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

Merge pull request #8545 from jordan-woyak/imu-cursor-centering 

WiimoteEmu: IMU pointing behavior improvements and code cleanup.
This commit is contained in:
Pierre Bourdon 2020-01-15 12:10:57 +01:00 committed by GitHub
parent ab07841e1b 0aacf3a627
commit 1ac3264d5d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 148 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
Source/Core/Common/Matrix.cpp
View File

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

3
Source/Core/Common/Matrix.h
View File

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

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

@ -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<RotationalState>* 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) *

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

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

3
Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
View File

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

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

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

6
Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
View File

@ -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<RotationalState> m_imu_cursor_state;
IMUCursorState m_imu_cursor_state;
};
} // namespace WiimoteEmu

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

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