diff --git a/include/mtl/mat.hpp b/include/mtl/mat.hpp new file mode 100644 index 0000000..1f48d22 --- /dev/null +++ b/include/mtl/mat.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include "mtl/target.hpp" + +#include + +#include "mtl/fixed.hpp" + +namespace mtl { + +template +class vec; + +template +class mat { +public: + fixed e[M][N]; + + constexpr mat() noexcept {} + + mat(const vec& v) noexcept { + for (size_t i = 0; i < M; ++i) { + e[i][0] = v[i]; + } + } + constexpr mat(const fixed(&_e)[M][N]) noexcept { + for (size_t i = 0; i < M; ++i) { + for (size_t j = 0; j < N; ++j) { + e[i][j] = _e[i][j]; + } + } + } + + vec row(size_t i) const noexcept { + return vec(e[i]); + } + vec col(size_t i) const noexcept { + vec r; + + for (size_t j = 0; j < M; ++j) { + r.e[j] = e[j][i]; + } + + return r; + } + + ARM_MODE GBA_IWRAM + mat operator+(const mat& rhs) const; + ARM_MODE GBA_IWRAM + mat operator-(const mat& rhs) const; + + ARM_MODE GBA_IWRAM + mat operator*(fixed rhs) const; + ARM_MODE GBA_IWRAM + mat operator/(fixed rhs) const; + + ARM_MODE GBA_IWRAM + vec operator*(const vec& rhs) const noexcept; + + ARM_MODE GBA_IWRAM + bool operator==(const mat& rhs) const; + bool operator!=(const mat& rhs) const { + return !(*this == rhs); + } + + template + ARM_MODE GBA_IWRAM + mat operator*(const mat& rhs) const noexcept; + + template + friend mat operator*(const vec& lhs, const mat<1, S>& rhs); + + template = true> + static mat identity() { + mat r; + + for (size_t i = 0; i < D; ++i) { + r.e[i][i] = 1; + } + + return r; + } + + template = true> + static mat translation(const vec& v); // Not defined in header because `v` will rarely be an r-value +}; + +template +ARM_MODE GBA_IWRAM +mat operator*(const vec& lhs, const mat<1, S>& rhs); + +} // namespace mtl + diff --git a/include/mtl/tests/mat.hpp b/include/mtl/tests/mat.hpp new file mode 100644 index 0000000..58c19a2 --- /dev/null +++ b/include/mtl/tests/mat.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include "mtl/test.hpp" +#include "mtl/target.hpp" + +namespace mtl { + +namespace test { + +class mat_suite : public suite { +public: + mat_suite() { + add_test(&construction_m2, "construction_m2"); + + add_test(&addition_m2, "addition_m2"); + add_test(&addition_m3, "addition_m3"); + add_test(&addition_m4, "addition_m4"); + + add_test(&subtraction_m2, "subtraction_m2"); + add_test(&subtraction_m3, "subtraction_m3"); + add_test(&subtraction_m4, "subtraction_m4"); + + add_test(&mult_vec_m2, "mult_vec_m2"); + add_test(&mult_vec_m3, "mult_vec_m3"); + add_test(&mult_vec_m4, "mult_vec_m4"); + + add_test(&mult_mat_m2, "mult_mat_m2"); + add_test(&mult_mat_m3, "mult_mat_m3"); + add_test(&mult_mat_m4, "mult_mat_m4"); + + add_test(&projection_build_m4, "projection_build_m4"); + add_test(&projection_calc_m4, "projection_calc_m4"); + } + + virtual string_view name() { + return "mat_suite"; + } + + GBA_IWRAM ARM_MODE static bool construction_m2(); + + GBA_IWRAM ARM_MODE static bool addition_m2(); + GBA_IWRAM ARM_MODE static bool addition_m3(); + GBA_IWRAM ARM_MODE static bool addition_m4(); + + GBA_IWRAM ARM_MODE static bool subtraction_m2(); + GBA_IWRAM ARM_MODE static bool subtraction_m3(); + GBA_IWRAM ARM_MODE static bool subtraction_m4(); + + GBA_IWRAM ARM_MODE static bool mult_vec_m2(); + GBA_IWRAM ARM_MODE static bool mult_vec_m3(); + GBA_IWRAM ARM_MODE static bool mult_vec_m4(); + + GBA_IWRAM ARM_MODE static bool mult_mat_m2(); + GBA_IWRAM ARM_MODE static bool mult_mat_m3(); + GBA_IWRAM ARM_MODE static bool mult_mat_m4(); + + GBA_IWRAM ARM_MODE static bool projection_build_m4(); + GBA_IWRAM ARM_MODE static bool projection_calc_m4(); +}; + +} // namespace test + +} // namespace mtl + diff --git a/src/mat.cpp b/src/mat.cpp new file mode 100644 index 0000000..2a6c1f2 --- /dev/null +++ b/src/mat.cpp @@ -0,0 +1,148 @@ +#pragma GCC push_options +#pragma GCC optimize("unroll-loops") + +#include "mtl/mat.hpp" + +#include "mtl/vec.hpp" + +namespace mtl { + +template +mat mat::operator+(const mat& rhs) const { + mat r; + + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < N; ++in) { + r.e[im][in] = e[im][in] + rhs.e[im][in]; + } + } + + return r; +} +template +mat mat::operator-(const mat& rhs) const { + mat r; + + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < N; ++in) { + r.e[im][in] = e[im][in] - rhs.e[im][in]; + } + } + + return r; +} + +template +mat mat::operator*(fixed rhs) const { + mat r; + + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < N; ++in) { + r.e[im][in] = e[im][in] * rhs; + } + } + + return r; +} +template +mat mat::operator/(fixed rhs) const { + mat r; + + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < N; ++in) { + r.e[im][in] = e[im][in] / rhs; + } + } + + return r; +} + +template +vec mat::operator*(const vec& rhs) const noexcept { + vec r; + + for (size_t i = 0; i < M; ++i) { + r.e[i] = row(i) * rhs; + } + + return r; +} + +template +template +mat mat::operator*(const mat& rhs) const noexcept { + mat r; + + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < RHS_N; ++in) { + r.e[im][in] = row(im) * rhs.col(in); + } + } + + return r; +} + +template +mat operator*(const vec& lhs, const mat<1, S>& rhs) { + mat r; + + for (size_t im = 0; im < S; ++im) { + for (size_t in = 0; in < S; ++in) { + r.e[im][in] = lhs[im] * rhs.e[0][in]; + } + } + + return r; +} + +template +bool mat::operator==(const mat& rhs) const { + for (size_t im = 0; im < M; ++im) { + for (size_t in = 0; in < N; ++in) { + if (e[im][in] != rhs.e[im][in]) { + return false; + } + } + } + + return true; +} + +template <> +template > +mat<4, 4> mat<4, 4>::translation(const vec& v) { + mat<4, 4> r; + + r.e[0][0] = 1; + r.e[1][1] = 1; + r.e[2][2] = 1; + r.e[3][3] = 1; + + r.e[0][3] = v.e[0]; + r.e[1][3] = v.e[1]; + r.e[2][3] = v.e[2]; + + return r; +} + +template class mat<2, 2>; +template class mat<3, 3>; +template class mat<4, 4>; +template class mat<1, 2>; +template class mat<1, 3>; +template class mat<1, 4>; + +template mat<2, 2> operator*(const vec<2>&, const mat<1, 2>&); +template mat<3, 3> operator*(const vec<3>&, const mat<1, 3>&); +template mat<4, 4> operator*(const vec<4>&, const mat<1, 4>&); + +template mat<4, 4> mat<4, 4>::operator*(const mat<4, 4>&) const; +template mat<3, 3> mat<3, 3>::operator*(const mat<3, 3>&) const; +template mat<2, 2> mat<2, 2>::operator*(const mat<2, 2>&) const; + +template mat<4, 4> mat<4, 4>::translation<4, true>(const vec<3>&); + +} // namespace mtl + +#pragma GCC pop_options + diff --git a/src/tests/mat.cpp b/src/tests/mat.cpp new file mode 100644 index 0000000..e8ca815 --- /dev/null +++ b/src/tests/mat.cpp @@ -0,0 +1,446 @@ +#include "mtl/tests/mat.hpp" + +#include "mtl/target.hpp" +#include "mtl/mat.hpp" +#include "mtl/vec.hpp" + +namespace mtl { +namespace test { + +bool mat_suite::construction_m2() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<2, 2> operator()(fixed a, fixed b, fixed c, fixed d) { + start_timer(); + mat<2, 2> r({ + { a, b }, + { c, d }, + }); + end_timer(); + return r; + } + } f; + + mat<2, 2> a = f(1, 2, 3, 4); + + return a.row(0)[0] == 1 && a.row(0)[1] == 2 && + a.row(1)[0] == 3 && a.row(1)[1] == 4; +} + +bool mat_suite::addition_m2() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<2, 2> operator()(mat<2, 2> a, mat<2, 2> b) { + start_timer(); + mat<2, 2> r = a + b; + end_timer(); + return r; + } + } f; + + mat<2, 2> a({ + { 1, 2 }, + { 3, 4 }, + }); + mat<2, 2> b({ + { 10, 20 }, + { 30, 40 }, + }); + + mat<2, 2> c = f(a, b); + + mat<2, 2> exp({ + { 11, 22 }, + { 33, 44 }, + }); + + return c == exp; +} +bool mat_suite::addition_m3() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<3, 3> operator()(mat<3, 3> a, mat<3, 3> b) { + start_timer(); + mat<3, 3> r = a + b; + end_timer(); + return r; + } + } f; + + mat<3, 3> a({ + { 1, 2, 3 }, + { 4, 5, 6 }, + { 7, 8, 9 }, + }); + mat<3, 3> b({ + { 10, 20, 30 }, + { 40, 50, 60 }, + { 70, 80, 90 }, + }); + + mat<3, 3> c = f(a, b); + + mat<3, 3> exp({ + { 11, 22, 33 }, + { 44, 55, 66 }, + { 77, 88, 99 }, + }); + + return c == exp; +} +bool mat_suite::addition_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<4, 4> operator()(mat<4, 4> a, mat<4, 4> b) { + start_timer(); + mat<4, 4> r = a + b; + end_timer(); + return r; + } + } f; + + mat<4, 4> a({ + { 1, 2, 3, 4 }, + { 5, 6, 7, 8 }, + { 9, 1, 2, 3 }, + { 4, 5, 6, 7 }, + }); + mat<4, 4> b({ + { 10, 20, 30, 40 }, + { 50, 60, 70, 80 }, + { 90, 10, 20, 30 }, + { 40, 50, 60, 70 }, + }); + + mat<4, 4> c = f(a, b); + + mat<4, 4> exp({ + { 11, 22, 33, 44 }, + { 55, 66, 77, 88 }, + { 99, 11, 22, 33 }, + { 44, 55, 66, 77 }, + }); + + return c == exp; +} + +bool mat_suite::subtraction_m2() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<2, 2> operator()(mat<2, 2> a, mat<2, 2> b) { + start_timer(); + mat<2, 2> r = a - b; + end_timer(); + return r; + } + } f; + + mat<2, 2> a({ + { 11, 22 }, + { 33, 44 }, + }); + mat<2, 2> b({ + { 1, 2 }, + { 3, 4 }, + }); + + mat<2, 2> c = f(a, b); + + mat<2, 2> exp({ + { 10, 20 }, + { 30, 40 }, + }); + + return c == exp; +} +bool mat_suite::subtraction_m3() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<3, 3> operator()(mat<3, 3> a, mat<3, 3> b) { + start_timer(); + mat<3, 3> r = a - b; + end_timer(); + return r; + } + } f; + + mat<3, 3> a({ + { 11, 22, 33 }, + { 44, 55, 66 }, + { 77, 88, 99 }, + }); + + mat<3, 3> b({ + { 1, 2, 3 }, + { 4, 5, 6 }, + { 7, 8, 9 }, + }); + + mat<3, 3> c = f(a, b); + + mat<3, 3> exp({ + { 10, 20, 30 }, + { 40, 50, 60 }, + { 70, 80, 90 }, + }); + + return c == exp; +} +bool mat_suite::subtraction_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<4, 4> operator()(mat<4, 4> a, mat<4, 4> b) { + start_timer(); + mat<4, 4> r = a - b; + end_timer(); + return r; + } + } f; + + mat<4, 4> a({ + { 11, 22, 33, 44 }, + { 55, 66, 77, 88 }, + { 99, 11, 22, 33 }, + { 44, 55, 66, 77 }, + }); + + mat<4, 4> b({ + { 1, 2, 3, 4 }, + { 5, 6, 7, 8 }, + { 9, 1, 2, 3 }, + { 4, 5, 6, 7 }, + }); + + mat<4, 4> c = f(a, b); + + mat<4, 4> exp({ + { 10, 20, 30, 40 }, + { 50, 60, 70, 80 }, + { 90, 10, 20, 30 }, + { 40, 50, 60, 70 }, + }); + + return c == exp; +} + +bool mat_suite::mult_vec_m2() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + vec2 operator()(mat<2, 2> a, vec2 b) { + start_timer(); + vec2 r = a * b; + end_timer(); + return r; + } + } f; + + mat<2, 2> a({ + { 1, 2 }, // col 1 + { 3, 4 } // col 2 + }); + + vec2 b(1, 2); + + vec2 c = f(a, b); + + return true; +} +bool mat_suite::mult_vec_m3() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + vec3 operator()(mat<3, 3> a, vec3 b) { + start_timer(); + vec3 r = a * b; + end_timer(); + return r; + } + } f; + + mat<3, 3> a({ + { 1, 2, 3 }, // col 1 + { 4, 5, 6 }, // col 2 + { 7, 8, 9 }, // col 3 + }); + + vec3 b(1, 2, 3); + + f(a, b); + + return true; +} +bool mat_suite::mult_vec_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + vec4 operator()(mat<4, 4> a, vec4 b) { + start_timer(); + vec4 r = a * b; + end_timer(); + return r; + } + } f; + + mat<4, 4> a({ + { 1, 2, 3, 4 }, // col 1 + { 5, 6, 7, 8 }, // col 2 + { 9, 1, 2, 3 }, // col 3 + { 4, 5, 6, 7 } // col 4 + }); + + vec4 b(1, 2, 3, 4); + + f(a, b); + + return true; +} + +bool mat_suite::mult_mat_m2() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<2, 2> operator()(mat<2, 2> a, mat<2, 2> b) { + start_timer(); + mat<2, 2> r = a * b; + end_timer(); + return r; + } + } f; + + mat<2, 2> a({ + { 1, 2 }, + { 3, 4 }, + }); + + mat<2, 2> b({ + { 11, 22 }, + { 33, 44 }, + }); + + mat<2, 2> exp({ + { 77, 110 }, + { 165, 242 }, + }); + + mat<2, 2> c = f(a, b); + + return c == exp; +} +bool mat_suite::mult_mat_m3() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<3, 3> operator()(mat<3, 3> a, mat<3, 3> b) { + start_timer(); + mat<3, 3> r = a * b; + end_timer(); + return r; + } + } f; + + mat<3, 3> a({ + { 1, 2, 3 }, + { 4, 5, 6 }, + { 7, 8, 9 }, + }); + + mat<3, 3> b({ + { 11, 22, 33 }, + { 44, 55, 66 }, + { 77, 88, 99 }, + }); + + mat<3, 3> exp({ + { 330, 396, 462 }, + { 726, 891, 1056 }, + { 1122, 1386, 1650 }, + }); + + mat<3, 3> c = f(a, b); + + return c == exp; +} +bool mat_suite::mult_mat_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<4, 4> operator()(mat<4, 4> a, mat<4, 4> b) { + start_timer(); + mat<4, 4> r = a * b; + end_timer(); + return r; + } + } f; + + mat<4, 4> a({ + { 1, 2, 3, 4 }, + { 5, 6, 7, 8 }, + { 9, 1, 2, 3 }, + { 4, 5, 6, 7 }, + }); + + mat<4, 4> b({ + { 11, 22, 33, 44 }, + { 55, 66, 77, 88 }, + { 99, 11, 22, 33 }, + { 44, 55, 66, 77 }, + }); + + mat<4, 4> exp({ + { 594, 407, 517, 627 }, + { 1430, 1023, 1309, 1595 }, + { 484, 451, 616, 781 }, + { 1221, 869, 1111, 1353 }, + }); + + mat<4, 4> c = f(a, b); + + return c == exp; +} + +bool mat_suite::projection_build_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + mat<4, 4> operator()(vec4 b) { + start_timer(); + mat<4, 4> r = b * b.transpose() / b.magnitude_sqr(); + end_timer(); + return r; + } + } f; + + vec4 b(-3, 4, 14, 0); + + f(b); + + return false; +} +bool mat_suite::projection_calc_m4() { + struct { + NOINLINE GBA_IWRAM ARM_MODE + vec4 operator()(mat<4, 4> A, vec4 b) { + start_timer(); + vec4 r = A * b; + end_timer(); + return r; + } + } f; + + vec4 a(8, 3, 7, 0); + vec4 b(-3, 4, 14, 0); + + mat<4, 4> A = b * b.transpose() / b.magnitude_sqr(); + + vec4 c = f(A, a); + + vec4 exp(fixed::from_raw(-74), fixed::from_raw(99), fixed::from_raw(348), 0); + + vec4 exact_value(-1.1674, 1.5566, 5.4480, 0); + + log::debug << "A = " << a << endl; + log::debug << "B = " << b << endl; + log::debug << "C = proj(A, B) = " << c << endl; + log::debug << "Exact value: " << exact_value << endl; + log::debug << "Divergence: " << exact_value - c << endl; + + return c == exp; +} + +} // namespace test +} // namespace mtl +