WiimoteEmu: Reimplement tilt/swing/camera/orientation data using matr…

…ix math.

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

Source/Core/Common/Matrix.h

@@ -6,40 +6,161 @@
 
 #include <array>
 #include <cmath>
+#include <type_traits>
 
 // Tiny matrix/vector library.
 // Used for things like Free-Look in the gfx backend.
 
 namespace Common
 {
-union Vec3
+template <typename T>
+union TVec3
 {
-  Vec3() = default;
-  Vec3(float _x, float _y, float _z) : data{_x, _y, _z} {}
+  TVec3() = default;
+  TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {}
+
+  T Dot(const TVec3& other) const { return x * other.x + y * other.y + z * other.z; }
+  T LengthSquared() const { return Dot(*this); }
+  T Length() const { return std::sqrt(LengthSquared()); }
+  TVec3 Normalized() const { return *this / Length(); }
 
-  Vec3& operator+=(const Vec3& rhs)
+  TVec3& operator+=(const TVec3& rhs)
   {
     x += rhs.x;
     y += rhs.y;
     z += rhs.z;
     return *this;
   }
 
-  std::array<float, 3> data = {};
+  TVec3& operator-=(const TVec3& rhs)
+  {
+    x -= rhs.x;
+    y -= rhs.y;
+    z -= rhs.z;
+    return *this;
+  }
+
+  TVec3& operator*=(const TVec3& rhs)
+  {
+    x *= rhs.x;
+    y *= rhs.y;
+    z *= rhs.z;
+    return *this;
+  }
+
+  TVec3& operator/=(const TVec3& rhs)
+  {
+    x /= rhs.x;
+    y /= rhs.y;
+    z /= rhs.z;
+    return *this;
+  }
+
+  TVec3 operator-() const { return {-x, -y, -z}; }
+
+  std::array<T, 3> data = {};
 
   struct
   {
-    float x;
-    float y;
-    float z;
+    T x;
+    T y;
+    T z;
   };
 };
 
-inline Vec3 operator+(Vec3 lhs, const Vec3& rhs)
+template <typename T>
+TVec3<T> operator+(TVec3<T> lhs, const TVec3<T>& rhs)
 {
   return lhs += rhs;
 }
 
+template <typename T>
+TVec3<T> operator-(TVec3<T> lhs, const TVec3<T>& rhs)
+{
+  return lhs -= rhs;
+}
+
+template <typename T>
+TVec3<T> operator*(TVec3<T> lhs, const TVec3<T>& rhs)
+{
+  return lhs *= rhs;
+}
+
+template <typename T>
+inline TVec3<T> operator/(TVec3<T> lhs, const TVec3<T>& rhs)
+{
+  return lhs /= rhs;
+}
+
+template <typename T>
+TVec3<T> operator*(TVec3<T> lhs, std::common_type_t<T> scalar)
+{
+  return lhs *= TVec3<T>{scalar, scalar, scalar};
+}
+
+template <typename T>
+TVec3<T> operator/(TVec3<T> lhs, std::common_type_t<T> scalar)
+{
+  return lhs /= TVec3<T>{scalar, scalar, scalar};
+}
+
+using Vec3 = TVec3<float>;
+using DVec3 = TVec3<double>;
+
+template <typename T>
+union TVec4
+{
+  TVec4() = default;
+  TVec4(TVec3<T> _vec, T _w) : TVec4{_vec.x, _vec.y, _vec.z, _w} {}
+  TVec4(T _x, T _y, T _z, T _w) : data{_x, _y, _z, _w} {}
+
+  TVec4& operator*=(const TVec4& rhs)
+  {
+    x *= rhs.x;
+    y *= rhs.y;
+    z *= rhs.z;
+    w *= rhs.w;
+    return *this;
+  }
+
+  TVec4& operator/=(const TVec4& rhs)
+  {
+    x /= rhs.x;
+    y /= rhs.y;
+    z /= rhs.z;
+    w /= rhs.w;
+    return *this;
+  }
+
+  TVec4& operator*=(T scalar) { return *this *= TVec4{scalar, scalar, scalar, scalar}; }
+  TVec4& operator/=(T scalar) { return *this /= TVec4{scalar, scalar, scalar, scalar}; }
+
+  std::array<T, 4> data = {};
+
+  struct
+  {
+    T x;
+    T y;
+    T z;
+    T w;
+  };
+};
+
+template <typename T>
+TVec4<T> operator*(TVec4<T> lhs, std::common_type_t<T> scalar)
+{
+  return lhs *= scalar;
+}
+
+template <typename T>
+TVec4<T> operator/(TVec4<T> lhs, std::common_type_t<T> scalar)
+{
+  return lhs /= scalar;
+}
+
+using Vec4 = TVec4<float>;
+using DVec4 = TVec4<double>;
+
 template <typename T>
 union TVec2
 {
@@ -73,6 +194,13 @@ union TVec2
     return *this;
   }
 
+  TVec2& operator/=(T scalar)
+  {
+    x /= scalar;
+    y /= scalar;
+    return *this;
+  }
+
   TVec2 operator-() const { return {-x, -y}; }
 
   std::array<T, 2> data = {};
@@ -102,6 +230,12 @@ TVec2<T> operator*(TVec2<T> lhs, T scalar)
   return lhs *= scalar;
 }
 
+template <typename T>
+TVec2<T> operator/(TVec2<T> lhs, T scalar)
+{
+  return lhs /= scalar;
+}
+
 using Vec2 = TVec2<float>;
 using DVec2 = TVec2<double>;
 
@@ -123,10 +257,11 @@ class Matrix33
 
   Matrix33& operator*=(const Matrix33& rhs)
   {
-    Multiply(Matrix33(*this), rhs, this);
+    Multiply(*this, rhs, this);
     return *this;
   }
 
+  // Note: Row-major storage order.
   std::array<float, 9> data;
 };
 
@@ -135,11 +270,10 @@ inline Matrix33 operator*(Matrix33 lhs, const Matrix33& rhs)
   return lhs *= rhs;
 }
 
-inline Vec3 operator*(const Matrix33& lhs, const Vec3& rhs)
+inline Vec3 operator*(const Matrix33& lhs, Vec3 rhs)
 {
-  Vec3 result;
-  Matrix33::Multiply(lhs, rhs, &result);
-  return result;
+  Matrix33::Multiply(lhs, rhs, &rhs);
+  return rhs;
 }
 
 class Matrix44
@@ -151,20 +285,33 @@ class Matrix44
 
   static Matrix44 Translate(const Vec3& vec);
   static Matrix44 Shear(const float a, const float b = 0);
+  static Matrix44 Perspective(float fov_y, float aspect_ratio, float z_near, float z_far);
 
   static void Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result);
+  static void Multiply(const Matrix44& a, const Vec4& vec, Vec4* result);
+
+  // For when a vec4 isn't needed a multiplication function that takes a Vec3 and w:
+  Vec3 Transform(const Vec3& point, float w) const;
 
   Matrix44& operator*=(const Matrix44& rhs)
   {
-    Multiply(Matrix44(*this), rhs, this);
+    Multiply(*this, rhs, this);
     return *this;
   }
 
+  // Note: Row-major storage order.
   std::array<float, 16> data;
 };
 
 inline Matrix44 operator*(Matrix44 lhs, const Matrix44& rhs)
 {
   return lhs *= rhs;
 }
+
+inline Vec4 operator*(const Matrix44& lhs, Vec4 rhs)
+{
+  Matrix44::Multiply(lhs, rhs, &rhs);
+  return rhs;
+}
+
 }  // namespace Common