From 86a0a606d37e225c2772a038abfd9ce5d5a04226 Mon Sep 17 00:00:00 2001 From: iwubcode Date: Wed, 4 Jun 2025 22:32:05 -0500 Subject: [PATCH] Core: add ability to invert a 4x4 matrix --- Source/Core/Common/Matrix.cpp | 49 +++++++++++++++++++++++++++++++++++ Source/Core/Common/Matrix.h | 1 + 2 files changed, 50 insertions(+) diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index fe587de7cb..dd5574b6d4 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -378,4 +378,53 @@ float Matrix44::Determinant() const m[0] * m[9] * m[6] * m[15] - m[4] * m[1] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15]; } +Matrix44 Matrix44::Inverted() const +{ + const auto m = [this](int x, int y) { return data[y + x * 4]; }; + + const auto invdet = 1 / Determinant(); + + Matrix44 result; + + const auto minv = [&result](int x, int y) -> auto& { return result.data[y + x * 4]; }; + + const double A2323 = m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2); + const double A1323 = m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1); + const double A1223 = m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1); + const double A0323 = m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0); + const double A0223 = m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0); + const double A0123 = m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0); + const double A2313 = m(1, 2) * m(3, 3) - m(1, 3) * m(3, 2); + const double A1313 = m(1, 1) * m(3, 3) - m(1, 3) * m(3, 1); + const double A1213 = m(1, 1) * m(3, 2) - m(1, 2) * m(3, 1); + const double A2312 = m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2); + const double A1312 = m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1); + const double A1212 = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1); + const double A0313 = m(1, 0) * m(3, 3) - m(1, 3) * m(3, 0); + const double A0213 = m(1, 0) * m(3, 2) - m(1, 2) * m(3, 0); + const double A0312 = m(1, 0) * m(2, 3) - m(1, 3) * m(2, 0); + const double A0212 = m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0); + const double A0113 = m(1, 0) * m(3, 1) - m(1, 1) * m(3, 0); + const double A0112 = m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0); + + minv(0, 0) = invdet * (m(1, 1) * A2323 - m(1, 2) * A1323 + m(1, 3) * A1223); + minv(0, 1) = invdet * -(m(0, 1) * A2323 - m(0, 2) * A1323 + m(0, 3) * A1223); + minv(0, 2) = invdet * (m(0, 1) * A2313 - m(0, 2) * A1313 + m(0, 3) * A1213); + minv(0, 3) = invdet * -(m(0, 1) * A2312 - m(0, 2) * A1312 + m(0, 3) * A1212); + minv(1, 0) = invdet * -(m(1, 0) * A2323 - m(1, 2) * A0323 + m(1, 3) * A0223); + minv(1, 1) = invdet * (m(0, 0) * A2323 - m(0, 2) * A0323 + m(0, 3) * A0223); + minv(1, 2) = invdet * -(m(0, 0) * A2313 - m(0, 2) * A0313 + m(0, 3) * A0213); + minv(1, 3) = invdet * (m(0, 0) * A2312 - m(0, 2) * A0312 + m(0, 3) * A0212); + minv(2, 0) = invdet * (m(1, 0) * A1323 - m(1, 1) * A0323 + m(1, 3) * A0123); + minv(2, 1) = invdet * -(m(0, 0) * A1323 - m(0, 1) * A0323 + m(0, 3) * A0123); + minv(2, 2) = invdet * (m(0, 0) * A1313 - m(0, 1) * A0313 + m(0, 3) * A0113); + minv(2, 3) = invdet * -(m(0, 0) * A1312 - m(0, 1) * A0312 + m(0, 3) * A0112); + minv(3, 0) = invdet * -(m(1, 0) * A1223 - m(1, 1) * A0223 + m(1, 2) * A0123); + minv(3, 1) = invdet * (m(0, 0) * A1223 - m(0, 1) * A0223 + m(0, 2) * A0123); + minv(3, 2) = invdet * -(m(0, 0) * A1213 - m(0, 1) * A0213 + m(0, 2) * A0113); + minv(3, 3) = invdet * (m(0, 0) * A1212 - m(0, 1) * A0212 + m(0, 2) * A0112); + + return result; +} + } // namespace Common diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index 455f2d4146..70ccc0afad 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -434,6 +434,7 @@ public: Vec3 Transform(const Vec3& point, float w) const; float Determinant() const; + Matrix44 Inverted() const; Matrix44& operator*=(const Matrix44& rhs) {