Merge pull request #9162 from jordan-woyak/quaternion

Replace stateful rotational matrices with quaternions.

10 files changed, 164 insertions, 48 deletions

diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index e64cc91be0..48adcba274 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/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,
diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h
index ff4ab71c73..bf1e80b567 100644
--- a/Source/Core/Common/Matrix.h
+++ b/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);
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index b84d638cfb..0f5532d58a 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -58,9 +58,9 @@ double CalculateStopDistance(double velocity, double max_accel)
 
 namespace WiimoteEmu
 {
-Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
-                                     const Common::Vec3& accelerometer, float accel_weight,
-                                     const Common::Vec3& accelerometer_normal)
+Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope,
+                                       const Common::Vec3& accelerometer, float accel_weight,
+                                       const Common::Vec3& accelerometer_normal)
 {
   const auto gyro_vec = gyroscope * accelerometer_normal;
   const auto normalized_accel = accelerometer.Normalized();
@@ -72,7 +72,7 @@ Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
   if (abs_cos_angle > 0 && abs_cos_angle < 1)
   {
     const auto axis = gyro_vec.Cross(normalized_accel).Normalized();
-    return Common::Matrix33::Rotate(std::acos(cos_angle) * accel_weight, axis) * gyroscope;
+    return Common::Quaternion::Rotate(std::acos(cos_angle) * accel_weight, axis) * gyroscope;
   }
   else
   {
@@ -80,7 +80,7 @@ Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
   }
 }
 
-IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
+IMUCursorState::IMUCursorState() : rotation{Common::Quaternion::Identity()}
 {
 }
 
@@ -311,7 +311,7 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
   }
 
   // Apply rotation from gyro data.
-  const auto gyro_rotation = GetMatrixFromGyroscope(*ang_vel * -1 * time_elapsed);
+  const auto gyro_rotation = GetRotationFromGyroscope(*ang_vel * -1 * time_elapsed);
   state->rotation = gyro_rotation * state->rotation;
 
   // If we have some non-zero accel data use it to adjust gyro drift.
@@ -334,7 +334,10 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
 
   // Adjust yaw as needed.
   if (yaw != target_yaw)
-    state->rotation *= Common::Matrix33::RotateZ(target_yaw - yaw);
+    state->rotation *= Common::Quaternion::RotateZ(target_yaw - yaw);
+
+  // Normalize for floating point inaccuracies.
+  state->rotation = state->rotation.Normalized();
 }
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
@@ -374,7 +377,7 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi
   }
 }
 
-Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
+Common::Quaternion GetRotationFromAcceleration(const Common::Vec3& accel)
 {
   const auto normalized_accel = accel.Normalized();
 
@@ -382,13 +385,13 @@ Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
   const auto axis = normalized_accel.Cross({0, 0, 1});
 
   // Check that axis is non-zero to handle perfect up/down orientations.
-  return Common::Matrix33::Rotate(angle,
-                                  axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
+  return Common::Quaternion::Rotate(angle, axis.LengthSquared() ? axis.Normalized() :
+                                                                  Common::Vec3{0, 1, 0});
 }
 
-Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro)
+Common::Quaternion GetRotationFromGyroscope(const Common::Vec3& gyro)
 {
-  return Common::Matrix33::FromQuaternion(gyro.x / 2, gyro.y / 2, gyro.z / 2, 1);
+  return Common::Quaternion{1, gyro.x / 2, gyro.y / 2, gyro.z / 2};
 }
 
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
@@ -397,19 +400,19 @@ Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
          Common::Matrix33::RotateX(angle.x);
 }
 
-float GetPitch(const Common::Matrix33& world_rotation)
+float GetPitch(const Common::Quaternion& world_rotation)
 {
   const auto vec = world_rotation * Common::Vec3{0, 0, 1};
   return std::atan2(vec.y, Common::Vec2(vec.x, vec.z).Length());
 }
 
-float GetRoll(const Common::Matrix33& world_rotation)
+float GetRoll(const Common::Quaternion& world_rotation)
 {
   const auto vec = world_rotation * Common::Vec3{0, 0, 1};
   return std::atan2(vec.x, vec.z);
 }
 
-float GetYaw(const Common::Matrix33& world_rotation)
+float GetYaw(const Common::Quaternion& world_rotation)
 {
   const auto vec = world_rotation.Inverted() * Common::Vec3{0, 1, 0};
   return std::atan2(vec.x, vec.y);
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
index 29ea03dea6..475d54ca1f 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
@@ -44,7 +44,7 @@ struct IMUCursorState
   IMUCursorState();
 
   // Rotation of world around device.
-  Common::Matrix33 rotation;
+  Common::Quaternion rotation;
 
   float recentered_pitch = {};
 };
@@ -57,22 +57,22 @@ struct MotionState : PositionalState, RotationalState
 // Note that 'gyroscope' is rotation of world around device.
 // Alternative accelerometer_normal can be supplied to correct from non-accelerometer data.
 // e.g. Used for yaw/pitch correction with IR data.
-Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
-                                     const Common::Vec3& accelerometer, float accel_weight,
-                                     const Common::Vec3& accelerometer_normal = {0, 0, 1});
+Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope,
+                                       const Common::Vec3& accelerometer, float accel_weight,
+                                       const Common::Vec3& accelerometer_normal = {0, 0, 1});
 
 // Estimate orientation from accelerometer data.
-Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel);
+Common::Quaternion GetRotationFromAcceleration(const Common::Vec3& accel);
 
-// Get a rotation matrix from current gyro data.
-Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro);
+// Get a quaternion from current gyro data.
+Common::Quaternion GetRotationFromGyroscope(const Common::Vec3& gyro);
 
 // Build a rotational matrix from euler angles.
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
 
-float GetPitch(const Common::Matrix33& world_rotation);
-float GetRoll(const Common::Matrix33& world_rotation);
-float GetYaw(const Common::Matrix33& world_rotation);
+float GetPitch(const Common::Quaternion& world_rotation);
+float GetRoll(const Common::Quaternion& world_rotation);
+float GetYaw(const Common::Quaternion& world_rotation);
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& target,
                               const Common::Vec3& max_jerk, float time_elapsed);
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 150bb9256a..48b69864d7 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -757,10 +757,10 @@ Common::Matrix44 Wiimote::GetTransformation(const Common::Matrix33& extra_rotati
          Common::Matrix44::Translate(-m_swing_state.position - m_point_state.position);
 }
 
-Common::Matrix33 Wiimote::GetOrientation() const
+Common::Quaternion Wiimote::GetOrientation() const
 {
-  return Common::Matrix33::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) *
-         Common::Matrix33::RotateX(float(MathUtil::TAU / 4 * IsUpright()));
+  return Common::Quaternion::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) *
+         Common::Quaternion::RotateX(float(MathUtil::TAU / 4 * IsUpright()));
 }
 
 Common::Vec3 Wiimote::GetTotalAcceleration() const
@@ -781,8 +781,9 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() const
 
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  return GetTransformation(m_imu_cursor_state.rotation *
-                           Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch));
+  return GetTransformation(Common::Matrix33::FromQuaternion(
+      m_imu_cursor_state.rotation *
+      Common::Quaternion::RotateX(m_imu_cursor_state.recentered_pitch)));
 }
 
 }  // namespace WiimoteEmu
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
index 13fd352617..604e0a05d7 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
@@ -162,7 +162,7 @@ private:
   GetTransformation(const Common::Matrix33& extra_rotation = Common::Matrix33::Identity()) const;
 
   // Returns the world rotation from the effects of sideways/upright settings.
-  Common::Matrix33 GetOrientation() const;
+  Common::Quaternion GetOrientation() const;
 
   Common::Vec3 GetTotalAcceleration() const;
   Common::Vec3 GetTotalAngularVelocity() const;
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index 842856d82e..a946059cab 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -579,7 +579,7 @@ void AccelerometerMappingIndicator::Draw()
   // UI axes are opposite that of Wii remote accelerometer.
   p.scale(-1.0, -1.0);
 
-  const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
+  const auto rotation = WiimoteEmu::GetRotationFromAcceleration(state);
 
   // Draw sphere.
   p.setPen(GetCosmeticPen(QPen(GetRawInputColor(), 0.5)));
@@ -650,8 +650,9 @@ void GyroMappingIndicator::Draw()
   const auto jitter = raw_gyro_state - m_previous_velocity;
   m_previous_velocity = raw_gyro_state;
 
-  m_state *= WiimoteEmu::GetMatrixFromGyroscope(angular_velocity * Common::Vec3(-1, +1, -1) /
-                                                INDICATOR_UPDATE_FREQ);
+  m_state *= WiimoteEmu::GetRotationFromGyroscope(angular_velocity * Common::Vec3(-1, +1, -1) /
+                                                  INDICATOR_UPDATE_FREQ);
+  m_state = m_state.Normalized();
 
   // Reset orientation when stable for a bit:
   constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ;
@@ -664,10 +665,11 @@ void GyroMappingIndicator::Draw()
     ++m_stable_steps;
 
   if (STABLE_RESET_STEPS == m_stable_steps)
-    m_state = Common::Matrix33::Identity();
+    m_state = Common::Quaternion::Identity();
 
   // Use an empty rotation matrix if gyroscope data is not present.
-  const auto rotation = (gyro_state.has_value() ? m_state : Common::Matrix33{});
+  const auto rotation =
+      (gyro_state.has_value() ? Common::Matrix33::FromQuaternion(m_state) : Common::Matrix33{});
 
   QPainter p(this);
   DrawBoundingBox(p);
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
index 9eeac84e2c..7e3fedfe75 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
@@ -176,7 +176,7 @@ private:
   void Draw() override;
 
   ControllerEmu::IMUGyroscope& m_gyro_group;
-  Common::Matrix33 m_state = Common::Matrix33::Identity();
+  Common::Quaternion m_state = Common::Quaternion::Identity();
   Common::Vec3 m_previous_velocity = {};
   u32 m_stable_steps = 0;
 };
diff --git a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp
index ef0d15090b..2a95fc0a57 100644
--- a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp
+++ b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp
@@ -1179,7 +1179,7 @@ void Device::UpdateOrientation()
 
   // Apply M+ gyro data to our orientation.
   m_orientation =
-      WiimoteEmu::GetMatrixFromGyroscope(m_mplus_state.gyro_data * -1 * elapsed_time.count()) *
+      WiimoteEmu::GetRotationFromGyroscope(m_mplus_state.gyro_data * -1 * elapsed_time.count()) *
       m_orientation;
 
   // When M+ data is not available give accel/ir data more weight.
@@ -1204,7 +1204,7 @@ void Device::UpdateOrientation()
                      m_ir_state.center_position.x * WiimoteEmu::CameraLogic::CAMERA_FOV_X) /
         2;
     const auto ir_normal = Common::Vec3(0, 1, 0);
-    const auto ir_vector = WiimoteEmu::GetMatrixFromGyroscope(-ir_rotation) * ir_normal;
+    const auto ir_vector = WiimoteEmu::GetRotationFromGyroscope(-ir_rotation) * ir_normal;
 
     // Pitch correction will be slightly wrong based on sensorbar height.
     // Keep weight below accelerometer weight for that reason.
@@ -1214,6 +1214,9 @@ void Device::UpdateOrientation()
     m_orientation = WiimoteEmu::ComplementaryFilter(m_orientation, ir_vector, ir_weight, ir_normal);
   }
 
+  // Normalize for floating point inaccuracies.
+  m_orientation = m_orientation.Normalized();
+
   // Update our (pitch, roll, yaw) inputs now that orientation has been adjusted.
   m_rotation_inputs =
       Common::Vec3{WiimoteEmu::GetPitch(m_orientation), WiimoteEmu::GetRoll(m_orientation),
diff --git a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h
index 75a5c447e7..0dc9082af6 100644
--- a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h
+++ b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h
@@ -275,7 +275,7 @@ private:
   std::list<ReportHandler> m_report_handlers;
 
   // World rotation. (used to rotate IR data and provide pitch, roll, yaw inputs)
-  Common::Matrix33 m_orientation = Common::Matrix33::Identity();
+  Common::Quaternion m_orientation = Common::Quaternion::Identity();
   Clock::time_point m_last_report_time = Clock::now();
 };