ShuriZma
/
dolphin
mirror of https://github.com/dolphin-emu/dolphin.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

InputCommon/WiimoteEmu: Replace stateful rotational matrices with quaternions.

This commit is contained in:
Jordan Woyak 2020-10-19 15:29:16 -05:00
parent 39030ea33c
commit 09431635f3
7 changed files with 43 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
View File

@ -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);

20
Source/Core/Core/HW/WiimoteEmu/Dynamics.h
View File

@ -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);

2
Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
View File

@ -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));
}

12
Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
View File

@ -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);

2
Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
View File

@ -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;
};

7
Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.cpp
View File

@ -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),

2
Source/Core/InputCommon/ControllerInterface/Wiimote/Wiimote.h
View File

@ -275,7 +275,7 @@ private:
std::list<ReportHandler> 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();
};