From 0aacf3a62768eac380800a1b8279aa642b17ca92 Mon Sep 17 00:00:00 2001 From: Jordan Woyak Date: Fri, 3 Jan 2020 18:24:41 -0600 Subject: [PATCH] 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