From 0fe66ad44a0e72a814830253de56ad1cac4749c9 Mon Sep 17 00:00:00 2001
From: THoehne <77296181+THoehne@users.noreply.github.com>
Date: Wed, 24 Jul 2024 23:01:25 +0200
Subject: [PATCH] Big fixes.

---
 .../Core/public/Math/Detail/Vector4Decl.inl   |   5 +-
 .../Core/public/Math/MathTypeConversion.hpp   |  12 +++
 .../Runtime/Core/public/Math/Vector2.hpp      |   4 +-
 .../Runtime/Core/public/Math/Vector3.hpp      |   6 +-
 .../Runtime/Core/public/Math/Vector4.hpp      | 101 +++++++++++++-----
 .../Runtime/Core/public/Math/Vector4.inl      |  16 +--
 6 files changed, 106 insertions(+), 38 deletions(-)

diff --git a/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl b/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
index 09e8d00..4c9d819 100644
--- a/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Detail/Vector4Decl.inl
@@ -38,7 +38,10 @@ namespace Phanes::Core::Math::Detail
     {
         static constexpr void map(Phanes::Core::Math::TVector4<T, false>& v1, const TVector4<T, false>& v2)
         {
-            memcpy(v1.data, v2.data, 4 * sizeof(T));
+            v1.x = v2.x;
+            v1.y = v2.y;
+            v1.z = v2.z;
+            v1.w = v2.w;
         }
 
 
diff --git a/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp b/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
index c468fae..39028c4 100644
--- a/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
@@ -86,6 +86,18 @@ namespace Phanes::Core::Math {
       return "(" + ToString(v.x) + ", " + ToString(v.y) + ", " + ToString(v.z) + ")";
   }
 
+  template<RealType T, bool S>
+  std::string ToString(const TVector4<T, S>& v)
+  {
+      return "(" + ToString(v.x) + ", " + ToString(v.y) + ", " + ToString(v.z) + ", " + ToString(v.w) + ")";
+  }
+
+  template<IntType T, bool S>
+  std::string ToString(const TIntVector4<T, S>& v)
+  {
+      return "(" + ToString(v.x) + ", " + ToString(v.y) + ", " + ToString(v.z) + ", " + ToString(v.w) + ")";
+  }
+
   //std::string toString(const Vector4& v);
 
 
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector2.hpp b/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
index faebf52..4c5bacf 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
@@ -420,7 +420,7 @@ namespace Phanes::Core::Math {
     TVector2<T, false> NormalizeV(TVector2<T, false>& v1)
     {
         float vecNorm = Magnitude(v1);
-        v1 /= (vecNorm < P_FLT_INAC) ? 1 : vecNorm;
+        v1 /= (vecNorm < P_FLT_INAC) ? (T)1.0 : vecNorm;
         return v1;
     }
 
@@ -953,7 +953,7 @@ namespace Phanes::Core::Math {
     TVector2<T, false> Normalize(const TVector2<T, false>& v1)
     {
         float vecNorm = Magnitude(v1);
-        return (vecNorm < P_FLT_INAC) ? PZeroVector2(T, false) : (v1 / vecNorm);
+        return (vecNorm < P_FLT_INAC) ? v1 : (v1 / vecNorm);
     }
 
     /**
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector3.hpp b/Engine/Source/Runtime/Core/public/Math/Vector3.hpp
index 37d2f5b..7ea7282 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector3.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Vector3.hpp
@@ -398,7 +398,7 @@ namespace Phanes::Core::Math {
     TVector3<T, false> NormalizeV(TVector3<T, false>& v1)
     {
         float vecNorm = Magnitude(v1);
-        v1 /= (vecNorm < P_FLT_INAC) ? 1 : vecNorm;
+        v1 /= (vecNorm < P_FLT_INAC) ? (T)1.0 : vecNorm;
 
         return v1;
     }
@@ -914,7 +914,7 @@ namespace Phanes::Core::Math {
     template<RealType T>
     inline bool IsNormalized(const TVector3<T, false>& v1, T threshold = P_FLT_INAC)
     {
-        return (abs(SqrMagnitude(v1) - 1) < threshold);
+        return (SqrMagnitude(v1) - 1 < threshold);
     }
 
     /**
@@ -962,7 +962,7 @@ namespace Phanes::Core::Math {
     TVector3<T, false> Normalize(const TVector3<T, false>& v1)
     {
         float vecNorm = Magnitude(v1);
-        return (vecNorm < P_FLT_INAC) ? PZeroVector3(T, false) : v1 / vecNorm;
+        return (vecNorm < P_FLT_INAC) ? v1 : v1 / vecNorm;
     }
 
     /**
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector4.hpp b/Engine/Source/Runtime/Core/public/Math/Vector4.hpp
index 5457030..ef9c018 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector4.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Vector4.hpp
@@ -10,7 +10,7 @@
 
 #include "Core/public/Math/Vector2.hpp"
 
-#define PZeroVector4(type, aligned)			Phanes::Core::Math::TVector4<##type, ##aligned>(0,0,0)
+#define PZeroVector4(type, aligned)			Phanes::Core::Math::TVector4<##type, ##aligned>(0,0,0,0)
 
 namespace Phanes::Core::Math
 {
@@ -203,7 +203,7 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector two</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator+ (TVector4<T, A>& v1, const TVector4<T, A>& v2);
+    TVector4<T, A> operator+ (const TVector4<T, A>& v1, const TVector4<T, A>& v2);
 
     /// <summary>
     /// Vector - scalar addition.
@@ -214,7 +214,7 @@ namespace Phanes::Core::Math
     /// <param name="s">Scalar</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator+ (TVector4<T, A>& v1, T s);
+    TVector4<T, A> operator+ (const TVector4<T, A>& v1, T s);
 
     /// <summary>
     /// Vector substraction.
@@ -225,7 +225,7 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector two</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator- (TVector4<T, A>& v1, const TVector4<T, A>& v2);
+    TVector4<T, A> operator- (const TVector4<T, A>& v1, const TVector4<T, A>& v2);
 
     /// <summary>
     /// Vector - scalar substraction.
@@ -236,7 +236,7 @@ namespace Phanes::Core::Math
     /// <param name="s">Scalar</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator- (TVector4<T, A>& v1, T s);
+    TVector4<T, A> operator- (const TVector4<T, A>& v1, T s);
 
     /// <summary>
     /// Vector - scalar multiplication.
@@ -247,7 +247,10 @@ namespace Phanes::Core::Math
     /// <param name="s">Scalar</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator* (TVector4<T, A>& v1, T s);
+    TVector4<T, A> operator* (const TVector4<T, A>& v1, T s);
+
+    template<RealType T, bool A>
+    FORCEINLINE TVector4<T, A> operator* (T s, const TVector4<T, A>& v1) { return v1 * s; };
 
     /// <summary>
     /// Scale vector by another vector componentwise.
@@ -258,7 +261,7 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector two</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator* (TVector4<T, A>& v1, const TVector4<T, A>& v2);
+    TVector4<T, A> operator* (const TVector4<T, A>& v1, const TVector4<T, A>& v2);
 
     /// <summary>
     /// Vector - scalar division.
@@ -269,7 +272,10 @@ namespace Phanes::Core::Math
     /// <param name="s">Scalar</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator/ (TVector4<T, A>& v1, T s);
+    TVector4<T, A> operator/ (const TVector4<T, A>& v1, T s);
+
+    template<RealType T, bool A>
+    FORCEINLINE TVector4<T, A> operator/ (T s, const TVector4<T, A>& v1) { return v1 / s; };
 
     /// <summary>
     /// Componentwise vector division.
@@ -280,7 +286,7 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector two</param>
     /// <returns>Computed vector.</returns>
     template<RealType T, bool A>
-    TVector4<T, A> operator/ (TVector4<T, A>& v1, const TVector4<T, A>& v2);
+    TVector4<T, A> operator/ (const TVector4<T, A>& v1, const TVector4<T, A>& v2);
 
 
 
@@ -460,9 +466,9 @@ namespace Phanes::Core::Math
     {
         T vecNorm = Magnitude(v1);
 
-        vecNorm = (vecNorm < P_FLT_INAC) ? Magnitude(v1) : (T)1.0;
+        vecNorm = (vecNorm < P_FLT_INAC) ? (T)1.0 : vecNorm;
 
-        return v1 / vecNorm;
+        return (v1 / vecNorm);
     }
 
     /// <summary>
@@ -477,7 +483,7 @@ namespace Phanes::Core::Math
     {
         T vecNorm = Magnitude(v1);
 
-        vecNorm = (vecNorm < P_FLT_INAC) ? Magnitude(v1) : (T)1.0;
+        vecNorm = (vecNorm < P_FLT_INAC) ? (T)1.0 : vecNorm;
 
         v1 /= vecNorm;
 
@@ -635,6 +641,8 @@ namespace Phanes::Core::Math
         v1.y = -v1.y;
         v1.z = -v1.z;
         v1.w = -v1.w;
+
+        return v1;
     }
 
     /// <summary>
@@ -686,7 +694,8 @@ namespace Phanes::Core::Math
     {
         T magnitude = Magnitude(v1);
 
-        const TVector3<T, false> unitVec = (magnitude > P_FLT_INAC) ? v1 / magnitude : PZeroVector3(T, false);
+        TVector4<T, false> unitVec;
+        unitVec = (magnitude > P_FLT_INAC) ? v1 / magnitude : PZeroVector4(T, false);
 
         Clamp(magnitude, min, max);
 
@@ -706,7 +715,7 @@ namespace Phanes::Core::Math
     {
         T magnitude = Magnitude(v1);
 
-        v1 = (magnitude > P_FLT_INAC) ? v1 / magnitude : PZeroVector3(T, false);
+        v1 = (magnitude > P_FLT_INAC) ? v1 / magnitude : PZeroVector4(T, false);
 
         v1 *= Clamp(magnitude, min, max);
 
@@ -738,7 +747,8 @@ namespace Phanes::Core::Math
     template<RealType T>
     TVector4<T, false> ScaleToMagnitudeV(TVector4<T, false>& v1, T s)
     {
-        NormalizeV(v1) *= s;
+        NormalizeV(v1);
+        v1 *= s;
         return v1;
     }
 
@@ -753,7 +763,7 @@ namespace Phanes::Core::Math
     template<RealType T>
     TVector4<T, false> Reflect(const TVector4<T, false>& v1, const TVector4<T, false> normal)
     {
-        return v1 - (2 * DotP(v1, normal) * normal);
+        return (2 * DotP(v1, normal) * normal) - v1;
     }
 
     /// <summary>
@@ -767,7 +777,7 @@ namespace Phanes::Core::Math
     template<RealType T>
     TVector4<T, false> ReflectV(TVector4<T, false>& v1, const TVector4<T, false> normal)
     {
-        Set(v1, v1 - (2 * DotP(v1, normal) * normal));
+        v1 = (2 * DotP(v1, normal) * normal) - v1;
         return v1;
     }
 
@@ -794,9 +804,11 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector to project on</param>
     /// <returns>Copy of v1.</returns>
     template<RealType T>
-    TVector4<T, false> ProjectV(const TVector4<T, false>& v1, const TVector4<T, false> v2)
+    TVector4<T, false> ProjectV(TVector4<T, false>& v1, const TVector4<T, false> v2)
     {
-        Set(v1, (DotP(v1, v2) / DotP(v2, v2)) * v2);
+        v1 = (DotP(v1, v2) / DotP(v2, v2)) * v2;
+
+        return v1;
     }
 
     /// <summary>
@@ -810,7 +822,7 @@ namespace Phanes::Core::Math
     template<RealType T>
     TVector4<T, false> Reject(const TVector4<T, false>& v1, const TVector4<T, false> v2)
     {
-        return v1 - (DotP(v1, v2) / DotP(v2, v2))* v2;
+        return v1 - (DotP(v1, v2) / DotP(v2, v2)) * v2;
     }
 
     /// <summary>
@@ -822,9 +834,9 @@ namespace Phanes::Core::Math
     /// <param name="v2">Vector to reject from</param>
     /// <returns>Copy of v1.</returns>
     template<RealType T>
-    TVector4<T, false> RejectV(const TVector4<T, false>& v1, const TVector4<T, false> v2)
+    TVector4<T, false> RejectV(TVector4<T, false>& v1, const TVector4<T, false> v2)
     {
-        Set(v1, v1 - (DotP(v1, v2) / DotP(v2, v2)) * v2);
+        v1 = v1 - (DotP(v1, v2) / DotP(v2, v2)) * v2;
 
         return v1;
     }
@@ -837,7 +849,7 @@ namespace Phanes::Core::Math
     /// <param name="v1">Vector</param>
     /// <returns>Perspective divided vector.</returns>
     template<RealType T>
-    TVector4<T, false> PrespectiveDivide(const TVector4<T, false>& v1)
+    TVector4<T, false> PerspectiveDivide(const TVector4<T, false>& v1)
     {
         float _1_w = (T)1.0 / v1.w;
         return TVector4<T, false>(
@@ -856,7 +868,7 @@ namespace Phanes::Core::Math
     /// <param name="v1">Vector</param>
     /// <returns>Copy of v1.</returns>
     template<RealType T>
-    TVector4<T, false> PrespectiveDivideV(TVector4<T, false>& v1)
+    TVector4<T, false> PerspectiveDivideV(TVector4<T, false>& v1)
     {
         float _1_w = (T)1.0 / v1.w;
         
@@ -867,6 +879,47 @@ namespace Phanes::Core::Math
 
         return v1;
     }
+
+    /// <summary>
+    /// Tests if vector is normalized.
+    /// </summary>
+    /// <typeparam name="T">Type of vector</typeparam>
+    /// <param name="v1">Vector</param>
+    /// <param name="threshold">Threshold to vector magnitude of 1.0</param>
+    /// <returns>True if vector is normalized, false if not.</returns>
+    template<RealType T>
+    FORCEINLINE TVector4<T, false> IsNormalized(const TVector4<T, false>& v1, T threshold = P_FLT_INAC)
+    {
+        return (SqrMagnitude(v1) - (T)1.0) < threshold;
+    }
+
+    /// <summary>
+    /// Tests if vectors are parallel.
+    /// </summary>
+    /// <typeparam name="T">Type of vector</typeparam>
+    /// <param name="v1">Vector</param>
+    /// <param name="v2">Vector</param>
+    /// <param name="threshold">Allowed T inaccuracy from one (e.g. 0.98f)</param>
+    /// <returns>True if parallel, false if not.</returns>
+    template<RealType T>
+    inline bool IsParallel(const TVector4<T, false>& v1, const TVector4<T, false>& v2, T threshold = 1.0f - P_FLT_INAC)
+    {
+        return (abs(DotP(v1, v2)) > threshold);
+    }
+
+    /// <summary>
+    /// Tests whether two vectors are coincident (Parallel and point in same direction).
+    /// </summary>
+    /// <typeparam name="T">Type of vector</typeparam>
+    /// <param name="v1">Vector</param>
+    /// <param name="v2">Vector</param>
+    /// <param name="threshold">Allowed T inaccuracy from one (e.g. 0.98f)</param>
+    /// <returns>True if coincident, false if not.</returns>
+    template<RealType T>
+    inline bool IsCoincident(const TVector4<T, false>& v1, const TVector4<T, false>& v2, T threshold = 1.0f - P_FLT_INAC)
+    {
+        return (DotP(v1, v2) > threshold);
+    }
 }
 
 
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector4.inl b/Engine/Source/Runtime/Core/public/Math/Vector4.inl
index 9a0808d..51ece7c 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector4.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Vector4.inl
@@ -98,7 +98,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator+(TVector4<T, S>& v1, const TVector4<T, S>& v2)
+    TVector4<T, S> operator+(const TVector4<T, S>& v1, const TVector4<T, S>& v2)
     {
         TVector4<T, S> r; 
         Detail::compute_vec4_add<T, S>::map(r, v1, v2);
@@ -106,7 +106,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator+(TVector4<T, S>& v1, T s)
+    TVector4<T, S> operator+(const TVector4<T, S>& v1, T s)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_add<T, S>::map(r, v1, s);
@@ -114,7 +114,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator-(TVector4<T, S>& v1, const TVector4<T, S>& v2)
+    TVector4<T, S> operator-(const TVector4<T, S>& v1, const TVector4<T, S>& v2)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_sub<T, S>::map(r, v1, v2);
@@ -122,7 +122,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator-(TVector4<T, S>& v1, T s)
+    TVector4<T, S> operator-(const TVector4<T, S>& v1, T s)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_sub<T, S>::map(r, v1, s);
@@ -130,7 +130,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator*(TVector4<T, S>& v1, const TVector4<T, S>& v2)
+    TVector4<T, S> operator*(const TVector4<T, S>& v1, const TVector4<T, S>& v2)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_mul<T, S>::map(r, v1, v2);
@@ -138,7 +138,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator*(TVector4<T, S>& v1, T s)
+    TVector4<T, S> operator*(const TVector4<T, S>& v1, T s)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_mul<T, S>::map(r, v1, s);
@@ -146,7 +146,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator/(TVector4<T, S>& v1, const TVector4<T, S>& v2)
+    TVector4<T, S> operator/(const TVector4<T, S>& v1, const TVector4<T, S>& v2)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_div<T, S>::map(r, v1, v2);
@@ -154,7 +154,7 @@ namespace Phanes::Core::Math
     }
 
     template<RealType T, bool S>
-    TVector4<T, S> operator/(TVector4<T, S>& v1, T s)
+    TVector4<T, S> operator/(const TVector4<T, S>& v1, T s)
     {
         TVector4<T, S> r;
         Detail::compute_vec4_div<T, S>::map(r, v1, s);