m3f inverse

1 files changed, 28 insertions, 0 deletions

diff --git a/maths.cpp b/maths.cpp
index 90d0e03..39a1163 100644
--- a/maths.cpp
+++ b/maths.cpp
@@ -446,3 +446,31 @@ v3f m3f::operator*(const v3f& other) const {
 		m[0][2] * other.x + m[1][2] * other.y + m[2][2] * other.z
 	);
 }
+
+m3f m3f::inverse() const {
+	m3f r;
+	r.m[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
+	r.m[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
+	r.m[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
+	r.m[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
+	r.m[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
+	r.m[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];
+	r.m[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
+	r.m[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
+	r.m[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
+	float d =
+		m[0][0] * r.m[0][0] +
+		m[0][1] * r.m[1][0] +
+		m[0][2] * r.m[2][0];
+	d = 1.0f / d;
+	r.m[0][0] *= d;
+	r.m[0][1] *= d;
+	r.m[0][2] *= d;
+	r.m[1][0] *= d;
+	r.m[1][1] *= d;
+	r.m[1][2] *= d;
+	r.m[2][0] *= d;
+	r.m[2][1] *= d;
+	r.m[2][2] *= d;
+	return r;
+}