WiimoteEmu: Make the "Total Yaw" setting work again.

Source/Core/Common/Matrix.cpp

@@ -136,6 +136,33 @@ void Matrix33::Multiply(const Matrix33& a, const Vec3& vec, Vec3* result)
   result->data = MatrixMultiply<3, 3, 1>(a.data, vec.data);
 }
 
+Matrix33 Matrix33::Inverted() const
+{
+  const auto m = [this](int x, int y) { return data[y + x * 3]; };
+
+  const auto det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
+                   m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
+                   m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
+
+  const auto invdet = 1 / det;
+
+  Matrix33 result;
+
+  const auto minv = [&result](int x, int y) -> auto& { return result.data[y + x * 3]; };
+
+  minv(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) * invdet;
+  minv(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * invdet;
+  minv(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * invdet;
+  minv(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * invdet;
+  minv(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * invdet;
+  minv(1, 2) = (m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2)) * invdet;
+  minv(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * invdet;
+  minv(2, 1) = (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) * invdet;
+  minv(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * invdet;
+
+  return result;
+}
+
 Matrix44 Matrix44::Identity()
 {
   Matrix44 mtx = {};

Source/Core/Common/Matrix.h

@@ -288,6 +288,8 @@ public:
   static void Multiply(const Matrix33& a, const Matrix33& b, Matrix33* result);
   static void Multiply(const Matrix33& a, const Vec3& vec, Vec3* result);
 
+  Matrix33 Inverted() const;
+
   Matrix33& operator*=(const Matrix33& rhs)
   {
     Multiply(*this, rhs, this);

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

@@ -54,6 +54,7 @@ double CalculateStopDistance(double velocity, double max_accel)
   return velocity * velocity / (2 * std::copysign(max_accel, velocity));
 }
 
+// Note that 'gyroscope' is rotation of world around device.
 Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
                                      const Common::Matrix33& gyroscope, float accel_weight)
 {
@@ -79,6 +80,10 @@ Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
 
 namespace WiimoteEmu
 {
+IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
+{
+}
+
 void EmulateShake(PositionalState* state, ControllerEmu::Shake* const shake_group,
                   float time_elapsed)
 {
@@ -296,13 +301,12 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
 {
-  auto ang_vel = imu_gyroscope_group->GetState();
+  const auto ang_vel = imu_gyroscope_group->GetState();
 
-  // Reset if we have no gyro data.
-  if (!ang_vel.has_value())
+  // Reset if pointing is disabled or we have no gyro data.
+  if (!imu_ir_group->enabled || !ang_vel.has_value())
   {
-    state->recentered_pitch = 0;
-    state->rotation = Common::Matrix33::Identity();
+    *state = {};
     return;
   }
 
@@ -312,21 +316,30 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
                                                               ang_vel->z * time_elapsed / -2, 1);
   state->rotation = gyro_rotation * state->rotation;
 
-  const auto yaw = std::asin((state->rotation * Common::Vec3{0, 1, 0}).x);
+  // If we have some non-zero accel data use it to adjust gyro drift.
+  constexpr auto ACCEL_WEIGHT = 0.02f;
+  auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
+  if (accel.LengthSquared())
+    state->rotation = ComplementaryFilter(accel, state->rotation, ACCEL_WEIGHT);
+
+  const auto inv_rotation = state->rotation.Inverted();
+
+  // Clamp yaw within configured bounds.
+  const auto yaw = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).x);
+  const auto max_yaw = float(imu_ir_group->GetTotalYaw() / 2);
+  auto target_yaw = std::clamp(yaw, -max_yaw, max_yaw);
 
   // Handle the "Recenter" button being pressed.
   if (imu_ir_group->controls[0]->control_ref->State() >
       ControllerEmu::Buttons::ACTIVATION_THRESHOLD)
   {
-    state->recentered_pitch = -std::asin((state->rotation * Common::Vec3{0, 1, 0}).z);
-    state->rotation *= Common::Matrix33::RotateZ(yaw);
+    state->recentered_pitch = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).z);
+    target_yaw = 0;
   }
 
-  // If we have usable accel data use it to adjust gyro drift.
-  constexpr float accel_weight = 0.02;
-  auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
-  if (accel.LengthSquared())
-    state->rotation = ComplementaryFilter(accel, state->rotation, accel_weight);
+  // Adjust yaw as needed.
+  if (yaw != target_yaw)
+    state->rotation *= Common::Matrix33::RotateZ(target_yaw - yaw);
 }
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,

Source/Core/Core/HW/WiimoteEmu/Dynamics.h

@@ -41,8 +41,12 @@ struct RotationalState
 
 struct IMUCursorState
 {
+  IMUCursorState();
+
+  // Rotation of world around device.
   Common::Matrix33 rotation;
-  float recentered_pitch;
+
+  float recentered_pitch = {};
 };
 
 // Contains both positional and rotational state.

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

@@ -595,6 +595,9 @@ void Wiimote::DoState(PointerWrap& p)
   p.Do(m_cursor_state);
   p.Do(m_shake_state);
 
+  // We'll consider the IMU state part of the user's physical controller state and not sync it.
+  // (m_imu_cursor_state)
+
   p.DoMarker("Wiimote");
 }
 

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

@@ -191,9 +191,7 @@ void Wiimote::Reset()
   m_cursor_state = {};
   m_shake_state = {};
 
-  // TODO: save state these
-  m_imu_cursor_state.rotation = Common::Matrix33::Identity();
-  m_imu_cursor_state.recentered_pitch = 0;
+  m_imu_cursor_state = {};
 }
 
 Wiimote::Wiimote(const unsigned int index) : m_index(index)
@@ -851,8 +849,8 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
 
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  return GetTransformation(Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch) *
-                           m_imu_cursor_state.rotation);
+  return GetTransformation(m_imu_cursor_state.rotation *
+                           Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch));
 }
 
 }  // namespace WiimoteEmu