From 8ab3694f51758a6859145b1a9467299e7c1e96f7 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Thu, 2 Jan 2020 15:16:37 -0600
Subject: [PATCH 1/6] Common: Add Matrix33::FromQuaternion.

---
 Source/Core/Common/Matrix.cpp | 16 ++++++++++++++++
 Source/Core/Common/Matrix.h   |  1 +
 2 files changed, 17 insertions(+)

diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index 9581a5a4ce..6ccfbe459c 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/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);
diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h
index ed58abdccb..b0ec811545 100644
--- a/Source/Core/Common/Matrix.h
+++ b/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);

From 540a3ce66529157b18cce539b5e337b4398b87d9 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Thu, 2 Jan 2020 15:46:18 -0600
Subject: [PATCH 2/6] DolphinQt: Use FromQuaternion for a more accurate gyro
 indicator.

---
 Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index d8fa5aaaaa..cf9c042599 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -797,9 +797,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) *
-             Common::Matrix33::RotateY(angular_velocity.y / INDICATOR_UPDATE_FREQ) *
-             Common::Matrix33::RotateZ(angular_velocity.z / -INDICATOR_UPDATE_FREQ);
+  m_state *= Common::Matrix33::FromQuaternion(angular_velocity.x / -INDICATOR_UPDATE_FREQ / 2,
+                                              angular_velocity.y / INDICATOR_UPDATE_FREQ / 2,
+                                              angular_velocity.z / -INDICATOR_UPDATE_FREQ / 2, 1);
 
   // Reset orientation when stable for a bit:
   constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ;

From 120c6dc8506647d3edd33e533bba65bb9bc081c8 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Fri, 3 Jan 2020 12:34:11 -0600
Subject: [PATCH 3/6] DolphinQt: Fix accelerometer indicator math.

---
 .../Core/DolphinQt/Config/Mapping/MappingIndicator.cpp   | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index cf9c042599..8e81b380d5 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -724,12 +724,11 @@ void AccelerometerMappingIndicator::paintEvent(QPaintEvent*)
   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}).Normalized();
+  const auto axis = state.Normalized().Cross({0, 0, 1});
 
-  // 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();
+  // 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});
 
   // Draw sphere.
   p.setPen(Qt::NoPen);

From 72c2be52eda47bf3e6b65e2f29c882aed0010415 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Fri, 3 Jan 2020 16:16:26 -0600
Subject: [PATCH 4/6] WiimoteEmu: Clean up ComplementaryFilter math.

---
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   | 128 +++++++-----------
 Source/Core/Core/HW/WiimoteEmu/Dynamics.h     |   5 +-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |  17 ++-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h   |   2 +-
 .../Config/Mapping/MappingIndicator.cpp       |   7 +-
 5 files changed, 64 insertions(+), 95 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{};
+  // 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;
 
-  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;
+  // 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);

From 723115b7b68dd64b9bff4ddbdf7e8d3fef07432a Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Fri, 3 Jan 2020 18:08:45 -0600
Subject: [PATCH 5/6] WiimoteEmu: Allow Recenter button to adjust the pitch.

---
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   | 32 ++++++++++++-------
 Source/Core/Core/HW/WiimoteEmu/Dynamics.h     |  8 ++++-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp | 16 ++++++----
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h   |  6 ++--
 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

From 0aacf3a62768eac380800a1b8279aa642b17ca92 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Fri, 3 Jan 2020 18:24:41 -0600
Subject: [PATCH 6/6] WiimoteEmu: Make the "Total Yaw" setting work again.

---
 Source/Core/Common/Matrix.cpp                 | 27 +++++++++++++
 Source/Core/Common/Matrix.h                   |  2 +
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   | 39 ++++++++++++-------
 Source/Core/Core/HW/WiimoteEmu/Dynamics.h     |  6 ++-
 .../Core/HW/WiimoteEmu/EmuSubroutines.cpp     |  3 ++
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |  8 ++--
 6 files changed, 66 insertions(+), 19 deletions(-)

diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index 6ccfbe459c..e64cc91be0 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/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 = {};
diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h
index b0ec811545..b47e24f03d 100644
--- a/Source/Core/Common/Matrix.h
+++ b/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);
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index 7ac6caf611..1c0da74a0f 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/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,
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
index 44ec6d698e..b72285a159 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
+++ b/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.
diff --git a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
index 8e1c6945c1..a3ea63161d 100644
--- a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
+++ b/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");
 }
 
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 53ccba0c8a..62b40e285c 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/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