squared distance with SIMD support (#96)

squared distance for vec4

include/cglm/call/vec4.h

@@ -132,10 +132,14 @@ glmc_vec4_negate(vec4 v);
 CGLM_EXPORT
 void
 glmc_vec4_negate_to(vec4 v, vec4 dest);
-
+    
 CGLM_EXPORT
 float
 glmc_vec4_distance(vec4 a, vec4 b);
+    
+CGLM_EXPORT
+float
+glmc_vec4_distance2(vec4 a, vec4 b);
 
 CGLM_EXPORT
 void

include/cglm/simd/arm.h

@@ -37,5 +37,11 @@ glmm_norm(float32x4_t a) {
   return sqrtf(glmm_dot(a, a));
 }
 
+static inline
+float
+glmm_norm2(float32x4_t a) {
+  return glmm_dot(a, a);
+}
+
 #endif
 #endif /* cglm_simd_arm_h */

include/cglm/simd/x86.h

@@ -113,6 +113,12 @@ glmm_norm(__m128 a) {
   return _mm_cvtss_f32(_mm_sqrt_ss(glmm_vhadds(_mm_mul_ps(a, a))));
 }
 
+static inline
+float
+glmm_norm2(__m128 a) {
+  return _mm_cvtss_f32(glmm_vhadds(_mm_mul_ps(a, a)));
+}
+
 static inline
 __m128
 glmm_load3(float v[3]) {

include/cglm/struct/vec3.h

@@ -45,13 +45,13 @@
    CGLM_INLINE vec3s glms_vec3_normalize(vec3s v);
    CGLM_INLINE vec3s glms_vec3_cross(vec3s a, vec3s b);
    CGLM_INLINE vec3s glms_vec3_crossn(vec3s a, vec3s b);
-   CGLM_INLINE float glms_vec3_distance(vec3s a, vec3s b);
    CGLM_INLINE float glms_vec3_angle(vec3s a, vec3s b);
    CGLM_INLINE vec3s glms_vec3_rotate(vec3s v, float angle, vec3s axis);
    CGLM_INLINE vec3s glms_vec3_rotate_m4(mat4s m, vec3s v);
    CGLM_INLINE vec3s glms_vec3_rotate_m3(mat3s m, vec3s v);
    CGLM_INLINE vec3s glms_vec3_proj(vec3s a, vec3s b);
    CGLM_INLINE vec3s glms_vec3_center(vec3s a, vec3s b);
+   CGLM_INLINE float glms_vec3_distance(vec3s a, vec3s b);
    CGLM_INLINE float glms_vec3_distance2(vec3s a, vec3s b);
    CGLM_INLINE vec3s glms_vec3_maxv(vec3s a, vec3s b);
    CGLM_INLINE vec3s glms_vec3_minv(vec3s a, vec3s b);
@@ -598,19 +598,6 @@ glms_vec3_center(vec3s a, vec3s b) {
   return r;
 }
 
-/**
- * @brief squared distance between two vectors
- *
- * @param[in] a vector1
- * @param[in] b vector2
- * @return      squared distance (distance * distance)
- */
-CGLM_INLINE
-float
-glms_vec3_distance2(vec3s a, vec3s b) {
-  return glm_vec3_distance2(a.raw, b.raw);
-}
-
 /**
  * @brief distance between two vectors
  *
@@ -624,6 +611,19 @@ glms_vec3_distance(vec3s a, vec3s b) {
   return glm_vec3_distance(a.raw, b.raw);
 }
 
+/**
+ * @brief squared distance between two vectors
+ *
+ * @param[in] a vector1
+ * @param[in] b vector2
+ * @return      squared distance (distance * distance)
+ */
+CGLM_INLINE
+float
+glms_vec3_distance2(vec3s a, vec3s b) {
+  return glm_vec3_distance2(a.raw, b.raw);
+}
+
 /*!
  * @brief max values of vectors
  *

include/cglm/struct/vec4.h

@@ -43,6 +43,7 @@
    CGLM_INLINE vec4s glms_vec4_inv(vec4s v);
    CGLM_INLINE vec4s glms_vec4_normalize(vec4s v);
    CGLM_INLINE float glms_vec4_distance(vec4s a, vec4s b);
+   CGLM_INLINE float glms_vec4_distance2(vec4s a, vec4s b);
    CGLM_INLINE vec4s glms_vec4_maxv(vec4s a, vec4s b);
    CGLM_INLINE vec4s glms_vec4_minv(vec4s a, vec4s b);
    CGLM_INLINE vec4s glms_vec4_clamp(vec4s v, float minVal, float maxVal);
@@ -501,6 +502,19 @@ glms_vec4_distance(vec4s a, vec4s b) {
   return glm_vec4_distance(a.raw, b.raw);
 }
 
+/**
+ * @brief squared distance between two vectors
+ *
+ * @param[in] a vector1
+ * @param[in] b vector2
+ * @return returns squared distance
+ */
+CGLM_INLINE
+float
+glms_vec4_distance2(vec4s a, vec4s b) {
+  return glm_vec4_distance2(a.raw, b.raw);
+}
+
 /*!
  * @brief max values of vectors
  *

include/cglm/vec3.h

@@ -48,13 +48,13 @@
    CGLM_INLINE void  glm_vec3_normalize_to(vec3 v, vec3 dest);
    CGLM_INLINE void  glm_vec3_cross(vec3 a, vec3 b, vec3 d);
    CGLM_INLINE void  glm_vec3_crossn(vec3 a, vec3 b, vec3 dest);
-   CGLM_INLINE float glm_vec3_distance(vec3 a, vec3 b);
    CGLM_INLINE float glm_vec3_angle(vec3 a, vec3 b);
    CGLM_INLINE void  glm_vec3_rotate(vec3 v, float angle, vec3 axis);
    CGLM_INLINE void  glm_vec3_rotate_m4(mat4 m, vec3 v, vec3 dest);
    CGLM_INLINE void  glm_vec3_rotate_m3(mat3 m, vec3 v, vec3 dest);
    CGLM_INLINE void  glm_vec3_proj(vec3 a, vec3 b, vec3 dest);
    CGLM_INLINE void  glm_vec3_center(vec3 a, vec3 b, vec3 dest);
+   CGLM_INLINE float glm_vec3_distance(vec3 a, vec3 b);
    CGLM_INLINE float glm_vec3_distance2(vec3 a, vec3 b);
    CGLM_INLINE void  glm_vec3_maxv(vec3 a, vec3 b, vec3 dest);
    CGLM_INLINE void  glm_vec3_minv(vec3 a, vec3 b, vec3 dest);
@@ -694,9 +694,9 @@ glm_vec3_center(vec3 a, vec3 b, vec3 dest) {
 CGLM_INLINE
 float
 glm_vec3_distance2(vec3 a, vec3 b) {
-  return glm_pow2(b[0] - a[0])
+  return glm_pow2(a[0] - b[0])
-       + glm_pow2(b[1] - a[1])
+       + glm_pow2(a[1] - b[1])
-       + glm_pow2(b[2] - a[2]);
+       + glm_pow2(a[2] - b[2]);
 }
 
 /**

include/cglm/vec4.h

@@ -43,6 +43,7 @@
    CGLM_INLINE void  glm_vec4_normalize(vec4 v);
    CGLM_INLINE void  glm_vec4_normalize_to(vec4 vec, vec4 dest);
    CGLM_INLINE float glm_vec4_distance(vec4 a, vec4 b);
+   CGLM_INLINE float glm_vec4_distance2(vec4 a, vec4 b);
    CGLM_INLINE void  glm_vec4_maxv(vec4 a, vec4 b, vec4 dest);
    CGLM_INLINE void  glm_vec4_minv(vec4 a, vec4 b, vec4 dest);
    CGLM_INLINE void  glm_vec4_clamp(vec4 v, float minVal, float maxVal);
@@ -697,14 +698,36 @@ CGLM_INLINE
 float
 glm_vec4_distance(vec4 a, vec4 b) {
 #if defined( __SSE__ ) || defined( __SSE2__ )
-  return glmm_norm(_mm_sub_ps(glmm_load(b), glmm_load(a)));
+  return glmm_norm(_mm_sub_ps(glmm_load(a), glmm_load(b)));
 #elif defined(CGLM_NEON_FP)
   return glmm_norm(vsubq_f32(glmm_load(a), glmm_load(b)));
 #else
-  return sqrtf(glm_pow2(b[0] - a[0])
+  return sqrtf(glm_pow2(a[0] - b[0])
-             + glm_pow2(b[1] - a[1])
+             + glm_pow2(a[1] - b[1])
-             + glm_pow2(b[2] - a[2])
+             + glm_pow2(a[2] - b[2])
-             + glm_pow2(b[3] - a[3]));
+             + glm_pow2(a[3] - b[3]));
+#endif
+}
+
+/**
+ * @brief squared distance between two vectors
+ *
+ * @param[in] a vector1
+ * @param[in] b vector2
+ * @return returns squared distance
+ */
+CGLM_INLINE
+float
+glm_vec4_distance2(vec4 a, vec4 b) {
+#if defined( __SSE__ ) || defined( __SSE2__ )
+  return glmm_norm2(_mm_sub_ps(glmm_load(a), glmm_load(b)));
+#elif defined(CGLM_NEON_FP)
+  return glmm_norm2(vsubq_f32(glmm_load(a), glmm_load(b)));
+#else
+  return glm_pow2(a[0] - b[0])
+       + glm_pow2(a[1] - b[1])
+       + glm_pow2(a[2] - b[2])
+       + glm_pow2(a[3] - b[3]);
 #endif
 }
 

src/vec3.c

@@ -214,14 +214,14 @@ glmc_vec3_center(vec3 a, vec3 b, vec3 dest) {
 
 CGLM_EXPORT
 float
-glmc_vec3_distance2(vec3 a, vec3 b) {
+glmc_vec3_distance(vec3 a, vec3 b) {
-  return glm_vec3_distance2(a, b);
+  return glm_vec3_distance(a, b);
 }
 
 CGLM_EXPORT
 float
-glmc_vec3_distance(vec3 a, vec3 b) {
+glmc_vec3_distance2(vec3 a, vec3 b) {
-  return glm_vec3_distance(a, b);
+  return glm_vec3_distance2(a, b);
 }
 
 CGLM_EXPORT

src/vec4.c

@@ -182,6 +182,12 @@ glmc_vec4_distance(vec4 a, vec4 b) {
   return glm_vec4_distance(a, b);
 }
 
+CGLM_EXPORT
+float
+glmc_vec4_distance2(vec4 a, vec4 b) {
+  return glm_vec4_distance2(a, b);
+}
+
 CGLM_EXPORT
 void
 glmc_vec4_maxv(vec4 a, vec4 b, vec4 dest) {