1 files changed, 43 insertions, 0 deletions
diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index 9581a5a4ce..e64cc91be0 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/Source/Core/Common/Matrix.cpp
@@ -45,6 +45,22 @@ Matrix33 Matrix33::Identity()
   return mtx;
 }
 
+Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw)
+{
+  // Normalize.
+  const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
+  qx *= n;
+  qy *= n;
+  qz *= n;
+  qw *= n;
+
+  return {
+      1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,     2 * qx * qz + 2 * qy * qw,
+      2 * qx * qy + 2 * qz * qw,     1 - 2 * qx * qx - 2 * qz * qz, 2 * qy * qz - 2 * qx * qw,
+      2 * qx * qz - 2 * qy * qw,     2 * qy * qz + 2 * qx * qw,     1 - 2 * qx * qx - 2 * qy * qy,
+  };
+}
+
 Matrix33 Matrix33::RotateX(float rad)
 {
   const float s = std::sin(rad);
@@ -120,6 +136,33 @@ void Matrix33::Multiply(const Matrix33& a, const Vec3& vec, Vec3* result)
   result->data = MatrixMultiply<3, 3, 1>(a.data, vec.data);
 }
 
+Matrix33 Matrix33::Inverted() const
+{
+  const auto m = [this](int x, int y) { return data[y + x * 3]; };
+
+  const auto det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
+                   m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
+                   m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
+
+  const auto invdet = 1 / det;
+
+  Matrix33 result;
+
+  const auto minv = [&result](int x, int y) -> auto& { return result.data[y + x * 3]; };
+
+  minv(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) * invdet;
+  minv(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * invdet;
+  minv(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * invdet;
+  minv(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * invdet;
+  minv(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * invdet;
+  minv(1, 2) = (m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2)) * invdet;
+  minv(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * invdet;
+  minv(2, 1) = (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) * invdet;
+  minv(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * invdet;
+
+  return result;
+}
+
 Matrix44 Matrix44::Identity()
 {
   Matrix44 mtx = {};