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

WiimoteEmu: Clean up ComplementaryFilter math.

This commit is contained in:
Jordan Woyak 2020-01-03 16:16:26 -06:00
parent 120c6dc850
commit 72c2be52ed
5 changed files with 64 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,6 +54,29 @@ double CalculateStopDistance(double velocity, double max_accel)
return velocity * velocity / (2 * std::copysign(max_accel, velocity));
}
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)
{
INFO_LOG(WIIMOTE, "angle diff: %lf", std::acos(cos_angle) * 360 / MathUtil::TAU);
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
@ -271,94 +294,31 @@ 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(Common::Matrix33* 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;
if (!imu_ir_group->enabled)
{
st = std::nullopt;
return;
}
auto accel = imu_accelerometer_group->GetState();
auto ang_vel = imu_gyroscope_group->GetState();
// The IMU Cursor requires both an accelerometer and a gyroscope to function correctly.
if (!(accel.has_value() && ang_vel.has_value()))
// Recenter if we have no gyro data or the "Recenter" button is pressed.
if (!ang_vel.has_value() || imu_ir_group->controls[0]->control_ref->State() >
ControllerEmu::Buttons::ACTIVATION_THRESHOLD)
{
st = std::nullopt;
*state = Common::Matrix33::Identity();
return;
}
if (!st.has_value())
st = RotationalState{};
// Apply rotation from gyro data.
const auto 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 * *state;
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;
// If we have usable accel data use it to adjust gyro drift.
constexpr float accel_weight = 0.02;
auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
if (accel.LengthSquared())
*state = ComplementaryFilter(accel, *state, accel_weight);
}
void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
@ -398,6 +358,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) *

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

@ -44,6 +44,9 @@ 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 +60,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(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);

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

@ -190,7 +190,7 @@ void Wiimote::Reset()
m_tilt_state = {};
m_cursor_state = {};
m_shake_state = {};
m_imu_cursor_state = {};
m_imu_cursor_state = Common::Matrix33::Identity();
}
Wiimote::Wiimote(const unsigned int index) : m_index(index)
@ -812,12 +812,13 @@ Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity)
Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 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(
m_imu_cursor_state * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle -
m_cursor_state.angle - extra_rotation)) *
Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
}
@ -836,6 +837,7 @@ Common::Vec3 Wiimote::GetTotalAcceleration()
return GetAcceleration();
}
// TODO: Kill this function.
Common::Vec3 Wiimote::GetTotalAngularVelocity()
{
auto ang_vel = m_imu_gyroscope->GetState();
@ -845,13 +847,10 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
return GetAngularVelocity();
}
// TODO: Kill this function.
Common::Matrix44 Wiimote::GetTotalTransformation() const
{
auto state = m_imu_cursor_state;
if (state.has_value())
return GetTransformation(state->angle);
else
return GetTransformation();
return GetTransformation();
}
} // namespace WiimoteEmu

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

@ -300,6 +300,6 @@ private:
RotationalState m_tilt_state;
MotionState m_cursor_state;
PositionalState m_shake_state;
std::optional<RotationalState> m_imu_cursor_state;
Common::Matrix33 m_imu_cursor_state;
};
} // namespace WiimoteEmu

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

@ -723,12 +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});
// Check that axis is non-zero to handle perfect up/down orientations.
const auto rotation = Common::Matrix33::Rotate(
angle, axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
// Draw sphere.
p.setPen(Qt::NoPen);