WiimoteEmu: Clean up ComplementaryFilter math.

5 files changed, 65 insertions, 96 deletions

diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index 0e33182cf2..1742d70c8e 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -54,6 +54,29 @@ double CalculateStopDistance(double velocity, double max_accel)
   return velocity * velocity / (2 * std::copysign(max_accel, velocity));
 }
 
+Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
+                                     const Common::Matrix33& gyroscope, float accel_weight)
+{
+  const auto gyro_vec = gyroscope * Common::Vec3{0, 0, 1};
+  const auto normalized_accel = accelerometer.Normalized();
+
+  const auto cos_angle = normalized_accel.Dot(gyro_vec);
+
+  // If gyro to accel angle difference is betwen 0 and 180 degrees we make an adjustment.
+  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;
+  }
+  else
+  {
+    return gyroscope;
+  }
+}
+
 }  // namespace
 
 namespace WiimoteEmu
@@ -271,94 +294,31 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
   }
 }
 
-static Common::Vec3 NormalizeAngle(Common::Vec3 angle)
-{
-  // TODO: There must be a more elegant way to do this
-  angle.x = fmod(angle.x, float(MathUtil::TAU));
-  angle.y = fmod(angle.y, float(MathUtil::TAU));
-  angle.z = fmod(angle.z, float(MathUtil::TAU));
-  angle.x += angle.x < 0 ? float(MathUtil::TAU) : 0;
-  angle.y += angle.y < 0 ? float(MathUtil::TAU) : 0;
-  angle.z += angle.z < 0 ? float(MathUtil::TAU) : 0;
-  return angle;
-}
-
-static Common::Vec3 ComplementaryFilter(const Common::Vec3& angle,
-                                        const Common::Vec3& accelerometer,
-                                        const Common::Vec3& gyroscope, float time_elapsed)
-{
-  Common::Vec3 gyroangle = angle + gyroscope * time_elapsed;
-  gyroangle = NormalizeAngle(gyroangle);
-
-  // Calculate accelerometer tilt angles
-  Common::Vec3 accangle = gyroangle;
-  if ((accelerometer.x != 0 && accelerometer.y != 0) || accelerometer.z != 0)
-  {
-    float accpitch = -atan2(accelerometer.y, -accelerometer.z) + float(MathUtil::PI);
-    float accroll = atan2(accelerometer.x, -accelerometer.z) + float(MathUtil::PI);
-    accangle = {accpitch, accroll, gyroangle.z};
-  }
-
-  // Massage accelerometer and gyroscope angle values so that averaging them works when they are on
-  // opposite sides of TAU / zero (which both represent the same angle)
-  // TODO: There must be a more elegant way to do this
-  constexpr float DEG360 = float(MathUtil::TAU);
-  constexpr float DEG270 = DEG360 * 0.75f;
-  constexpr float DEG90 = DEG360 * 0.25f;
-  if (accangle.x < DEG90 && gyroangle.x > DEG270)
-    accangle.x += DEG360;
-  else if (gyroangle.x < DEG90 && accangle.x > DEG270)
-    gyroangle.x += DEG360;
-  if (accangle.y < DEG90 && gyroangle.y > DEG270)
-    accangle.y += DEG360;
-  else if (gyroangle.y < DEG90 && accangle.y > DEG270)
-    gyroangle.y += DEG360;
-
-  // Combine accelerometer and gyroscope angles
-  return NormalizeAngle((gyroangle * 0.98f) + (accangle * 0.02f));
-}
-
-void EmulateIMUCursor(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group,
+void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
 {
-  // Avoid having to double dereference
-  auto& st = *state;
-
-  if (!imu_ir_group->enabled)
-  {
-    st = std::nullopt;
-    return;
-  }
-
-  auto accel = imu_accelerometer_group->GetState();
   auto ang_vel = imu_gyroscope_group->GetState();
 
-  // The IMU Cursor requires both an accelerometer and a gyroscope to function correctly.
-  if (!(accel.has_value() && ang_vel.has_value()))
+  // 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)
   {
-    st = std::nullopt;
+    *state = Common::Matrix33::Identity();
     return;
   }
 
-  if (!st.has_value())
-    st = RotationalState{};
-
-  st->angle = ComplementaryFilter(st->angle, accel.value(), ang_vel.value(), time_elapsed);
-
-  // Reset camera yaw angle
-  constexpr ControlState BUTTON_THRESHOLD = 0.5;
-  if (imu_ir_group->controls[0]->control_ref->State() > BUTTON_THRESHOLD)
-    st->angle.z = 0;
-
-  // Limit camera yaw angle
-  float totalyaw = float(imu_ir_group->GetTotalYaw());
-  float yawmax = totalyaw / 2;
-  float yawmin = float(MathUtil::TAU) - totalyaw / 2;
-  if (st->angle.z > yawmax && st->angle.z <= float(MathUtil::PI))
-    st->angle.z = yawmax;
-  if (st->angle.z < yawmin && st->angle.z > float(MathUtil::PI))
-    st->angle.z = yawmin;
+  // 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;
+
+  // 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);
 }
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
@@ -398,6 +358,18 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi
   }
 }
 
+Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
+{
+  const auto normalized_accel = accel.Normalized();
+
+  const auto angle = std::acos(normalized_accel.Dot({0, 0, 1}));
+  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});
+}
+
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
 {
   return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
index ecf55d4ee5..42cc60067a 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
@@ -44,6 +44,9 @@ struct MotionState : PositionalState, RotationalState
 {
 };
 
+// Estimate orientation from accelerometer data.
+Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel);
+
 // Build a rotational matrix from euler angles.
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
 
@@ -57,7 +60,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(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group,
+void EmulateIMUCursor(Common::Matrix33* 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 5ffc8aaf98..7869bf3b62 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -190,7 +190,7 @@ void Wiimote::Reset()
   m_tilt_state = {};
   m_cursor_state = {};
   m_shake_state = {};
-  m_imu_cursor_state = {};
+  m_imu_cursor_state = Common::Matrix33::Identity();
 }
 
 Wiimote::Wiimote(const unsigned int index) : m_index(index)
@@ -812,12 +812,13 @@ Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity)
 Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
 {
   // Includes positional and rotational effects of:
-  // Cursor, Swing, Tilt, Shake
+  // Point, Swing, Tilt, Shake
 
   // TODO: Think about and clean up matrix order + make nunchuk match.
   return Common::Matrix44::Translate(-m_shake_state.position) *
-         Common::Matrix44::FromMatrix33(GetRotationalMatrix(
-             -m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle - extra_rotation)) *
+         Common::Matrix44::FromMatrix33(
+             m_imu_cursor_state * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle -
+                                                      m_cursor_state.angle - extra_rotation)) *
          Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
 }
 
@@ -836,6 +837,7 @@ Common::Vec3 Wiimote::GetTotalAcceleration()
     return GetAcceleration();
 }
 
+// TODO: Kill this function.
 Common::Vec3 Wiimote::GetTotalAngularVelocity()
 {
   auto ang_vel = m_imu_gyroscope->GetState();
@@ -845,13 +847,10 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
     return GetAngularVelocity();
 }
 
+// TODO: Kill this function.
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  auto state = m_imu_cursor_state;
-  if (state.has_value())
-    return GetTransformation(state->angle);
-  else
-    return GetTransformation();
+  return GetTransformation();
 }
 
 }  // namespace WiimoteEmu
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
index 1f13d29d2e..32489db229 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
@@ -300,6 +300,6 @@ private:
   RotationalState m_tilt_state;
   MotionState m_cursor_state;
   PositionalState m_shake_state;
-  std::optional<RotationalState> m_imu_cursor_state;
+  Common::Matrix33 m_imu_cursor_state;
 };
 }  // namespace WiimoteEmu
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index 8e81b380d5..150b035791 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -723,12 +723,7 @@ void AccelerometerMappingIndicator::paintEvent(QPaintEvent*)
   p.setRenderHint(QPainter::Antialiasing, true);
   p.setRenderHint(QPainter::SmoothPixmapTransform, true);
 
-  const auto angle = std::acos(state.Normalized().Dot({0, 0, 1}));
-  const auto axis = state.Normalized().Cross({0, 0, 1});
-
-  // Check that axis is non-zero to handle perfect up/down orientations.
-  const auto rotation = Common::Matrix33::Rotate(
-      angle, axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
+  const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
 
   // Draw sphere.
   p.setPen(Qt::NoPen);