WiimoteEmu: Improve emulated swing.

Source/Core/Common/MathUtil.h

@@ -24,6 +24,18 @@ constexpr T Clamp(const T val, const T& min, const T& max)
   return std::max(min, std::min(max, val));
 }
 
+template <typename T>
+constexpr auto Sign(const T& val) -> decltype((T{} < val) - (val < T{}))
+{
+  return (T{} < val) - (val < T{});
+}
+
+template <typename T, typename F>
+constexpr auto Lerp(const T& x, const T& y, const F& a) -> decltype(x + (y - x) * a)
+{
+  return x + (y - x) * a;
+}
+
 template <typename T>
 constexpr bool IsPow2(T imm)
 {

Source/Core/Common/Matrix.h

@@ -6,6 +6,7 @@
 
 #include <array>
 #include <cmath>
+#include <functional>
 #include <type_traits>
 
 // Tiny matrix/vector library.
@@ -58,6 +59,25 @@ union TVec3
 
   TVec3 operator-() const { return {-x, -y, -z}; }
 
+  // Apply function to each element and return the result.
+  template <typename F>
+  auto Map(F&& f) const -> TVec3<decltype(f(T{}))>
+  {
+    return {f(x), f(y), f(z)};
+  }
+
+  template <typename F, typename T2>
+  auto Map(F&& f, const TVec3<T2>& t) const -> TVec3<decltype(f(T{}, t.x))>
+  {
+    return {f(x, t.x), f(y, t.y), f(z, t.z)};
+  }
+
+  template <typename F, typename T2>
+  auto Map(F&& f, T2 scalar) const -> TVec3<decltype(f(T{}, scalar))>
+  {
+    return {f(x, scalar), f(y, scalar), f(z, scalar)};
+  }
+
   std::array<T, 3> data = {};
 
   struct
@@ -69,39 +89,45 @@ union TVec3
 };
 
 template <typename T>
-TVec3<T> operator+(TVec3<T> lhs, const TVec3<T>& rhs)
+TVec3<bool> operator<(const TVec3<T>& lhs, const TVec3<T>& rhs)
 {
-  return lhs += rhs;
+  return lhs.Map(std::less<T>{}, rhs);
 }
 
 template <typename T>
-TVec3<T> operator-(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator+(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x + rhs.x)>
 {
-  return lhs -= rhs;
+  return lhs.Map(std::plus<decltype(lhs.x + rhs.x)>{}, rhs);
 }
 
 template <typename T>
-TVec3<T> operator*(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator-(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x - rhs.x)>
 {
-  return lhs *= rhs;
+  return lhs.Map(std::minus<decltype(lhs.x - rhs.x)>{}, rhs);
+}
+
+template <typename T1, typename T2>
+auto operator*(const TVec3<T1>& lhs, const TVec3<T2>& rhs) -> TVec3<decltype(lhs.x * rhs.x)>
+{
+  return lhs.Map(std::multiplies<decltype(lhs.x * rhs.x)>{}, rhs);
 }
 
 template <typename T>
-inline TVec3<T> operator/(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator/(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x / rhs.x)>
 {
-  return lhs /= rhs;
+  return lhs.Map(std::divides<decltype(lhs.x / rhs.x)>{}, rhs);
 }
 
-template <typename T>
+template <typename T1, typename T2>
-TVec3<T> operator*(TVec3<T> lhs, std::common_type_t<T> scalar)
+auto operator*(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x * scalar)>
 {
-  return lhs *= TVec3<T>{scalar, scalar, scalar};
+  return lhs.Map(std::multiplies<decltype(lhs.x * scalar)>{}, scalar);
 }
 
-template <typename T>
+template <typename T1, typename T2>
-TVec3<T> operator/(TVec3<T> lhs, std::common_type_t<T> scalar)
+auto operator/(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x / scalar)>
 {
-  return lhs /= TVec3<T>{scalar, scalar, scalar};
+  return lhs.Map(std::divides<decltype(lhs.x / scalar)>{}, scalar);
 }
 
 using Vec3 = TVec3<float>;

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

@@ -57,7 +57,7 @@ namespace WiimoteEmu
 void EmulateShake(PositionalState* state, ControllerEmu::Shake* const shake_group,
                   float time_elapsed)
 {
-  auto target_position = shake_group->GetState() * shake_group->GetIntensity() / 2;
+  auto target_position = shake_group->GetState() * float(shake_group->GetIntensity() / 2);
   for (std::size_t i = 0; i != target_position.data.size(); ++i)
   {
     if (state->velocity.data[i] * std::copysign(1.f, target_position.data[i]) < 0 ||
@@ -90,7 +90,9 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
   const ControlState roll = target.x * MathUtil::PI;
   const ControlState pitch = target.y * MathUtil::PI;
 
-  // TODO: expose this setting in UI:
+  // Higher values will be more responsive but will increase rate of M+ "desync".
+  // I'd rather not expose this value in the UI if not needed.
+  // Desync caused by tilt seems not as severe as accelerometer data can estimate pitch/yaw.
   constexpr auto MAX_ACCEL = float(MathUtil::TAU * 50);
 
   ApproachAngleWithAccel(state, Common::Vec3(pitch, -roll, 0), MAX_ACCEL, time_elapsed);
@@ -98,22 +100,80 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
 
 void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed)
 {
-  const auto target = swing_group->GetState();
+  const auto input_state = swing_group->GetState();
+  const float max_distance = swing_group->GetMaxDistance();
+  const float max_angle = swing_group->GetTwistAngle();
 
-  // Note. Y/Z swapped because X/Y axis to the swing_group is X/Z to the wiimote.
+  // Note: Y/Z swapped because X/Y axis to the swing_group is X/Z to the wiimote.
   // X is negated because Wiimote X+ is to the left.
-  ApproachPositionWithJerk(state, {-target.x, -target.z, target.y},
+  const auto target_position = Common::Vec3{-input_state.x, -input_state.z, input_state.y};
-                           Common::Vec3{1, 1, 1} * swing_group->GetMaxJerk(), time_elapsed);
 
-  // Just jump to our target angle scaled by our progress to the target position.
+  // Jerk is scaled based on input distance from center.
-  // TODO: If we wanted to be less hacky we could use ApproachAngleWithAccel.
+  // X and Z scale is connected for sane movement about the circle.
-  const auto angle = state->position / swing_group->GetMaxDistance() * swing_group->GetTwistAngle();
+  const auto xz_target_dist = Common::Vec2{target_position.x, target_position.z}.Length();
+  const auto y_target_dist = std::abs(target_position.y);
+  const auto target_dist = Common::Vec3{xz_target_dist, y_target_dist, xz_target_dist};
+  const auto speed = MathUtil::Lerp(Common::Vec3{1, 1, 1} * float(swing_group->GetReturnSpeed()),
+                                    Common::Vec3{1, 1, 1} * float(swing_group->GetSpeed()),
+                                    target_dist / max_distance);
 
-  const auto old_angle = state->angle;
+  // Convert our m/s "speed" to the jerk required to reach this speed when traveling 1 meter.
-  state->angle = {-angle.z, 0, angle.x};
+  const auto max_jerk = speed * speed * speed * 4;
 
-  // Update velocity based on change in angle.
+  // Rotational acceleration to approximately match the completion time of our swing.
-  state->angular_velocity = state->angle - old_angle;
+  const auto max_accel = max_angle * speed.x * speed.x;
+
+  // Apply rotation based on amount of swing.
+  const auto target_angle =
+      Common::Vec3{-target_position.z, 0, target_position.x} / max_distance * max_angle;
+
+  // Angular acceleration * 2 seems to reduce "spurious stabs" in ZSS.
+  // TODO: Fix properly.
+  ApproachAngleWithAccel(state, target_angle, max_accel * 2, time_elapsed);
+
+  // Clamp X and Z rotation.
+  for (const int c : {0, 2})
+  {
+    if (std::abs(state->angle.data[c] / max_angle) > 1 &&
+        MathUtil::Sign(state->angular_velocity.data[c]) == MathUtil::Sign(state->angle.data[c]))
+    {
+      state->angular_velocity.data[c] = 0;
+    }
+  }
+
+  // Adjust target position backwards based on swing progress and max angle
+  // to simulate a swing with an outstretched arm.
+  const auto backwards_angle = std::max(std::abs(state->angle.x), std::abs(state->angle.z));
+  const auto backwards_movement = (1 - std::cos(backwards_angle)) * max_distance;
+
+  // TODO: Backswing jerk should be based on x/z speed.
+
+  ApproachPositionWithJerk(state, target_position + Common::Vec3{0, backwards_movement, 0},
+                           max_jerk, time_elapsed);
+
+  // Clamp Left/Right/Up/Down movement within the configured circle.
+  const auto xz_progress =
+      Common::Vec2{state->position.x, state->position.z}.Length() / max_distance;
+  if (xz_progress > 1)
+  {
+    state->position.x /= xz_progress;
+    state->position.z /= xz_progress;
+
+    state->acceleration.x = state->acceleration.z = 0;
+    state->velocity.x = state->velocity.z = 0;
+  }
+
+  // Clamp Forward/Backward movement within the configured distance.
+  // We allow additional backwards movement for the back swing.
+  const auto y_progress = state->position.y / max_distance;
+  const auto max_y_progress = 2 - std::cos(max_angle);
+  if (y_progress > max_y_progress || y_progress < -1)
+  {
+    state->position.y =
+        MathUtil::Clamp(state->position.y, -1.f * max_distance, max_y_progress * max_distance);
+    state->velocity.y = 0;
+    state->acceleration.y = 0;
+  }
 }
 
 WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
@@ -174,8 +234,10 @@ void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float ti
   state->acceleration = new_position - state->position;
   state->position = new_position;
 
-  // TODO: expose this setting in UI:
+  // Higher values will be more responsive but increase rate of M+ "desync".
-  constexpr auto MAX_ACCEL = float(MathUtil::TAU * 100);
+  // I'd rather not expose this value in the UI if not needed.
+  // At this value, sync is very good and responsiveness still appears instant.
+  constexpr auto MAX_ACCEL = float(MathUtil::TAU * 8);
 
   ApproachAngleWithAccel(state, target_angle, MAX_ACCEL, time_elapsed);
 }
@@ -190,10 +252,7 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
 
   const auto offset = angle_target - state->angle;
   const auto stop_offset = offset - stop_distance;
-
+  const auto accel = MathUtil::Sign(stop_offset) * max_accel;
-  const Common::Vec3 accel{std::copysign(max_accel, stop_offset.x),
-                           std::copysign(max_accel, stop_offset.y),
-                           std::copysign(max_accel, stop_offset.z)};
 
   state->angular_velocity += accel * time_elapsed;
 
@@ -202,11 +261,11 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
 
   for (std::size_t i = 0; i != offset.data.size(); ++i)
   {
-    // If new velocity will overshoot assume we would have stopped right on target.
+    // If new angle will overshoot stop right on target.
-    // TODO: Improve check to see if less accel would have caused undershoot.
+    if (std::abs(offset.data[i]) < 0.0001 || (change_in_angle.data[i] / offset.data[i] > 1.0))
-    if ((change_in_angle.data[i] / offset.data[i]) > 1.0)
     {
-      state->angular_velocity.data[i] = 0;
+      state->angular_velocity.data[i] =
+          (angle_target.data[i] - state->angle.data[i]) / time_elapsed;
       state->angle.data[i] = angle_target.data[i];
     }
     else
@@ -226,10 +285,7 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi
 
   const auto offset = position_target - state->position;
   const auto stop_offset = offset - stop_distance;
-
+  const auto jerk = MathUtil::Sign(stop_offset) * max_jerk;
-  const Common::Vec3 jerk{std::copysign(max_jerk.x, stop_offset.x),
-                          std::copysign(max_jerk.y, stop_offset.y),
-                          std::copysign(max_jerk.z, stop_offset.z)};
 
   state->acceleration += jerk * time_elapsed;
 

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

@@ -646,11 +646,6 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
     roll_value = MathUtil::Clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE);
     pitch_value = MathUtil::Clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE);
 
-    // TODO: Remove before merge.
-    // INFO_LOG(WIIMOTE, "M+ YAW: 0x%x slow:%d", yaw_value, mplus_data.yaw_slow);
-    // INFO_LOG(WIIMOTE, "M+ ROL: 0x%x slow:%d", roll_value, mplus_data.roll_slow);
-    // INFO_LOG(WIIMOTE, "M+ PIT: 0x%x slow:%d", pitch_value, mplus_data.pitch_slow);
-
     // Bits 0-7
     mplus_data.yaw1 = u8(yaw_value);
     mplus_data.roll1 = u8(roll_value);

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

@@ -688,15 +688,28 @@ void Wiimote::StepDynamics()
 
 Common::Vec3 Wiimote::GetAcceleration()
 {
-  // TODO: Cursor movement should produce acceleration.
+  // TODO: Cursor forward/backward movement should produce acceleration.
 
   Common::Vec3 accel =
       GetOrientation() *
       GetTransformation().Transform(
           m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0);
 
+  // Our shake effects have never been affected by orientation. Should they be?
   accel += m_shake_state.acceleration;
 
+  // Simulate centripetal acceleration caused by an offset of the accelerometer sensor.
+  // Estimate of sensor position based on an image of the wii remote board:
+  constexpr float ACCELEROMETER_Y_OFFSET = 0.1f;
+
+  const auto angular_velocity = GetAngularVelocity();
+  const auto centripetal_accel =
+      // TODO: Is this the proper way to combine the x and z angular velocities?
+      std::pow(std::abs(angular_velocity.x) + std::abs(angular_velocity.z), 2) *
+      ACCELEROMETER_Y_OFFSET;
+
+  accel.y += centripetal_accel;
+
   return accel;
 }
 
@@ -709,7 +722,7 @@ Common::Vec3 Wiimote::GetAngularVelocity()
 Common::Matrix44 Wiimote::GetTransformation() const
 {
   // Includes positional and rotational effects of:
-  // IR, Swing, Tilt, Shake
+  // Cursor, Swing, Tilt, Shake
 
   // TODO: think about and clean up matrix order, make nunchuk match.
   return Common::Matrix44::Translate(-m_shake_state.position) *

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

@@ -492,11 +492,19 @@ void MappingIndicator::DrawForce(ControllerEmu::Force& force)
       QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
 
   // Adjusted Z:
-  if (adj_coord.y)
+  const auto curve_point =
+      std::max(std::abs(m_motion_state.angle.x), std::abs(m_motion_state.angle.z)) / MathUtil::TAU;
+  if (adj_coord.y || curve_point)
   {
-    p.setBrush(GetAdjustedInputColor());
+    // Show off the angle somewhat with a curved line.
-    p.drawRect(
+    QPainterPath path;
-        QRectF(-scale, adj_coord.y * -scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
+    path.moveTo(-scale, (adj_coord.y + curve_point) * -scale);
+    path.quadTo({0, (adj_coord.y - curve_point) * -scale},
+                {scale, (adj_coord.y + curve_point) * -scale});
+
+    p.setBrush(Qt::NoBrush);
+    p.setPen(QPen(GetAdjustedInputColor(), INPUT_DOT_RADIUS));
+    p.drawPath(path);
   }
 
   // Draw "gate" shape.

Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp

@@ -31,16 +31,30 @@ Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType
               _trans("cm"),
               // i18n: Refering to emulated wii remote swing movement.
               _trans("Distance of travel from neutral position.")},
-             25, 0, 100);
+             50, 1, 100);
 
-  AddSetting(&m_jerk_setting,
+  // These speed settings are used to calculate a maximum jerk (change in acceleration).
-             // i18n: "Jerk" as it relates to physics. The time derivative of acceleration.
+  // The calculation uses a travel distance of 1 meter.
-             {_trans("Jerk"),
+  // The maximum value of 40 m/s is the approximate speed of the head of a golf club.
-              // i18n: The symbol/abbreviation for meters per second to the 3rd power.
+  // Games seem to not even properly detect motions at this speed.
-              _trans("m/s³"),
+  // Values result in an exponentially increasing jerk.
+
+  AddSetting(&m_speed_setting,
+             {_trans("Speed"),
+              // i18n: The symbol/abbreviation for meters per second.
+              _trans("m/s"),
               // i18n: Refering to emulated wii remote swing movement.
-              _trans("Maximum change in acceleration.")},
+              _trans("Peak velocity of outward swing movements.")},
-             500, 1, 1000);
+             16, 1, 40);
+
+  // "Return Speed" allows for a "slow return" that won't trigger additional actions.
+  AddSetting(&m_return_speed_setting,
+             {_trans("Return Speed"),
+              // i18n: The symbol/abbreviation for meters per second.
+              _trans("m/s"),
+              // i18n: Refering to emulated wii remote swing movement.
+              _trans("Peak velocity of movements to neutral position.")},
+             2, 1, 40);
 
   AddSetting(&m_angle_setting,
              {_trans("Angle"),
@@ -48,7 +62,7 @@ Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType
               _trans("°"),
               // i18n: Refering to emulated wii remote swing movement.
               _trans("Rotation applied at extremities of swing.")},
-             45, 0, 180);
+             90, 1, 180);
 }
 
 Force::ReshapeData Force::GetReshapableState(bool adjusted)
@@ -70,8 +84,8 @@ Force::StateData Force::GetState(bool adjusted)
 
   if (adjusted)
   {
-    // Apply deadzone to z.
+    // Apply deadzone to z and scale.
-    z = ApplyDeadzone(z, GetDeadzonePercentage());
+    z = ApplyDeadzone(z, GetDeadzonePercentage()) * GetMaxDistance();
   }
 
   return {float(state.x), float(state.y), float(z)};
@@ -83,9 +97,14 @@ ControlState Force::GetGateRadiusAtAngle(double) const
   return GetMaxDistance();
 }
 
-ControlState Force::GetMaxJerk() const
+ControlState Force::GetSpeed() const
 {
-  return m_jerk_setting.GetValue();
+  return m_speed_setting.GetValue();
+}
+
+ControlState Force::GetReturnSpeed() const
+{
+  return m_return_speed_setting.GetValue();
 }
 
 ControlState Force::GetTwistAngle() const

Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h

@@ -26,8 +26,9 @@ public:
 
   StateData GetState(bool adjusted = true);
 
-  // Return jerk in m/s^3.
+  // Velocities returned in m/s.
-  ControlState GetMaxJerk() const;
+  ControlState GetSpeed() const;
+  ControlState GetReturnSpeed() const;
 
   // Return twist angle in radians.
   ControlState GetTwistAngle() const;
@@ -37,7 +38,8 @@ public:
 
 private:
   SettingValue<double> m_distance_setting;
-  SettingValue<double> m_jerk_setting;
+  SettingValue<double> m_speed_setting;
+  SettingValue<double> m_return_speed_setting;
   SettingValue<double> m_angle_setting;
 };