Common/Matrix: Add Quaternion class.

Source/Core/Common/Matrix.cpp

@@ -36,6 +36,84 @@ auto MatrixMultiply(const std::array<T, N * M>& a, const std::array<T, M * P>& b
 
 namespace Common
 {
+Quaternion Quaternion::Identity()
+{
+  return Quaternion(1, 0, 0, 0);
+}
+
+Quaternion Quaternion::RotateX(float rad)
+{
+  return Rotate(rad, Vec3(1, 0, 0));
+}
+
+Quaternion Quaternion::RotateY(float rad)
+{
+  return Rotate(rad, Vec3(0, 1, 0));
+}
+
+Quaternion Quaternion::RotateZ(float rad)
+{
+  return Rotate(rad, Vec3(0, 0, 1));
+}
+
+Quaternion Quaternion::Rotate(float rad, const Vec3& axis)
+{
+  const auto sin_angle_2 = std::sin(rad / 2);
+
+  return Quaternion(std::cos(rad / 2), axis.x * sin_angle_2, axis.y * sin_angle_2,
+                    axis.z * sin_angle_2);
+}
+
+Quaternion::Quaternion(float w, float x, float y, float z) : data(x, y, z, w)
+{
+}
+
+float Quaternion::Norm() const
+{
+  return data.Dot(data);
+}
+
+Quaternion Quaternion::Normalized() const
+{
+  Quaternion result(*this);
+  result.data /= Norm();
+  return result;
+}
+
+Quaternion Quaternion::Conjugate() const
+{
+  return Quaternion(data.w, -1 * data.x, -1 * data.y, -1 * data.z);
+}
+
+Quaternion Quaternion::Inverted() const
+{
+  return Normalized().Conjugate();
+}
+
+Quaternion& Quaternion::operator*=(const Quaternion& rhs)
+{
+  auto& a = data;
+  auto& b = rhs.data;
+
+  data = Vec4{a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
+              a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
+              a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
+              // W
+              a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z};
+  return *this;
+}
+
+Quaternion operator*(Quaternion lhs, const Quaternion& rhs)
+{
+  return lhs *= rhs;
+}
+
+Vec3 operator*(const Quaternion& lhs, const Vec3& rhs)
+{
+  const auto result = lhs * Quaternion(0, rhs.x, rhs.y, rhs.z) * lhs.Conjugate();
+  return Vec3(result.data.x, result.data.y, result.data.z);
+}
+
 Matrix33 Matrix33::Identity()
 {
   Matrix33 mtx = {};
@@ -45,14 +123,12 @@ Matrix33 Matrix33::Identity()
   return mtx;
 }
 
-Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw)
+Matrix33 Matrix33::FromQuaternion(const Quaternion& q)
 {
-  // Normalize.
-  const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
-  qx *= n;
-  qy *= n;
-  qz *= n;
-  qw *= n;
+  const auto qx = q.data.x;
+  const auto qy = q.data.y;
+  const auto qz = q.data.z;
+  const auto qw = q.data.w;
 
   return {
       1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,     2 * qx * qz + 2 * qy * qw,

Source/Core/Common/Matrix.h

@@ -154,6 +154,8 @@ union TVec4
   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} {}
 
+  T Dot(const TVec4& other) const { return x * other.x + y * other.y + z * other.z + w * other.w; }
+
   TVec4& operator*=(const TVec4& rhs)
   {
     x *= rhs.x;
@@ -321,11 +323,40 @@ auto operator/(TVec2<T> lhs, T2 scalar)
 using Vec2 = TVec2<float>;
 using DVec2 = TVec2<double>;
 
+class Matrix33;
+
+class Quaternion
+{
+public:
+  static Quaternion Identity();
+
+  static Quaternion RotateX(float rad);
+  static Quaternion RotateY(float rad);
+  static Quaternion RotateZ(float rad);
+
+  static Quaternion Rotate(float rad, const Vec3& axis);
+
+  Quaternion() = default;
+  Quaternion(float w, float x, float y, float z);
+
+  float Norm() const;
+  Quaternion Normalized() const;
+  Quaternion Conjugate() const;
+  Quaternion Inverted() const;
+
+  Quaternion& operator*=(const Quaternion& rhs);
+
+  Vec4 data;
+};
+
+Quaternion operator*(Quaternion lhs, const Quaternion& rhs);
+Vec3 operator*(const Quaternion& lhs, const Vec3& rhs);
+
 class Matrix33
 {
 public:
   static Matrix33 Identity();
-  static Matrix33 FromQuaternion(float x, float y, float z, float w);
+  static Matrix33 FromQuaternion(const Quaternion&);
 
   // Return a rotation matrix around the x,y,z axis
   static Matrix33 RotateX(float rad);