diff --git a/include/math/seadMatrix.h b/include/math/seadMatrix.h index fd5af108..3422fc50 100644 --- a/include/math/seadMatrix.h +++ b/include/math/seadMatrix.h @@ -198,6 +198,8 @@ class Matrix34 : public Policies::Mtx34Base Vec3 getRotation() const; void getBase(Vec3& o, s32 axis) const; + template + void getBase(Vec3& o) const; void getRow(Vec4& o, s32 row) const; void getTranslation(Vec3& o) const; void getRotation(Vec3& o) const; diff --git a/include/math/seadMatrix.hpp b/include/math/seadMatrix.hpp index 858f1555..2b112d04 100644 --- a/include/math/seadMatrix.hpp +++ b/include/math/seadMatrix.hpp @@ -563,6 +563,13 @@ inline void Matrix34::getBase(Vec3& o, s32 axis) const Matrix34CalcCommon::getBase(o, *this, axis); } +template +template +inline void Matrix34::getBase(Vec3& o) const +{ + Matrix34CalcCommon::template getBase(o, *this); +} + template inline void Matrix34::getRow(Vec4& o, s32 row) const { diff --git a/include/math/seadMatrixCalcCommon.h b/include/math/seadMatrixCalcCommon.h index 427442d7..0f079d28 100644 --- a/include/math/seadMatrixCalcCommon.h +++ b/include/math/seadMatrixCalcCommon.h @@ -113,6 +113,8 @@ class Matrix34CalcCommon static void toQuat(Quat& q, const Base& n); static void getBase(Vec3& v, const Base& n, s32 axis); + template + static void getBase(Vec3& v, const Base& n); static void getRow(Vec4& v, const Base& n, s32 row); static void getTranslation(Vec3& v, const Base& n); static void getRotation(Vec3& v, const Base& n); diff --git a/include/math/seadMatrixCalcCommon.hpp b/include/math/seadMatrixCalcCommon.hpp index de6a03f2..73d7c446 100644 --- a/include/math/seadMatrixCalcCommon.hpp +++ b/include/math/seadMatrixCalcCommon.hpp @@ -6,6 +6,7 @@ #ifdef __aarch64__ #include +#include #endif #include @@ -1807,6 +1808,38 @@ void Matrix34CalcCommon::getBase(Vec3& v, const Base& n, s32 axis) v.z = n.m[2][axis]; } +template +template +void Matrix34CalcCommon::getBase(Vec3& v, const Base& n) +{ + v.x = n.m[0][Axis]; + v.y = n.m[1][Axis]; + v.z = n.m[2][Axis]; +} + +#ifdef __aarch64__ +template <> +template +void Matrix34CalcCommon::getBase(Vec3& v, const Base& n) +{ + const f32* next_row; + if (Axis == 0) + next_row = &n.m[1][Axis]; + + f32 first; + __atomic_load(&n.m[0][Axis], &first, __ATOMIC_RELAXED); + float32x2_t xy = {first, 0.0f}; + + if (Axis != 0) + next_row = &n.m[1][Axis]; + + xy[1] = *next_row; + const f32 z = n.m[2][Axis]; + MemUtil::copy(&v.x, &xy, sizeof(xy)); + v.z = z; +} +#endif + template void Matrix34CalcCommon::getRow(Vec4& v, const Base& n, s32 row) {