LinearAlgebra/LinearAlgebra-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added EulerXYZ

Browse Source
This commit is contained in:
Pascal Serrarens 2022-10-27 10:00:47 +02:00
parent d3e2ded5d7
commit 00b62c9baa
2 changed files with 550 additions and 488 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
include/Quaternion.h
View File

@ -32,7 +32,6 @@ extern "C" {
} Quat;
}
/// <summary>
/// A quaternion
/// </summary>
@ -104,18 +103,18 @@ public:
/// Check the equality of two quaternions
/// </summary>
/// <param name="quaternion">The quaternion to compare to</param>
/// <returns>True when the components of the quaternions are identical</returns>
/// Note that this does not compare the rotations themselves.
/// Two quaternions with the same rotational effect may have different
/// components. Use Quaternion::Angle to check if the rotations are the same.
/// <returns>True when the components of the quaternions are
/// identical</returns> Note that this does not compare the rotations
/// themselves. Two quaternions with the same rotational effect may have
/// different components. Use Quaternion::Angle to check if the rotations are
/// the same.
bool operator==(const Quaternion& quaternion);
/// <summary>
/// The inverse of quaterion
/// </summary>
/// <param name="quaternion">The quaternion for which the inverse is needed</param>
/// <returns>The inverted quaternion</returns>
/// <param name="quaternion">The quaternion for which the inverse is
/// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion);
/// <summary>
@ -124,14 +123,17 @@ public:
/// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards);
static Quaternion LookRotation(const Vector3& forward,
const Vector3& upwards);
/// <summary>
/// Creates a quaternion with the given forward direction with up = Vector3::up
/// Creates a quaternion with the given forward direction with up =
/// Vector3::up
/// </summary>
/// <param name="forward">The look direction</param>
/// <returns>The rotation for this direction</returns>
/// For the rotation, Vector::up is used for the up direction.
/// Note: if the forward direction == Vector3::up, the result is Quaternion::identity
/// Note: if the forward direction == Vector3::up, the result is
/// Quaternion::identity
static Quaternion LookRotation(const Vector3& forward);
/// <summary>
@ -147,9 +149,11 @@ public:
/// </summary>
/// <param name="from">The from rotation</param>
/// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to rotate</param>
/// <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta);
/// <param name="maxDegreesDelta">The maximum amount of degrees to
/// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta);
/// <summary>
/// Convert an angle/axis representation to a quaternion
@ -170,7 +174,8 @@ public:
/// </summary>
/// <param name="orientation1">The first orientation</param>
/// <param name="orientation2">The second orientation</param>
/// <returns>The smallest angle in degrees between the two orientations</returns>
/// <returns>The smallest angle in degrees between the two
/// orientations</returns>
static float Angle(Quaternion orientation1, Quaternion orientation2);
/// <summary>
/// Sherical lerp between two rotations
@ -180,7 +185,9 @@ public:
/// <param name="factor">The factor between 0 and 1.</param>
/// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2.
static Quaternion Slerp(const Quaternion& rotation1, const Quaternion& rotation2, float factor);
static Quaternion Slerp(const Quaternion& rotation1,
const Quaternion& rotation2,
float factor);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
@ -190,7 +197,9 @@ public:
/// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2.
/// Values outside the 0..1 range will result in extrapolated rotations
static Quaternion SlerpUnclamped(const Quaternion& rotation1, const Quaternion& rotation2, float factor);
static Quaternion SlerpUnclamped(const Quaternion& rotation1,
const Quaternion& rotation2,
float factor);
/// <summary>
/// Create a rotation from euler angles
@ -199,28 +208,45 @@ public:
/// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order z, X, Y.
/// Rotation are appied in the order Z, X, Y.
static Quaternion Euler(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order z, X, Y.
/// Rotation are appied in the order Z, X, Y.
static Quaternion Euler(Vector3 eulerAngles);
/// <summary>
/// Create a rotation from euler angles
/// </summary>
/// <param name="x">The angle around the right axis</param>
/// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(Vector3 eulerAngles);
/// <summary>
/// Returns the angle of around the give axis for a rotation
/// </summary>
/// <param name="axis">The axis around which the angle should be computed</param>
/// <param name="rotation">The source rotation</param>
/// <param name="axis">The axis around which the angle should be
/// computed</param> <param name="rotation">The source rotation</param>
/// <returns>The signed angle around the axis</returns>
static float GetAngleAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Returns the rotation limited around the given axis
/// </summary>
/// <param name="axis">The axis which which the rotation should be limited</param>
/// <param name="rotation">The source rotation</param>
/// <param name="axis">The axis which which the rotation should be
/// limited</param> <param name="rotation">The source rotation</param>
/// <returns>The rotation around the given axis</returns>
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
/// <summary>
@ -228,9 +254,13 @@ public:
/// </summary>
/// <param name="axis">The base direction for the decomposition</param>
/// <param name="rotation">The source rotation</param>
/// <param name="swing">A pointer to the quaternion for the swing result</param>
/// <param name="twist">A pointer to the quaternion for the twist result</param>
static void GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist);
/// <param name="swing">A pointer to the quaternion for the swing
/// result</param> <param name="twist">A pointer to the quaternion for the
/// twist result</param>
static void GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
/// <summary>
/// Calculate the dot product of two quaternions
@ -247,7 +277,7 @@ private:
void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
static Quaternion FromEulerRad(Vector3 euler);
static Quaternion FromEulerRadXYZ(Vector3 euler);
Vector3 xyz() const;
};

142
src/Quaternion.cpp
View File

@ -2,9 +2,9 @@
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include <math.h>
#include <float.h>
#include "Quaternion.h"
#include <float.h>
#include <math.h>
#include "Vector3.h"
void CopyQuat(const Quat& q1, Quat& q2) {
@ -72,7 +72,6 @@ Quaternion Quaternion::Normalize(const Quaternion& q) {
return result;
};
float Quaternion::Dot(Quaternion a, Quaternion b) {
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
@ -80,29 +79,20 @@ float Quaternion::Dot(Quaternion a, Quaternion b) {
Vector3 Quaternion::ToAngles(const Quaternion& q1) {
float test = q1.x * q1.y + q1.z * q1.w;
if (test > 0.499) { // singularity at north pole
return Vector3(
0,
2 * (float)atan2(q1.x, q1.w) * Rad2Deg,
90
);
}
else if (test < -0.499) { // singularity at south pole
return Vector3(
0,
-2 * (float)atan2(q1.x, q1.w) * Rad2Deg,
-90
);
}
else {
return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
} else if (test < -0.499) { // singularity at south pole
return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
} else {
float sqx = q1.x * q1.x;
float sqy = q1.y * q1.y;
float sqz = q1.z * q1.z;
return Vector3(
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) * Rad2Deg,
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) * Rad2Deg,
asinf(2 * test) * Rad2Deg
);
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) *
Rad2Deg,
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) *
Rad2Deg,
asinf(2 * test) * Rad2Deg);
}
}
@ -111,8 +101,7 @@ Quaternion Quaternion::operator *(const Quaternion& r2) const {
this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
-this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w
);
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w);
};
Vector3 Quaternion::operator*(const Vector3& p) const {
@ -129,9 +118,12 @@ Vector3 Quaternion::operator *(const Vector3& p) const {
float num11 = this->w * num2;
float num12 = this->w * num3;
Vector3 result = Vector3::zero;
result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z;
result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z;
result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z;
result.x =
(1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z;
result.y =
(num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z;
result.z =
(num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z;
return result;
}
@ -162,7 +154,6 @@ Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
float m21 = nForward.y;
float m22 = nForward.z;
float num8 = (m00 + m11) + m22;
Quaternion quaternion = Quaternion();
if (num8 > 0) {
@ -201,7 +192,8 @@ Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
return quaternion;
}
Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) {
Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
Vector3 toDirection) {
Vector3 axis = Vector3::Cross(fromDirection, toDirection);
axis = Vector3::Normalize(axis);
float angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
@ -209,7 +201,9 @@ Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection
return rotation;
}
Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) {
Quaternion Quaternion::RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta) {
float num = Quaternion::Angle(from, to);
if (num == 0) {
return to;
@ -240,37 +234,35 @@ float Quaternion::Angle(Quaternion a, Quaternion b) {
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
}
void Quaternion::ToAngleAxis(float* angle, Vector3* axis)
{
void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
Quaternion::ToAxisAngleRad(*this, axis, angle);
*angle *= Rad2Deg;
}
void Quaternion::ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle)
{
void Quaternion::ToAxisAngleRad(const Quaternion& q,
Vector3* const axis,
float* angle) {
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
*angle = 2.0f * acosf(q1.w); // angle
float den = sqrtf(1.0F - q1.w * q1.w);
if (den > 0.0001f)
{
if (den > 0.0001f) {
*axis = q1.xyz() / den;
}
else
{
} else {
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
*axis = Vector3(1, 0, 0);
}
}
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) {
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
const Quaternion& b,
float t) {
// if either input is zero, return the other.
if (Quaternion::GetLengthSquared(a) == 0.0) {
if (Quaternion::GetLengthSquared(b) == 0.0) {
return Quaternion();
}
return b;
}
else if (Quaternion::GetLengthSquared(b) == 0.0f) {
} else if (Quaternion::GetLengthSquared(b) == 0.0f) {
return a;
}
@ -282,8 +274,7 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b,
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
// angle = 0.0f, so just return one input.
return a;
}
else if (cosHalfAngle < 0.0f) {
} else if (cosHalfAngle < 0.0f) {
b2.x = -b.x;
b2.y = -b.y;
b2.z = -b.z;
@ -300,8 +291,7 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b,
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
}
else {
} else {
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
@ -314,9 +304,13 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b,
return Quaternion();
}
Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) {
if (t > 1) t = 1;
if (t < 0) t = 0;
Quaternion Quaternion::Slerp(const Quaternion& a,
const Quaternion& b,
float t) {
if (t > 1)
t = 1;
if (t < 0)
t = 0;
return Quaternion::SlerpUnclamped(a, b, t);
}
@ -341,10 +335,45 @@ Quaternion Quaternion::FromEulerRad(Vector3 euler) {
float sinYawOver2 = (float)sin((float)yawOver2);
float cosYawOver2 = (float)cos((float)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
Quaternion Quaternion::EulerXYZ(float x, float y, float z) {
return Quaternion::EulerXYZ(Vector3(x, y, z));
}
Quaternion Quaternion::EulerXYZ(Vector3 euler) {
return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
float yaw = euler.x;
float pitch = euler.y;
float roll = euler.z;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)sin((float)rollOver2);
float cosRollOver2 = (float)cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)sin((float)pitchOver2);
float cosPitchOver2 = (float)cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)sin((float)yawOver2);
float cosYawOver2 = (float)cos((float)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 -
cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 +
sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
@ -363,14 +392,17 @@ float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
Vector3 p = Vector3::Project(ra, axis); // return projection ra on to axis (parallel component)
Vector3 p = Vector3::Project(
ra, axis); // return projection ra on to axis (parallel component)
Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w);
twist = Quaternion::Normalize(twist);
return twist;
}
void Quaternion::GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist) {
void Quaternion::GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist) {
*twist = GetRotationAround(axis, rotation);
*swing = rotation * Quaternion::Inverse(*twist);
}