From 95c5073d86cf4ca7a30ee3aa8dfb2230d20fdda0 Mon Sep 17 00:00:00 2001
From: Orange <orange-cpp@yandex.ru>
Date: Wed, 4 Dec 2024 06:43:54 +0300
Subject: [PATCH] added rotation matrix

---
 include/omath/Mat.hpp                     | 19 ++++++----
 include/omath/engines/Source/Formulas.hpp | 43 ++++-------------------
 2 files changed, 18 insertions(+), 44 deletions(-)

diff --git a/include/omath/Mat.hpp b/include/omath/Mat.hpp
index 3059dd3..7f6aae5 100644
--- a/include/omath/Mat.hpp
+++ b/include/omath/Mat.hpp
@@ -349,19 +349,19 @@ namespace omath
 
     template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR, class Angle>
     [[nodiscard]]
-    constexpr Mat<3, 3, Type, St> RotationMatrixAxisX(const Angle& roll) noexcept
+    Mat<3, 3, Type, St> RotationMatAxisX(const Angle& roll) noexcept
     {
         return
         {
-            {1,    0,          0},
-            {0, 0, roll.Cos(), -roll.Sin()},
-            {0, 0, roll.Sin(), roll.Cos()},
+            {1, 0,          0},
+            {0, roll.Cos(), -roll.Sin()},
+            {0, roll.Sin(), roll.Cos()},
         };
     }
 
     template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR, class Angle>
     [[nodiscard]]
-    constexpr Mat<3, 3, Type, St> RotationMatrixAxisY(const Angle& pitch) noexcept
+    Mat<3, 3, Type, St> RotationMatAxisY(const Angle& pitch) noexcept
     {
         return
         {
@@ -373,7 +373,7 @@ namespace omath
 
     template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR, class Angle>
     [[nodiscard]]
-    constexpr Mat<3, 3, Type, St> RotationMatrixAxisZ(const Angle& Yaw) noexcept
+    Mat<3, 3, Type, St> RotationMatAxisZ(const Angle& Yaw) noexcept
     {
         return
         {
@@ -383,5 +383,10 @@ namespace omath
         };
     }
 
-
+    template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR, class ViewAngles>
+    [[nodiscard]]
+    Mat<3, 3, Type, St> RotationMat(const ViewAngles& angles) noexcept
+    {
+        return RotationMatAxisY(angles.pitch) * RotationMatAxisZ(angles.yaw) * RotationMatAxisX(angles.roll);
+    }
 } // namespace omath
diff --git a/include/omath/engines/Source/Formulas.hpp b/include/omath/engines/Source/Formulas.hpp
index 39e16c3..0721d96 100644
--- a/include/omath/engines/Source/Formulas.hpp
+++ b/include/omath/engines/Source/Formulas.hpp
@@ -10,56 +10,25 @@ namespace omath::source
     [[nodiscard]]
     inline Vector3 ForwardVector(const ViewAngles& angles)
     {
-        const auto cosPitch = angles.pitch.Cos();
-        const auto sinPitch = angles.pitch.Sin();
+        const auto vec = RotationMat(angles) * Mat<3, 1>({{kAbsForward.x}, {kAbsForward.y}, {kAbsForward.z}});
 
-        const auto cosYaw = angles.yaw.Cos();
-        const auto sinYaw = angles.yaw.Sin();
-
-
-        return {cosPitch * cosYaw, cosPitch * sinYaw, -sinPitch};
+        return {vec.At(0, 0), vec.At(1, 0), vec.At(2, 0)};
     }
 
     [[nodiscard]]
     inline Vector3 RightVector(const ViewAngles& angles)
     {
-        const auto cosPitch = angles.pitch.Cos();
-        const auto sinPitch = angles.pitch.Sin();
+        const auto vec = RotationMat(angles) * Mat<3, 1>({{kAbsRight.x}, {kAbsRight.y}, {kAbsRight.z}});
 
-        const auto cosYaw = angles.yaw.Cos();
-        const auto sinYaw = angles.yaw.Sin();
-
-        const auto cosRoll = angles.roll.Cos();
-        const auto sinRoll = angles.roll.Sin();
-
-
-        return
-        {
-            -1 * sinRoll * sinPitch * cosYaw + -1 * cosRoll * -sinYaw,
-            -1 * sinRoll * sinPitch * sinYaw + -1 * cosRoll * cosYaw,
-            -1 * sinRoll * cosPitch
-        };
+        return {vec.At(0, 0), vec.At(1, 0), vec.At(2, 0)};
     }
 
     [[nodiscard]]
     inline Vector3 UpVector(const ViewAngles& angles)
     {
-        const auto cosPitch = angles.pitch.Cos();
-        const auto sinPitch = angles.pitch.Sin();
+        const auto vec = RotationMat(angles) * Mat<3, 1>({{kAbsUp.x}, {kAbsUp.y}, {kAbsUp.z}});
 
-        const auto cosYaw = angles.yaw.Cos();
-        const auto sinYaw = angles.yaw.Sin();
-
-        const auto cosRoll = angles.roll.Cos();
-        const auto sinRoll = angles.roll.Sin();
-
-
-        return
-        {
-            cosRoll * sinPitch * cosYaw + - sinRoll * -sinYaw,
-            cosRoll * sinPitch * sinYaw + - sinRoll * cosYaw,
-            cosRoll * cosPitch,
-        };
+        return {vec.At(0, 0), vec.At(1, 0), vec.At(2, 0)};
     }
 
     [[nodiscard]]