WiimoteEmu: Allow Recenter button to adjust the pitch.

4 files changed, 40 insertions, 22 deletions

diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index 1742d70c8e..7ac6caf611 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -66,8 +66,6 @@ Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
   const auto abs_cos_angle = std::abs(cos_angle);
   if (abs_cos_angle > 0 && abs_cos_angle < 1)
   {
-    INFO_LOG(WIIMOTE, "angle diff: %lf", std::acos(cos_angle) * 360 / MathUtil::TAU);
-
     const auto axis = gyro_vec.Cross(normalized_accel).Normalized();
     return Common::Matrix33::Rotate(std::acos(cos_angle) * accel_weight, axis) * gyroscope;
   }
@@ -294,31 +292,41 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
   }
 }
 
-void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
+void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_group,
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
 {
   auto ang_vel = imu_gyroscope_group->GetState();
 
-  // Recenter if we have no gyro data or the "Recenter" button is pressed.
-  if (!ang_vel.has_value() || imu_ir_group->controls[0]->control_ref->State() >
-                                  ControllerEmu::Buttons::ACTIVATION_THRESHOLD)
+  // Reset if we have no gyro data.
+  if (!ang_vel.has_value())
   {
-    *state = Common::Matrix33::Identity();
+    state->recentered_pitch = 0;
+    state->rotation = Common::Matrix33::Identity();
     return;
   }
 
   // Apply rotation from gyro data.
-  const auto rotation = Common::Matrix33::FromQuaternion(ang_vel->x * time_elapsed / -2,
-                                                         ang_vel->y * time_elapsed / -2,
-                                                         ang_vel->z * time_elapsed / -2, 1);
-  *state = rotation * *state;
+  const auto gyro_rotation = Common::Matrix33::FromQuaternion(ang_vel->x * time_elapsed / -2,
+                                                              ang_vel->y * time_elapsed / -2,
+                                                              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);
+
+  // 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);
+  }
 
   // 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 = ComplementaryFilter(accel, *state, accel_weight);
+    state->rotation = ComplementaryFilter(accel, state->rotation, accel_weight);
 }
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
index 42cc60067a..44ec6d698e 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
@@ -39,6 +39,12 @@ struct RotationalState
   Common::Vec3 angular_velocity;
 };
 
+struct IMUCursorState
+{
+  Common::Matrix33 rotation;
+  float recentered_pitch;
+};
+
 // Contains both positional and rotational state.
 struct MotionState : PositionalState, RotationalState
 {
@@ -60,7 +66,7 @@ void EmulateShake(PositionalState* state, ControllerEmu::Shake* shake_group, flo
 void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed);
 void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed);
 void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed);
-void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
+void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_group,
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
 
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 7869bf3b62..53ccba0c8a 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -190,7 +190,10 @@ void Wiimote::Reset()
   m_tilt_state = {};
   m_cursor_state = {};
   m_shake_state = {};
-  m_imu_cursor_state = Common::Matrix33::Identity();
+
+  // TODO: save state these
+  m_imu_cursor_state.rotation = Common::Matrix33::Identity();
+  m_imu_cursor_state.recentered_pitch = 0;
 }
 
 Wiimote::Wiimote(const unsigned int index) : m_index(index)
@@ -809,7 +812,7 @@ Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity)
                              m_cursor_state.angular_velocity + extra_angular_velocity);
 }
 
-Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
+Common::Matrix44 Wiimote::GetTransformation(const Common::Matrix33& extra_rotation) const
 {
   // Includes positional and rotational effects of:
   // Point, Swing, Tilt, Shake
@@ -817,8 +820,8 @@ Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
   // TODO: Think about and clean up matrix order + make nunchuk match.
   return Common::Matrix44::Translate(-m_shake_state.position) *
          Common::Matrix44::FromMatrix33(
-             m_imu_cursor_state * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle -
-                                                      m_cursor_state.angle - extra_rotation)) *
+             extra_rotation * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle -
+                                                  m_cursor_state.angle)) *
          Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
 }
 
@@ -837,7 +840,6 @@ Common::Vec3 Wiimote::GetTotalAcceleration()
     return GetAcceleration();
 }
 
-// TODO: Kill this function.
 Common::Vec3 Wiimote::GetTotalAngularVelocity()
 {
   auto ang_vel = m_imu_gyroscope->GetState();
@@ -847,10 +849,10 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
     return GetAngularVelocity();
 }
 
-// TODO: Kill this function.
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  return GetTransformation();
+  return GetTransformation(Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch) *
+                           m_imu_cursor_state.rotation);
 }
 
 }  // namespace WiimoteEmu
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
index 32489db229..0a5ea3c040 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
@@ -159,7 +159,8 @@ private:
   // Returns the transformation of the world around the wiimote.
   // Used for simulating camera data and for rotating acceleration data.
   // Does not include orientation transformations.
-  Common::Matrix44 GetTransformation(Common::Vec3 extra_rotation = {}) const;
+  Common::Matrix44
+  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;
@@ -300,6 +301,7 @@ private:
   RotationalState m_tilt_state;
   MotionState m_cursor_state;
   PositionalState m_shake_state;
-  Common::Matrix33 m_imu_cursor_state;
+
+  IMUCursorState m_imu_cursor_state;
 };
 }  // namespace WiimoteEmu