From 53d072876505c9661e7ee9f9961a8f78670e2536 Mon Sep 17 00:00:00 2001
From: THoehne <77296181+THoehne@users.noreply.github.com>
Date: Mon, 24 Jun 2024 22:36:23 +0200
Subject: [PATCH] Finish Matrix4. Bug Fixes.

---
 Engine/Source/Runtime/Core/Include.h          |   2 +-
 .../Core/public/Math/Detail/Matrix4Decl.inl   |   4 +-
 .../Source/Runtime/Core/public/Math/Include.h |  27 ++-
 .../Source/Runtime/Core/public/Math/MathFwd.h |  35 ++++
 .../Source/Runtime/Core/public/Math/MathPCH.h |   3 +
 .../Core/public/Math/MathTypeConversion.hpp   |  27 ++-
 .../Runtime/Core/public/Math/Matrix3.hpp      |  88 +++++++--
 .../Runtime/Core/public/Math/Matrix4.hpp      | 180 ++++++++++++++++--
 .../public/Math/SIMD/PhanesVectorMathFPU.hpp  |   8 +
 .../public/Math/SIMD/PhanesVectorMathSSE.hpp  |   6 +
 .../Runtime/Core/public/Math/Vector2.hpp      | 177 +++++------------
 .../Runtime/Core/public/Math/Vector4.inl      |   6 +
 12 files changed, 380 insertions(+), 183 deletions(-)

diff --git a/Engine/Source/Runtime/Core/Include.h b/Engine/Source/Runtime/Core/Include.h
index 55e8d35..c28cc1f 100644
--- a/Engine/Source/Runtime/Core/Include.h
+++ b/Engine/Source/Runtime/Core/Include.h
@@ -13,7 +13,7 @@
 
 // --- OSAL ----------------------------------------
 
-#include "Core/public/OSAL/PlatformTypes.h"
+#include "Core/public/HAL/PlatformTypes.h"
 
 
 #ifdef P_USE_NAMESPACE_ALIAS
diff --git a/Engine/Source/Runtime/Core/public/Math/Detail/Matrix4Decl.inl b/Engine/Source/Runtime/Core/public/Math/Detail/Matrix4Decl.inl
index 7fe4274..94369a9 100644
--- a/Engine/Source/Runtime/Core/public/Math/Detail/Matrix4Decl.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Detail/Matrix4Decl.inl
@@ -18,7 +18,7 @@ namespace Phanes::Core::Math::Detail
     template<RealType T>
     struct compute_mat4_det<T, false>
     {
-        static constexpr T map(Phanes::Core::Math::TMatrix4<T, S>& m)
+        static constexpr T map(Phanes::Core::Math::TMatrix4<T, false>& m)
         {
             const TVector3<T, false>& a = reinterpret_cast<TVector3<T, false>&>(m[0]);
             const TVector3<T, false>& b = reinterpret_cast<TVector3<T, false>&>(m[1]);
@@ -87,7 +87,7 @@ namespace Phanes::Core::Math::Detail
     template<RealType T>
     struct compute_mat4_transpose<T, false>
     {
-        static constexpr void map(Phanes::Core::Math::TMatrix4<T, S>& r, const Phanes::Core::Math::TMatrix4<T, S>& m)
+        static constexpr void map(Phanes::Core::Math::TMatrix4<T, false>& r, const Phanes::Core::Math::TMatrix4<T, false>& m)
         {
             r = Phanes::Core::Math::TMatrix4<T, false>(m(0, 0), m(1, 0), m(2, 0), m(3, 0),
                                                        m(0, 1), m(1, 1), m(2, 1), m(3, 1),
diff --git a/Engine/Source/Runtime/Core/public/Math/Include.h b/Engine/Source/Runtime/Core/public/Math/Include.h
index 1015147..9c540df 100644
--- a/Engine/Source/Runtime/Core/public/Math/Include.h
+++ b/Engine/Source/Runtime/Core/public/Math/Include.h
@@ -1,14 +1,31 @@
 #pragma once
 
+#ifndef P_DEBUG
+#pragma warning(disable : 4244)
+#endif
+
+#include "Core/public/Math/MathFwd.h"
+
+
 // --- Vectors ------------------------
 
-#include "Core/public/Math/Vector2.hpp" // <-- Includes Vector3/4 automatically
-#include "Core/public/Math/Vector3.hpp" // <-- Includes Vector3/4 automatically
-#include "Core/public/Math/Vector4.hpp" // <-- Includes Vector3/4 automatically
-
+#include "Core/public/Math/Vector2.hpp"
+#include "Core/public/Math/Vector3.hpp"
+#include "Core/public/Math/Vector4.hpp"
 
 #include "Core/public/Math/IntVector2.hpp"
 #include "Core/public/Math/IntVector3.hpp"
 #include "Core/public/Math/IntVector4.hpp"
 
-#include "Core/public/Math/Matrix2.hpp"
\ No newline at end of file
+
+// --- Matrices ------------------------
+
+#include "Core/public/Math/Matrix2.hpp"
+#include "Core/public/Math/Matrix3.hpp"
+#include "Core/public/Math/Matrix4.hpp"
+
+
+// --- Misc -----------------
+
+#include "Core/public/Math/MathTypeConversion.hpp"
+#include "Core/public/Math/MathUnitConversion.hpp"
diff --git a/Engine/Source/Runtime/Core/public/Math/MathFwd.h b/Engine/Source/Runtime/Core/public/Math/MathFwd.h
index 6d4c093..41b41a3 100644
--- a/Engine/Source/Runtime/Core/public/Math/MathFwd.h
+++ b/Engine/Source/Runtime/Core/public/Math/MathFwd.h
@@ -10,6 +10,7 @@
 #define MATH_FWD_H
 
 #include "Core/public/Math/Boilerplate.h"
+#include "Core/public/Math/SIMD/PhanesSIMDTypes.h"
 
 /**
  * Includes forward declarations, as well as certain useful typedefs.
@@ -50,6 +51,40 @@ namespace Phanes::Core::Math {
     /**
      * Specific instantiation of forward declarations.
      */
+
+    // Vector2
+
+    typedef TVector2<float, false>      Vector2;
+    typedef TVector2<float, false>      Vector2f;
+    typedef TVector2<double, false>     Vector2d;
+
+    typedef TVector2<double, SIMD::use_simd<double, 2, true>::value>        Vector2Regf64;
+    typedef TVector2<double, SIMD::use_simd<double, 2, true>::value>        Vector2Reg;
+    typedef TVector2<double, SIMD::use_simd<double, 2, true>::value>        Vector2Regd;
+
+
+    // Vector3
+
+    typedef TVector3<float, false>      Vector3;
+    typedef TVector3<float, false>      Vector3f;
+    typedef TVector3<double, false>     Vector3d;
+
+    typedef TVector3<float, SIMD::use_simd<float, 3, true>::value>          Vector3Reg;
+    typedef TVector3<float, SIMD::use_simd<float, 3, true>::value>          Vector3Regf32;
+    typedef TVector3<double, SIMD::use_simd<double, 3, true>::value>        Vector3Regd;
+    typedef TVector3<double, SIMD::use_simd<double, 3, true>::value>        Vector3Regf64;
+
+
+    // Vector4
+
+    typedef TVector4<float, false>      Vector4;
+    typedef TVector4<float, false>      Vector4f;
+    typedef TVector4<double, false>     Vector4d;
+
+    typedef TVector4<float, SIMD::use_simd<float, 4, true>::value>          Vector4Reg;
+    typedef TVector4<float, SIMD::use_simd<float, 4, true>::value>          Vector4Regf32;
+    typedef TVector4<double, SIMD::use_simd<double, 4, true>::value>        Vector4Regd;
+    typedef TVector4<double, SIMD::use_simd<double, 4, true>::value>        Vector4Regf64;
     
 
 } // Phanes::Core::Math::coretypes
diff --git a/Engine/Source/Runtime/Core/public/Math/MathPCH.h b/Engine/Source/Runtime/Core/public/Math/MathPCH.h
index ca313f7..a338f31 100644
--- a/Engine/Source/Runtime/Core/public/Math/MathPCH.h
+++ b/Engine/Source/Runtime/Core/public/Math/MathPCH.h
@@ -7,13 +7,16 @@
 #include "Plane.hpp"
 #include "Vector2.hpp"
 #include "Vector3.hpp"
+#include "Vector4.hpp"
 
 #include "IntPoint.hpp"
 #include "IntVector2.hpp"
 #include "IntVector3.hpp"
+#include "IntVector4.hpp"
 
 #include "Matrix2.hpp"
 #include "Matrix3.hpp"
+#include "Matrix4.hpp"
 
 #include "MathCommon.hpp"
 #include "MathTypeConversion.hpp"
diff --git a/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp b/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
index 488e940..c468fae 100644
--- a/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/MathTypeConversion.hpp
@@ -17,11 +17,13 @@
 #include "Core/public/Math/MathAbstractTypes.h"
 #include "Core/public/Math/Vector2.hpp"
 #include "Core/public/Math/Vector3.hpp"
-//#include "Core/public/Math/Vector4.h"
+#include "Core/public/Math/Vector4.hpp"
 #include "Core/public/Math/Matrix2.hpp"
-//#include "Core/public/Math/Matrix3.h"
+#include "Core/public/Math/Matrix3.hpp"
+#include "Core/public/Math/Matrix4.hpp"
 #include "Core/public/Math/IntVector2.hpp"
 #include "Core/public/Math/IntVector3.hpp"
+#include "Core/public/Math/IntVector4.hpp"
 
 #ifndef MATH_TYPE_CONVERSION_H
 #define MATH_TYPE_CONVERSION_H
@@ -60,37 +62,32 @@ namespace Phanes::Core::Math {
   //   ToString   //
   // ============ //
 
-  template<RealType T>
-  std::string ToString(const TVector2<T>& v)
+  template<RealType T, bool S>
+  std::string ToString(const TVector2<T, S>& v)
   {
       return "(" + ToString(v.x) + ", " + ToString(v.y) + ")";
   }
 
-  template<IntType T>
-  std::string ToString(const TIntVector2<T>& v)
+  template<IntType T, bool S>
+  std::string ToString(const TIntVector2<T, S>& v)
   {
       return "(" + ToString(v.x) + ", " + ToString(v.y) + ")";
   }
 
-  template<RealType T>
-  std::string ToString(const TVector3<T>& v)
+  template<RealType T, bool S>
+  std::string ToString(const TVector3<T, S>& v)
   {
       return "(" + ToString(v.x) + ", " + ToString(v.y) + ", " + ToString(v.z) + ")";
   }
 
-  template<IntType T>
-  std::string ToString(const TIntVector3<T>& v)
+  template<IntType T, bool S>
+  std::string ToString(const TIntVector3<T, S>& v)
   {
       return "(" + ToString(v.x) + ", " + ToString(v.y) + ", " + ToString(v.z) + ")";
   }
 
   //std::string toString(const Vector4& v);
 
-  template<RealType T>
-  std::string toString(const TMatrix2<T>& m)
-  {
-      return "[[" + ToString(m.m(0, 0)) + " " + ToString(m.m(0, 1)) + "], [" + ToString(m.m(0, 0)) + " " + ToString(m.m(0, 1)) + "]]";
-  }
 
   //std::string toString(const Matrix3& v);
 
diff --git a/Engine/Source/Runtime/Core/public/Math/Matrix3.hpp b/Engine/Source/Runtime/Core/public/Math/Matrix3.hpp
index 1ff5178..47d1aef 100644
--- a/Engine/Source/Runtime/Core/public/Math/Matrix3.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Matrix3.hpp
@@ -214,9 +214,9 @@ namespace Phanes::Core::Math {
     template<RealType T, bool S>
     TMatrix3<T, S> operator*= (TMatrix3<T, S>& m1, T s)
     {
-        m1.c0 *= ss;
-        m1.c1 *= ss;
-        m1.c2 *= ss;
+        m1.c0 *= s;
+        m1.c1 *= s;
+        m1.c2 *= s;
 
         return m1;
     }
@@ -238,6 +238,41 @@ namespace Phanes::Core::Math {
         return m1;
     }
 
+    /**
+     * Multiply matrix with scalar
+     *
+     * @param(m1) Matrix
+     * @param(s) Scalar
+     */
+
+    template<RealType T, bool S>
+    TMatrix3<T, S> operator/= (TMatrix3<T, S>& m1, T s)
+    {
+        s = (T)1.0 / s;
+        m1.c0 *= s;
+        m1.c1 *= s;
+        m1.c2 *= s;
+
+        return m1;
+    }
+
+    /**
+     * Matrix on matrix (componentwise)
+     *
+     * @param(m1) Matrix
+     * @param(m2) Matrix
+     */
+
+    template<RealType T, bool S>
+    TMatrix3<T, S> operator/= (TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
+    {
+        m1.c0 /= m2.c0;
+        m1.c1 /= m2.c1;
+        m1.c2 /= m2.c2;
+
+        return m1;
+    }
+
     /**
      * Add scalar to matrix componentwise
      *
@@ -278,9 +313,9 @@ namespace Phanes::Core::Math {
     template<RealType T, bool S>
     TMatrix3<T, S> operator- (const TMatrix3<T, S>& m, T s)
     {
-        return TMatrix3<T, S>(m1.c0 - s,
-                              m1.c1 - s,
-                              m1.c2 - s);
+        return TMatrix3<T, S>(m.c0 - s,
+                              m.c1 - s,
+                              m.c2 - s);
     }
 
     /**
@@ -313,6 +348,37 @@ namespace Phanes::Core::Math {
                               m.c2 * s);
     }
 
+    /**
+     * Multiplay matrix by matrix (componentwise)
+     *
+     * @param(m1) Matrix
+     * @param(m2) Matrix
+     */
+
+    template<RealType T, bool S>
+    TMatrix3<T, S> operator/ (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
+    {
+        return TMatrix3<T, S>(m1.c0 / m2.c0,
+                              m1.c1 / m2.c1,
+                              m1.c2 / m2.c2);
+    }
+
+    /**
+     * Multiply scalar with matrix
+     *
+     * @param(m) Matrix
+     * @param(s) Scalar
+     */
+
+    template<RealType T, bool S>
+    TMatrix3<T, S> operator/ (const TMatrix3<T, S>& m, float s)
+    {
+        s = (T)1.0 / s;
+        return TMatrix3<T, S>(m.c0 * s,
+                              m.c1 * s,
+                              m.c2 * s);
+    }
+
     /**
      * Multiplay matrix by matrix (componentwise)
      *
@@ -323,9 +389,9 @@ namespace Phanes::Core::Math {
     template<RealType T, bool S>
     TMatrix3<T, S> operator* (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
     {
-        return TMatrix3<T, S>(m1.c0 + m2.c0,
-                              m1.c1 + m2.c1,
-                              m1.c2 + m2.c2);
+        return TMatrix3<T, S>(m1.c0 * m2.c0,
+            m1.c1 * m2.c1,
+            m1.c2 * m2.c2);
     }
 
     /**
@@ -338,7 +404,7 @@ namespace Phanes::Core::Math {
     template<RealType T, bool S>
     bool operator== (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
     {
-        return (m1[0] == m2[0] && m1[1] == m2[1] && m1[2] == m2[2]);
+        return (m1.c0 == m2.c0 && m1.c1 == m2.c1 && m1.c2 == m2.c2);
     }
 
     /**
@@ -351,7 +417,7 @@ namespace Phanes::Core::Math {
     template<RealType T, bool S>
     bool operator!= (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
     {
-        return (m1[0] != m2[0] || m1[1] != m2[1] || m1[2] != m2[2]);
+        return (m1.c0 != m2.c0 || m1.c1 != m2.c1 || m1.c2 != m2.c2);
     }
 
 
diff --git a/Engine/Source/Runtime/Core/public/Math/Matrix4.hpp b/Engine/Source/Runtime/Core/public/Math/Matrix4.hpp
index 25c1379..73b10e5 100644
--- a/Engine/Source/Runtime/Core/public/Math/Matrix4.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Matrix4.hpp
@@ -57,49 +57,196 @@ namespace Phanes::Core::Math {
 	// ==================== //
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator+= (TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator+= (TMatrix4<T, S>& m1, T s)
+	{
+		m1.c0 += s;
+		m1.c1 += s;
+		m1.c2 += s;
+		m1.c3 += s;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator+= (TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator+= (TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		m1.c0 += m2.c0;
+		m1.c1 += m2.c1;
+		m1.c2 += m2.c2;
+		m1.c3 += m2.c3;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator-= (TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator-= (TMatrix4<T, S>& m1, T s)
+	{
+		m1.c0 -= s;
+		m1.c1 -= s;
+		m1.c2 -= s;
+		m1.c3 -= s;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator-= (TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator-= (TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		m1.c0 -= m2.c0;
+		m1.c1 -= m2.c1;
+		m1.c2 -= m2.c2;
+		m1.c3 -= m2.c3;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator*= (TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator*= (TMatrix4<T, S>& m1, T s)
+	{
+		m1.c0 *= s;
+		m1.c1 *= s;
+		m1.c2 *= s;
+		m1.c3 *= s;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator*= (TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator*= (TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		m1.c0 *= m2.c0;
+		m1.c1 *= m2.c1;
+		m1.c2 *= m2.c2;
+		m1.c3 *= m2.c3;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator+ (const TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator/= (TMatrix4<T, S>& m1, T s)
+	{
+		s = (T)1.0 / s;
+		m1.c0 *= s;
+		m1.c1 *= s;
+		m1.c2 *= s;
+		m1.c3 *= s;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator+ (const TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator/= (TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		m1.c0 /= m2.c0;
+		m1.c1 /= m2.c1;
+		m1.c2 /= m2.c2;
+		m1.c3 /= m2.c3;
+
+		return m1;
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator- (const TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator+ (const TMatrix4<T, S>& m1, T s)
+	{
+		return TMatrix4<T, S>(m1.c0 + s,
+							  m1.c1 + s,
+							  m1.c2 + s,
+							  m1.c3 + s
+						      );
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator- (const TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator+ (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return TMatrix4<T, S>(m1.c0 + m2.c0,
+							  m1.c1 + m2.c1,
+							  m1.c2 + m2.c2,
+							  m1.c3 + m2.c3
+					);
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator* (const TMatrix4<T, S>& a, T s);
+	TMatrix4<T, S> operator- (const TMatrix4<T, S>& m1, T s)
+	{
+		return TMatrix4<T, S>(m1.c0 - s,
+							  m1.c1 - s,
+							  m1.c2 - s,
+							  m1.c3 - s
+			);
+	}
 
 	template<RealType T, bool S>
-	TMatrix4<T, S> operator* (const TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator- (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return TMatrix4<T, S>(m1.c0 - m2.c0,
+							  m1.c1 - m2.c1,
+							  m1.c2 - m2.c2,
+							  m1.c3 - m2.c3
+		);
+	}
 
 	template<RealType T, bool S>
-	TVector4<T, S> operator* (const TMatrix4<T, S>& a, const TVector4<T, S>& v);
+	TMatrix4<T, S> operator* (const TMatrix4<T, S>& m1, T s)
+	{
+		return TMatrix4<T, S>(m1.c0 * s,
+						      m1.c1 * s,
+						      m1.c2 * s,
+						      m1.c3 * s
+						      );
+	}
 
 	template<RealType T, bool S>
-	bool operator== (const TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator* (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return TMatrix4<T, S>(m1.c0 * m2.c0,
+							  m1.c1 * m2.c1,
+							  m1.c2 * m2.c2,
+							  m1.c3 * m2.c3
+		);
+	}
 
 	template<RealType T, bool S>
-	bool operator!= (const TMatrix4<T, S>& a, const TMatrix4<T, S>& b);
+	TMatrix4<T, S> operator/ (const TMatrix4<T, S>& m1, T s)
+	{
+		s = (T)1.0 / s;
+		return TMatrix4<T, S>(m1.c0 * s,
+							  m1.c1 * s,
+							  m1.c2 * s,
+							  m1.c3 * s
+		);
+	}
+
+	template<RealType T, bool S>
+	TMatrix4<T, S> operator/ (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return TMatrix4<T, S>(m1.c0 / m2.c0,
+						      m1.c1 / m2.c1,
+						      m1.c2 / m2.c2,
+						      m1.c3 / m2.c3
+		);
+	}
+
+	template<RealType T, bool S>
+	TVector4<T, S> operator* (const TMatrix4<T, S>& m1, const TVector4<T, S>& v)
+	{
+		return TVector4<T, S>(DotP(m1.c0, v),
+						      DotP(m1.c1, v),
+						      DotP(m1.c2, v),
+						      DotP(m1.c3, v)
+							  );
+	}
+
+	template<RealType T, bool S>
+	bool operator== (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return (m1.c0 == m2.c0 && m1.c1 == m2.c1 && m1.c2 == m2.c2 && m1.c3 == m2.c3);
+	}
+
+	template<RealType T, bool S>
+	bool operator!= (const TMatrix4<T, S>& m1, const TMatrix4<T, S>& m2)
+	{
+		return (m1.c0 != m2.c0 || m1.c1 != m2.c1 || m1.c2 != m2.c2 || m1.c3 != m2.c3);
+	}
 
 
 	// ================================ //
@@ -115,6 +262,7 @@ namespace Phanes::Core::Math {
 	template<RealType T, bool S>
 	TMatrix4<T, S> TransposeV(TMatrix4<T, S>& a);
 
+
 	// =============== //
 	//   WITH RETURN   //
 	// =============== //
@@ -127,7 +275,7 @@ namespace Phanes::Core::Math {
 	TMatrix4<T, S> Transpose(const TMatrix4<T, S>& a);
 
 	template<RealType T, bool S>
-	FORCEINLINE bool IsIndentityMatrix(const TMatrix4<T, S>& a)
+	FORCEINLINE bool IsIndentityMatrix(const TMatrix4<T, S>& m1)
 	{
 		return (abs(m1(0, 0) - (T)1.0) < P_FLT_INAC && abs(m1(0, 1) - (T)0.0) < P_FLT_INAC && abs(m1(0, 2) - (T)0.0) < P_FLT_INAC && abs(m1(0, 3) - (T)0.0) < P_FLT_INAC && 
 				abs(m1(1, 0) - (T)0.0) < P_FLT_INAC && abs(m1(1, 1) - (T)1.0) < P_FLT_INAC && abs(m1(1, 2) - (T)0.0) < P_FLT_INAC && abs(m1(1, 3) - (T)0.0) < P_FLT_INAC && 
diff --git a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathFPU.hpp b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathFPU.hpp
index 3c6bd20..3dde2f6 100644
--- a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathFPU.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathFPU.hpp
@@ -63,4 +63,12 @@ namespace Phanes::Core::Math::SIMD
     {
         return v1.data[0] * v1.data[0] + v1.data[1] * v2.data[1] + v1.data[2] * v2.data[2] + v1.data[3] * v2.data[3];
     }
+
+    Phanes::Core::Types::Vec2f64Reg vec2_eq(const Phanes::Core::Types::Vec2f64Reg v1, const Phanes::Core::Types::Vec2f64Reg v2)
+    {
+        Phanes::Core::Types::Vec4f64Reg r;
+
+        r.data[0] = (Phanes::Core::Math::Abs(v1.data[0] - v2.data[0]) < P_FLT_INAC) ? 0xFFFFFFFF : 0;
+        r.data[1] = (Phanes::Core::Math::Abs(v1.data[1] - v2.data[1]) < P_FLT_INAC) ? 0xFFFFFFFF : 0;
+    }
 }
\ No newline at end of file
diff --git a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
index 78dae78..3e554c4 100644
--- a/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/SIMD/PhanesVectorMathSSE.hpp
@@ -97,6 +97,11 @@ namespace Phanes::Core::Math::SIMD
     {
         return vec4_hadd_cvtf32(_mm_mul_ps(v1, v2));
     }
+
+    Phanes::Core::Types::Vec2f64Reg vec2_eq(const Phanes::Core::Types::Vec2f64Reg v1, const Phanes::Core::Types::Vec2f64Reg v2)
+    {
+        return _mm_cmpeq_pd(v1, v2);
+    }
 }
 
 
@@ -379,6 +384,7 @@ namespace Phanes::Core::Math::Detail
         }
     };
 
+
     // =============== //
     //   TIntVector4   //
     // =============== //
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector2.hpp b/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
index 8cfd950..6372399 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
+++ b/Engine/Source/Runtime/Core/public/Math/Vector2.hpp
@@ -120,84 +120,54 @@ namespace Phanes::Core::Math {
     // ====================== //
     
     /**
-     * Addition operation on same TVector2<T, false> (this) by a floating point value.
+     * Addition operation on same TVector2<T, S> (this) by a floating point value.
      * 
      * @param(v1) Vector to add to
      * @param(s) Floating point to add
      */
 
-    template<RealType T>
-    TVector2<T, false> operator+= (TVector2<T, false>& v1, T s)
-    {
-        v1.x += s;
-        v1.y += s;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator+= (TVector2<T, S>& v1, T s);
 
     /**
-     * Addition operation on same TVector2<T, false> (this) by a another TVector2<T, false>.
+     * Addition operation on same TVector2<T, S> (this) by a another TVector2<T, S>.
      *
      * @param(v1) Vector to add to
      * @param(v2) Vector to add
      */
 
-    template<RealType T>
-    TVector2<T, false> operator+= (TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        v1.x += v2.x;
-        v1.y += v2.y;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator+= (TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
     /**
-     * Substraction operation on same TVector2<T, false> (this) by a floating point.
+     * Substraction operation on same TVector2<T, S> (this) by a floating point.
      *
      * @param(v1) Vector to substract from
      * @param(v2) Floating point to substract
      */
 
-    template<RealType T>
-    TVector2<T, false> operator-= (TVector2<T, false>& v1, T s)
-    {
-        v1.x -= s;
-        v1.y -= s;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator-= (TVector2<T, S>& v1, T s);
 
     /**
-     * Substraction operation on same TVector2<T, false> (this) by a another TVector2<T, false>.
+     * Substraction operation on same TVector2<T, S> (this) by a another TVector2<T, S>.
      *
      * @param(v1) Vector to substract from
      * @param(v2) Vector to substract
      */
 
-    template<RealType T>
-    TVector2<T, false> operator-= (TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        v1.x -= v2.x;
-        v1.y -= v2.y;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator-= (TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
     /**
-     * Multiplication of TVector2<T, false> (this) with a floating point. 
+     * Multiplication of TVector2<T, S> (this) with a floating point. 
      *
      * @param(v1) Vector to multiply with
      * @param(s Floating point to multiply with
      */
 
-    template<RealType T>
-    TVector2<T, false> operator*= (TVector2<T, false>& v1, T s)
-    {
-        v1.x *= s;
-        v1.y *= s;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator*= (TVector2<T, S>& v1, T s);
 
     /**
      * Devision of Vector (this) by floating point.
@@ -206,18 +176,11 @@ namespace Phanes::Core::Math {
      * @param(s Floating point to divide with
      */
 
-    template<RealType T>
-    TVector2<T, false> operator/= (TVector2<T, false>& v1, T s)
-    {
-        s = 1.0f / s;
-        v1.x *= s;
-        v1.y *= s;
-
-        return v1;
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator/= (TVector2<T, S>& v1, T s);
 
     /**
-     * Scale of Vector by floating point. (> Creates a new TVector2<T, false>)
+     * Scale of Vector by floating point. (> Creates a new TVector2<T, S>)
      *
      * @param(v1) Vector to multiply with
      * @param(s Floating point to multiply with
@@ -225,14 +188,11 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator* (const TVector2<T, false>& v1, T s)
-    {
-        return TVector2<T, false>(v1.x * s, v1.y * s);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator* (const TVector2<T, S>& v1, T s);
 
     /**
-     * Division of Vector by floating point. (> Creates another TVector2<T, false>)
+     * Division of Vector by floating point. (> Creates another TVector2<T, S>)
      *
      * @param(v1) Vector to multiply with
      * @param(s Floating point to divide with
@@ -240,15 +200,11 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator/ (const TVector2<T, false>& v1, T s)
-    {
-        s = 1.0f / s;
-        return TVector2<T, false>(v1.x * s, v1.y * s);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator/ (const TVector2<T, S>& v1, T s);
 
     /**
-     * Scale of Vector by floating point. (> Creates a new TVector2<T, false>)
+     * Scale of Vector by floating point. (> Creates a new TVector2<T, S>)
      *
      * @param(v1) Vector to multiply with
      * @param(s Floating point to multiply with
@@ -256,11 +212,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    inline TVector2<T, false> operator* (T s, const TVector2<T, false>& v1)
-    {
-        return v1 * s;
-    }
+    template<RealType T, bool S>
+    inline TVector2<T, S> operator* (T s, const TVector2<T, S>& v1);
 
     /**
      * Division of Vector by floating point. (> For convenience not arithmethicaly correct. Works like overloaded counterpart.)
@@ -271,29 +224,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    inline TVector2<T, false> operator/ (T s, const TVector2<T, false>& v1)
-    {
-        s = 1.0f / s;
-        return v1 * s;
-    }
-
-    /**
-     * Dot product between two Vectors.
-     *
-     * @see [FUNC]DotP
-     * 
-     * @param(v1) Vector one
-     * @param(v2) Vector two
-     * 
-     * @result Dot product 
-     */
-
-    template<RealType T>
-    inline T operator* (const TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        return v1.x * v2.x + v1.y * v2.y;
-    }
+    template<RealType T, bool S>
+    inline TVector2<T, S> operator/ (T s, const TVector2<T, S>& v1);
 
     /**
      * Componentwise addition of Vector with floating point. 
@@ -304,11 +236,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator+ (const TVector2<T, false>& v1, T s)
-    {
-        return TVector2<T, false>(v1.x + s, v1.y + s);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator+ (const TVector2<T, S>& v1, T s);
                 
     /**
      * Componentwise addition of Vector with floating point. 
@@ -319,11 +248,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator+ (const TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        return TVector2<T, false>(v1.x + v2.x, v1.y + v2.y);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator+ (const TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
     /**
      * Componentwise substraction of Vector with floating point.
@@ -334,11 +260,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator- (const TVector2<T, false>& v1, T s)
-    {
-        return TVector2<T, false>(v1.x - s, v1.y - s);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator- (const TVector2<T, S>& v1, T s);
 
     /**
      * Componentwise substraction of Vector with Vector.
@@ -349,11 +272,8 @@ namespace Phanes::Core::Math {
      * @return Result Vector
      */
 
-    template<RealType T>
-    TVector2<T, false> operator- (const TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        return TVector2<T, false>(v1.x - v2.x, v1.y - v2.y);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator- (const TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
     /**
      * Negate Vector.
@@ -361,11 +281,8 @@ namespace Phanes::Core::Math {
      * @param(v1) Vector to negate
      */
 
-    template<RealType T>
-    TVector2<T, false> operator- (const TVector2<T, false>& v1)
-    {
-        return TVector2<T, false>&(-v1.x, -v1.y);
-    }
+    template<RealType T, bool S>
+    TVector2<T, S> operator- (const TVector2<T, S>& v1);
 
 
     /**
@@ -378,11 +295,8 @@ namespace Phanes::Core::Math {
      * @return true if equal, false if inequal
      */
 
-    template<RealType T>
-    bool operator== (const TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        return (abs(v1.x - v1.x) < P_FLT_INAC && abs(v1.y - v1.y) < P_FLT_INAC);
-    }
+    template<RealType T, bool S>
+    bool operator== (const TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
 
     /**
@@ -395,11 +309,8 @@ namespace Phanes::Core::Math {
         * @return true if inequal, false if equal
         */
 
-    template<RealType T>
-    bool operator!= (const TVector2<T, false>& v1, const TVector2<T, false>& v2)
-    {
-        return (abs(v1.x - v1.x) > P_FLT_INAC || abs(v1.y - v1.y) > P_FLT_INAC);
-    }
+    template<RealType T, bool S>
+    bool operator!= (const TVector2<T, S>& v1, const TVector2<T, S>& v2);
 
 
     // ============================================ //
@@ -486,7 +397,7 @@ namespace Phanes::Core::Math {
     template<RealType T>
     T Angle(const TVector2<T, false>& v1, const TVector2<T, false>& v2)
     {
-        return acos((v1 * v2) / Magnitude(v1) * Magnitude(v2));
+        return acos(DotP(v1, v2) / (Magnitude(v1) * Magnitude(v2)));
     }
 
     /**
@@ -784,7 +695,7 @@ namespace Phanes::Core::Math {
     template<RealType T>
     inline bool IsNormalized(const TVector2<T, false>& v1, T threshold = P_FLT_INAC)
     {
-        return (SqrMagnitude(v1) < threshold);
+        return (abs(SqrMagnitude(v1) - 1) < threshold);
     }
 
     /**
diff --git a/Engine/Source/Runtime/Core/public/Math/Vector4.inl b/Engine/Source/Runtime/Core/public/Math/Vector4.inl
index 1b1a2db..9a0808d 100644
--- a/Engine/Source/Runtime/Core/public/Math/Vector4.inl
+++ b/Engine/Source/Runtime/Core/public/Math/Vector4.inl
@@ -205,4 +205,10 @@ namespace Phanes::Core::Math
     {
         return --v1;
     }
+
+    template<RealType T>
+    T DotP(const TVector4<T, true>& v1, const TVector4<T, true>& v2)
+    {
+        return vec4_dot_cvtf32(v1.data, v2.data);
+    }
 }
\ No newline at end of file