From 7a1fb0daa3616698496ffbf0914b48ae6dc91430 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Lo=C3=AFc=20GUEZO?= <loicguezo@gmail.com>
Date: Sat, 21 Jun 2025 22:45:07 +0200
Subject: [PATCH] feat: add dot function

---
 src/math/vec3.c | 14 ++++++------
 src/math/vec3.h | 10 ++++-----
 src/math/vec4.c | 60 +++++++++++++++++++++++++++++++------------------
 src/math/vec4.h | 10 ++++-----
 4 files changed, 55 insertions(+), 39 deletions(-)

diff --git a/src/math/vec3.c b/src/math/vec3.c
index 4dbc9eb..39ef50c 100644
--- a/src/math/vec3.c
+++ b/src/math/vec3.c
@@ -15,7 +15,7 @@ Vec3f_t vec3f(float x, float y, float z)
     return (Vec3f_t){.x = x, .y = y, .z = z};
 }
 
-Vec3f_t vec3f_from_array(const float *restrict val)
+Vec3f_t vec3f_from_array(const float *__restrict val)
 {
     Vec3f_t vec;
 #if defined (SIMD_X86)
@@ -54,7 +54,7 @@ Vec3f_t vec3f_zero(void)
     return vec3f_scalar(0.f);
 }
 
-Vec3f_t vec3f_add_r(Vec3f_t *restrict out, Vec3f_t a)
+Vec3f_t vec3f_add_r(Vec3f_t *__restrict out, Vec3f_t a)
 {
 #if defined (SIMD_X86)
     __m128 va = _mm_load_ps(a.data);
@@ -79,7 +79,7 @@ Vec3f_t vec3f_add(Vec3f_t a, Vec3f_t b)
     return vec3f_add_r(&a, b);
 }
 
-Vec3f_t vec3f_sub_r(Vec3f_t *restrict out, Vec3f_t a)
+Vec3f_t vec3f_sub_r(Vec3f_t *__restrict out, Vec3f_t a)
 {
 #if defined (SIMD_X86)
     __m128 va = _mm_load_ps(out->data);
@@ -104,7 +104,7 @@ Vec3f_t vec3f_sub(Vec3f_t a, Vec3f_t b)
     return vec3f_sub_r(&a, b);
 }
 
-Vec3f_t vec3f_scale_r(Vec3f_t *restrict out, float scalar)
+Vec3f_t vec3f_scale_r(Vec3f_t *__restrict out, float scalar)
 {
 #if defined (SIMD_X86)
     __m128 va = _mm_load_ps(out->data);
@@ -130,11 +130,11 @@ Vec3f_t vec3f_scale(Vec3f_t a, float scalar)
 }
 
 
-//Vec3f_t vec3f_add_r(Vec3f_t *restrict out, Vec3f_t a);
+//Vec3f_t vec3f_add_r(Vec3f_t *__restrict out, Vec3f_t a);
 //Vec3f_t vec3f_add(Vec3f_t a, Vec3f_t b);
 //
-//Vec3f_t vec3f_sub_r(Vec3f_t *restrict out, Vec3f_t a);
+//Vec3f_t vec3f_sub_r(Vec3f_t *__restrict out, Vec3f_t a);
 //Vec3f_t vec3f_sub(Vec3f_t a, Vec3f_t b);
 //
-//Vec3f_t vec3f_scale_r(Vec3f_t *restrict out, float scale);
+//Vec3f_t vec3f_scale_r(Vec3f_t *__restrict out, float scale);
 //Vec3f_t vec3f_scale(Vec3f_t a, float scale);
diff --git a/src/math/vec3.h b/src/math/vec3.h
index 3ffd2e6..544d0ff 100644
--- a/src/math/vec3.h
+++ b/src/math/vec3.h
@@ -16,25 +16,25 @@ typedef union
     float data[4];
 } ALIGN16 Vec3f_t;
 
-Vec3f_t vec3f_from_array(const float *restrict val);
+Vec3f_t vec3f_from_array(const float *__restrict val);
 Vec3f_t vec3f(float x, float y, float z);
 // (f, f, f)
 Vec3f_t vec3f_scalar(float f);
 // (0, 0, 0)
 Vec3f_t vec3f_zero(void);
 
-inline static Vec3f_t vec3f_clone(const Vec3f_t *restrict v)
+inline static Vec3f_t vec3f_clone(const Vec3f_t *__restrict v)
 {
     return *v;
 }
 
-Vec3f_t vec3f_add_r(Vec3f_t *restrict out, Vec3f_t a);
+Vec3f_t vec3f_add_r(Vec3f_t *__restrict out, Vec3f_t a);
 Vec3f_t vec3f_add(Vec3f_t a, Vec3f_t b);
 
-Vec3f_t vec3f_sub_r(Vec3f_t *restrict out, Vec3f_t a);
+Vec3f_t vec3f_sub_r(Vec3f_t *__restrict out, Vec3f_t a);
 Vec3f_t vec3f_sub(Vec3f_t a, Vec3f_t b);
 
-Vec3f_t vec3f_scale_r(Vec3f_t *restrict out, float scale);
+Vec3f_t vec3f_scale_r(Vec3f_t *__restrict out, float scale);
 Vec3f_t vec3f_scale(Vec3f_t a, float scale);
 
 #endif /* vec3_h */
diff --git a/src/math/vec4.c b/src/math/vec4.c
index 59c267a..4a8c101 100644
--- a/src/math/vec4.c
+++ b/src/math/vec4.c
@@ -7,7 +7,7 @@ Vec4f_t vec4f(float x, float y, float z, float w)
     return (Vec4f_t){.x = x, .y = y, .z = z, .w = w};
 }
 
-Vec4f_t vec4f_from_array(const float *restrict val)
+Vec4f_t vec4f_from_array(const float *__restrict val)
 {
     Vec4f_t vec;
 #if defined (SIMD_X86)
@@ -52,7 +52,7 @@ Vec4f_t vec4f_zero(void)
     return vec4f_scalar(0.f);
 }
 
-Vec4f_t vec4f_add_r(Vec4f_t *restrict out, Vec4f_t a)
+Vec4f_t vec4f_add_r(Vec4f_t *__restrict out, Vec4f_t a)
 {
 #if defined (SIMD_X86)
     __m128 va = _mm_load_ps(a.data);
@@ -79,7 +79,7 @@ Vec4f_t vec4f_add(Vec4f_t a, Vec4f_t b)
     return a;
 }
 
-Vec4f_t vec4f_sub_r(Vec4f_t *restrict out, Vec4f_t a)
+Vec4f_t vec4f_sub_r(Vec4f_t *__restrict out, Vec4f_t a)
 {
 #if defined (SIMD_X86)   
     __m128 va = _mm_load_ps(out->data);
@@ -107,7 +107,7 @@ Vec4f_t vec4f_sub(Vec4f_t a, Vec4f_t b)
     return a;
 }
 
-Vec4f_t vec4f_scale_r(Vec4f_t *restrict out, float scalar)
+Vec4f_t vec4f_scale_r(Vec4f_t *__restrict out, float scalar)
 {
 #if defined (SIMD_X86)
     __m128 va = _mm_load_ps(out->data);
@@ -134,24 +134,40 @@ Vec4f_t vec4f_scale(Vec4f_t a, float scalar)
     return a;
 }
 
-//float vec4f_dot(Vec4f_t a, Vec4f_t b)
-//{
-//    float result;
-//#if defined (SIMD_X86)
-//    __m128 va = _mm_load_ps(a.data);
-//    __m128 vb = _mm_load_ps(b.data);
-//    __m128 vres = _mm_mul_ps(va, vb);
-//    
-//    __m128 shuf = 
-//    result = 0.f;
-//    
-//#elif defined (SIMD_ARCH)
-//    result = 0.f;
-//#else
-//    result = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
-//#endif
-//    return result;
-//}
+float vec4f_dot(Vec4f_t a, Vec4f_t b)
+{
+    float result;
+#if defined (SIMD_X86)
+    __m128 va = _mm_load_ps(a.data);
+    __m128 vb = _mm_load_ps(b.data);
+    __m128 vmul = _mm_mul_ps(va, vb);
+    
+//    [y*y y*y, w*w, w*w]
+    __m128 shuf = _mm_movehdup_ps(vmul);
+//    [x*x+y*y, y*y+y*y, z*z+w*w, w*w+w*w]
+    __m128 sum = _mm_add_ps(vmul, shuf);
+    
+//    [z*z+w*w, w*w+w*w, ?, ?]
+    shuf = _mm_movehl_ps(shuf, sum);
+//    [x*x+y*y+z*z+w*w, ?, ?, ?]
+    sum = _mm_add_ss(sums, shuf);
+    
+    result = __mm_cvtss_f32(sum);
+    
+#elif defined (SIMD_ARCH)
+    float32x4_t va = vld1q_f32(a.data);
+    float32x4_t vb = vld1q_f32(b.data);
+    
+    float32x4_t vmul = vmulq_f32(va, vb);
+    float32x2_t sum_pair = vadd_f32(vget_low_f32(vmul), vget_high_f32(vmul));
+    float32x2_t final_sum = vpadd_f32(sum_pair, sum_pair);
+    
+    result = vget_lane_f32(final_sum, 0);
+#else
+    result = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+#endif
+    return result;
+}
 
 // float vec4_dot(Vec4_t a, Vec4_t b)
 // {
diff --git a/src/math/vec4.h b/src/math/vec4.h
index de56081..a746532 100644
--- a/src/math/vec4.h
+++ b/src/math/vec4.h
@@ -10,25 +10,25 @@ typedef union
     float data[4];
 } ALIGN16 Vec4f_t;
 
-Vec4f_t vec4f_from_array(const float *restrict val);
+Vec4f_t vec4f_from_array(const float *__restrict val);
 Vec4f_t vec4f(float x, float y, float z, float w);
 // (f, f, f, f)
 Vec4f_t vec4f_scalar(float f);
 // (0, 0, 0, 0)
 Vec4f_t vec4f_zero(void);
 
-inline static Vec4f_t vec4f_clone(const Vec4f_t *restrict v)
+inline static Vec4f_t vec4f_clone(const Vec4f_t *__restrict v)
 {
     return *v;
 }
 
-Vec4f_t vec4f_add_r(Vec4f_t *restrict out, Vec4f_t a);
+Vec4f_t vec4f_add_r(Vec4f_t *__restrict out, Vec4f_t a);
 Vec4f_t vec4f_add(Vec4f_t a, Vec4f_t b);
 
-Vec4f_t vec4f_sub_r(Vec4f_t *restrict out, Vec4f_t a);
+Vec4f_t vec4f_sub_r(Vec4f_t *__restrict out, Vec4f_t a);
 Vec4f_t vec4f_sub(Vec4f_t a, Vec4f_t b);
 
-Vec4f_t vec4f_scale_r(Vec4f_t *restrict out, float scale);
+Vec4f_t vec4f_scale_r(Vec4f_t *__restrict out, float scale);
 Vec4f_t vec4f_scale(Vec4f_t a, float scale);
 
 float vec4_dot(Vec4f_t a, Vec4f_t b);