From 89d96bbdf96c8c4953c8c17c9ef34960e24d1471 Mon Sep 17 00:00:00 2001
From: THoehne <77296181+THoehne@users.noreply.github.com>
Date: Sun, 13 Oct 2024 17:54:58 +0200
Subject: [PATCH] Add SIMD to Pmath::Plane.

---
 .../Core/public/Math/Detail/PlaneDecl.inl     |  9 --
 .../Core/public/Math/Detail/Vector4Decl.inl   |  7 ++
 .../Source/Runtime/Core/public/Math/Plane.hpp | 17 +---
 .../Source/Runtime/Core/public/Math/Plane.inl | 12 ---
 .../public/Math/SIMD/PhanesVectorMathSSE.hpp  | 84 ++++++++++++++++++-
 MathTestFPU/test.cpp                          |  4 +-
 6 files changed, 93 insertions(+), 40 deletions(-)

diff --git a/Engine/Source/Runtime/Core/public/Math/Detail/PlaneDecl.inl b/Engine/Source/Runtime/Core/public/Math/Detail/PlaneDecl.inl
index fd4b606..eb7b740 100644
--- a/Engine/Source/Runtime/Core/public/Math/Detail/PlaneDecl.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Detail/PlaneDecl.inl
@@ -24,15 +24,6 @@ namespace Phanes::Core::Math::Detail
     template<RealType T>
     struct construct_plane<T, false>
     {
-        static constexpr void map(TPlane<T, false>& r, const TPlane<T, false>& pl1)
-        {
-            r.comp = std::copy(pl1.comp);
-        }
-
-        static constexpr void map(TPlane<T, false>& r, TPlane<T, false>&& pl1)
-        {
-            r.comp = std::move(pl1.comp);
-        }
 
         static constexpr void map(TPlane<T, false>& r, const TVector3<T, false>& normal, T d)
         {
diff --git a/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl b/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
index d4ae4bd..2a3a3b7 100644
--- a/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
@@ -93,6 +93,13 @@ namespace Phanes::Core::Math::Detail
             r.w = v3.y;
         }
 
+        static constexpr void map(Phanes::Core::Math::TVector4<T, false>& r, const Phanes::Core::Math::TVector3<T, false>& v, T w)
+        {
+            r.x = v.x;
+            r.y = v.y;
+            r.z = v.z;
+            r.w = w;
+        }
 
         static constexpr void map(Phanes::Core::Math::TVector4<T, false>& r, const T* comp)
         {
diff --git a/Engine/Source/Runtime/Core/public/Math/Plane.hpp b/Engine/Source/Runtime/Core/public/Math/Plane.hpp
index 84bdc81..56c9e35 100644
--- a/Engine/Source/Runtime/Core/public/Math/Plane.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Plane.hpp
@@ -62,19 +62,6 @@ namespace Phanes::Core::Math {
 
         TPlane() = default;
 
-        /**
-         * Copy constructor
-         */
-
-        TPlane(const TPlane<Real, S>& plane);
-
-        /**
-         * Move constructor 
-         */
-
-        TPlane(TPlane<Real, S>&& plane);
-
-
         /**
          * Construct plane from normal and d 
          * 
@@ -96,7 +83,7 @@ namespace Phanes::Core::Math {
         TPlane(const TVector3<Real, S>& normal, const TVector3<Real, S>& base);
 
         /**
-         * Construct plane from coefficients
+         * Construct plane from coefficients. The components should be normalized.
          * 
          * @param(x) X coefficient
          * @param(y) Y coefficient
@@ -911,7 +898,7 @@ namespace Phanes::Core::Math {
     template<RealType T>
     TVector3<T, false> PointProjectOntoPlane(const TVector3<T, false>& p1, const TPlane<T, false>& pl1)
     {
-        p1 - PointDistance(pl1, p1) * pl1.normal;
+        return p1 - PointDistance(pl1, p1) * pl1.normal;
     }
 
     /**
diff --git a/Engine/Source/Runtime/Core/public/Math/Plane.inl b/Engine/Source/Runtime/Core/public/Math/Plane.inl
index 01aa78b..9cc77dc 100644
--- a/Engine/Source/Runtime/Core/public/Math/Plane.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Plane.inl
@@ -11,18 +11,6 @@
 
 namespace Phanes::Core::Math
 {
-    template<RealType T, bool S>
-    TPlane<T, S>::TPlane(const TPlane<T, S>& plane)
-    {
-        Detail::construct_plane<T, S>::map(*this, plane);
-    }
-
-    template<RealType T, bool S>
-    TPlane<T, S>::TPlane(TPlane<T, S>&& plane)
-    {
-        Detail::construct_plane<T, S>::map(*this, plane);
-    }
-
     template<RealType T, bool S>
     TPlane<T, S>::TPlane(const TVector3<T, S>& normal, Real d)
     {
diff --git a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
index 203d5e5..26ff481 100644
--- a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
@@ -11,6 +11,8 @@
 #include "Core/public/Math/Vector3.hpp"
 #include "Core/public/Math/Vector4.hpp"
 
+#include "Core/public/Math/Plane.hpp"
+
 #include "Core/public/Math/IntVector2.hpp"
 #include "Core/public/Math/IntVector3.hpp"
 #include "Core/public/Math/IntVector4.hpp"
@@ -19,6 +21,7 @@
 #include "Core/public/Math/Matrix4.hpp"
 
 
+
 // ========== //
 //   Common   //
 // ========== //
@@ -110,7 +113,7 @@ namespace Phanes::Core::Math::SIMD
     /// <param name="v1"></param>
     void vec3_fix(Phanes::Core::Types::Vec4f32Reg v1)
     {
-        v1 = _mm_blend_ps(v1, _mm_setzero_ps(), 0x1);
+        v1.m128_f32[3] = 0.0f;
     }
 }
 
@@ -155,6 +158,11 @@ namespace Phanes::Core::Math::Detail
         {
             v1.comp = _mm_loadu_ps(s);
         }
+
+        static FORCEINLINE void map(Phanes::Core::Math::TVector4<float, true>& r, const Phanes::Core::Math::TVector3<float, true>& v, float w)
+        {
+            r.comp = _mm_set_ps(w, v.z, v.y, v.x);
+        }
     };
 
     template<>
@@ -302,8 +310,8 @@ namespace Phanes::Core::Math::Detail
     {
         static FORCEINLINE void map(Phanes::Core::Math::TVector4<float, true>& r, const Phanes::Core::Math::TVector4<float, true>& v1)
         {
-            __m128 tmp = _mm_div_ps(v1.data, _mm_set_ps1(v1.w));
-            r.data = _mm_blend_ps(tmp, _mm_setzero_ps(), 0x1);
+            r.data = _mm_div_ps(v1.data, _mm_set_ps1(v1.w));
+            r.w = 0.0f;
         }
     };
 
@@ -553,6 +561,76 @@ namespace Phanes::Core::Math::Detail
     };
 
 
+    // ========= //
+    //   Plane   //
+    // ========= //
+
+    template<>
+    struct construct_plane<float, true> 
+    {
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& pl, const TVector3<float, true>& v1, float d)
+        {
+            pl.comp.data = v1.data;
+            pl.comp.w = d;
+        }
+
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& pl, const TVector3<float, true>& normal, const TVector3<float, true>& base)
+        {
+            pl.comp.data = normal.data;
+            pl.comp.w = DotP(normal, base);
+        }
+
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& pl, float x, float y, float z, float d)
+        {
+
+            pl.comp.data = _mm_set_ps(x, y, z, d);
+        }
+
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& pl, const TVector3<float, true>& v1, const TVector3<float, true>& v2, const TVector3<float, true>& v3)
+        {
+            TVector4<float, false> tmp;
+            
+        }
+        
+                
+    };
+
+    template<>
+    struct compute_plane_add<float, true> 
+    {
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& r, Phanes::Core::Math::TPlane<float, true>& pl1, Phanes::Core::Math::TPlane<float, true>& pl2)
+        {
+            r.comp.data = _mm_add_ps(pl1.comp.data, pl2.comp.data);
+        }
+    };
+
+    template<>
+    struct compute_plane_sub<float, true> 
+    {
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& r, Phanes::Core::Math::TPlane<float, true>& pl1, Phanes::Core::Math::TPlane<float, true>& pl2)
+        {
+            r.comp.data = _mm_sub_ps(pl1.comp.data, pl2.comp.data);
+        }
+    };
+
+    template<>
+    struct compute_plane_mul<float, true> 
+    {
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& r, Phanes::Core::Math::TPlane<float, true>& pl1, Phanes::Core::Math::TPlane<float, true>& pl2)
+        {
+            r.comp.data = _mm_mul_ps(pl1.comp.data, pl2.comp.data);
+        }
+    };
+
+    template<>
+    struct compute_plane_div<float, true> 
+    {
+        static FORCEINLINE void map(Phanes::Core::Math::TPlane<float, true>& r, Phanes::Core::Math::TPlane<float, true>& pl1, Phanes::Core::Math::TPlane<float, true>& pl2)
+        {
+            r.comp.data = _mm_div_ps(pl1.comp.data, pl2.comp.data);
+        }
+    };
+
     // =============== //
     //   TIntVector4   //
     // =============== //
diff --git a/MathTestFPU/test.cpp b/MathTestFPU/test.cpp
index bf21292..a092999 100644
--- a/MathTestFPU/test.cpp
+++ b/MathTestFPU/test.cpp
@@ -1262,6 +1262,8 @@ namespace Plane
 {
     TEST(Plane, OperatorTests)
     {
-        
+        PMath::Plane pl1(3.0f / 5.4772255750f, -2.0f / 5.4772255750f, -3.0f / 5.4772255750f, 4.0f);
+        PMath::Plane pl2(-0.526316f, -0.442105f, -0.726316f, 6.0f);
+
     }
 }
\ No newline at end of file