diff --git a/src/lol/math/transform.h b/src/lol/math/transform.h index bf951f63..5fc7bb2a 100644 --- a/src/lol/math/transform.h +++ b/src/lol/math/transform.h @@ -460,6 +460,24 @@ static inline quat_t<T> operator /(quat_t<T> const &x, quat_t<T> const &y) template<typename T> extern quat_t<T> slerp(quat_t<T> const &qa, quat_t<T> const &qb, T f); +/* + * SQTs only + */ + +template<typename T> +static inline sqt_t<T> inverse(sqt_t<T> const &tr) +{ + auto inv_s = T(1) / tr.s; + auto inv_q = re(tr.q); + return sqt_t<T>(inv_s, inv_q, inv_q * tr.t * -inv_s); +} + +template<typename T> +static inline sqt_t<T> operator /(sqt_t<T> const &x, sqt_t<T> const &y) +{ + return x * inverse(y); +} + #if !LOL_FEATURE_CXX11_CONSTEXPR #undef constexpr #endif diff --git a/src/t/math/sqt.cpp b/src/t/math/sqt.cpp index 5bdff20c..6e170a0b 100644 --- a/src/t/math/sqt.cpp +++ b/src/t/math/sqt.cpp @@ -72,6 +72,19 @@ lolunit_declare_fixture(sqt_test) } } + lolunit_declare_test(sqt_inverse) + { + for (vec4 v0 : test_vec4s) + { + vec4 v1 = inverse(test_sqt_1) * (test_sqt_1 * v0); + + lolunit_assert_doubles_equal(v0.x, v1.x, 1e-5f); + lolunit_assert_doubles_equal(v0.y, v1.y, 1e-5f); + lolunit_assert_doubles_equal(v0.z, v1.z, 1e-5f); + lolunit_assert_doubles_equal(v0.w, v1.w, 1e-5f); + } + } + lolunit_declare_test(sqt_composition_vec4) { /* We check that transforming a vec4 with an SQT and a