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

106
include/Quaternion.h
View File

@ -8,11 +8,11 @@
#include "Vector3.h" #include "Vector3.h"
extern "C" { extern "C" {
/// <summary> /// <summary>
/// A quaternion /// A quaternion
/// </summary> /// </summary>
/// This is a C-style implementation /// This is a C-style implementation
typedef struct Quat { typedef struct Quat {
/// <summary> /// <summary>
/// The x component /// The x component
/// </summary> /// </summary>
@ -29,15 +29,14 @@ extern "C" {
/// The w component /// The w component
/// </summary> /// </summary>
float w; float w;
} Quat; } Quat;
} }
/// <summary> /// <summary>
/// A quaternion /// A quaternion
/// </summary> /// </summary>
struct Quaternion : Quat { struct Quaternion : Quat {
public: public:
/// <summary> /// <summary>
/// Create a new identity quaternion /// Create a new identity quaternion
/// </summary> /// </summary>
@ -90,7 +89,7 @@ public:
/// </summary> /// </summary>
/// <param name="vector">The vector to rotate</param> /// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns> /// <returns>The rotated vector</returns>
Vector3 operator *(const Vector3& vector) const; Vector3 operator*(const Vector3& vector) const;
/// <summary> /// <summary>
/// Multiply this quaternion with another quaternion /// Multiply this quaternion with another quaternion
/// </summary> /// </summary>
@ -98,24 +97,24 @@ public:
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to /// The result will be this quaternion rotated according to
/// the give rotation. /// the give rotation.
Quaternion operator *(const Quaternion& rotation) const; Quaternion operator*(const Quaternion& rotation) const;
/// <summary> /// <summary>
/// Check the equality of two quaternions /// Check the equality of two quaternions
/// </summary> /// </summary>
/// <param name="quaternion">The quaternion to compare to</param> /// <param name="quaternion">The quaternion to compare to</param>
/// <returns>True when the components of the quaternions are identical</returns> /// <returns>True when the components of the quaternions are
/// Note that this does not compare the rotations themselves. /// identical</returns> Note that this does not compare the rotations
/// Two quaternions with the same rotational effect may have different /// themselves. Two quaternions with the same rotational effect may have
/// components. Use Quaternion::Angle to check if the rotations are the same. /// different components. Use Quaternion::Angle to check if the rotations are
bool operator ==(const Quaternion& quaternion); /// the same.
bool operator==(const Quaternion& quaternion);
/// <summary> /// <summary>
/// The inverse of quaterion /// The inverse of quaterion
/// </summary> /// </summary>
/// <param name="quaternion">The quaternion for which the inverse is needed</param> /// <param name="quaternion">The quaternion for which the inverse is
/// <returns>The inverted quaternion</returns> /// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion); static Quaternion Inverse(Quaternion quaternion);
/// <summary> /// <summary>
@ -124,14 +123,17 @@ public:
/// <param name="forward">The look direction</param> /// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param> /// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns> /// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards); static Quaternion LookRotation(const Vector3& forward,
const Vector3& upwards);
/// <summary> /// <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> /// </summary>
/// <param name="forward">The look direction</param> /// <param name="forward">The look direction</param>
/// <returns>The rotation for this direction</returns> /// <returns>The rotation for this direction</returns>
/// For the rotation, Vector::up is used for the up direction. /// 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); static Quaternion LookRotation(const Vector3& forward);
/// <summary> /// <summary>
@ -147,9 +149,11 @@ public:
/// </summary> /// </summary>
/// <param name="from">The from rotation</param> /// <param name="from">The from rotation</param>
/// <param name="to">The destination rotation</param> /// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to rotate</param> /// <param name="maxDegreesDelta">The maximum amount of degrees to
/// <returns>The possibly limited rotation</returns> /// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta); static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta);
/// <summary> /// <summary>
/// Convert an angle/axis representation to a quaternion /// Convert an angle/axis representation to a quaternion
@ -170,7 +174,8 @@ public:
/// </summary> /// </summary>
/// <param name="orientation1">The first orientation</param> /// <param name="orientation1">The first orientation</param>
/// <param name="orientation2">The second 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); static float Angle(Quaternion orientation1, Quaternion orientation2);
/// <summary> /// <summary>
/// Sherical lerp between two rotations /// Sherical lerp between two rotations
@ -180,7 +185,9 @@ public:
/// <param name="factor">The factor between 0 and 1.</param> /// <param name="factor">The factor between 0 and 1.</param>
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2. /// 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> /// <summary>
/// Unclamped sherical lerp between two rotations /// Unclamped sherical lerp between two rotations
/// </summary> /// </summary>
@ -190,7 +197,9 @@ public:
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2. /// A factor 0 returns rotation1, factor1 returns rotation2.
/// Values outside the 0..1 range will result in extrapolated rotations /// 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> /// <summary>
/// Create a rotation from euler angles /// Create a rotation from euler angles
@ -199,28 +208,45 @@ public:
/// <param name="y">The angle around the upward axis</param> /// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param> /// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns> /// <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); static Quaternion Euler(float x, float y, float z);
/// <summary> /// <summary>
/// Create a rotation from a vector containing euler angles /// Create a rotation from a vector containing euler angles
/// </summary> /// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param> /// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns> /// <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); 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> /// <summary>
/// Returns the angle of around the give axis for a rotation /// Returns the angle of around the give axis for a rotation
/// </summary> /// </summary>
/// <param name="axis">The axis around which the angle should be computed</param> /// <param name="axis">The axis around which the angle should be
/// <param name="rotation">The source rotation</param> /// computed</param> <param name="rotation">The source rotation</param>
/// <returns>The signed angle around the axis</returns> /// <returns>The signed angle around the axis</returns>
static float GetAngleAround(Vector3 axis, Quaternion rotation); static float GetAngleAround(Vector3 axis, Quaternion rotation);
/// <summary> /// <summary>
/// Returns the rotation limited around the given axis /// Returns the rotation limited around the given axis
/// </summary> /// </summary>
/// <param name="axis">The axis which which the rotation should be limited</param> /// <param name="axis">The axis which which the rotation should be
/// <param name="rotation">The source rotation</param> /// limited</param> <param name="rotation">The source rotation</param>
/// <returns>The rotation around the given axis</returns> /// <returns>The rotation around the given axis</returns>
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation); static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
/// <summary> /// <summary>
@ -228,9 +254,13 @@ public:
/// </summary> /// </summary>
/// <param name="axis">The base direction for the decomposition</param> /// <param name="axis">The base direction for the decomposition</param>
/// <param name="rotation">The source rotation</param> /// <param name="rotation">The source rotation</param>
/// <param name="swing">A pointer to the quaternion for the swing result</param> /// <param name="swing">A pointer to the quaternion for the swing
/// <param name="twist">A pointer to the quaternion for the twist result</param> /// result</param> <param name="twist">A pointer to the quaternion for the
static void GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist); /// twist result</param>
static void GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
/// <summary> /// <summary>
/// Calculate the dot product of two quaternions /// Calculate the dot product of two quaternions
@ -240,14 +270,14 @@ public:
/// <returns></returns> /// <returns></returns>
static float Dot(Quaternion rotation1, Quaternion rotation2); static float Dot(Quaternion rotation1, Quaternion rotation2);
private: private:
float GetLength() const; float GetLength() const;
float GetLengthSquared() const; float GetLengthSquared() const;
static float GetLengthSquared(const Quaternion& q); static float GetLengthSquared(const Quaternion& q);
void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle); void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
static Quaternion FromEulerRad(Vector3 euler); static Quaternion FromEulerRad(Vector3 euler);
static Quaternion FromEulerRadXYZ(Vector3 euler);
Vector3 xyz() const; Vector3 xyz() const;
}; };

146
src/Quaternion.cpp
View File

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