Merge pull request #8545 from jordan-woyak/imu-cursor-centering

WiimoteEmu: IMU pointing behavior improvements and code cleanup.

Source/Core/Common/Matrix.cpp

@@ -45,6 +45,22 @@ Matrix33 Matrix33::Identity()
   return mtx;
 }
 
+Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw)
+{
+  // Normalize.
+  const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
+  qx *= n;
+  qy *= n;
+  qz *= n;
+  qw *= n;
+
+  return {
+      1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,     2 * qx * qz + 2 * qy * qw,
+      2 * qx * qy + 2 * qz * qw,     1 - 2 * qx * qx - 2 * qz * qz, 2 * qy * qz - 2 * qx * qw,
+      2 * qx * qz - 2 * qy * qw,     2 * qy * qz + 2 * qx * qw,     1 - 2 * qx * qx - 2 * qy * qy,
+  };
+}
+
 Matrix33 Matrix33::RotateX(float rad)
 {
   const float s = std::sin(rad);
@@ -120,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

@@ -273,6 +273,7 @@ class Matrix33
 {
 public:
   static Matrix33 Identity();
+  static Matrix33 FromQuaternion(float x, float y, float z, float w);
 
   // Return a rotation matrix around the x,y,z axis
   static Matrix33 RotateX(float rad);
@@ -287,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,10 +54,36 @@ 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)
+{
+  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)
+  {
+    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
 {
+IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
+{
+}
+
 void EmulateShake(PositionalState* state, ControllerEmu::Shake* const shake_group,
                   float time_elapsed)
 {
@@ -271,94 +297,49 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
   }
 }
 
-static Common::Vec3 NormalizeAngle(Common::Vec3 angle)
+void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_group,
-{
-  // 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,
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
 {
-  // Avoid having to double dereference
+  const auto ang_vel = imu_gyroscope_group->GetState();
-  auto& st = *state;
 
-  if (!imu_ir_group->enabled)
+  // Reset if pointing is disabled or we have no gyro data.
+  if (!imu_ir_group->enabled || !ang_vel.has_value())
   {
-    st = std::nullopt;
+    *state = {};
     return;
   }
 
-  auto accel = imu_accelerometer_group->GetState();
+  // Apply rotation from gyro data.
-  auto ang_vel = imu_gyroscope_group->GetState();
+  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;
 
-  // The IMU Cursor requires both an accelerometer and a gyroscope to function correctly.
+  // If we have some non-zero accel data use it to adjust gyro drift.
-  if (!(accel.has_value() && ang_vel.has_value()))
+  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)
   {
-    st = std::nullopt;
+    state->recentered_pitch = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).z);
-    return;
+    target_yaw = 0;
   }
 
-  if (!st.has_value())
+  // Adjust yaw as needed.
-    st = RotationalState{};
+  if (yaw != target_yaw)
-
+    state->rotation *= Common::Matrix33::RotateZ(target_yaw - yaw);
-  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;
 }
 
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
@@ -398,6 +379,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) *

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

@@ -39,11 +39,24 @@ struct RotationalState
   Common::Vec3 angular_velocity;
 };
 
+struct IMUCursorState
+{
+  IMUCursorState();
+
+  // Rotation of world around device.
+  Common::Matrix33 rotation;
+
+  float recentered_pitch = {};
+};
+
 // Contains both positional and rotational state.
 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 +70,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(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_group,
                       ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
 

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

@@ -190,6 +190,7 @@ void Wiimote::Reset()
   m_tilt_state = {};
   m_cursor_state = {};
   m_shake_state = {};
+
   m_imu_cursor_state = {};
 }
 
@@ -809,15 +810,16 @@ 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:
-  // 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(
+         Common::Matrix44::FromMatrix33(
-             -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);
 }
 
@@ -847,11 +849,8 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
 
 Common::Matrix44 Wiimote::GetTotalTransformation() const
 {
-  auto state = m_imu_cursor_state;
+  return GetTransformation(m_imu_cursor_state.rotation *
-  if (state.has_value())
+                           Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch));
-    return GetTransformation(state->angle);
-  else
-    return GetTransformation();
 }
 
 }  // namespace WiimoteEmu

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;
-  std::optional<RotationalState> m_imu_cursor_state;
+
+  IMUCursorState m_imu_cursor_state;
 };
 }  // namespace WiimoteEmu

Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp

@@ -723,13 +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 rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
-  const auto axis = state.Normalized().Cross({0, 0, 1}).Normalized();
-
-  // Odd checks to handle case of 0g (draw no sphere) and perfect up/down orientation.
-  const auto rotation = (!state.LengthSquared() || axis.LengthSquared() < 2) ?
-                            Common::Matrix33::Rotate(angle, axis) :
-                            Common::Matrix33::Identity();
 
   // Draw sphere.
   p.setPen(Qt::NoPen);
@@ -797,9 +791,9 @@ void GyroMappingIndicator::paintEvent(QPaintEvent*)
   const auto gyro_state = m_gyro_group.GetState();
   const auto angular_velocity = gyro_state.value_or(Common::Vec3{});
 
-  m_state *= Common::Matrix33::RotateX(angular_velocity.x / -INDICATOR_UPDATE_FREQ) *
+  m_state *= Common::Matrix33::FromQuaternion(angular_velocity.x / -INDICATOR_UPDATE_FREQ / 2,
-             Common::Matrix33::RotateY(angular_velocity.y / INDICATOR_UPDATE_FREQ) *
+                                              angular_velocity.y / INDICATOR_UPDATE_FREQ / 2,
-             Common::Matrix33::RotateZ(angular_velocity.z / -INDICATOR_UPDATE_FREQ);
+                                              angular_velocity.z / -INDICATOR_UPDATE_FREQ / 2, 1);
 
   // Reset orientation when stable for a bit:
   constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ;