diff --git a/include/Quaternion.h b/include/Quaternion.h
index 8e17a65..eb40a5d 100644
--- a/include/Quaternion.h
+++ b/include/Quaternion.h
@@ -8,248 +8,278 @@
#include "Vector3.h"
extern "C" {
- ///
- /// A quaternion
- ///
- /// This is a C-style implementation
- typedef struct Quat {
- ///
- /// The x component
- ///
- float x;
- ///
- /// The y component
- ///
- float y;
- ///
- /// The z component
- ///
- float z;
- ///
- /// The w component
- ///
- float w;
- } Quat;
+///
+/// A quaternion
+///
+/// This is a C-style implementation
+typedef struct Quat {
+ ///
+ /// The x component
+ ///
+ float x;
+ ///
+ /// The y component
+ ///
+ float y;
+ ///
+ /// The z component
+ ///
+ float z;
+ ///
+ /// The w component
+ ///
+ float w;
+} Quat;
}
-
///
/// A quaternion
///
struct Quaternion : Quat {
-public:
- ///
- /// Create a new identity quaternion
- ///
- Quaternion();
- ///
- /// create a new quaternion with the given values
- ///
- /// x component
- /// y component
- /// z component
- /// w component
- Quaternion(float _x, float _y, float _z, float _w);
- ///
- /// Create a quaternion from C-style Quat
- ///
- ///
- Quaternion(Quat q);
- ~Quaternion();
+ public:
+ ///
+ /// Create a new identity quaternion
+ ///
+ Quaternion();
+ ///
+ /// create a new quaternion with the given values
+ ///
+ /// x component
+ /// y component
+ /// z component
+ /// w component
+ Quaternion(float _x, float _y, float _z, float _w);
+ ///
+ /// Create a quaternion from C-style Quat
+ ///
+ ///
+ Quaternion(Quat q);
+ ~Quaternion();
- ///
- /// An identity quaternion
- ///
- const static Quaternion identity;
+ ///
+ /// An identity quaternion
+ ///
+ const static Quaternion identity;
- ///
- /// Convert to unit quaternion
- ///
- /// This will preserve the orientation,
- /// but ensures that it is a unit quaternion.
- void Normalize();
- ///
- /// Convert to unity quaternion
- ///
- /// The quaternion to convert
- /// A unit quaternion
- /// This will preserve the orientation,
- /// but ensures that it is a unit quaternion.
- static Quaternion Normalize(const Quaternion& q);
+ ///
+ /// Convert to unit quaternion
+ ///
+ /// This will preserve the orientation,
+ /// but ensures that it is a unit quaternion.
+ void Normalize();
+ ///
+ /// Convert to unity quaternion
+ ///
+ /// The quaternion to convert
+ /// A unit quaternion
+ /// This will preserve the orientation,
+ /// but ensures that it is a unit quaternion.
+ static Quaternion Normalize(const Quaternion& q);
- ///
- /// Convert to euler angles
- ///
- /// The quaternion to convert
- /// A vector containing euler angles
- /// The euler angles performed in the order: Z, X, Y
- static Vector3 ToAngles(const Quaternion& q);
+ ///
+ /// Convert to euler angles
+ ///
+ /// The quaternion to convert
+ /// A vector containing euler angles
+ /// The euler angles performed in the order: Z, X, Y
+ static Vector3 ToAngles(const Quaternion& q);
- ///
- /// Rotate a vector using this quaterion
- ///
- /// The vector to rotate
- /// The rotated vector
- Vector3 operator *(const Vector3& vector) const;
- ///
- /// Multiply this quaternion with another quaternion
- ///
- /// The quaternion to multiply with
- /// The resulting rotation
- /// The result will be this quaternion rotated according to
- /// the give rotation.
- Quaternion operator *(const Quaternion& rotation) const;
+ ///
+ /// Rotate a vector using this quaterion
+ ///
+ /// The vector to rotate
+ /// The rotated vector
+ Vector3 operator*(const Vector3& vector) const;
+ ///
+ /// Multiply this quaternion with another quaternion
+ ///
+ /// The quaternion to multiply with
+ /// The resulting rotation
+ /// The result will be this quaternion rotated according to
+ /// the give rotation.
+ Quaternion operator*(const Quaternion& rotation) const;
- ///
- /// Check the equality of two quaternions
- ///
- /// The quaternion to compare to
- /// True when the components of the quaternions are identical
- /// 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);
+ ///
+ /// Check the equality of two quaternions
+ ///
+ /// The quaternion to compare to
+ /// True when the components of the quaternions are
+ /// identical 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);
+ ///
+ /// The inverse of quaterion
+ ///
+ /// The quaternion for which the inverse is
+ /// needed The inverted quaternion
+ static Quaternion Inverse(Quaternion quaternion);
- ///
- /// The inverse of quaterion
- ///
- /// The quaternion for which the inverse is needed
- /// The inverted quaternion
- static Quaternion Inverse(Quaternion quaternion);
+ ///
+ /// A rotation which looks in the given direction
+ ///
+ /// The look direction
+ /// The up direction
+ /// The look rotation
+ static Quaternion LookRotation(const Vector3& forward,
+ const Vector3& upwards);
+ ///
+ /// Creates a quaternion with the given forward direction with up =
+ /// Vector3::up
+ ///
+ /// The look direction
+ /// The rotation for this direction
+ /// 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);
- ///
- /// A rotation which looks in the given direction
- ///
- /// The look direction
- /// The up direction
- /// The look rotation
- static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards);
- ///
- /// Creates a quaternion with the given forward direction with up = Vector3::up
- ///
- /// The look direction
- /// The rotation for this direction
- /// 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);
+ ///
+ /// Calculat the rotation from on vector to another
+ ///
+ /// The from direction
+ /// The to direction
+ /// The rotation from the first to the second vector
+ static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
- ///
- /// Calculat the rotation from on vector to another
- ///
- /// The from direction
- /// The to direction
- /// The rotation from the first to the second vector
- static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
+ ///
+ /// Rotate form one orientation to anther with a maximum amount of degrees
+ ///
+ /// The from rotation
+ /// The destination rotation
+ /// The maximum amount of degrees to
+ /// rotate The possibly limited rotation
+ static Quaternion RotateTowards(const Quaternion& from,
+ const Quaternion& to,
+ float maxDegreesDelta);
- ///
- /// Rotate form one orientation to anther with a maximum amount of degrees
- ///
- /// The from rotation
- /// The destination rotation
- /// The maximum amount of degrees to rotate
- /// The possibly limited rotation
- static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta);
+ ///
+ /// Convert an angle/axis representation to a quaternion
+ ///
+ /// The angle
+ /// The axis
+ /// The resulting quaternion
+ static Quaternion AngleAxis(float angle, const Vector3& axis);
+ ///
+ /// Convert this quaternion to angle/axis representation
+ ///
+ /// A pointer to the angle for the result
+ /// A pointer to the axis for the result
+ void ToAngleAxis(float* angle, Vector3* axis);
- ///
- /// Convert an angle/axis representation to a quaternion
- ///
- /// The angle
- /// The axis
- /// The resulting quaternion
- static Quaternion AngleAxis(float angle, const Vector3& axis);
- ///
- /// Convert this quaternion to angle/axis representation
- ///
- /// A pointer to the angle for the result
- /// A pointer to the axis for the result
- void ToAngleAxis(float* angle, Vector3* axis);
+ ///
+ /// Get the angle between two orientations
+ ///
+ /// The first orientation
+ /// The second orientation
+ /// The smallest angle in degrees between the two
+ /// orientations
+ static float Angle(Quaternion orientation1, Quaternion orientation2);
+ ///
+ /// Sherical lerp between two rotations
+ ///
+ /// The first rotation
+ /// The second rotation
+ /// The factor between 0 and 1.
+ /// The resulting rotation
+ /// A factor 0 returns rotation1, factor1 returns rotation2.
+ static Quaternion Slerp(const Quaternion& rotation1,
+ const Quaternion& rotation2,
+ float factor);
+ ///
+ /// Unclamped sherical lerp between two rotations
+ ///
+ /// The first rotation
+ /// The second rotation
+ /// The factor
+ /// The resulting rotation
+ /// 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);
- ///
- /// Get the angle between two orientations
- ///
- /// The first orientation
- /// The second orientation
- /// The smallest angle in degrees between the two orientations
- static float Angle(Quaternion orientation1, Quaternion orientation2);
- ///
- /// Sherical lerp between two rotations
- ///
- /// The first rotation
- /// The second rotation
- /// The factor between 0 and 1.
- /// The resulting rotation
- /// A factor 0 returns rotation1, factor1 returns rotation2.
- static Quaternion Slerp(const Quaternion& rotation1, const Quaternion& rotation2, float factor);
- ///
- /// Unclamped sherical lerp between two rotations
- ///
- /// The first rotation
- /// The second rotation
- /// The factor
- /// The resulting rotation
- /// 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);
+ ///
+ /// Create a rotation from euler angles
+ ///
+ /// The angle around the right axis
+ /// The angle around the upward axis
+ /// The angle around the forward axis
+ /// The resulting quaternion
+ /// Rotation are appied in the order Z, X, Y.
+ static Quaternion Euler(float x, float y, float z);
+ ///
+ /// Create a rotation from a vector containing euler angles
+ ///
+ /// Vector with the euler angles
+ /// The resulting quaternion
+ /// Rotation are appied in the order Z, X, Y.
+ static Quaternion Euler(Vector3 eulerAngles);
- ///
- /// Create a rotation from euler angles
- ///
- /// The angle around the right axis
- /// The angle around the upward axis
- /// The angle around the forward axis
- /// The resulting quaternion
- /// Rotation are appied in the order z, X, Y.
- static Quaternion Euler(float x, float y, float z);
- ///
- /// Create a rotation from a vector containing euler angles
- ///
- /// Vector with the euler angles
- /// The resulting quaternion
- /// Rotation are appied in the order z, X, Y.
- static Quaternion Euler(Vector3 eulerAngles);
+ ///
+ /// Create a rotation from euler angles
+ ///
+ /// The angle around the right axis
+ /// The angle around the upward axis
+ /// The angle around the forward axis
+ /// The resulting quaternion
+ /// Rotation are appied in the order X, Y, Z.
+ static Quaternion EulerXYZ(float x, float y, float z);
+ ///
+ /// Create a rotation from a vector containing euler angles
+ ///
+ /// Vector with the euler angles
+ /// The resulting quaternion
+ /// Rotation are appied in the order X, Y, Z.
+ static Quaternion EulerXYZ(Vector3 eulerAngles);
- ///
- /// Returns the angle of around the give axis for a rotation
- ///
- /// The axis around which the angle should be computed
- /// The source rotation
- /// The signed angle around the axis
- static float GetAngleAround(Vector3 axis, Quaternion rotation);
- ///
- /// Returns the rotation limited around the given axis
- ///
- /// The axis which which the rotation should be limited
- /// The source rotation
- /// The rotation around the given axis
- static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
- ///
- /// Swing-twist decomposition of a rotation
- ///
- /// The base direction for the decomposition
- /// The source rotation
- /// A pointer to the quaternion for the swing result
- /// A pointer to the quaternion for the twist result
- static void GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist);
+ ///
+ /// Returns the angle of around the give axis for a rotation
+ ///
+ /// The axis around which the angle should be
+ /// computed The source rotation
+ /// The signed angle around the axis
+ static float GetAngleAround(Vector3 axis, Quaternion rotation);
+ ///
+ /// Returns the rotation limited around the given axis
+ ///
+ /// The axis which which the rotation should be
+ /// limited The source rotation
+ /// The rotation around the given axis
+ static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
+ ///
+ /// Swing-twist decomposition of a rotation
+ ///
+ /// The base direction for the decomposition
+ /// The source rotation
+ /// A pointer to the quaternion for the swing
+ /// result A pointer to the quaternion for the
+ /// twist result
+ static void GetSwingTwist(Vector3 axis,
+ Quaternion rotation,
+ Quaternion* swing,
+ Quaternion* twist);
- ///
- /// Calculate the dot product of two quaternions
- ///
- /// The first rotation
- /// The second rotation
- ///
- static float Dot(Quaternion rotation1, Quaternion rotation2);
+ ///
+ /// Calculate the dot product of two quaternions
+ ///
+ /// The first rotation
+ /// The second rotation
+ ///
+ static float Dot(Quaternion rotation1, Quaternion rotation2);
-private:
- float GetLength() const;
- float GetLengthSquared() const;
- static float GetLengthSquared(const Quaternion& q);
+ private:
+ float GetLength() const;
+ float GetLengthSquared() const;
+ static float GetLengthSquared(const Quaternion& q);
- void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
- static Quaternion FromEulerRad(Vector3 euler);
+ void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
+ static Quaternion FromEulerRad(Vector3 euler);
+ static Quaternion FromEulerRadXYZ(Vector3 euler);
-
- Vector3 xyz() const;
+ Vector3 xyz() const;
};
#endif
\ No newline at end of file
diff --git a/src/Quaternion.cpp b/src/Quaternion.cpp
index 645b10e..c1c4b06 100644
--- a/src/Quaternion.cpp
+++ b/src/Quaternion.cpp
@@ -2,40 +2,40 @@
// 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
-#include
#include "Quaternion.h"
+#include
+#include
#include "Vector3.h"
void CopyQuat(const Quat& q1, Quat& q2) {
- q2.x = q1.x;
- q2.y = q1.y;
- q2.z = q1.z;
- q2.w = q1.w;
+ q2.x = q1.x;
+ q2.y = q1.y;
+ q2.z = q1.z;
+ q2.w = q1.w;
}
const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F;
Quaternion::Quaternion() {
- x = 0;
- y = 0;
- z = 0;
- w = 1;
+ x = 0;
+ y = 0;
+ z = 0;
+ w = 1;
}
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
- x = _x;
- y = _y;
- z = _z;
- w = _w;
+ x = _x;
+ y = _y;
+ z = _z;
+ w = _w;
}
Quaternion::Quaternion(Quat q) {
- x = q.x;
- y = q.y;
- z = q.z;
- w = q.w;
+ x = q.x;
+ y = q.y;
+ z = q.z;
+ w = q.w;
}
Quaternion::~Quaternion() {}
@@ -43,334 +43,366 @@ Quaternion::~Quaternion() {}
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
Vector3 Quaternion::xyz() const {
- return Vector3(x, y, z);
+ return Vector3(x, y, z);
}
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 {
- return x * x + y * y + z * z + w * w;
+ return x * x + y * y + z * z + w * w;
}
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() {
- float length = GetLength();
- x /= length;
- y /= length;
- z /= length;
- w /= length;
+ float length = GetLength();
+ x /= length;
+ y /= length;
+ z /= length;
+ w /= length;
}
Quaternion Quaternion::Normalize(const Quaternion& q) {
- Quaternion result;
- float length = q.GetLength();
- result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
- return result;
+ Quaternion result;
+ float length = q.GetLength();
+ result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
+ 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;
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
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 {
- float sqx = q1.x * q1.x;
- float sqy = q1.y * q1.y;
- float sqz = q1.z * q1.z;
+ 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 {
+ 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
- );
- }
+ 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);
+ }
}
-Quaternion Quaternion::operator *(const Quaternion& r2) const {
- return Quaternion(
- 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
- );
+Quaternion Quaternion::operator*(const Quaternion& r2) const {
+ return Quaternion(
+ 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);
};
-Vector3 Quaternion::operator *(const Vector3& p) const {
- float num = this->x * 2;
- float num2 = this->y * 2;
- float num3 = this->z * 2;
- float num4 = this->x * num;
- float num5 = this->y * num2;
- float num6 = this->z * num3;
- float num7 = this->x * num2;
- float num8 = this->x * num3;
- float num9 = this->y * num3;
- float num10 = this->w * num;
- 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;
- return result;
+Vector3 Quaternion::operator*(const Vector3& p) const {
+ float num = this->x * 2;
+ float num2 = this->y * 2;
+ float num3 = this->z * 2;
+ float num4 = this->x * num;
+ float num5 = this->y * num2;
+ float num6 = this->z * num3;
+ float num7 = this->x * num2;
+ float num8 = this->x * num3;
+ float num9 = this->y * num3;
+ float num10 = this->w * num;
+ 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;
+ return result;
}
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) {
- 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);
+ 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);
}
Quaternion Quaternion::LookRotation(const Vector3& forward) {
- Vector3 up = Vector3(0, 1, 0);
- return LookRotation(forward, up);
+ Vector3 up = Vector3(0, 1, 0);
+ return LookRotation(forward, up);
}
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
- Vector3 nForward = Vector3::Normalize(forward);
- Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
- Vector3 nUp = Vector3::Cross(nForward, nRight);
- float m00 = nRight.x;
- float m01 = nRight.y;
- float m02 = nRight.z;
- float m10 = nUp.x;
- float m11 = nUp.y;
- float m12 = nUp.z;
- float m20 = nForward.x;
- float m21 = nForward.y;
- float m22 = nForward.z;
+ Vector3 nForward = Vector3::Normalize(forward);
+ Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
+ Vector3 nUp = Vector3::Cross(nForward, nRight);
+ float m00 = nRight.x;
+ float m01 = nRight.y;
+ float m02 = nRight.z;
+ float m10 = nUp.x;
+ float m11 = nUp.y;
+ float m12 = nUp.z;
+ float m20 = nForward.x;
+ float m21 = nForward.y;
+ float m22 = nForward.z;
-
- float num8 = (m00 + m11) + m22;
- Quaternion quaternion = Quaternion();
- if (num8 > 0) {
- float num = sqrtf(num8 + 1);
- quaternion.w = num * 0.5f;
- num = 0.5f / num;
- quaternion.x = (m12 - m21) * num;
- quaternion.y = (m20 - m02) * num;
- quaternion.z = (m01 - m10) * num;
- return quaternion;
- }
- if ((m00 >= m11) && (m00 >= m22)) {
- float num7 = sqrtf(((1 + m00) - m11) - m22);
- float num4 = 0.5F / num7;
- quaternion.x = 0.5f * num7;
- quaternion.y = (m01 + m10) * num4;
- quaternion.z = (m02 + m20) * num4;
- quaternion.w = (m12 - m21) * num4;
- return quaternion;
- }
- if (m11 > m22) {
- float num6 = sqrtf(((1 + m11) - m00) - m22);
- float num3 = 0.5F / num6;
- quaternion.x = (m10 + m01) * num3;
- quaternion.y = 0.5F * num6;
- quaternion.z = (m21 + m12) * num3;
- quaternion.w = (m20 - m02) * num3;
- return quaternion;
- }
- float num5 = sqrtf(((1 + m22) - m00) - m11);
- float num2 = 0.5F / num5;
- quaternion.x = (m20 + m02) * num2;
- quaternion.y = (m21 + m12) * num2;
- quaternion.z = 0.5F * num5;
- quaternion.w = (m01 - m10) * num2;
- return quaternion;
+ float num8 = (m00 + m11) + m22;
+ Quaternion quaternion = Quaternion();
+ if (num8 > 0) {
+ float num = sqrtf(num8 + 1);
+ quaternion.w = num * 0.5f;
+ num = 0.5f / num;
+ quaternion.x = (m12 - m21) * num;
+ quaternion.y = (m20 - m02) * num;
+ quaternion.z = (m01 - m10) * num;
+ return quaternion;
+ }
+ if ((m00 >= m11) && (m00 >= m22)) {
+ float num7 = sqrtf(((1 + m00) - m11) - m22);
+ float num4 = 0.5F / num7;
+ quaternion.x = 0.5f * num7;
+ quaternion.y = (m01 + m10) * num4;
+ quaternion.z = (m02 + m20) * num4;
+ quaternion.w = (m12 - m21) * num4;
+ return quaternion;
+ }
+ if (m11 > m22) {
+ float num6 = sqrtf(((1 + m11) - m00) - m22);
+ float num3 = 0.5F / num6;
+ quaternion.x = (m10 + m01) * num3;
+ quaternion.y = 0.5F * num6;
+ quaternion.z = (m21 + m12) * num3;
+ quaternion.w = (m20 - m02) * num3;
+ return quaternion;
+ }
+ float num5 = sqrtf(((1 + m22) - m00) - m11);
+ float num2 = 0.5F / num5;
+ quaternion.x = (m20 + m02) * num2;
+ quaternion.y = (m21 + m12) * num2;
+ quaternion.z = 0.5F * num5;
+ quaternion.w = (m01 - m10) * num2;
+ return quaternion;
}
-Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) {
- Vector3 axis = Vector3::Cross(fromDirection, toDirection);
- axis = Vector3::Normalize(axis);
- float angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
- Quaternion rotation = Quaternion::AngleAxis(angle, axis);
- return rotation;
+Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
+ Vector3 toDirection) {
+ Vector3 axis = Vector3::Cross(fromDirection, toDirection);
+ axis = Vector3::Normalize(axis);
+ float angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
+ Quaternion rotation = Quaternion::AngleAxis(angle, axis);
+ return rotation;
}
-Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) {
- float num = Quaternion::Angle(from, to);
- if (num == 0) {
- return to;
- }
- float t = (float)fmin(1, maxDegreesDelta / num);
- return SlerpUnclamped(from, to, t);
+Quaternion Quaternion::RotateTowards(const Quaternion& from,
+ const Quaternion& to,
+ float maxDegreesDelta) {
+ float num = Quaternion::Angle(from, to);
+ if (num == 0) {
+ return to;
+ }
+ float t = (float)fmin(1, maxDegreesDelta / num);
+ return SlerpUnclamped(from, to, t);
}
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
- if (Vector3::SqrMagnitude(axis) == 0.0)
- return Quaternion();
+ if (Vector3::SqrMagnitude(axis) == 0.0)
+ return Quaternion();
- Quaternion result = Quaternion();
- float radians = angle * Deg2Rad;
- radians *= 0.5;
+ Quaternion result = Quaternion();
+ float radians = angle * Deg2Rad;
+ radians *= 0.5;
- Vector3 axis2 = axis * (float)sin(radians);
- result.x = axis2.x;
- result.y = axis2.y;
- result.z = axis2.z;
- result.w = (float)cos(radians);
+ Vector3 axis2 = axis * (float)sin(radians);
+ result.x = axis2.x;
+ result.y = axis2.y;
+ result.z = axis2.z;
+ result.w = (float)cos(radians);
- return Quaternion::Normalize(result);
+ return Quaternion::Normalize(result);
}
float Quaternion::Angle(Quaternion a, Quaternion b) {
- float f = Quaternion::Dot(a, b);
- return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
+ float f = Quaternion::Dot(a, b);
+ return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
}
-void Quaternion::ToAngleAxis(float* angle, Vector3* axis)
-{
- Quaternion::ToAxisAngleRad(*this, axis, angle);
- *angle *= Rad2Deg;
+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)
-{
- 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)
- {
- *axis = q1.xyz() / den;
- }
- else
- {
- // This occurs when the angle is zero.
- // Not a problem: just set an arbitrary normalized axis.
- *axis = Vector3(1, 0, 0);
- }
+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) {
+ *axis = q1.xyz() / den;
+ } 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) {
- // 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) {
- return a;
- }
+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) {
+ return a;
+ }
- const Vector3 axyz = a.xyz();
- const Vector3 bxyz = b.xyz();
- float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
+ const Vector3 axyz = a.xyz();
+ const Vector3 bxyz = b.xyz();
+ float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
- Quaternion b2 = b;
- if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
- // angle = 0.0f, so just return one input.
- return a;
- }
- else if (cosHalfAngle < 0.0f) {
- b2.x = -b.x;
- b2.y = -b.y;
- b2.z = -b.z;
- b2.w = -b.w;
- cosHalfAngle = -cosHalfAngle;
- }
+ Quaternion b2 = b;
+ if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
+ // angle = 0.0f, so just return one input.
+ return a;
+ } else if (cosHalfAngle < 0.0f) {
+ b2.x = -b.x;
+ b2.y = -b.y;
+ b2.z = -b.z;
+ b2.w = -b.w;
+ cosHalfAngle = -cosHalfAngle;
+ }
- float blendA;
- float blendB;
- if (cosHalfAngle < 0.99f) {
- // do proper slerp for big angles
- float halfAngle = acosf(cosHalfAngle);
- float sinHalfAngle = sinf(halfAngle);
- float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
- blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
- blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
- }
- else {
- // do lerp if angle is really small.
- blendA = 1.0f - t;
- blendB = t;
- }
- Vector3 v = axyz * blendA + b2.xyz() * blendB;
- Quaternion result = Quaternion(v.x, v.y, v.z, blendA * a.w + blendB * b2.w);
- if (result.GetLengthSquared() > 0.0f)
- return Quaternion::Normalize(result);
- else
- return Quaternion();
+ float blendA;
+ float blendB;
+ if (cosHalfAngle < 0.99f) {
+ // do proper slerp for big angles
+ float halfAngle = acosf(cosHalfAngle);
+ float sinHalfAngle = sinf(halfAngle);
+ float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
+ blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
+ blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
+ } else {
+ // do lerp if angle is really small.
+ blendA = 1.0f - t;
+ blendB = t;
+ }
+ Vector3 v = axyz * blendA + b2.xyz() * blendB;
+ Quaternion result = Quaternion(v.x, v.y, v.z, blendA * a.w + blendB * b2.w);
+ if (result.GetLengthSquared() > 0.0f)
+ return Quaternion::Normalize(result);
+ else
+ return Quaternion();
}
-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);
+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);
}
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) {
- return Quaternion::FromEulerRad(euler * Deg2Rad);
+ return Quaternion::FromEulerRad(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRad(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 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;
+}
+
+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) {
- Quaternion secondaryRotation = GetRotationAround(axis, rotation);
- float rotationAngle;
- Vector3 rotationAxis;
- secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
+ Quaternion secondaryRotation = GetRotationAround(axis, rotation);
+ float rotationAngle;
+ Vector3 rotationAxis;
+ secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
- // Do the axis point in opposite directions?
- if (Vector3::Dot(axis, rotationAxis) < 0)
- rotationAngle = -rotationAngle;
+ // Do the axis point in opposite directions?
+ if (Vector3::Dot(axis, rotationAxis) < 0)
+ rotationAngle = -rotationAngle;
- return rotationAngle;
+ return rotationAngle;
}
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)
- Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w);
- twist = Quaternion::Normalize(twist);
- return twist;
+ 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)
+ 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) {
- *twist = GetRotationAround(axis, rotation);
- *swing = rotation * Quaternion::Inverse(*twist);
+void Quaternion::GetSwingTwist(Vector3 axis,
+ Quaternion rotation,
+ Quaternion* swing,
+ Quaternion* twist) {
+ *twist = GetRotationAround(axis, rotation);
+ *swing = rotation * Quaternion::Inverse(*twist);
}
-