diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp index b84d638cfb..0f5532d58a 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp @@ -58,9 +58,9 @@ double CalculateStopDistance(double velocity, double max_accel) namespace WiimoteEmu { -Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope, - const Common::Vec3& accelerometer, float accel_weight, - const Common::Vec3& accelerometer_normal) +Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope, + const Common::Vec3& accelerometer, float accel_weight, + const Common::Vec3& accelerometer_normal) { const auto gyro_vec = gyroscope * accelerometer_normal; const auto normalized_accel = accelerometer.Normalized(); @@ -72,7 +72,7 @@ Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope, 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; + return Common::Quaternion::Rotate(std::acos(cos_angle) * accel_weight, axis) * gyroscope; } else { @@ -80,7 +80,7 @@ Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope, } } -IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()} +IMUCursorState::IMUCursorState() : rotation{Common::Quaternion::Identity()} { } @@ -311,7 +311,7 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr } // Apply rotation from gyro data. - const auto gyro_rotation = GetMatrixFromGyroscope(*ang_vel * -1 * time_elapsed); + const auto gyro_rotation = GetRotationFromGyroscope(*ang_vel * -1 * time_elapsed); state->rotation = gyro_rotation * state->rotation; // If we have some non-zero accel data use it to adjust gyro drift. @@ -334,7 +334,10 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr // Adjust yaw as needed. if (yaw != target_yaw) - state->rotation *= Common::Matrix33::RotateZ(target_yaw - yaw); + state->rotation *= Common::Quaternion::RotateZ(target_yaw - yaw); + + // Normalize for floating point inaccuracies. + state->rotation = state->rotation.Normalized(); } void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target, @@ -374,7 +377,7 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi } } -Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel) +Common::Quaternion GetRotationFromAcceleration(const Common::Vec3& accel) { const auto normalized_accel = accel.Normalized(); @@ -382,13 +385,13 @@ Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel) 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}); + return Common::Quaternion::Rotate(angle, axis.LengthSquared() ? axis.Normalized() : + Common::Vec3{0, 1, 0}); } -Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro) +Common::Quaternion GetRotationFromGyroscope(const Common::Vec3& gyro) { - return Common::Matrix33::FromQuaternion(gyro.x / 2, gyro.y / 2, gyro.z / 2, 1); + return Common::Quaternion{1, gyro.x / 2, gyro.y / 2, gyro.z / 2}; } Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle) @@ -397,19 +400,19 @@ Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle) Common::Matrix33::RotateX(angle.x); } -float GetPitch(const Common::Matrix33& world_rotation) +float GetPitch(const Common::Quaternion& world_rotation) { const auto vec = world_rotation * Common::Vec3{0, 0, 1}; return std::atan2(vec.y, Common::Vec2(vec.x, vec.z).Length()); } -float GetRoll(const Common::Matrix33& world_rotation) +float GetRoll(const Common::Quaternion& world_rotation) { const auto vec = world_rotation * Common::Vec3{0, 0, 1}; return std::atan2(vec.x, vec.z); } -float GetYaw(const Common::Matrix33& world_rotation) +float GetYaw(const Common::Quaternion& world_rotation) { const auto vec = world_rotation.Inverted() * Common::Vec3{0, 1, 0}; return std::atan2(vec.x, vec.y); diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h index 29ea03dea6..475d54ca1f 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h @@ -44,7 +44,7 @@ struct IMUCursorState IMUCursorState(); // Rotation of world around device. - Common::Matrix33 rotation; + Common::Quaternion rotation; float recentered_pitch = {}; }; @@ -57,22 +57,22 @@ struct MotionState : PositionalState, RotationalState // Note that 'gyroscope' is rotation of world around device. // Alternative accelerometer_normal can be supplied to correct from non-accelerometer data. // e.g. Used for yaw/pitch correction with IR data. -Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope, - const Common::Vec3& accelerometer, float accel_weight, - const Common::Vec3& accelerometer_normal = {0, 0, 1}); +Common::Quaternion ComplementaryFilter(const Common::Quaternion& gyroscope, + const Common::Vec3& accelerometer, float accel_weight, + const Common::Vec3& accelerometer_normal = {0, 0, 1}); // Estimate orientation from accelerometer data. -Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel); +Common::Quaternion GetRotationFromAcceleration(const Common::Vec3& accel); -// Get a rotation matrix from current gyro data. -Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro); +// Get a quaternion from current gyro data. +Common::Quaternion GetRotationFromGyroscope(const Common::Vec3& gyro); // Build a rotational matrix from euler angles. Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle); -float GetPitch(const Common::Matrix33& world_rotation); -float GetRoll(const Common::Matrix33& world_rotation); -float GetYaw(const Common::Matrix33& world_rotation); +float GetPitch(const Common::Quaternion& world_rotation); +float GetRoll(const Common::Quaternion& world_rotation); +float GetYaw(const Common::Quaternion& world_rotation); void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& target, const Common::Vec3& max_jerk, float time_elapsed); diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp index 549251aed1..3bc2fe0b63 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -758,7 +758,7 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() const Common::Matrix44 Wiimote::GetTotalTransformation() const { - return GetTransformation(m_imu_cursor_state.rotation * + return GetTransformation(Common::Matrix33::FromQuaternion(m_imu_cursor_state.rotation) * Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch)); } diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp index eb56434bd1..c55c28f975 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp @@ -579,7 +579,7 @@ void AccelerometerMappingIndicator::Draw() // UI axes are opposite that of Wii remote accelerometer. p.scale(-1.0, -1.0); - const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state); + const auto rotation = WiimoteEmu::GetRotationFromAcceleration(state); // Draw sphere. p.setPen(GetCosmeticPen(QPen(GetRawInputColor(), 0.5))); @@ -650,8 +650,9 @@ void GyroMappingIndicator::Draw() const auto jitter = raw_gyro_state - m_previous_velocity; m_previous_velocity = raw_gyro_state; - m_state *= WiimoteEmu::GetMatrixFromGyroscope(angular_velocity * Common::Vec3(-1, +1, -1) / - INDICATOR_UPDATE_FREQ); + m_state *= WiimoteEmu::GetRotationFromGyroscope(angular_velocity * Common::Vec3(-1, +1, -1) / + INDICATOR_UPDATE_FREQ); + m_state = m_state.Normalized(); // Reset orientation when stable for a bit: constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ; @@ -664,10 +665,11 @@ void GyroMappingIndicator::Draw() ++m_stable_steps; if (STABLE_RESET_STEPS == m_stable_steps) - m_state = Common::Matrix33::Identity(); + m_state = Common::Quaternion::Identity(); // Use an empty rotation matrix if gyroscope data is not present. - const auto rotation = (gyro_state.has_value() ? m_state : Common::Matrix33{}); + const auto rotation = + (gyro_state.has_value() ? Common::Matrix33::FromQuaternion(m_state) : Common::Matrix33{}); QPainter p(this); DrawBoundingBox(p); diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h index 763c9e3db7..e5abb12e25 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h @@ -176,7 +176,7 @@ private: void Draw() override; ControllerEmu::IMUGyroscope& m_gyro_group; - Common::Matrix33 m_state = Common::Matrix33::Identity(); + Common::Quaternion m_state = Common::Quaternion::Identity(); Common::Vec3 m_previous_velocity = {}; u32 m_stable_steps = 0; }; diff --git a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp index ef0d15090b..2a95fc0a57 100644 --- a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp +++ b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp @@ -1179,7 +1179,7 @@ void Device::UpdateOrientation() // Apply M+ gyro data to our orientation. m_orientation = - WiimoteEmu::GetMatrixFromGyroscope(m_mplus_state.gyro_data * -1 * elapsed_time.count()) * + WiimoteEmu::GetRotationFromGyroscope(m_mplus_state.gyro_data * -1 * elapsed_time.count()) * m_orientation; // When M+ data is not available give accel/ir data more weight. @@ -1204,7 +1204,7 @@ void Device::UpdateOrientation() m_ir_state.center_position.x * WiimoteEmu::CameraLogic::CAMERA_FOV_X) / 2; const auto ir_normal = Common::Vec3(0, 1, 0); - const auto ir_vector = WiimoteEmu::GetMatrixFromGyroscope(-ir_rotation) * ir_normal; + const auto ir_vector = WiimoteEmu::GetRotationFromGyroscope(-ir_rotation) * ir_normal; // Pitch correction will be slightly wrong based on sensorbar height. // Keep weight below accelerometer weight for that reason. @@ -1214,6 +1214,9 @@ void Device::UpdateOrientation() m_orientation = WiimoteEmu::ComplementaryFilter(m_orientation, ir_vector, ir_weight, ir_normal); } + // Normalize for floating point inaccuracies. + m_orientation = m_orientation.Normalized(); + // Update our (pitch, roll, yaw) inputs now that orientation has been adjusted. m_rotation_inputs = Common::Vec3{WiimoteEmu::GetPitch(m_orientation), WiimoteEmu::GetRoll(m_orientation), diff --git a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h index 75a5c447e7..0dc9082af6 100644 --- a/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h +++ b/Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h @@ -275,7 +275,7 @@ private: std::list m_report_handlers; // World rotation. (used to rotate IR data and provide pitch, roll, yaw inputs) - Common::Matrix33 m_orientation = Common::Matrix33::Identity(); + Common::Quaternion m_orientation = Common::Quaternion::Identity(); Clock::time_point m_last_report_time = Clock::now(); };