nv_euclidean_distance.c

#include "nv_core.h"
#include "nv_num_distance.h"
#if NV_ENABLE_SSE2
#include <emmintrin.h>
#include <xmmintrin.h>
#endif

// ユークリッド距離
float nv_euclidean(const nv_matrix_t *vec1, int m1, const nv_matrix_t *vec2, int m2)
{
	return sqrtf(nv_euclidean2(vec1, m1, vec2, m2));
}

// ユークリッド距離^2
float nv_euclidean2(const nv_matrix_t *vec1, int m1, const nv_matrix_t *vec2, int m2)
{
	int n;
	float dist = 0.0f;

	assert(vec1->n == vec2->n);

#if NV_ENABLE_SSE2
	{
		NV_ALIGNED(float, mm[4], 16);
		__m128 w, x, u;
		int n;
		int pk_lp = (vec1->n & 0xfffffffc);
		int nm_lp = (vec1->n & 0x3);

		if (NV_VALID_SSE(vec1) && NV_VALID_SSE(vec2)) {
			u = _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f);
			for (n = 0; n < pk_lp; n += 4) {
				w = _mm_load_ps(&NV_MAT_V(vec1, m1, n));
				x = _mm_load_ps(&NV_MAT_V(vec2, m2, n));
				x = _mm_sub_ps(x, w);
				x = _mm_mul_ps(x, x);
				u = _mm_add_ps(u, x);
			}
			_mm_store_ps(mm, u);
			dist = mm[0] + mm[1] + mm[2] + mm[3];
		} else {
			u = _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f);
			for (n = 0; n < pk_lp; n += 4) {
				w = _mm_loadu_ps(&NV_MAT_V(vec1, m1, n));
				x = _mm_loadu_ps(&NV_MAT_V(vec2, m2, n));
				x = _mm_sub_ps(x, w);
				x = _mm_mul_ps(x, x);
				u = _mm_add_ps(u, x);
			}
			_mm_store_ps(mm, u);
			dist = mm[0] + mm[1] + mm[2] + mm[3];
			for (n = pk_lp; n < vec1->n; ++n) {
				dist += (NV_MAT_V(vec1, m1, n) - NV_MAT_V(vec2, m2, n)) 
					* (NV_MAT_V(vec1, m1, n) - NV_MAT_V(vec2, m2, n)) ;
			}
		}
	}
#else
	for (n = 0; n < vec1->n; ++n) {
		dist += (NV_MAT_V(vec1, m1, n) - NV_MAT_V(vec2, m2, n))
			* (NV_MAT_V(vec1, m1, n) - NV_MAT_V(vec2, m2, n));
	}
#endif
	return dist;
}