Merge pull request #191 from orange-cpp/feature/mat_improvement

added improvement

include/omath/engines/cry_engine/formulas.hpp

@@ -21,6 +21,15 @@ namespace omath::cry_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Mat4X4 calc_perspective_projection_matrix(float field_of_view, float aspect_ratio, float near, float far,
                                              NDCDepthRange ndc_depth_range = NDCDepthRange::ZERO_TO_ONE) noexcept;

include/omath/engines/frostbite_engine/formulas.hpp

@@ -21,6 +21,15 @@ namespace omath::frostbite_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Mat4X4 calc_perspective_projection_matrix(float field_of_view, float aspect_ratio, float near, float far,
                                              NDCDepthRange ndc_depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE) noexcept;

include/omath/engines/iw_engine/formulas.hpp

@@ -19,6 +19,15 @@ namespace omath::iw_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]] Mat4X4 calc_view_matrix(const ViewAngles& angles, const Vector3<float>& cam_origin) noexcept;
 
     [[nodiscard]]

include/omath/engines/opengl_engine/formulas.hpp

@@ -20,6 +20,15 @@ namespace omath::opengl_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Mat4X4 calc_perspective_projection_matrix(float field_of_view, float aspect_ratio, float near, float far,
                                              NDCDepthRange ndc_depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE) noexcept;

include/omath/engines/source_engine/formulas.hpp

@@ -12,6 +12,15 @@ namespace omath::source_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Vector3<float> right_vector(const ViewAngles& angles) noexcept;
 

include/omath/engines/unity_engine/formulas.hpp

@@ -21,6 +21,15 @@ namespace omath::unity_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Mat4X4 calc_perspective_projection_matrix(float field_of_view, float aspect_ratio, float near, float far,
                                              NDCDepthRange ndc_depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE) noexcept;

include/omath/engines/unreal_engine/formulas.hpp

@@ -21,6 +21,15 @@ namespace omath::unreal_engine
     [[nodiscard]]
     Mat4X4 rotation_matrix(const ViewAngles& angles) noexcept;
 
+    [[nodiscard]]
+    Vector3<double> extract_origin(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    Vector3<double> extract_scale(const Mat4X4& mat) noexcept;
+
+    [[nodiscard]]
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept;
+
     [[nodiscard]]
     Mat4X4 calc_perspective_projection_matrix(double field_of_view, double aspect_ratio, double near, double far,
                                              NDCDepthRange ndc_depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE) noexcept;

include/omath/linear_algebra/mat.hpp

@@ -5,6 +5,7 @@
 #include "vector3.hpp"
 #include <algorithm>
 #include <array>
+#include <cmath>
 #include <iomanip>
 #include <numeric>
 #include <sstream>
@@ -612,6 +613,53 @@ namespace omath
         };
     }
 
+    template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR>
+    [[nodiscard]]
+    constexpr Vector3<Type> mat_extract_origin(const Mat<4, 4, Type, St>& mat) noexcept
+    {
+        return {mat.at(0, 3), mat.at(1, 3), mat.at(2, 3)};
+    }
+
+    template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR>
+    [[nodiscard]]
+    Vector3<Type> mat_extract_scale(const Mat<4, 4, Type, St>& mat) noexcept
+    {
+        auto column_length = [](const Type x, const Type y, const Type z) {
+            return static_cast<Type>(std::sqrt(x * x + y * y + z * z));
+        };
+
+        const auto scale_x = column_length(mat.at(0, 0), mat.at(1, 0), mat.at(2, 0));
+        const auto scale_y = column_length(mat.at(0, 1), mat.at(1, 1), mat.at(2, 1));
+        const auto scale_z = column_length(mat.at(0, 2), mat.at(1, 2), mat.at(2, 2));
+
+        constexpr auto epsilon = std::numeric_limits<Type>::epsilon();
+
+        return {
+                std::abs(scale_x) < epsilon ? Type{1} : scale_x,
+                std::abs(scale_y) < epsilon ? Type{1} : scale_y,
+                std::abs(scale_z) < epsilon ? Type{1} : scale_z,
+        };
+    }
+
+    template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR>
+    requires std::is_floating_point_v<Type>
+    [[nodiscard]]
+    Vector3<Type> mat_extract_rotation_zyx(const Mat<4, 4, Type, St>& mat) noexcept
+    {
+        const auto scale = mat_extract_scale(mat);
+        const auto m00 = mat.at(0, 0) / scale.x;
+        const auto m10 = mat.at(1, 0) / scale.x;
+        const auto m20 = mat.at(2, 0) / scale.x;
+        const auto m21 = mat.at(2, 1) / scale.y;
+        const auto m22 = mat.at(2, 2) / scale.z;
+
+        return {
+                angles::radians_to_degrees(std::atan2(m21, m22)),
+                angles::radians_to_degrees(std::asin(std::clamp(-m20, Type{-1}, Type{1}))),
+                angles::radians_to_degrees(std::atan2(m10, m00)),
+        };
+    }
+
     template<class Type = float, MatStoreType St = MatStoreType::ROW_MAJOR, class Angle>
     [[nodiscard]]
     Mat<4, 4, Type, St> mat_rotation_axis_x(const Angle& angle) noexcept
@@ -854,4 +902,4 @@ struct std::formatter<omath::Mat<Rows, Columns, Type, StoreType>> // NOLINT(*-dc
         if constexpr (std::is_same_v<typename FormatContext::char_type, char8_t>)
             return std::format_to(ctx.out(), u8"{}", mat.to_u8string());
     }
-};
+};

source/engines/cry_engine/formulas.cpp

@@ -34,6 +34,27 @@ namespace omath::cry_engine
                * mat_rotation_axis_y<float, MatStoreType::ROW_MAJOR>(angles.roll)
                * mat_rotation_axis_x<float, MatStoreType::ROW_MAJOR>(angles.pitch);
     }
+
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.x),
+                YawAngle::from_degrees(angles.z),
+                RollAngle::from_degrees(angles.y),
+        };
+    }
+
     Mat4X4 calc_perspective_projection_matrix(const float field_of_view, const float aspect_ratio, const float near,
                                               const float far, const NDCDepthRange ndc_depth_range) noexcept
     {

source/engines/frostbite_engine/formulas.cpp

@@ -34,6 +34,27 @@ namespace omath::frostbite_engine
                * mat_rotation_axis_y<float, MatStoreType::ROW_MAJOR>(angles.yaw)
                * mat_rotation_axis_x<float, MatStoreType::ROW_MAJOR>(angles.pitch);
     }
+
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.x),
+                YawAngle::from_degrees(angles.y),
+                RollAngle::from_degrees(angles.z),
+        };
+    }
+
     Mat4X4 calc_perspective_projection_matrix(const float field_of_view, const float aspect_ratio, const float near,
                                               const float far, const NDCDepthRange ndc_depth_range) noexcept
     {

source/engines/iw_engine/formulas.cpp

@@ -30,6 +30,26 @@ namespace omath::iw_engine
         return mat_rotation_axis_z(angles.yaw) * mat_rotation_axis_y(angles.pitch) * mat_rotation_axis_x(angles.roll);
     }
 
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.y),
+                YawAngle::from_degrees(angles.z),
+                RollAngle::from_degrees(angles.x),
+        };
+    }
+
     Mat4X4 calc_view_matrix(const ViewAngles& angles, const Vector3<float>& cam_origin) noexcept
     {
         return mat_camera_view(forward_vector(angles), right_vector(angles), up_vector(angles), cam_origin);

source/engines/opengl_engine/formulas.cpp

@@ -36,6 +36,27 @@ namespace omath::opengl_engine
                * mat_rotation_axis_y<float, MatStoreType::COLUMN_MAJOR>(angles.yaw)
                * mat_rotation_axis_x<float, MatStoreType::COLUMN_MAJOR>(angles.pitch);
     }
+
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.x),
+                YawAngle::from_degrees(angles.y),
+                RollAngle::from_degrees(angles.z),
+        };
+    }
+
     Mat4X4 calc_perspective_projection_matrix(const float field_of_view, const float aspect_ratio, const float near,
                                               const float far, const NDCDepthRange ndc_depth_range) noexcept
     {

source/engines/source_engine/formulas.cpp

@@ -17,6 +17,26 @@ namespace omath::source_engine
         return mat_rotation_axis_z(angles.yaw) * mat_rotation_axis_y(angles.pitch) * mat_rotation_axis_x(angles.roll);
     }
 
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.y),
+                YawAngle::from_degrees(angles.z),
+                RollAngle::from_degrees(angles.x),
+        };
+    }
+
     Vector3<float> right_vector(const ViewAngles& angles) noexcept
     {
         const auto vec = rotation_matrix(angles) * mat_column_from_vector(k_abs_right);

source/engines/unity_engine/formulas.cpp

@@ -34,6 +34,27 @@ namespace omath::unity_engine
                * mat_rotation_axis_y<float, MatStoreType::ROW_MAJOR>(angles.yaw)
                * mat_rotation_axis_x<float, MatStoreType::ROW_MAJOR>(angles.pitch);
     }
+
+    Vector3<float> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<float> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(angles.x),
+                YawAngle::from_degrees(angles.y),
+                RollAngle::from_degrees(angles.z),
+        };
+    }
+
     Mat4X4 calc_perspective_projection_matrix(const float field_of_view, const float aspect_ratio, const float near,
                                               const float far, const NDCDepthRange ndc_depth_range) noexcept
     {

source/engines/unreal_engine/formulas.cpp

@@ -39,6 +39,26 @@ namespace omath::unreal_engine
     }
 
 
+    Vector3<double> extract_origin(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_origin(mat);
+    }
+
+    Vector3<double> extract_scale(const Mat4X4& mat) noexcept
+    {
+        return mat_extract_scale(mat);
+    }
+
+    ViewAngles extract_rotation_angles(const Mat4X4& mat) noexcept
+    {
+        const auto angles = mat_extract_rotation_zyx(mat);
+        return {
+                PitchAngle::from_degrees(-angles.y),
+                YawAngle::from_degrees(angles.z),
+                RollAngle::from_degrees(-angles.x),
+        };
+    }
+
     Mat4X4 calc_perspective_projection_matrix(const double field_of_view, const double aspect_ratio, const double near,
                                               const double far, const NDCDepthRange ndc_depth_range) noexcept
     {

tests/engines/unit_test_transform_extraction.cpp

@@ -0,0 +1,126 @@
+#include <gtest/gtest.h>
+#include <omath/engines/cry_engine/formulas.hpp>
+#include <omath/engines/frostbite_engine/formulas.hpp>
+#include <omath/engines/iw_engine/formulas.hpp>
+#include <omath/engines/opengl_engine/formulas.hpp>
+#include <omath/engines/source_engine/formulas.hpp>
+#include <omath/engines/unity_engine/formulas.hpp>
+#include <omath/engines/unreal_engine/formulas.hpp>
+
+template<class Type>
+static void expect_vector_near(const omath::Vector3<Type>& actual, const omath::Vector3<Type>& expected,
+                               const double epsilon)
+{
+    EXPECT_NEAR(static_cast<double>(actual.x), static_cast<double>(expected.x), epsilon);
+    EXPECT_NEAR(static_cast<double>(actual.y), static_cast<double>(expected.y), epsilon);
+    EXPECT_NEAR(static_cast<double>(actual.z), static_cast<double>(expected.z), epsilon);
+}
+
+template<class Mat4X4, class ViewAnglesType, class RotationMatrixFn, class ExtractOriginFn, class ExtractScaleFn,
+         class ExtractRotationAnglesFn>
+static void verify_transform_extractors(const omath::Vector3<typename Mat4X4::ContainedType>& origin,
+                                        const omath::Vector3<typename Mat4X4::ContainedType>& scale,
+                                        const ViewAnglesType& angles, RotationMatrixFn rotation_matrix,
+                                        ExtractOriginFn extract_origin, ExtractScaleFn extract_scale,
+                                        ExtractRotationAnglesFn extract_rotation_angles)
+{
+    using Type = typename Mat4X4::ContainedType;
+    constexpr auto store_ordering = Mat4X4::get_store_ordering();
+    const auto transform = omath::mat_translation<Type, store_ordering>(origin) * rotation_matrix(angles)
+                           * omath::mat_scale<Type, store_ordering>(scale);
+
+    expect_vector_near(extract_origin(transform), origin, 1e-4);
+    expect_vector_near(extract_scale(transform), scale, 1e-4);
+    expect_vector_near(extract_rotation_angles(transform).as_vector3(), angles.as_vector3(), 1e-3);
+}
+
+TEST(TransformExtraction, SourceEngine)
+{
+    namespace e = omath::source_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(20.f),
+            e::YawAngle::from_degrees(-35.f),
+            e::RollAngle::from_degrees(15.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({12.f, -3.f, 8.f}, {2.f, 3.f, 4.f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, IwEngine)
+{
+    namespace e = omath::iw_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(-18.f),
+            e::YawAngle::from_degrees(42.f),
+            e::RollAngle::from_degrees(-11.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({-7.f, 5.f, 13.f}, {1.5f, 2.5f, 3.5f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, UnrealEngine)
+{
+    namespace e = omath::unreal_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(30.0),
+            e::YawAngle::from_degrees(45.0),
+            e::RollAngle::from_degrees(-20.0),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({100.0, -50.0, 25.0}, {2.0, 4.0, 6.0}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, UnityEngine)
+{
+    namespace e = omath::unity_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(15.f),
+            e::YawAngle::from_degrees(-25.f),
+            e::RollAngle::from_degrees(35.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({4.f, 9.f, -2.f}, {0.5f, 3.f, 7.f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, FrostbiteEngine)
+{
+    namespace e = omath::frostbite_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(-12.f),
+            e::YawAngle::from_degrees(33.f),
+            e::RollAngle::from_degrees(-27.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({6.f, -8.f, 10.f}, {1.25f, 2.75f, 4.25f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, CryEngine)
+{
+    namespace e = omath::cry_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(-18.f),
+            e::YawAngle::from_degrees(40.f),
+            e::RollAngle::from_degrees(22.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({3.f, 14.f, -6.f}, {2.f, 5.f, 8.f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}
+
+TEST(TransformExtraction, OpenGlEngine)
+{
+    namespace e = omath::opengl_engine;
+    const e::ViewAngles angles{
+            e::PitchAngle::from_degrees(24.f),
+            e::YawAngle::from_degrees(-31.f),
+            e::RollAngle::from_degrees(17.f),
+    };
+
+    verify_transform_extractors<e::Mat4X4>({-9.f, 2.f, 11.f}, {3.f, 6.f, 9.f}, angles, e::rotation_matrix,
+                                          e::extract_origin, e::extract_scale, e::extract_rotation_angles);
+}