diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index e64cc91be0..48adcba274 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -36,6 +36,84 @@ auto MatrixMultiply(const std::array& a, const std::array& b namespace Common { +Quaternion Quaternion::Identity() +{ + return Quaternion(1, 0, 0, 0); +} + +Quaternion Quaternion::RotateX(float rad) +{ + return Rotate(rad, Vec3(1, 0, 0)); +} + +Quaternion Quaternion::RotateY(float rad) +{ + return Rotate(rad, Vec3(0, 1, 0)); +} + +Quaternion Quaternion::RotateZ(float rad) +{ + return Rotate(rad, Vec3(0, 0, 1)); +} + +Quaternion Quaternion::Rotate(float rad, const Vec3& axis) +{ + const auto sin_angle_2 = std::sin(rad / 2); + + return Quaternion(std::cos(rad / 2), axis.x * sin_angle_2, axis.y * sin_angle_2, + axis.z * sin_angle_2); +} + +Quaternion::Quaternion(float w, float x, float y, float z) : data(x, y, z, w) +{ +} + +float Quaternion::Norm() const +{ + return data.Dot(data); +} + +Quaternion Quaternion::Normalized() const +{ + Quaternion result(*this); + result.data /= Norm(); + return result; +} + +Quaternion Quaternion::Conjugate() const +{ + return Quaternion(data.w, -1 * data.x, -1 * data.y, -1 * data.z); +} + +Quaternion Quaternion::Inverted() const +{ + return Normalized().Conjugate(); +} + +Quaternion& Quaternion::operator*=(const Quaternion& rhs) +{ + auto& a = data; + auto& b = rhs.data; + + data = Vec4{a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + // W + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z}; + return *this; +} + +Quaternion operator*(Quaternion lhs, const Quaternion& rhs) +{ + return lhs *= rhs; +} + +Vec3 operator*(const Quaternion& lhs, const Vec3& rhs) +{ + const auto result = lhs * Quaternion(0, rhs.x, rhs.y, rhs.z) * lhs.Conjugate(); + return Vec3(result.data.x, result.data.y, result.data.z); +} + Matrix33 Matrix33::Identity() { Matrix33 mtx = {}; @@ -45,14 +123,12 @@ Matrix33 Matrix33::Identity() return mtx; } -Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw) +Matrix33 Matrix33::FromQuaternion(const Quaternion& q) { - // Normalize. - const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); - qx *= n; - qy *= n; - qz *= n; - qw *= n; + const auto qx = q.data.x; + const auto qy = q.data.y; + const auto qz = q.data.z; + const auto qw = q.data.w; return { 1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw, diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index ff4ab71c73..bf1e80b567 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -154,6 +154,8 @@ union TVec4 TVec4(TVec3 _vec, T _w) : TVec4{_vec.x, _vec.y, _vec.z, _w} {} TVec4(T _x, T _y, T _z, T _w) : data{_x, _y, _z, _w} {} + T Dot(const TVec4& other) const { return x * other.x + y * other.y + z * other.z + w * other.w; } + TVec4& operator*=(const TVec4& rhs) { x *= rhs.x; @@ -321,11 +323,40 @@ auto operator/(TVec2 lhs, T2 scalar) using Vec2 = TVec2; using DVec2 = TVec2; +class Matrix33; + +class Quaternion +{ +public: + static Quaternion Identity(); + + static Quaternion RotateX(float rad); + static Quaternion RotateY(float rad); + static Quaternion RotateZ(float rad); + + static Quaternion Rotate(float rad, const Vec3& axis); + + Quaternion() = default; + Quaternion(float w, float x, float y, float z); + + float Norm() const; + Quaternion Normalized() const; + Quaternion Conjugate() const; + Quaternion Inverted() const; + + Quaternion& operator*=(const Quaternion& rhs); + + Vec4 data; +}; + +Quaternion operator*(Quaternion lhs, const Quaternion& rhs); +Vec3 operator*(const Quaternion& lhs, const Vec3& rhs); + class Matrix33 { public: static Matrix33 Identity(); - static Matrix33 FromQuaternion(float x, float y, float z, float w); + static Matrix33 FromQuaternion(const Quaternion&); // Return a rotation matrix around the x,y,z axis static Matrix33 RotateX(float rad); 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 150bb9256a..48b69864d7 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -757,10 +757,10 @@ Common::Matrix44 Wiimote::GetTransformation(const Common::Matrix33& extra_rotati Common::Matrix44::Translate(-m_swing_state.position - m_point_state.position); } -Common::Matrix33 Wiimote::GetOrientation() const +Common::Quaternion Wiimote::GetOrientation() const { - return Common::Matrix33::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) * - Common::Matrix33::RotateX(float(MathUtil::TAU / 4 * IsUpright())); + return Common::Quaternion::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) * + Common::Quaternion::RotateX(float(MathUtil::TAU / 4 * IsUpright())); } Common::Vec3 Wiimote::GetTotalAcceleration() const @@ -781,8 +781,9 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity() const Common::Matrix44 Wiimote::GetTotalTransformation() const { - return GetTransformation(m_imu_cursor_state.rotation * - Common::Matrix33::RotateX(m_imu_cursor_state.recentered_pitch)); + return GetTransformation(Common::Matrix33::FromQuaternion( + m_imu_cursor_state.rotation * + Common::Quaternion::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 13fd352617..604e0a05d7 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h @@ -162,7 +162,7 @@ private: 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; + Common::Quaternion GetOrientation() const; Common::Vec3 GetTotalAcceleration() const; Common::Vec3 GetTotalAngularVelocity() const; diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp index 842856d82e..a946059cab 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 9eeac84e2c..7e3fedfe75 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(); };