diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index 9581a5a4ce..e64cc91be0 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -45,6 +45,22 @@ Matrix33 Matrix33::Identity() return mtx; } +Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw) +{ + // Normalize. + const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); + qx *= n; + qy *= n; + qz *= n; + qw *= n; + + return { + 1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw, + 2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz, 2 * qy * qz - 2 * qx * qw, + 2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx * qx - 2 * qy * qy, + }; +} + Matrix33 Matrix33::RotateX(float rad) { const float s = std::sin(rad); @@ -120,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 ed58abdccb..b47e24f03d 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -273,6 +273,7 @@ class Matrix33 { public: static Matrix33 Identity(); + static Matrix33 FromQuaternion(float x, float y, float z, float w); // Return a rotation matrix around the x,y,z axis static Matrix33 RotateX(float rad); @@ -287,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 0e33182cf2..1c0da74a0f 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp @@ -54,10 +54,36 @@ 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) +{ + 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) + { + 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 { +IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()} +{ +} + void EmulateShake(PositionalState* state, ControllerEmu::Shake* const shake_group, float time_elapsed) { @@ -271,94 +297,49 @@ 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(IMUCursorState* 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; + const auto ang_vel = imu_gyroscope_group->GetState(); - if (!imu_ir_group->enabled) + // Reset if pointing is disabled or we have no gyro data. + if (!imu_ir_group->enabled || !ang_vel.has_value()) { - st = std::nullopt; + *state = {}; return; } - auto accel = imu_accelerometer_group->GetState(); - auto ang_vel = imu_gyroscope_group->GetState(); + // Apply rotation from gyro data. + 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; - // The IMU Cursor requires both an accelerometer and a gyroscope to function correctly. - if (!(accel.has_value() && ang_vel.has_value())) + // 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) { - st = std::nullopt; - return; + state->recentered_pitch = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).z); + target_yaw = 0; } - if (!st.has_value()) - st = RotationalState{}; - - 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; + // 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, @@ -398,6 +379,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..b72285a159 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h @@ -39,11 +39,24 @@ struct RotationalState Common::Vec3 angular_velocity; }; +struct IMUCursorState +{ + IMUCursorState(); + + // Rotation of world around device. + Common::Matrix33 rotation; + + float recentered_pitch = {}; +}; + // Contains both positional and rotational state. 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 +70,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(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/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 755baf242b..997582595c 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -190,6 +190,7 @@ void Wiimote::Reset() m_tilt_state = {}; m_cursor_state = {}; m_shake_state = {}; + m_imu_cursor_state = {}; } @@ -809,15 +810,16 @@ 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: - // 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( + 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); } @@ -847,11 +849,8 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() Common::Matrix44 Wiimote::GetTotalTransformation() const { - auto state = m_imu_cursor_state; - if (state.has_value()) - return GetTransformation(state->angle); - else - return GetTransformation(); + return GetTransformation(m_imu_cursor_state.rotation * + Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch)); } } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h index 1f13d29d2e..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; - std::optional m_imu_cursor_state; + + IMUCursorState 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 d8fa5aaaaa..150b035791 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp @@ -723,13 +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}).Normalized(); - - // Odd checks to handle case of 0g (draw no sphere) and perfect up/down orientation. - const auto rotation = (!state.LengthSquared() || axis.LengthSquared() < 2) ? - Common::Matrix33::Rotate(angle, axis) : - Common::Matrix33::Identity(); + const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state); // Draw sphere. p.setPen(Qt::NoPen); @@ -797,9 +791,9 @@ void GyroMappingIndicator::paintEvent(QPaintEvent*) const auto gyro_state = m_gyro_group.GetState(); const auto angular_velocity = gyro_state.value_or(Common::Vec3{}); - m_state *= Common::Matrix33::RotateX(angular_velocity.x / -INDICATOR_UPDATE_FREQ) * - Common::Matrix33::RotateY(angular_velocity.y / INDICATOR_UPDATE_FREQ) * - Common::Matrix33::RotateZ(angular_velocity.z / -INDICATOR_UPDATE_FREQ); + m_state *= Common::Matrix33::FromQuaternion(angular_velocity.x / -INDICATOR_UPDATE_FREQ / 2, + angular_velocity.y / INDICATOR_UPDATE_FREQ / 2, + angular_velocity.z / -INDICATOR_UPDATE_FREQ / 2, 1); // Reset orientation when stable for a bit: constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ;