WiimoteEmu: Clean up ComplementaryFilter math.
This commit is contained in:
parent
120c6dc850
commit
72c2be52ed
|
@ -54,6 +54,29 @@ double CalculateStopDistance(double velocity, double max_accel)
|
||||||
return velocity * velocity / (2 * std::copysign(max_accel, velocity));
|
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
|
||||||
|
|
||||||
namespace WiimoteEmu
|
namespace WiimoteEmu
|
||||||
|
@ -271,94 +294,31 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static Common::Vec3 NormalizeAngle(Common::Vec3 angle)
|
void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
|
||||||
{
|
|
||||||
// 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,
|
|
||||||
ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
|
ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
|
||||||
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
|
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();
|
auto ang_vel = imu_gyroscope_group->GetState();
|
||||||
|
|
||||||
// The IMU Cursor requires both an accelerometer and a gyroscope to function correctly.
|
// Recenter if we have no gyro data or the "Recenter" button is pressed.
|
||||||
if (!(accel.has_value() && ang_vel.has_value()))
|
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;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!st.has_value())
|
// Apply rotation from gyro data.
|
||||||
st = RotationalState{};
|
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);
|
// If we have usable accel data use it to adjust gyro drift.
|
||||||
|
constexpr float accel_weight = 0.02;
|
||||||
// Reset camera yaw angle
|
auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
|
||||||
constexpr ControlState BUTTON_THRESHOLD = 0.5;
|
if (accel.LengthSquared())
|
||||||
if (imu_ir_group->controls[0]->control_ref->State() > BUTTON_THRESHOLD)
|
*state = ComplementaryFilter(accel, *state, accel_weight);
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
|
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)
|
Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
|
||||||
{
|
{
|
||||||
return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *
|
return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *
|
||||||
|
|
|
@ -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.
|
// Build a rotational matrix from euler angles.
|
||||||
Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
|
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 EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed);
|
||||||
void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_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 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::IMUAccelerometer* imu_accelerometer_group,
|
||||||
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
|
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
|
||||||
|
|
||||||
|
|
|
@ -190,7 +190,7 @@ void Wiimote::Reset()
|
||||||
m_tilt_state = {};
|
m_tilt_state = {};
|
||||||
m_cursor_state = {};
|
m_cursor_state = {};
|
||||||
m_shake_state = {};
|
m_shake_state = {};
|
||||||
m_imu_cursor_state = {};
|
m_imu_cursor_state = Common::Matrix33::Identity();
|
||||||
}
|
}
|
||||||
|
|
||||||
Wiimote::Wiimote(const unsigned int index) : m_index(index)
|
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
|
Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
|
||||||
{
|
{
|
||||||
// Includes positional and rotational effects of:
|
// 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.
|
// TODO: Think about and clean up matrix order + make nunchuk match.
|
||||||
return Common::Matrix44::Translate(-m_shake_state.position) *
|
return Common::Matrix44::Translate(-m_shake_state.position) *
|
||||||
Common::Matrix44::FromMatrix33(GetRotationalMatrix(
|
Common::Matrix44::FromMatrix33(
|
||||||
-m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle - extra_rotation)) *
|
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);
|
Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -836,6 +837,7 @@ Common::Vec3 Wiimote::GetTotalAcceleration()
|
||||||
return GetAcceleration();
|
return GetAcceleration();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO: Kill this function.
|
||||||
Common::Vec3 Wiimote::GetTotalAngularVelocity()
|
Common::Vec3 Wiimote::GetTotalAngularVelocity()
|
||||||
{
|
{
|
||||||
auto ang_vel = m_imu_gyroscope->GetState();
|
auto ang_vel = m_imu_gyroscope->GetState();
|
||||||
|
@ -845,12 +847,9 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
|
||||||
return GetAngularVelocity();
|
return GetAngularVelocity();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO: Kill this function.
|
||||||
Common::Matrix44 Wiimote::GetTotalTransformation() const
|
Common::Matrix44 Wiimote::GetTotalTransformation() const
|
||||||
{
|
{
|
||||||
auto state = m_imu_cursor_state;
|
|
||||||
if (state.has_value())
|
|
||||||
return GetTransformation(state->angle);
|
|
||||||
else
|
|
||||||
return GetTransformation();
|
return GetTransformation();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -300,6 +300,6 @@ private:
|
||||||
RotationalState m_tilt_state;
|
RotationalState m_tilt_state;
|
||||||
MotionState m_cursor_state;
|
MotionState m_cursor_state;
|
||||||
PositionalState m_shake_state;
|
PositionalState m_shake_state;
|
||||||
std::optional<RotationalState> m_imu_cursor_state;
|
Common::Matrix33 m_imu_cursor_state;
|
||||||
};
|
};
|
||||||
} // namespace WiimoteEmu
|
} // namespace WiimoteEmu
|
||||||
|
|
|
@ -723,12 +723,7 @@ void AccelerometerMappingIndicator::paintEvent(QPaintEvent*)
|
||||||
p.setRenderHint(QPainter::Antialiasing, true);
|
p.setRenderHint(QPainter::Antialiasing, true);
|
||||||
p.setRenderHint(QPainter::SmoothPixmapTransform, true);
|
p.setRenderHint(QPainter::SmoothPixmapTransform, true);
|
||||||
|
|
||||||
const auto angle = std::acos(state.Normalized().Dot({0, 0, 1}));
|
const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
|
||||||
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});
|
|
||||||
|
|
||||||
// Draw sphere.
|
// Draw sphere.
|
||||||
p.setPen(Qt::NoPen);
|
p.setPen(Qt::NoPen);
|
||||||
|
|
Loading…
Reference in New Issue