From ebdcb4f4957c573fe375443d954ccaa94e62cedc Mon Sep 17 00:00:00 2001
From: BlackMATov <blackmat@mail.ru>
Date: Sat, 27 Feb 2021 07:35:53 +0700
Subject: [PATCH] trs function

---
 README.md                       | 18 +++++++++++++
 headers/vmath.hpp/vmath_ext.hpp | 48 +++++++++++++++++++++++++++++++++
 untests/vmath_ext_tests.cpp     | 23 ++++++++++++++++
 untests/vmath_fix_tests.cpp     |  8 ++++++
 4 files changed, 97 insertions(+)

diff --git a/README.md b/README.md
index e6f9265..bd4d9f6 100644
--- a/README.md
+++ b/README.md
@@ -1799,6 +1799,18 @@ qua<T> imag(qua<T> q, const vec<T, 3>& imag);
 ### Matrix Transform 3D
 
 ```cpp
+template < typename T >
+mat<T, 4> trs(const vec<T, 3>& t, const mat<T, 3>& r);
+
+template < typename T >
+mat<T, 4> trs(const vec<T, 3>& t, const mat<T, 3>& r, const vec<T, 3>& s);
+
+template < typename T >
+mat<T, 4> trs(const vec<T, 3>& t, const qua<T>& r);
+
+template < typename T >
+mat<T, 4> trs(const vec<T, 3>& t, const qua<T>& r, const vec<T, 3>& s);
+
 template < typename T >
 mat<T, 4> translate(const vec<T, 3>& v);
 
@@ -1893,6 +1905,12 @@ mat<T, 4> look_at_rh(const vec<T, 3>& eye, const vec<T, 3>& at, const vec<T, 3>&
 ### Matrix Transform 2D
 
 ```cpp
+template < typename T >
+mat<T, 3> trs(const vec<T, 2>& t, const mat<T, 2>& r);
+
+template < typename T >
+mat<T, 3> trs(const vec<T, 2>& t, const mat<T, 2>& r, const vec<T, 2>& s);
+
 template < typename T >
 mat<T, 3> translate(const vec<T, 2>& v);
 
diff --git a/headers/vmath.hpp/vmath_ext.hpp b/headers/vmath.hpp/vmath_ext.hpp
index 7883705..defb556 100644
--- a/headers/vmath.hpp/vmath_ext.hpp
+++ b/headers/vmath.hpp/vmath_ext.hpp
@@ -230,6 +230,36 @@ namespace vmath_hpp
 
 namespace vmath_hpp
 {
+    // trs
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 4> trs(const vec<T, 3>& t, const mat<T, 3>& r) {
+        return {
+            { r[0], T{0} },
+            { r[1], T{0} },
+            { r[2], T{0} },
+            { t,    T{1} }};
+    }
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 4> trs(const vec<T, 3>& t, const mat<T, 3>& r, const vec<T, 3>& s) {
+        return {
+            { r[0] * s[0], T{0} },
+            { r[1] * s[1], T{0} },
+            { r[2] * s[2], T{0} },
+            { t,           T{1} }};
+    }
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 4> trs(const vec<T, 3>& t, const qua<T>& r) {
+        return trs(t, rotate(r));
+    }
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 4> trs(const vec<T, 3>& t, const qua<T>& r, const vec<T, 3>& s) {
+        return trs(t, rotate(r), s);
+    }
+
     // translate
 
     template < typename T >
@@ -555,6 +585,24 @@ namespace vmath_hpp
 
 namespace vmath_hpp
 {
+    // trs
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 3> trs(const vec<T, 2>& t, const mat<T, 2>& r) {
+        return {
+            { r[0], T{0} },
+            { r[1], T{0} },
+            { t,    T{1} }};
+    }
+
+    template < typename T >
+    [[nodiscard]] constexpr mat<T, 3> trs(const vec<T, 2>& t, const mat<T, 2>& r, const vec<T, 2>& s) {
+        return {
+            { r[0] * s[0], T{0} },
+            { r[1] * s[1], T{0} },
+            { t,           T{1} }};
+    }
+
     // translate
 
     template < typename T >
diff --git a/untests/vmath_ext_tests.cpp b/untests/vmath_ext_tests.cpp
index 694204e..0b71573 100644
--- a/untests/vmath_ext_tests.cpp
+++ b/untests/vmath_ext_tests.cpp
@@ -218,6 +218,29 @@ TEST_CASE("vmath/ext/access") {
 }
 
 TEST_CASE("vmath/ext/matrix_transform") {
+    SUBCASE("trs") {
+        CHECK(all(approx(
+            trs(fvec3(1,2,3), rotate(pi, fvec3{1,2,3})),
+            rotate4(pi, fvec3{1,2,3}) * translate(fvec3(1,2,3)))));
+        CHECK(all(approx(
+            trs(fvec3(1,2,3), rotate(pi, fvec3{1,2,3}), fvec3(2,3,4)),
+            scale4(fvec3(2,3,4)) * rotate4(pi, fvec3(1,2,3)) * translate(fvec3(1,2,3)))));
+
+        CHECK(all(approx(
+            trs(fvec3(1,2,3), qrotate(pi, fvec3{1,2,3})),
+            rotate4(qrotate(pi, fvec3{1,2,3})) * translate(fvec3(1,2,3)))));
+        CHECK(all(approx(
+            trs(fvec3(1,2,3), qrotate(pi, fvec3{1,2,3}), fvec3(2,3,4)),
+            scale4(fvec3(2,3,4)) * rotate4(qrotate(pi, fvec3{1,2,3})) * translate(fvec3(1,2,3)))));
+
+        CHECK(all(approx(
+            trs(fvec2(1,2), rotate(pi)),
+            rotate3(pi) * translate(fvec2(1,2)))));
+        CHECK(all(approx(
+            trs(fvec2(1,2), rotate(pi), fvec2(2,3)),
+            scale3(fvec2(2,3)) * rotate3(pi) * translate(fvec2(1,2)))));
+    }
+
     SUBCASE("translate") {
         STATIC_CHECK(fvec3(2.f,3.f,1.f) * translate(fvec2{1.f,2.f}) == uapprox3(3.f,5.f,1.f));
         STATIC_CHECK(fvec3(2.f,3.f,1.f) * translate(translate(fvec2{1.f,2.f}), fvec2{1.f,2.f}) == uapprox3(4.f,7.f,1.f));
diff --git a/untests/vmath_fix_tests.cpp b/untests/vmath_fix_tests.cpp
index 8c786df..dafbd34 100644
--- a/untests/vmath_fix_tests.cpp
+++ b/untests/vmath_fix_tests.cpp
@@ -383,6 +383,11 @@ namespace vmath_hpp
 
 namespace vmath_hpp
 {
+    template fix4x4f trs(const fix3f&, const fix3x3f&);
+    template fix4x4f trs(const fix3f&, const fix3x3f&, const fix3f&);
+    template fix4x4f trs(const fix3f&, const qfix&);
+    template fix4x4f trs(const fix3f&, const qfix&, const fix3f&);
+
     template fix4x4f translate(const fix3f&);
     template fix4x4f translate(const fix4x4f&, const fix3f&);
 
@@ -429,6 +434,9 @@ namespace vmath_hpp
 
 namespace vmath_hpp
 {
+    template fix3x3f trs(const fix2f&, const fix2x2f&);
+    template fix3x3f trs(const fix2f&, const fix2x2f&, const fix2f&);
+
     template fix3x3f translate(const fix2f&);
     template fix3x3f translate(const fix3x3f&, const fix2f&);