From 72c2be52eda47bf3e6b65e2f29c882aed0010415 Mon Sep 17 00:00:00 2001 From: Jordan Woyak Date: Fri, 3 Jan 2020 16:16:26 -0600 Subject: [PATCH] WiimoteEmu: Clean up ComplementaryFilter math. --- Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp | 128 +++++++----------- Source/Core/Core/HW/WiimoteEmu/Dynamics.h | 5 +- Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp | 17 ++- Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h | 2 +- .../Config/Mapping/MappingIndicator.cpp | 7 +- 5 files changed, 64 insertions(+), 95 deletions(-) diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp index 0e33182cf2..1742d70c8e 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp @@ -54,6 +54,29 @@ double CalculateStopDistance(double velocity, double max_accel) return velocity * velocity / (2 * std::copysign(max_accel, velocity)); } +Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer, + const Common::Matrix33& gyroscope, float accel_weight) +{ + const auto gyro_vec = gyroscope * Common::Vec3{0, 0, 1}; + const auto normalized_accel = accelerometer.Normalized(); + + const auto cos_angle = normalized_accel.Dot(gyro_vec); + + // If gyro to accel angle difference is betwen 0 and 180 degrees we make an adjustment. + 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; + } + else + { + return gyroscope; + } +} + } // namespace namespace WiimoteEmu @@ -271,94 +294,31 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta } } -static Common::Vec3 NormalizeAngle(Common::Vec3 angle) -{ - // TODO: There must be a more elegant way to do this - angle.x = fmod(angle.x, float(MathUtil::TAU)); - angle.y = fmod(angle.y, float(MathUtil::TAU)); - angle.z = fmod(angle.z, float(MathUtil::TAU)); - angle.x += angle.x < 0 ? float(MathUtil::TAU) : 0; - angle.y += angle.y < 0 ? float(MathUtil::TAU) : 0; - angle.z += angle.z < 0 ? float(MathUtil::TAU) : 0; - return angle; -} - -static Common::Vec3 ComplementaryFilter(const Common::Vec3& angle, - const Common::Vec3& accelerometer, - const Common::Vec3& gyroscope, float time_elapsed) -{ - Common::Vec3 gyroangle = angle + gyroscope * time_elapsed; - gyroangle = NormalizeAngle(gyroangle); - - // Calculate accelerometer tilt angles - Common::Vec3 accangle = gyroangle; - if ((accelerometer.x != 0 && accelerometer.y != 0) || accelerometer.z != 0) - { - float accpitch = -atan2(accelerometer.y, -accelerometer.z) + float(MathUtil::PI); - float accroll = atan2(accelerometer.x, -accelerometer.z) + float(MathUtil::PI); - accangle = {accpitch, accroll, gyroangle.z}; - } - - // Massage accelerometer and gyroscope angle values so that averaging them works when they are on - // opposite sides of TAU / zero (which both represent the same angle) - // TODO: There must be a more elegant way to do this - constexpr float DEG360 = float(MathUtil::TAU); - constexpr float DEG270 = DEG360 * 0.75f; - constexpr float DEG90 = DEG360 * 0.25f; - if (accangle.x < DEG90 && gyroangle.x > DEG270) - accangle.x += DEG360; - else if (gyroangle.x < DEG90 && accangle.x > DEG270) - gyroangle.x += DEG360; - if (accangle.y < DEG90 && gyroangle.y > DEG270) - accangle.y += DEG360; - else if (gyroangle.y < DEG90 && accangle.y > DEG270) - gyroangle.y += DEG360; - - // Combine accelerometer and gyroscope angles - return NormalizeAngle((gyroangle * 0.98f) + (accangle * 0.02f)); -} - -void EmulateIMUCursor(std::optional* state, ControllerEmu::IMUCursor* imu_ir_group, +void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group, ControllerEmu::IMUAccelerometer* imu_accelerometer_group, ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed) { - // Avoid having to double dereference - auto& st = *state; - - if (!imu_ir_group->enabled) - { - st = std::nullopt; - return; - } - - auto accel = imu_accelerometer_group->GetState(); auto ang_vel = imu_gyroscope_group->GetState(); - // The IMU Cursor requires both an accelerometer and a gyroscope to function correctly. - if (!(accel.has_value() && ang_vel.has_value())) + // 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) { - st = std::nullopt; + *state = Common::Matrix33::Identity(); return; } - if (!st.has_value()) - st = RotationalState{}; + // 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; - st->angle = ComplementaryFilter(st->angle, accel.value(), ang_vel.value(), time_elapsed); - - // Reset camera yaw angle - constexpr ControlState BUTTON_THRESHOLD = 0.5; - if (imu_ir_group->controls[0]->control_ref->State() > BUTTON_THRESHOLD) - st->angle.z = 0; - - // Limit camera yaw angle - float totalyaw = float(imu_ir_group->GetTotalYaw()); - float yawmax = totalyaw / 2; - float yawmin = float(MathUtil::TAU) - totalyaw / 2; - if (st->angle.z > yawmax && st->angle.z <= float(MathUtil::PI)) - st->angle.z = yawmax; - if (st->angle.z < yawmin && st->angle.z > float(MathUtil::PI)) - st->angle.z = yawmin; + // 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); } void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target, @@ -398,6 +358,18 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi } } +Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel) +{ + const auto normalized_accel = accel.Normalized(); + + const auto angle = std::acos(normalized_accel.Dot({0, 0, 1})); + const auto axis = normalized_accel.Cross({0, 0, 1}); + + // Check that axis is non-zero to handle perfect up/down orientations. + return Common::Matrix33::Rotate(angle, + axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0}); +} + Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle) { return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) * diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h index ecf55d4ee5..42cc60067a 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h @@ -44,6 +44,9 @@ struct MotionState : PositionalState, RotationalState { }; +// Estimate orientation from accelerometer data. +Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel); + // Build a rotational matrix from euler angles. Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle); @@ -57,7 +60,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(std::optional* state, ControllerEmu::IMUCursor* imu_ir_group, +void EmulateIMUCursor(Common::Matrix33* 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 5ffc8aaf98..7869bf3b62 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -190,7 +190,7 @@ void Wiimote::Reset() m_tilt_state = {}; m_cursor_state = {}; m_shake_state = {}; - m_imu_cursor_state = {}; + m_imu_cursor_state = Common::Matrix33::Identity(); } Wiimote::Wiimote(const unsigned int index) : m_index(index) @@ -812,12 +812,13 @@ Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity) Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const { // Includes positional and rotational effects of: - // Cursor, Swing, Tilt, Shake + // Point, Swing, Tilt, Shake // TODO: Think about and clean up matrix order + make nunchuk match. return Common::Matrix44::Translate(-m_shake_state.position) * - Common::Matrix44::FromMatrix33(GetRotationalMatrix( - -m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle - extra_rotation)) * + Common::Matrix44::FromMatrix33( + m_imu_cursor_state * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle - + m_cursor_state.angle - extra_rotation)) * Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position); } @@ -836,6 +837,7 @@ Common::Vec3 Wiimote::GetTotalAcceleration() return GetAcceleration(); } +// TODO: Kill this function. Common::Vec3 Wiimote::GetTotalAngularVelocity() { auto ang_vel = m_imu_gyroscope->GetState(); @@ -845,13 +847,10 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() return GetAngularVelocity(); } +// TODO: Kill this function. Common::Matrix44 Wiimote::GetTotalTransformation() const { - auto state = m_imu_cursor_state; - if (state.has_value()) - return GetTransformation(state->angle); - else - return GetTransformation(); + return GetTransformation(); } } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h index 1f13d29d2e..32489db229 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h @@ -300,6 +300,6 @@ private: RotationalState m_tilt_state; MotionState m_cursor_state; PositionalState m_shake_state; - std::optional m_imu_cursor_state; + Common::Matrix33 m_imu_cursor_state; }; } // namespace WiimoteEmu diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp index 8e81b380d5..150b035791 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp @@ -723,12 +723,7 @@ void AccelerometerMappingIndicator::paintEvent(QPaintEvent*) p.setRenderHint(QPainter::Antialiasing, true); p.setRenderHint(QPainter::SmoothPixmapTransform, true); - const auto angle = std::acos(state.Normalized().Dot({0, 0, 1})); - const auto axis = state.Normalized().Cross({0, 0, 1}); - - // Check that axis is non-zero to handle perfect up/down orientations. - const auto rotation = Common::Matrix33::Rotate( - angle, axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0}); + const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state); // Draw sphere. p.setPen(Qt::NoPen);