Torque3D/Engine/source/math/isa/sse2/float3.cpp

#include "float3_dispatch.h"
#include <emmintrin.h> // SSE2 intrinsics

namespace
{
   typedef __m128 f32x4;

   // Load 3 floats into 4-wide SIMD, zero the 4th lane
   inline f32x4 v_load3(const float* p) { return _mm_set_ps(0.0f, p[2], p[1], p[0]); }

   // Store 3 floats from SIMD register back to memory
   inline void v_store3(float* dst, f32x4 v)
   {
      alignas(16) float tmp[4];   // temp storage
      _mm_store_ps(tmp, v);        // store all 4 lanes
      dst[0] = tmp[0];
      dst[1] = tmp[1];
      dst[2] = tmp[2];
   }

   // extract just the first lane.
   inline float v_extract0(f32x4 v) { return _mm_cvtss_f32(v); }

   // Broadcast a single float across all 4 lanes
   inline f32x4 v_set1(float s) { return _mm_set1_ps(s); }

   // Element-wise multiply
   inline f32x4 v_mul(f32x4 a, f32x4 b) { return _mm_mul_ps(a, b); }

   // Element-wise divide fast (1/b)
   inline f32x4 v_div_fast(f32x4 a, f32x4 b)
   {
      f32x4 rcp = _mm_rcp_ps(b);
      // Optional refinement here
      return _mm_mul_ps(a, rcp);
   }

   // Element-wise divide (to change from fast use _mm_div_ps(a,b)
   inline f32x4 v_div(f32x4 a, f32x4 b) { return v_div_fast(a, b); }

   // Element-wise add
   inline f32x4 v_add(f32x4 a, f32x4 b) { return _mm_add_ps(a, b); }

   // Element-wise subtract
   inline f32x4 v_sub(f32x4 a, f32x4 b) { return _mm_sub_ps(a, b); }

   // Horizontal sum of all elements (for dot product, length, etc.)
   inline f32x4 v_hadd3(f32x4 a)
   {
      __m128 shuf = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 3, 0, 1));   // swap pairs
      __m128 sums = _mm_add_ps(a, shuf);                             // sums: [a0+a1 a1+a0 a2+a3 a3+a2]
      shuf = _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(1, 0, 3, 2));    // move high pair to low
      sums = _mm_add_ps(sums, shuf);                                 // total sum in lower float
      return sums;
   }

   inline f32x4 v_cross(f32x4 a, f32x4 b)
   {
      f32x4 a_yzx = _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1));
      f32x4 b_yzx = _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1));

      f32x4 c = _mm_sub_ps(
         _mm_mul_ps(a, b_yzx),
         _mm_mul_ps(a_yzx, b)
      );

      return _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 0, 2, 1));
   }
}

#include "float3_impl.inl"

namespace math_backend::float3::dispatch
{
   // Install SSE2 backend
   void install_sse2()
   {
      gFloat3.add = float3_add_impl;
      gFloat3.sub = float3_sub_impl;
      gFloat3.mul = float3_mul_impl;
      gFloat3.mul_scalar = float3_mul_scalar_impl;
      gFloat3.div = float3_div_impl;
      gFloat3.div_scalar = float3_div_scalar_impl;
      gFloat3.dot = float3_dot_impl;
      gFloat3.length = float3_length_impl;
      gFloat3.lengthSquared = float3_length_squared_impl;
      gFloat3.normalize = float3_normalize_impl;
      gFloat3.normalize_mag = float3_normalize_mag_impl;
      gFloat3.lerp = float3_lerp_impl;
      gFloat3.cross = float3_cross_impl;
   }
}