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

Change Vector3::Angle return type to AngleOf<float>

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-05 12:16:27 +02:00
parent 18cf653aff
commit c72bba4f7a
4 changed files with 81 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
Quaternion.cpp
View File

@ -3,11 +3,12 @@
// 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 "Angle.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,7 +43,9 @@ 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);
@ -51,7 +54,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;
}
@ -63,7 +66,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);
@ -74,11 +77,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;
@ -94,7 +97,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,
@ -102,7 +105,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;
@ -133,7 +136,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);
}
@ -142,23 +145,23 @@ 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);
float m00 = nRight.Right(); // x;
float m01 = nRight.Up(); // y;
float m02 = nRight.Forward(); // z;
float m10 = nUp.Right(); // x;
float m11 = nUp.Up(); // y;
float m12 = nUp.Forward(); // z;
float m20 = nForward.Right(); // x;
float m21 = nForward.Up(); // y;
float m22 = nForward.Forward(); // z;
float m00 = nRight.Right(); // x;
float m01 = nRight.Up(); // y;
float m02 = nRight.Forward(); // z;
float m10 = nUp.Right(); // x;
float m11 = nUp.Up(); // y;
float m12 = nUp.Forward(); // z;
float m20 = nForward.Right(); // x;
float m21 = nForward.Up(); // y;
float m22 = nForward.Forward(); // z;
float num8 = (m00 + m11) + m22;
Quaternion quaternion = Quaternion();
@ -202,13 +205,13 @@ 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);
AngleOf<float> angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
Quaternion rotation = Quaternion::AngleAxis(angle.ToFloat(), axis);
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) {
@ -218,7 +221,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();
@ -227,9 +230,9 @@ Quaternion Quaternion::AngleAxis(float angle, const Vector3 &axis) {
radians *= 0.5f;
Vector3 axis2 = axis * (float)sin(radians);
result.x = axis2.Right(); // x;
result.y = axis2.Up(); // y;
result.z = axis2.Forward(); // z;
result.x = axis2.Right(); // x;
result.y = axis2.Up(); // y;
result.z = axis2.Forward(); // z;
result.w = (float)cos(radians);
return Quaternion::Normalize(result);
@ -240,15 +243,16 @@ 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 = Vector3::Normalize(q1.xyz() / den);
@ -258,7 +262,8 @@ void Quaternion::ToAxisAngleRad(const Quaternion &q, Vector3 *const 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) {
@ -309,7 +314,8 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion &a, const Quaternion &b,
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;
@ -357,9 +363,9 @@ Quaternion Quaternion::EulerXYZ(Vector3 euler) {
return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
float yaw = euler.Right(); // x;
float pitch = euler.Up(); // y;
float roll = euler.Forward(); // z;
float yaw = euler.Right(); // x;
float pitch = euler.Up(); // y;
float roll = euler.Forward(); // z;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)sin((float)rollOver2);
float cosRollOver2 = (float)cos((float)rollOver2);
@ -395,16 +401,18 @@ 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.Right(), p.Up(), p.Forward(), 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);
}

16
Vector3.cpp
View File

@ -188,7 +188,7 @@ float clamp(float x, float lower, float upper) {
return upperClamp;
}
float Vector3::Angle(const Vector3& v1, const Vector3& v2) {
AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
if (denominator < epsilon)
return 0;
@ -200,23 +200,23 @@ float Vector3::Angle(const Vector3& v1, const Vector3& v2) {
float cdot = clamp(fraction, -1.0, 1.0);
float r = ((float)acos(cdot)) * Rad2Deg;
return r;
return AngleOf<float>(r);
}
float Vector3::SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis) {
AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis) {
// angle in [0,180]
float angle = Vector3::Angle(v1, v2);
AngleOf<float> angle = Vector3::Angle(v1, v2);
Vector3 cross = Vector3::Cross(v1, v2);
float b = Vector3::Dot(axis, cross);
float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
// angle in [-179,180]
float signed_angle = angle * signd;
AngleOf<float> signed_angle = angle * signd;
return signed_angle;
return AngleOf<float>(signed_angle);
}
Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {

8
Vector3.h
View File

@ -204,15 +204,15 @@ struct Vector3 : Vec3 {
/// @remark This reterns an unsigned angle which is the shortest distance
/// between the two vectors. Use Vector3::SignedAngle if a signed angle is
/// needed.
static float Angle(const Vector3& v1, const Vector3& v2);
static AngleOf<float> Angle(const Vector3& v1, const Vector3& v2);
/// @brief The signed angle between two vectors
/// @param v1 The starting vector
/// @param v2 The ending vector
/// @param axis The axis to rotate around
/// @return The signed angle between the two vectors
static float SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis);
static AngleOf<float> SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector

33
test/Vector3_test.cc
View File

@ -1,7 +1,7 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <limits>
#include "Vector3.h"
@ -488,29 +488,29 @@ TEST(Vector3, ProjectOnPlane) {
TEST(Vector3, Angle) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
float f = 0;
AngleOf<float> f = 0;
bool r = false;
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 12.9331388F) << "Angle(4 5 6, 1 2 3)";
EXPECT_FLOAT_EQ(f.ToFloat(), 12.9331388F) << "Angle(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 167.066864F) << "Angle(4 5 6, -1 -2 -3)";
EXPECT_FLOAT_EQ(f.ToFloat(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "Angle(4 5 6, 0 0 0)";
EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "Angle(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::Angle(v1, v2);
r = isnan(f);
r = isnan(f.ToFloat());
EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::Angle(v1, v2);
r = isnan(f);
r = isnan(f.ToFloat());
EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
@ -519,39 +519,42 @@ TEST(Vector3, SignedAngle) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v3 = Vector3(7, 8, -9);
float f = 0;
AngleOf<float> f = 0;
bool r = false;
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f, -12.9331388F) << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)";
EXPECT_FLOAT_EQ(f.ToFloat(), -12.9331388F)
<< "SignedAngle(4 5 6, 1 2 3, 7 8 -9)";
v2 = Vector3(-1, -2, -3);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f, 167.066864F) << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)";
EXPECT_FLOAT_EQ(f.ToFloat(), 167.066864F)
<< "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)";
v2 = Vector3(0, 0, 0);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )";
EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )";
v2 = Vector3(1, 2, 3);
v3 = Vector3(-7, -8, 9);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f, 12.9331388F) << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)";
EXPECT_FLOAT_EQ(f.ToFloat(), 12.9331388F)
<< "SignedAngle(4 5 6, 1 2 3, -7 -8 9)";
v3 = Vector3(0, 0, 0);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)";
EXPECT_FLOAT_EQ(f.ToFloat(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::SignedAngle(v1, v2, v3);
r = isnan(f);
r = isnan(f.ToFloat());
EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::SignedAngle(v1, v2, v3);
r = isnan(f);
r = isnan(f.ToFloat());
EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}