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

normalize toAngleAxis result

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-26 12:12:22 +02:00
parent 4385befa11
commit 64ca76830c
1 changed files with 27 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
Quaternion.cpp
View File

@ -3,11 +3,11 @@
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Quaternion.h"
#include "Vector3.h"
#include <float.h>
#include <math.h>
#include "Vector3.h"
void CopyQuat(const Quat& q1, Quat& q2) {
void CopyQuat(const Quat &q1, Quat &q2) {
q2.x = q1.x;
q2.y = q1.y;
q2.z = q1.z;
@ -42,9 +42,7 @@ Quaternion::~Quaternion() {}
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
Vector3 Quaternion::xyz() const {
return Vector3(x, y, z);
}
Vector3 Quaternion::xyz() const { return Vector3(x, y, z); }
float Quaternion::GetLength() const {
return sqrtf(x * x + y * y + z * z + w * w);
@ -53,7 +51,7 @@ float Quaternion::GetLength() const {
float Quaternion::GetLengthSquared() const {
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;
}
@ -65,7 +63,7 @@ void Quaternion::Normalize() {
w /= length;
}
Quaternion Quaternion::Normalize(const Quaternion& q) {
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);
@ -76,11 +74,11 @@ float Quaternion::Dot(Quaternion a, Quaternion b) {
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;
if (test > 0.499f) { // singularity at north pole
if (test > 0.499f) { // singularity at north pole
return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
} else if (test < -0.499f) { // singularity at south pole
} else if (test < -0.499f) { // singularity at south pole
return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
} else {
float sqx = q1.x * q1.x;
@ -96,7 +94,7 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
}
}
Quaternion Quaternion::operator*(const Quaternion& r2) const {
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,
@ -104,7 +102,7 @@ Quaternion Quaternion::operator*(const Quaternion& r2) const {
-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 num2 = this->y * 2;
float num3 = this->z * 2;
@ -127,7 +125,7 @@ Vector3 Quaternion::operator*(const Vector3& p) const {
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);
}
@ -136,11 +134,11 @@ Quaternion Quaternion::Inverse(Quaternion r) {
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);
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 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
Vector3 nUp = Vector3::Cross(nForward, nRight);
@ -201,8 +199,8 @@ Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
return rotation;
}
Quaternion Quaternion::RotateTowards(const Quaternion& from,
const Quaternion& to,
Quaternion Quaternion::RotateTowards(const Quaternion &from,
const Quaternion &to,
float maxDegreesDelta) {
float num = Quaternion::Angle(from, to);
if (num == 0) {
@ -212,7 +210,7 @@ Quaternion Quaternion::RotateTowards(const Quaternion& from,
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.0f)
return Quaternion();
@ -234,27 +232,25 @@ float Quaternion::Angle(Quaternion a, Quaternion b) {
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
}
void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
void Quaternion::ToAngleAxis(float *angle, Vector3 *axis) {
Quaternion::ToAxisAngleRad(*this, axis, angle);
*angle *= Rad2Deg;
}
void Quaternion::ToAxisAngleRad(const Quaternion& q,
Vector3* const axis,
float* angle) {
void Quaternion::ToAxisAngleRad(const Quaternion &q, Vector3 *const axis,
float *angle) {
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
*angle = 2.0f * acosf(q1.w); // angle
*angle = 2.0f * acosf(q1.w); // angle
float den = sqrtf(1.0F - q1.w * q1.w);
if (den > 0.0001f) {
*axis = q1.xyz() / den;
*axis = Vector3::Normalize(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,
Quaternion Quaternion::SlerpUnclamped(const Quaternion &a, const Quaternion &b,
float t) {
// if either input is zero, return the other.
if (Quaternion::GetLengthSquared(a) == 0.0f) {
@ -304,8 +300,7 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
return Quaternion();
}
Quaternion Quaternion::Slerp(const Quaternion& a,
const Quaternion& b,
Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b,
float t) {
if (t > 1)
t = 1;
@ -391,18 +386,16 @@ float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
}
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
Vector3 p = Vector3::Project(
ra, axis); // return projection ra on to axis (parallel component)
ra, axis); // return projection ra on to axis (parallel component)
Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w);
twist = Quaternion::Normalize(twist);
return twist;
}
void Quaternion::GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist) {
void Quaternion::GetSwingTwist(Vector3 axis, Quaternion rotation,
Quaternion *swing, Quaternion *twist) {
*twist = GetRotationAround(axis, rotation);
*swing = rotation * Quaternion::Inverse(*twist);
}