diff --git a/Source/Core/Common/MathUtil.h b/Source/Core/Common/MathUtil.h index bfbf0a8a77..990a5dd145 100644 --- a/Source/Core/Common/MathUtil.h +++ b/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 +constexpr auto Sign(const T& val) -> decltype((T{} < val) - (val < T{})) +{ + return (T{} < val) - (val < T{}); +} + +template +constexpr auto Lerp(const T& x, const T& y, const F& a) -> decltype(x + (y - x) * a) +{ + return x + (y - x) * a; +} + template constexpr bool IsPow2(T imm) { diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index f96624fef3..bf23963b3d 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -6,6 +6,7 @@ #include #include +#include #include // 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 + auto Map(F&& f) const -> TVec3 + { + return {f(x), f(y), f(z)}; + } + + template + auto Map(F&& f, const TVec3& t) const -> TVec3 + { + return {f(x, t.x), f(y, t.y), f(z, t.z)}; + } + + template + auto Map(F&& f, T2 scalar) const -> TVec3 + { + return {f(x, scalar), f(y, scalar), f(z, scalar)}; + } + std::array data = {}; struct @@ -69,39 +89,45 @@ union TVec3 }; template -TVec3 operator+(TVec3 lhs, const TVec3& rhs) +TVec3 operator<(const TVec3& lhs, const TVec3& rhs) { - return lhs += rhs; + return lhs.Map(std::less{}, rhs); } template -TVec3 operator-(TVec3 lhs, const TVec3& rhs) +auto operator+(const TVec3& lhs, const TVec3& rhs) -> TVec3 { - return lhs -= rhs; + return lhs.Map(std::plus{}, rhs); } template -TVec3 operator*(TVec3 lhs, const TVec3& rhs) +auto operator-(const TVec3& lhs, const TVec3& rhs) -> TVec3 { - return lhs *= rhs; + return lhs.Map(std::minus{}, rhs); +} + +template +auto operator*(const TVec3& lhs, const TVec3& rhs) -> TVec3 +{ + return lhs.Map(std::multiplies{}, rhs); } template -inline TVec3 operator/(TVec3 lhs, const TVec3& rhs) +auto operator/(const TVec3& lhs, const TVec3& rhs) -> TVec3 { - return lhs /= rhs; + return lhs.Map(std::divides{}, rhs); } -template -TVec3 operator*(TVec3 lhs, std::common_type_t scalar) +template +auto operator*(const TVec3& lhs, T2 scalar) -> TVec3 { - return lhs *= TVec3{scalar, scalar, scalar}; + return lhs.Map(std::multiplies{}, scalar); } -template -TVec3 operator/(TVec3 lhs, std::common_type_t scalar) +template +auto operator/(const TVec3& lhs, T2 scalar) -> TVec3 { - return lhs /= TVec3{scalar, scalar, scalar}; + return lhs.Map(std::divides{}, scalar); } using Vec3 = TVec3; diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp index c109e2dcf6..e4c74668fa 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp +++ b/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}, - Common::Vec3{1, 1, 1} * swing_group->GetMaxJerk(), time_elapsed); + const auto target_position = Common::Vec3{-input_state.x, -input_state.z, input_state.y}; - // Just jump to our target angle scaled by our progress to the target position. - // TODO: If we wanted to be less hacky we could use ApproachAngleWithAccel. - const auto angle = state->position / swing_group->GetMaxDistance() * swing_group->GetTwistAngle(); + // Jerk is scaled based on input distance from center. + // X and Z scale is connected for sane movement about the circle. + 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; - state->angle = {-angle.z, 0, angle.x}; + // Convert our m/s "speed" to the jerk required to reach this speed when traveling 1 meter. + const auto max_jerk = speed * speed * speed * 4; - // Update velocity based on change in angle. - state->angular_velocity = state->angle - old_angle; + // Rotational acceleration to approximately match the completion time of our swing. + 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: - constexpr auto MAX_ACCEL = float(MathUtil::TAU * 100); + // Higher values will be more responsive but increase rate of M+ "desync". + // 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 Common::Vec3 accel{std::copysign(max_accel, stop_offset.x), - std::copysign(max_accel, stop_offset.y), - std::copysign(max_accel, stop_offset.z)}; + const auto accel = MathUtil::Sign(stop_offset) * max_accel; 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. - // TODO: Improve check to see if less accel would have caused undershoot. - if ((change_in_angle.data[i] / offset.data[i]) > 1.0) + // If new angle will overshoot stop right on target. + if (std::abs(offset.data[i]) < 0.0001 || (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 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)}; + const auto jerk = MathUtil::Sign(stop_offset) * max_jerk; state->acceleration += jerk * time_elapsed; diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp index 1afe31b2ad..de5e55f69e 100644 --- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp +++ b/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); diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp index 096247e8e6..6d24824608 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/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) * diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp index e69b93c9d1..2cd768feea 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp +++ b/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()); - p.drawRect( - QRectF(-scale, adj_coord.y * -scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS)); + // Show off the angle somewhat with a curved line. + QPainterPath path; + 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. diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp index 93a9d06cb3..7ddbeba335 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp +++ b/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, - // i18n: "Jerk" as it relates to physics. The time derivative of acceleration. - {_trans("Jerk"), - // i18n: The symbol/abbreviation for meters per second to the 3rd power. - _trans("m/s³"), + // These speed settings are used to calculate a maximum jerk (change in acceleration). + // The calculation uses a travel distance of 1 meter. + // The maximum value of 40 m/s is the approximate speed of the head of a golf club. + // Games seem to not even properly detect motions at this speed. + // 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.")}, - 500, 1, 1000); + _trans("Peak velocity of outward swing movements.")}, + 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. - z = ApplyDeadzone(z, GetDeadzonePercentage()); + // Apply deadzone to z and scale. + 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 diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h index da66dc034e..ae9cabe340 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h +++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h @@ -26,8 +26,9 @@ public: StateData GetState(bool adjusted = true); - // Return jerk in m/s^3. - ControlState GetMaxJerk() const; + // Velocities returned in m/s. + ControlState GetSpeed() const; + ControlState GetReturnSpeed() const; // Return twist angle in radians. ControlState GetTwistAngle() const; @@ -37,7 +38,8 @@ public: private: SettingValue m_distance_setting; - SettingValue m_jerk_setting; + SettingValue m_speed_setting; + SettingValue m_return_speed_setting; SettingValue m_angle_setting; };