InputCommon/WiimoteEmu: Replace stateful rotational matrices with quaternions.

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,
+Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope,
-                                     const Common::Vec3& accelerometer, float accel_weight,
+                                       const Common::Vec3& accelerometer, float accel_weight,
-                                     const Common::Vec3& accelerometer_normal)
+                                       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,
+  return Common::Quaternion::Rotate(angle, axis.LengthSquared() ? axis.Normalized() :
-                                  axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
+                                                                  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);

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,
+Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope,
-                                     const Common::Vec3& accelerometer, float accel_weight,
+                                       const Common::Vec3& accelerometer, float accel_weight,
-                                     const Common::Vec3& accelerometer_normal = {0, 0, 1});
+                                       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.
+// Get a quaternion from current gyro data.
-Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro);
+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 GetPitch(const Common::Quaternion& world_rotation);
-float GetRoll(const Common::Matrix33& world_rotation);
+float GetRoll(const Common::Quaternion& world_rotation);
-float GetYaw(const Common::Matrix33& 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);

Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp

@@ -758,7 +758,7 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() const
 
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  return GetTransformation(m_imu_cursor_state.rotation *
+  return GetTransformation(Common::Matrix33::FromQuaternion(m_imu_cursor_state.rotation) *
                            Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch));
 }
 

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) /
+  m_state *= WiimoteEmu::GetRotationFromGyroscope(angular_velocity * Common::Vec3(-1, +1, -1) /
-                                                INDICATOR_UPDATE_FREQ);
+                                                  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);

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;
 };

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),

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();
 };