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"
extern "C" {
/// <summary>
/// A quaternion
/// </summary>
/// This is a C-style implementation
typedef struct Quat {
/// <summary>
/// The x component
/// </summary>
float x;
/// <summary>
/// The y component
/// </summary>
float y;
/// <summary>
/// The z component
/// </summary>
float z;
/// <summary>
/// The w component
/// </summary>
float w;
} Quat;
/// <summary>
/// A quaternion
/// </summary>
/// This is a C-style implementation
typedef struct Quat {
/// <summary>
/// The x component
/// </summary>
float x;
/// <summary>
/// The y component
/// </summary>
float y;
/// <summary>
/// The z component
/// </summary>
float z;
/// <summary>
/// The w component
/// </summary>
float w;
} Quat;
}
/// <summary>
/// A quaternion
/// </summary>
struct Quaternion : Quat {
public:
/// <summary>
/// Create a new identity quaternion
/// </summary>
Quaternion();
/// <summary>
/// create a new quaternion with the given values
/// </summary>
/// <param name="_x">x component</param>
/// <param name="_y">y component</param>
/// <param name="_z">z component</param>
/// <param name="_w">w component</param>
Quaternion(float _x, float _y, float _z, float _w);
/// <summary>
/// Create a quaternion from C-style Quat
/// </summary>
/// <param name="q"></param>
Quaternion(Quat q);
~Quaternion();
public:
/// <summary>
/// Create a new identity quaternion
/// </summary>
Quaternion();
/// <summary>
/// create a new quaternion with the given values
/// </summary>
/// <param name="_x">x component</param>
/// <param name="_y">y component</param>
/// <param name="_z">z component</param>
/// <param name="_w">w component</param>
Quaternion(float _x, float _y, float _z, float _w);
/// <summary>
/// Create a quaternion from C-style Quat
/// </summary>
/// <param name="q"></param>
Quaternion(Quat q);
~Quaternion();
/// <summary>
/// An identity quaternion
/// </summary>
const static Quaternion identity;
/// <summary>
/// An identity quaternion
/// </summary>
const static Quaternion identity;
/// <summary>
/// Convert to unit quaternion
/// </summary>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
void Normalize();
/// <summary>
/// Convert to unity quaternion
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>A unit quaternion</returns>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion& q);
/// <summary>
/// Convert to unit quaternion
/// </summary>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
void Normalize();
/// <summary>
/// Convert to unity quaternion
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>A unit quaternion</returns>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion& q);
/// <summary>
/// Convert to euler angles
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>A vector containing euler angles</returns>
/// The euler angles performed in the order: Z, X, Y
static Vector3 ToAngles(const Quaternion& q);
/// <summary>
/// Convert to euler angles
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>A vector containing euler angles</returns>
/// The euler angles performed in the order: Z, X, Y
static Vector3 ToAngles(const Quaternion& q);
/// <summary>
/// Rotate a vector using this quaterion
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
Vector3 operator *(const Vector3& vector) const;
/// <summary>
/// Multiply this quaternion with another quaternion
/// </summary>
/// <param name="rotation">The quaternion to multiply with</param>
/// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to
/// the give rotation.
Quaternion operator *(const Quaternion& rotation) const;
/// <summary>
/// Rotate a vector using this quaterion
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
Vector3 operator*(const Vector3& vector) const;
/// <summary>
/// Multiply this quaternion with another quaternion
/// </summary>
/// <param name="rotation">The quaternion to multiply with</param>
/// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to
/// the give rotation.
Quaternion operator*(const Quaternion& rotation) const;
/// <summary>
/// Check the equality of two quaternions
/// </summary>
/// <param name="quaternion">The quaternion to compare to</param>
/// <returns>True when the components of the quaternions are identical</returns>
/// Note that this does not compare the rotations themselves.
/// Two quaternions with the same rotational effect may have different
/// components. Use Quaternion::Angle to check if the rotations are the same.
bool operator ==(const Quaternion& quaternion);
/// <summary>
/// Check the equality of two quaternions
/// </summary>
/// <param name="quaternion">The quaternion to compare to</param>
/// <returns>True when the components of the quaternions are
/// identical</returns> Note that this does not compare the rotations
/// themselves. Two quaternions with the same rotational effect may have
/// different components. Use Quaternion::Angle to check if the rotations are
/// the same.
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>
/// 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>
/// A rotation which looks in the given direction
/// </summary>
/// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward,
const Vector3& upwards);
/// <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>
/// A rotation which looks in the given direction
/// </summary>
/// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards);
/// <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>
/// Calculat the rotation from on vector to another
/// </summary>
/// <param name="fromDirection">The from direction</param>
/// <param name="toDirection">The to direction</param>
/// <returns>The rotation from the first to the second vector</returns>
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
/// <summary>
/// Calculat the rotation from on vector to another
/// </summary>
/// <param name="fromDirection">The from direction</param>
/// <param name="toDirection">The to direction</param>
/// <returns>The rotation from the first to the second vector</returns>
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
/// <summary>
/// Rotate form one orientation to anther with a maximum amount of degrees
/// </summary>
/// <param name="from">The from rotation</param>
/// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to
/// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta);
/// <summary>
/// Rotate form one orientation to anther with a maximum amount of degrees
/// </summary>
/// <param name="from">The from rotation</param>
/// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to rotate</param>
/// <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta);
/// <summary>
/// Convert an angle/axis representation to a quaternion
/// </summary>
/// <param name="angle">The angle</param>
/// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns>
static Quaternion AngleAxis(float angle, const Vector3& axis);
/// <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>
/// Convert an angle/axis representation to a quaternion
/// </summary>
/// <param name="angle">The angle</param>
/// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns>
static Quaternion AngleAxis(float angle, const Vector3& axis);
/// <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>
/// Get the angle between two orientations
/// </summary>
/// <param name="orientation1">The first orientation</param>
/// <param name="orientation2">The second orientation</param>
/// <returns>The smallest angle in degrees between the two
/// orientations</returns>
static float Angle(Quaternion orientation1, Quaternion orientation2);
/// <summary>
/// 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 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>
/// Get the angle between two orientations
/// </summary>
/// <param name="orientation1">The first orientation</param>
/// <param name="orientation2">The second orientation</param>
/// <returns>The smallest angle in degrees between the two orientations</returns>
static float Angle(Quaternion orientation1, Quaternion orientation2);
/// <summary>
/// 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 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>
/// 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 Z, X, Y.
static Quaternion Euler(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order Z, X, Y.
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 z, X, Y.
static Quaternion Euler(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order z, X, Y.
static Quaternion Euler(Vector3 eulerAngles);
/// <summary>
/// Create a rotation from euler angles
/// </summary>
/// <param name="x">The angle around the right axis</param>
/// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(Vector3 eulerAngles);
/// <summary>
/// Returns the angle of around the give axis for a rotation
/// </summary>
/// <param name="axis">The axis around which the angle should be computed</param>
/// <param name="rotation">The source rotation</param>
/// <returns>The signed angle around the axis</returns>
static float GetAngleAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Returns the rotation limited around the given axis
/// </summary>
/// <param name="axis">The axis which which the rotation should be limited</param>
/// <param name="rotation">The source rotation</param>
/// <returns>The rotation around the given axis</returns>
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Swing-twist decomposition of a rotation
/// </summary>
/// <param name="axis">The base direction for the decomposition</param>
/// <param name="rotation">The source rotation</param>
/// <param name="swing">A pointer to the quaternion for the swing result</param>
/// <param name="twist">A pointer to the quaternion for the twist result</param>
static void GetSwingTwist(Vector3 axis, Quaternion rotation, Quaternion* swing, Quaternion* twist);
/// <summary>
/// Returns the angle of around the give axis for a rotation
/// </summary>
/// <param name="axis">The axis around which the angle should be
/// computed</param> <param name="rotation">The source rotation</param>
/// <returns>The signed angle around the axis</returns>
static float GetAngleAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Returns the rotation limited around the given axis
/// </summary>
/// <param name="axis">The axis which which the rotation should be
/// limited</param> <param name="rotation">The source rotation</param>
/// <returns>The rotation around the given axis</returns>
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Swing-twist decomposition of a rotation
/// </summary>
/// <param name="axis">The base direction for the decomposition</param>
/// <param name="rotation">The source rotation</param>
/// <param name="swing">A pointer to the quaternion for the swing
/// result</param> <param name="twist">A pointer to the quaternion for the
/// twist result</param>
static void GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
/// <summary>
/// Calculate the dot product of two quaternions
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <returns></returns>
static float Dot(Quaternion rotation1, Quaternion rotation2);
/// <summary>
/// Calculate the dot product of two quaternions
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <returns></returns>
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

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
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include <math.h>
#include <float.h>
#include "Quaternion.h"
#include <float.h>
#include <math.h>
#include "Vector3.h"
void CopyQuat(const Quat& q1, Quat& q2) {
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);
}