RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-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

466
include/Quaternion.h
View File

@ -8,248 +8,278 @@
#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>
float x; float x;
/// <summary> /// <summary>
/// The y component /// The y component
/// </summary> /// </summary>
float y; float y;
/// <summary> /// <summary>
/// The z component /// The z component
/// </summary> /// </summary>
float z; float z;
/// <summary> /// <summary>
/// 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>
Quaternion(); Quaternion();
/// <summary> /// <summary>
/// create a new quaternion with the given values /// create a new quaternion with the given values
/// </summary> /// </summary>
/// <param name="_x">x component</param> /// <param name="_x">x component</param>
/// <param name="_y">y component</param> /// <param name="_y">y component</param>
/// <param name="_z">z component</param> /// <param name="_z">z component</param>
/// <param name="_w">w component</param> /// <param name="_w">w component</param>
Quaternion(float _x, float _y, float _z, float _w); Quaternion(float _x, float _y, float _z, float _w);
/// <summary> /// <summary>
/// Create a quaternion from C-style Quat /// Create a quaternion from C-style Quat
/// </summary> /// </summary>
/// <param name="q"></param> /// <param name="q"></param>
Quaternion(Quat q); Quaternion(Quat q);
~Quaternion(); ~Quaternion();
/// <summary> /// <summary>
/// An identity quaternion /// An identity quaternion
/// </summary> /// </summary>
const static Quaternion identity; const static Quaternion identity;
/// <summary> /// <summary>
/// Convert to unit quaternion /// Convert to unit quaternion
/// </summary> /// </summary>
/// This will preserve the orientation, /// This will preserve the orientation,
/// but ensures that it is a unit quaternion. /// but ensures that it is a unit quaternion.
void Normalize(); void Normalize();
/// <summary> /// <summary>
/// Convert to unity quaternion /// Convert to unity quaternion
/// </summary> /// </summary>
/// <param name="q">The quaternion to convert</param> /// <param name="q">The quaternion to convert</param>
/// <returns>A unit quaternion</returns> /// <returns>A unit quaternion</returns>
/// This will preserve the orientation, /// This will preserve the orientation,
/// but ensures that it is a unit quaternion. /// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion& q); static Quaternion Normalize(const Quaternion& q);
/// <summary> /// <summary>
/// Convert to euler angles /// Convert to euler angles
/// </summary> /// </summary>
/// <param name="q">The quaternion to convert</param> /// <param name="q">The quaternion to convert</param>
/// <returns>A vector containing euler angles</returns> /// <returns>A vector containing euler angles</returns>
/// The euler angles performed in the order: Z, X, Y /// The euler angles performed in the order: Z, X, Y
static Vector3 ToAngles(const Quaternion& q); static Vector3 ToAngles(const Quaternion& q);
/// <summary> /// <summary>
/// Rotate a vector using this quaterion /// Rotate a vector using this quaterion
/// </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>
/// <param name="rotation">The quaternion to multiply with</param> /// <param name="rotation">The quaternion to multiply with</param>
/// <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>
/// The inverse of quaterion
/// </summary>
/// <param name="quaternion">The quaternion for which the inverse is
/// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion);
/// <summary> /// <summary>
/// The inverse of quaterion /// A rotation which looks in the given direction
/// </summary> /// </summary>
/// <param name="quaternion">The quaternion for which the inverse is needed</param> /// <param name="forward">The look direction</param>
/// <returns>The inverted quaternion</returns> /// <param name="upwards">The up direction</param>
static Quaternion Inverse(Quaternion quaternion); /// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward,
const Vector3& upwards);
/// <summary>
/// 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
static Quaternion LookRotation(const Vector3& forward);
/// <summary> /// <summary>
/// A rotation which looks in the given direction /// Calculat the rotation from on vector to another
/// </summary> /// </summary>
/// <param name="forward">The look direction</param> /// <param name="fromDirection">The from direction</param>
/// <param name="upwards">The up direction</param> /// <param name="toDirection">The to direction</param>
/// <returns>The look rotation</returns> /// <returns>The rotation from the first to the second vector</returns>
static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards); static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
/// <summary>
/// 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
static Quaternion LookRotation(const Vector3& forward);
/// <summary> /// <summary>
/// Calculat the rotation from on vector to another /// Rotate form one orientation to anther with a maximum amount of degrees
/// </summary> /// </summary>
/// <param name="fromDirection">The from direction</param> /// <param name="from">The from rotation</param>
/// <param name="toDirection">The to direction</param> /// <param name="to">The destination rotation</param>
/// <returns>The rotation from the first to the second vector</returns> /// <param name="maxDegreesDelta">The maximum amount of degrees to
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection); /// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta);
/// <summary> /// <summary>
/// Rotate form one orientation to anther with a maximum amount of degrees /// Convert an angle/axis representation to a quaternion
/// </summary> /// </summary>
/// <param name="from">The from rotation</param> /// <param name="angle">The angle</param>
/// <param name="to">The destination rotation</param> /// <param name="axis">The axis</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to rotate</param> /// <returns>The resulting quaternion</returns>
/// <returns>The possibly limited rotation</returns> static Quaternion AngleAxis(float angle, const Vector3& axis);
static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta); /// <summary>
/// Convert this quaternion to angle/axis representation
/// </summary>
/// <param name="angle">A pointer to the angle for the result</param>
/// <param name="axis">A pointer to the axis for the result</param>
void ToAngleAxis(float* angle, Vector3* axis);
/// <summary> /// <summary>
/// Convert an angle/axis representation to a quaternion /// Get the angle between two orientations
/// </summary> /// </summary>
/// <param name="angle">The angle</param> /// <param name="orientation1">The first orientation</param>
/// <param name="axis">The axis</param> /// <param name="orientation2">The second orientation</param>
/// <returns>The resulting quaternion</returns> /// <returns>The smallest angle in degrees between the two
static Quaternion AngleAxis(float angle, const Vector3& axis); /// orientations</returns>
/// <summary> static float Angle(Quaternion orientation1, Quaternion orientation2);
/// Convert this quaternion to angle/axis representation /// <summary>
/// </summary> /// Sherical lerp between two rotations
/// <param name="angle">A pointer to the angle for the result</param> /// </summary>
/// <param name="axis">A pointer to the axis for the result</param> /// <param name="rotation1">The first rotation</param>
void ToAngleAxis(float* angle, Vector3* axis); /// <param name="rotation2">The second rotation</param>
/// <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);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <param name="factor">The factor</param>
/// <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);
/// <summary> /// <summary>
/// Get the angle between two orientations /// Create a rotation from euler angles
/// </summary> /// </summary>
/// <param name="orientation1">The first orientation</param> /// <param name="x">The angle around the right axis</param>
/// <param name="orientation2">The second orientation</param> /// <param name="y">The angle around the upward axis</param>
/// <returns>The smallest angle in degrees between the two orientations</returns> /// <param name="z">The angle around the forward axis</param>
static float Angle(Quaternion orientation1, Quaternion orientation2); /// <returns>The resulting quaternion</returns>
/// <summary> /// Rotation are appied in the order Z, X, Y.
/// Sherical lerp between two rotations static Quaternion Euler(float x, float y, float z);
/// </summary> /// <summary>
/// <param name="rotation1">The first rotation</param> /// Create a rotation from a vector containing euler angles
/// <param name="rotation2">The second rotation</param> /// </summary>
/// <param name="factor">The factor between 0 and 1.</param> /// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting rotation</returns> /// <returns>The resulting quaternion</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2. /// Rotation are appied in the order Z, X, Y.
static Quaternion Slerp(const Quaternion& rotation1, const Quaternion& rotation2, float factor); static Quaternion Euler(Vector3 eulerAngles);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <param name="factor">The factor</param>
/// <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);
/// <summary> /// <summary>
/// Create a rotation from euler angles /// Create a rotation from euler angles
/// </summary> /// </summary>
/// <param name="x">The angle around the right axis</param> /// <param name="x">The angle around the right axis</param>
/// <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 X, Y, Z.
static Quaternion Euler(float x, float y, float z); static Quaternion EulerXYZ(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 X, Y, Z.
static Quaternion Euler(Vector3 eulerAngles); 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>
/// Swing-twist decomposition of a rotation /// Swing-twist decomposition of a rotation
/// </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
/// </summary> /// </summary>
/// <param name="rotation1">The first rotation</param> /// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param> /// <param name="rotation2">The second rotation</param>
/// <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;
}; };
#endif #endif

572
src/Quaternion.cpp
View File

@ -2,40 +2,40 @@
// 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) {
q2.x = q1.x; q2.x = q1.x;
q2.y = q1.y; q2.y = q1.y;
q2.z = q1.z; q2.z = q1.z;
q2.w = q1.w; q2.w = q1.w;
} }
const float Deg2Rad = 0.0174532924F; const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F; const float Rad2Deg = 57.29578F;
Quaternion::Quaternion() { Quaternion::Quaternion() {
x = 0; x = 0;
y = 0; y = 0;
z = 0; z = 0;
w = 1; w = 1;
} }
Quaternion::Quaternion(float _x, float _y, float _z, float _w) { Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
x = _x; x = _x;
y = _y; y = _y;
z = _z; z = _z;
w = _w; w = _w;
} }
Quaternion::Quaternion(Quat q) { Quaternion::Quaternion(Quat q) {
x = q.x; x = q.x;
y = q.y; y = q.y;
z = q.z; z = q.z;
w = q.w; w = q.w;
} }
Quaternion::~Quaternion() {} Quaternion::~Quaternion() {}
@ -43,334 +43,366 @@ Quaternion::~Quaternion() {}
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1); const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
Vector3 Quaternion::xyz() const { Vector3 Quaternion::xyz() const {
return Vector3(x, y, z); return Vector3(x, y, z);
} }
float Quaternion::GetLength() const { float Quaternion::GetLength() const {
return sqrtf(x * x + y * y + z * z + w * w); return sqrtf(x * x + y * y + z * z + w * w);
} }
float Quaternion::GetLengthSquared() const { float Quaternion::GetLengthSquared() const {
return x * x + y * y + z * z + w * w; return x * x + y * y + z * z + w * w;
} }
float Quaternion::GetLengthSquared(const Quaternion& q) { float Quaternion::GetLengthSquared(const Quaternion& q) {
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
} }
void Quaternion::Normalize() { void Quaternion::Normalize() {
float length = GetLength(); float length = GetLength();
x /= length; x /= length;
y /= length; y /= length;
z /= length; z /= length;
w /= length; w /= length;
} }
Quaternion Quaternion::Normalize(const Quaternion& q) { Quaternion Quaternion::Normalize(const Quaternion& q) {
Quaternion result; Quaternion result;
float length = q.GetLength(); float length = q.GetLength();
result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length); result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
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;
} }
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 {
); float sqx = q1.x * q1.x;
} float sqy = q1.y * q1.y;
else if (test < -0.499) { // singularity at south pole float sqz = q1.z * q1.z;
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( 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;
float num4 = this->x * num; float num4 = this->x * num;
float num5 = this->y * num2; float num5 = this->y * num2;
float num6 = this->z * num3; float num6 = this->z * num3;
float num7 = this->x * num2; float num7 = this->x * num2;
float num8 = this->x * num3; float num8 = this->x * num3;
float num9 = this->y * num3; float num9 = this->y * num3;
float num10 = this->w * num; float num10 = this->w * num;
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 =
return result; (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;
} }
bool Quaternion::operator==(const Quaternion& q) { bool Quaternion::operator==(const Quaternion& q) {
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w); return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
} }
Quaternion Quaternion::Inverse(Quaternion r) { Quaternion Quaternion::Inverse(Quaternion r) {
float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w); float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n); return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
} }
Quaternion Quaternion::LookRotation(const Vector3& forward) { Quaternion Quaternion::LookRotation(const Vector3& forward) {
Vector3 up = Vector3(0, 1, 0); Vector3 up = Vector3(0, 1, 0);
return LookRotation(forward, up); return LookRotation(forward, up);
} }
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) { Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
Vector3 nForward = Vector3::Normalize(forward); Vector3 nForward = Vector3::Normalize(forward);
Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward)); Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
Vector3 nUp = Vector3::Cross(nForward, nRight); Vector3 nUp = Vector3::Cross(nForward, nRight);
float m00 = nRight.x; float m00 = nRight.x;
float m01 = nRight.y; float m01 = nRight.y;
float m02 = nRight.z; float m02 = nRight.z;
float m10 = nUp.x; float m10 = nUp.x;
float m11 = nUp.y; float m11 = nUp.y;
float m12 = nUp.z; float m12 = nUp.z;
float m20 = nForward.x; float m20 = nForward.x;
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) { float num = sqrtf(num8 + 1);
float num = sqrtf(num8 + 1); quaternion.w = num * 0.5f;
quaternion.w = num * 0.5f; num = 0.5f / num;
num = 0.5f / num; quaternion.x = (m12 - m21) * num;
quaternion.x = (m12 - m21) * num; quaternion.y = (m20 - m02) * num;
quaternion.y = (m20 - m02) * num; quaternion.z = (m01 - m10) * num;
quaternion.z = (m01 - m10) * num; return quaternion;
return quaternion; }
} if ((m00 >= m11) && (m00 >= m22)) {
if ((m00 >= m11) && (m00 >= m22)) { float num7 = sqrtf(((1 + m00) - m11) - m22);
float num7 = sqrtf(((1 + m00) - m11) - m22); float num4 = 0.5F / num7;
float num4 = 0.5F / num7; quaternion.x = 0.5f * num7;
quaternion.x = 0.5f * num7; quaternion.y = (m01 + m10) * num4;
quaternion.y = (m01 + m10) * num4; quaternion.z = (m02 + m20) * num4;
quaternion.z = (m02 + m20) * num4; quaternion.w = (m12 - m21) * num4;
quaternion.w = (m12 - m21) * num4; return quaternion;
return quaternion; }
} if (m11 > m22) {
if (m11 > m22) { float num6 = sqrtf(((1 + m11) - m00) - m22);
float num6 = sqrtf(((1 + m11) - m00) - m22); float num3 = 0.5F / num6;
float num3 = 0.5F / num6; quaternion.x = (m10 + m01) * num3;
quaternion.x = (m10 + m01) * num3; quaternion.y = 0.5F * num6;
quaternion.y = 0.5F * num6; quaternion.z = (m21 + m12) * num3;
quaternion.z = (m21 + m12) * num3; quaternion.w = (m20 - m02) * num3;
quaternion.w = (m20 - m02) * num3; return quaternion;
return quaternion; }
} float num5 = sqrtf(((1 + m22) - m00) - m11);
float num5 = sqrtf(((1 + m22) - m00) - m11); float num2 = 0.5F / num5;
float num2 = 0.5F / num5; quaternion.x = (m20 + m02) * num2;
quaternion.x = (m20 + m02) * num2; quaternion.y = (m21 + m12) * num2;
quaternion.y = (m21 + m12) * num2; quaternion.z = 0.5F * num5;
quaternion.z = 0.5F * num5; quaternion.w = (m01 - m10) * num2;
quaternion.w = (m01 - m10) * num2; return quaternion;
return quaternion;
} }
Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) { Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
Vector3 axis = Vector3::Cross(fromDirection, toDirection); Vector3 toDirection) {
axis = Vector3::Normalize(axis); Vector3 axis = Vector3::Cross(fromDirection, toDirection);
float angle = Vector3::SignedAngle(fromDirection, toDirection, axis); axis = Vector3::Normalize(axis);
Quaternion rotation = Quaternion::AngleAxis(angle, axis); float angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
return rotation; Quaternion rotation = Quaternion::AngleAxis(angle, axis);
return rotation;
} }
Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) { Quaternion Quaternion::RotateTowards(const Quaternion& from,
float num = Quaternion::Angle(from, to); const Quaternion& to,
if (num == 0) { float maxDegreesDelta) {
return to; float num = Quaternion::Angle(from, to);
} if (num == 0) {
float t = (float)fmin(1, maxDegreesDelta / num); return to;
return SlerpUnclamped(from, to, t); }
float t = (float)fmin(1, maxDegreesDelta / num);
return SlerpUnclamped(from, to, t);
} }
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) { Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
if (Vector3::SqrMagnitude(axis) == 0.0) if (Vector3::SqrMagnitude(axis) == 0.0)
return Quaternion(); return Quaternion();
Quaternion result = Quaternion(); Quaternion result = Quaternion();
float radians = angle * Deg2Rad; float radians = angle * Deg2Rad;
radians *= 0.5; radians *= 0.5;
Vector3 axis2 = axis * (float)sin(radians); Vector3 axis2 = axis * (float)sin(radians);
result.x = axis2.x; result.x = axis2.x;
result.y = axis2.y; result.y = axis2.y;
result.z = axis2.z; result.z = axis2.z;
result.w = (float)cos(radians); result.w = (float)cos(radians);
return Quaternion::Normalize(result); return Quaternion::Normalize(result);
} }
float Quaternion::Angle(Quaternion a, Quaternion b) { float Quaternion::Angle(Quaternion a, Quaternion b) {
float f = Quaternion::Dot(a, b); float f = Quaternion::Dot(a, 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,
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q; float* angle) {
*angle = 2.0f * acosf(q1.w); // angle Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
float den = sqrtf(1.0F - q1.w * q1.w); *angle = 2.0f * acosf(q1.w); // angle
if (den > 0.0001f) float den = sqrtf(1.0F - q1.w * q1.w);
{ if (den > 0.0001f) {
*axis = q1.xyz() / den; *axis = q1.xyz() / den;
} } else {
else // This occurs when the angle is zero.
{ // Not a problem: just set an arbitrary normalized axis.
// This occurs when the angle is zero. *axis = Vector3(1, 0, 0);
// 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,
// if either input is zero, return the other. const Quaternion& b,
if (Quaternion::GetLengthSquared(a) == 0.0) { float t) {
if (Quaternion::GetLengthSquared(b) == 0.0) { // if either input is zero, return the other.
return Quaternion(); if (Quaternion::GetLengthSquared(a) == 0.0) {
} if (Quaternion::GetLengthSquared(b) == 0.0) {
return b; return Quaternion();
} }
else if (Quaternion::GetLengthSquared(b) == 0.0f) { return b;
return a; } else if (Quaternion::GetLengthSquared(b) == 0.0f) {
} return a;
}
const Vector3 axyz = a.xyz(); const Vector3 axyz = a.xyz();
const Vector3 bxyz = b.xyz(); const Vector3 bxyz = b.xyz();
float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz); float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
Quaternion b2 = b; Quaternion b2 = 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; b2.w = -b.w;
b2.w = -b.w; cosHalfAngle = -cosHalfAngle;
cosHalfAngle = -cosHalfAngle; }
}
float blendA; float blendA;
float blendB; float blendB;
if (cosHalfAngle < 0.99f) { if (cosHalfAngle < 0.99f) {
// do proper slerp for big angles // do proper slerp for big angles
float halfAngle = acosf(cosHalfAngle); float halfAngle = acosf(cosHalfAngle);
float sinHalfAngle = sinf(halfAngle); float sinHalfAngle = sinf(halfAngle);
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; }
} Vector3 v = axyz * blendA + b2.xyz() * blendB;
Vector3 v = axyz * blendA + b2.xyz() * blendB; Quaternion result = Quaternion(v.x, v.y, v.z, blendA * a.w + blendB * b2.w);
Quaternion result = Quaternion(v.x, v.y, v.z, blendA * a.w + blendB * b2.w); if (result.GetLengthSquared() > 0.0f)
if (result.GetLengthSquared() > 0.0f) return Quaternion::Normalize(result);
return Quaternion::Normalize(result); else
else 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) {
return Quaternion::SlerpUnclamped(a, b, t); if (t > 1)
t = 1;
if (t < 0)
t = 0;
return Quaternion::SlerpUnclamped(a, b, t);
} }
Quaternion Quaternion::Euler(float x, float y, float z) { Quaternion Quaternion::Euler(float x, float y, float z) {
return Quaternion::Euler(Vector3(x, y, z)); return Quaternion::Euler(Vector3(x, y, z));
} }
Quaternion Quaternion::Euler(Vector3 euler) { Quaternion Quaternion::Euler(Vector3 euler) {
return Quaternion::FromEulerRad(euler * Deg2Rad); return Quaternion::FromEulerRad(euler * Deg2Rad);
} }
Quaternion Quaternion::FromEulerRad(Vector3 euler) { Quaternion Quaternion::FromEulerRad(Vector3 euler) {
float yaw = euler.x; float yaw = euler.x;
float pitch = euler.y; float pitch = euler.y;
float roll = euler.z; float roll = euler.z;
float rollOver2 = roll * 0.5f; float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)sin((float)rollOver2); float sinRollOver2 = (float)sin((float)rollOver2);
float cosRollOver2 = (float)cos((float)rollOver2); float cosRollOver2 = (float)cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f; float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)sin((float)pitchOver2); float sinPitchOver2 = (float)sin((float)pitchOver2);
float cosPitchOver2 = (float)cos((float)pitchOver2); float cosPitchOver2 = (float)cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f; float yawOver2 = yaw * 0.5f;
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;
return result; 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;
} }
float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) { float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
Quaternion secondaryRotation = GetRotationAround(axis, rotation); Quaternion secondaryRotation = GetRotationAround(axis, rotation);
float rotationAngle; float rotationAngle;
Vector3 rotationAxis; Vector3 rotationAxis;
secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis); secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
// Do the axis point in opposite directions? // Do the axis point in opposite directions?
if (Vector3::Dot(axis, rotationAxis) < 0) if (Vector3::Dot(axis, rotationAxis) < 0)
rotationAngle = -rotationAngle; rotationAngle = -rotationAngle;
return rotationAngle; return rotationAngle;
} }
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(
Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w); ra, axis); // return projection ra on to axis (parallel component)
twist = Quaternion::Normalize(twist); Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w);
return twist; twist = Quaternion::Normalize(twist);
return twist;
} }
void Quaternion::GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist) { void Quaternion::GetSwingTwist(Vector3 axis,
*twist = GetRotationAround(axis, rotation); Quaternion rotation,
*swing = rotation * Quaternion::Inverse(*twist); Quaternion* swing,
Quaternion* twist) {
*twist = GetRotationAround(axis, rotation);
*swing = rotation * Quaternion::Inverse(*twist);
} }