Skip to content
Permalink
Browse files

Merge pull request #7861 from jordan-woyak/mplus-emu

WiimoteEmu: Emulated MotionPlus and improved emulated swing.
  • Loading branch information...
JMC47 committed Apr 26, 2019
2 parents 057fa6c + ba1b335 commit 664376dae15ef187df4e4c8c7c5e241b9a68fb5d
@@ -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)
{
@@ -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 T>
inline TVec3<T> operator/(TVec3<T> lhs, const TVec3<T>& rhs)
template <typename T1, typename T2>
auto operator*(const TVec3<T1>& lhs, const TVec3<T2>& rhs) -> TVec3<decltype(lhs.x * rhs.x)>
{
return lhs /= rhs;
return lhs.Map(std::multiplies<decltype(lhs.x * rhs.x)>{}, rhs);
}

template <typename T>
TVec3<T> operator*(TVec3<T> lhs, std::common_type_t<T> scalar)
auto operator/(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x / rhs.x)>
{
return lhs *= TVec3<T>{scalar, scalar, scalar};
return lhs.Map(std::divides<decltype(lhs.x / rhs.x)>{}, rhs);
}

template <typename T>
TVec3<T> operator/(TVec3<T> lhs, std::common_type_t<T> scalar)
template <typename T1, typename T2>
auto operator*(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x * scalar)>
{
return lhs.Map(std::multiplies<decltype(lhs.x * scalar)>{}, scalar);
}

template <typename T1, typename T2>
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>;
@@ -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,30 +90,90 @@ 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);
}

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,
@@ -129,10 +189,17 @@ WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3&
u16(MathUtil::Clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))};
}

Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group)
void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed)
{
using Common::Matrix33;
using Common::Matrix44;
const auto cursor = ir_group->GetState(true);

if (!cursor.IsVisible())
{
// Move the wiimote a kilometer forward so the sensor bar is always behind it.
*state = {};
state->position = {0, -1000, 0};
return;
}

// Nintendo recommends a distance of 1-3 meters.
constexpr float NEUTRAL_DISTANCE = 2.f;
@@ -147,12 +214,32 @@ Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group)
const float yaw_scale = ir_group->GetTotalYaw() / 2;
const float pitch_scale = ir_group->GetTotalPitch() / 2;

const auto cursor = ir_group->GetState(true);
// TODO: Move state out of ControllerEmu::Cursor
// TODO: Use ApproachPositionWithJerk
// TODO: Move forward/backward after rotation.
const auto new_position =
Common::Vec3(0, NEUTRAL_DISTANCE - MOVE_DISTANCE * float(cursor.z), -height);

const auto target_angle = Common::Vec3(pitch_scale * -cursor.y, 0, yaw_scale * -cursor.x);

// If cursor was hidden, jump to the target position/angle immediately.
if (state->position.y < 0)
{
state->position = new_position;
state->angle = target_angle;

return;
}

state->acceleration = new_position - state->position;
state->position = new_position;

return Matrix44::Translate({0, MOVE_DISTANCE * float(cursor.z), 0}) *
Matrix44::FromMatrix33(Matrix33::RotateX(pitch_scale * cursor.y) *
Matrix33::RotateZ(yaw_scale * cursor.x)) *
Matrix44::Translate({0, -NEUTRAL_DISTANCE, height});
// 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);
}

void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_target,
@@ -165,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;

@@ -177,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
@@ -201,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;

@@ -19,14 +19,19 @@ constexpr double GRAVITY_ACCELERATION = 9.80665;

struct PositionalState
{
// meters
Common::Vec3 position;
// meters/second
Common::Vec3 velocity;
// meters/second^2
Common::Vec3 acceleration;
};

struct RotationalState
{
// radians
Common::Vec3 angle;
// radians/second
Common::Vec3 angular_velocity;
};

@@ -47,11 +52,10 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& target,
void EmulateShake(PositionalState* state, ControllerEmu::Shake* shake_group, float time_elapsed);
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);

// Convert m/s/s acceleration data to the format used by Wiimote/Nunchuk (10-bit unsigned integers).
WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
u16 one_g);

Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group);

} // namespace WiimoteEmu

0 comments on commit 664376d

Please sign in to comment.
You can’t perform that action at this time.