From 723115b7b68dd64b9bff4ddbdf7e8d3fef07432a Mon Sep 17 00:00:00 2001 From: Jordan Woyak Date: Fri, 3 Jan 2020 18:08:45 -0600 Subject: [PATCH] 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