WiimoteEmu: Reimplement tilt/swing/camera/orientation data using matrix math.

1 files changed, 48 insertions, 24 deletions

diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index dd394b7be5..7efac12f78 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/Source/Core/Common/Matrix.cpp
@@ -9,21 +9,29 @@
 
 namespace
 {
-void MatrixMul(int n, const float* a, const float* b, float* result)
+// Multiply a NxM matrix by a NxP matrix.
+template <int N, int M, int P, typename T>
+auto MatrixMultiply(const std::array<T, N * M>& a, const std::array<T, M * P>& b)
+    -> std::array<T, N * P>
 {
-  for (int i = 0; i < n; ++i)
+  std::array<T, N * P> result;
+
+  for (int n = 0; n != N; ++n)
   {
-    for (int j = 0; j < n; ++j)
+    for (int p = 0; p != P; ++p)
     {
-      float temp = 0;
-      for (int k = 0; k < n; ++k)
+      T temp = {};
+      for (int m = 0; m != M; ++m)
       {
-        temp += a[i * n + k] * b[k * n + j];
+        temp += a[n * M + m] * b[m * P + p];
       }
-      result[i * n + j] = temp;
+      result[n * P + p] = temp;
     }
   }
+
+  return result;
 }
+
 }  // namespace
 
 namespace Common
@@ -39,8 +47,8 @@ Matrix33 Matrix33::Identity()
 
 Matrix33 Matrix33::RotateX(float rad)
 {
-  const float s = sin(rad);
-  const float c = cos(rad);
+  const float s = std::sin(rad);
+  const float c = std::cos(rad);
   Matrix33 mtx = {};
   mtx.data[0] = 1;
   mtx.data[4] = c;
@@ -52,8 +60,8 @@ Matrix33 Matrix33::RotateX(float rad)
 
 Matrix33 Matrix33::RotateY(float rad)
 {
-  const float s = sin(rad);
-  const float c = cos(rad);
+  const float s = std::sin(rad);
+  const float c = std::cos(rad);
   Matrix33 mtx = {};
   mtx.data[0] = c;
   mtx.data[2] = s;
@@ -65,8 +73,8 @@ Matrix33 Matrix33::RotateY(float rad)
 
 Matrix33 Matrix33::RotateZ(float rad)
 {
-  const float s = sin(rad);
-  const float c = cos(rad);
+  const float s = std::sin(rad);
+  const float c = std::cos(rad);
   Matrix33 mtx = {};
   mtx.data[0] = c;
   mtx.data[1] = -s;
@@ -87,20 +95,12 @@ Matrix33 Matrix33::Scale(const Vec3& vec)
 
 void Matrix33::Multiply(const Matrix33& a, const Matrix33& b, Matrix33* result)
 {
-  MatrixMul(3, a.data.data(), b.data.data(), result->data.data());
+  result->data = MatrixMultiply<3, 3, 3>(a.data, b.data);
 }
 
 void Matrix33::Multiply(const Matrix33& a, const Vec3& vec, Vec3* result)
 {
-  for (int i = 0; i < 3; ++i)
-  {
-    result->data[i] = 0;
-
-    for (int k = 0; k < 3; ++k)
-    {
-      result->data[i] += a.data[i * 3 + k] * vec.data[k];
-    }
-  }
+  result->data = MatrixMultiply<3, 3, 1>(a.data, vec.data);
 }
 
 Matrix44 Matrix44::Identity()
@@ -157,8 +157,32 @@ Matrix44 Matrix44::Shear(const float a, const float b)
   return mtx;
 }
 
+Matrix44 Matrix44::Perspective(float fov_y, float aspect_ratio, float z_near, float z_far)
+{
+  Matrix44 mtx{};
+  const float tan_half_fov_y = std::tan(fov_y / 2);
+  mtx.data[0] = 1 / (aspect_ratio * tan_half_fov_y);
+  mtx.data[5] = 1 / tan_half_fov_y;
+  mtx.data[10] = -(z_far + z_near) / (z_far - z_near);
+  mtx.data[11] = -(2 * z_far * z_near) / (z_far - z_near);
+  mtx.data[14] = -1;
+  return mtx;
+}
+
 void Matrix44::Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result)
 {
-  MatrixMul(4, a.data.data(), b.data.data(), result->data.data());
+  result->data = MatrixMultiply<4, 4, 4>(a.data, b.data);
+}
+
+Vec3 Matrix44::Transform(const Vec3& v, float w) const
+{
+  const auto result = MatrixMultiply<4, 4, 1>(data, {v.x, v.y, v.z, w});
+  return Vec3{result[0], result[1], result[2]};
 }
+
+void Matrix44::Multiply(const Matrix44& a, const Vec4& vec, Vec4* result)
+{
+  result->data = MatrixMultiply<4, 4, 1>(a.data, vec.data);
+}
+
 }  // namespace Common