Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
Merge pull request #9162 from jordan-woyak/quaternion 
Replace stateful rotational matrices with quaternions.
  • Loading branch information
@lioncash
lioncash committed Oct 21, 2020
2 parents e553197 + 907fdd2 commit de96fe0
Show file tree
Hide file tree
Showing 10 changed files with 164 additions and 48 deletions.
90 changes: 83 additions & 7 deletions Source/Core/Common/Matrix.cpp
View file
Expand Up @@ -36,6 +36,84 @@ auto MatrixMultiply(const std::array<T, N * M>& a, const std::array<T, M * P>& 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 = {};
Expand All @@ -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,
Expand Down
33 changes: 32 additions & 1 deletion Source/Core/Common/Matrix.h
View file
Expand Up @@ -154,6 +154,8 @@ union TVec4
TVec4(TVec3<T> _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;
Expand Down Expand Up @@ -321,11 +323,40 @@ auto operator/(TVec2<T> lhs, T2 scalar)
using Vec2 = TVec2<float>;
using DVec2 = TVec2<double>;

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);
Expand Down
33 changes: 18 additions & 15 deletions Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
View file
Expand Up @@ -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();
Expand All @@ -72,15 +72,15 @@ 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
{
return gyroscope;
}
}

IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
IMUCursorState::IMUCursorState() : rotation{Common::Quaternion::Identity()}
{
}

Expand Down Expand Up @@ -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.
Expand All @@ -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,
Expand Down Expand Up @@ -374,21 +377,21 @@ 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();

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});
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)
Expand All @@ -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);
Expand Down
20 changes: 10 additions & 10 deletions Source/Core/Core/HW/WiimoteEmu/Dynamics.h
View file
Expand Up @@ -44,7 +44,7 @@ struct IMUCursorState
IMUCursorState();

// Rotation of world around device.
Common::Matrix33 rotation;
Common::Quaternion rotation;

float recentered_pitch = {};
};
Expand All @@ -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);
Expand Down
11 changes: 6 additions & 5 deletions Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
View file
Expand Up @@ -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
Expand All @@ -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
2 changes: 1 addition & 1 deletion Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
View file
Expand Up @@ -162,7 +162,7 @@ class Wiimote : public ControllerEmu::EmulatedController, public WiimoteCommon::
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;
Expand Down
12 changes: 7 additions & 5 deletions Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
View file
Expand Up @@ -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)));
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
View file
Expand Up @@ -176,7 +176,7 @@ class GyroMappingIndicator : public SquareIndicator
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;
};
Expand Down

0 comments on commit de96fe0

Please sign in to comment.