Merge pull request #9162 from jordan-woyak/quaternion

Replace stateful rotational matrices with quaternions.

2 files changed, 6 insertions, 3 deletions

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