Remove EulerAngle & Add EulerAngleXYZ.
All checks were successful
Run ReCI Build Test / Explore-Gitea-Actions (push) Successful in 1m38s
Build Docs With Doxygen / Explore-Gitea-Actions (push) Successful in 26s

This commit is contained in:
2024-11-18 20:37:52 -05:00
parent 80b752128c
commit bb1b1b5a13
16 changed files with 97 additions and 328 deletions

View File

@@ -84,6 +84,9 @@ namespace J3ML::Math::Constants {
constexpr float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364;
/// pi - https://www.mathsisfun.com/numbers/pi.html
constexpr float Pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
/// pi * 0.5.
constexpr float HalfPi = 1.5707963267948966192313216916397514420985846996875529104874722961539082031431044993140174126710585;
/// e - https://www.mathsisfun.com/numbers/e-eulers-number.html
constexpr float EulersNumber = 2.7182818284590452353602874713526624977572470936999595749669676277240766303535475945713821785251664274;
/// 2pi - The ratio of a circle's circumferecne to its radius, and the number of radians in one turn.

View File

@@ -2,28 +2,23 @@
#include <J3ML/LinearAlgebra/EulerAngle.hpp>
#include <J3ML/LinearAlgebra/Quaternion.hpp>
#include <J3ML/LinearAlgebra/AxisAngle.hpp>
#include <J3ML/LinearAlgebra/Vector3.hpp>
namespace J3ML::LinearAlgebra
{
namespace J3ML::LinearAlgebra {
class AxisAngle;
}
/// Transitional datatype, not useful for internal representation of rotation
/// But has uses for conversion and manipulation.
class AxisAngle {
public:
Vector3 axis;
float angle;
public:
AxisAngle();
explicit AxisAngle(const Quaternion& q);
explicit AxisAngle(const EulerAngle& e);
AxisAngle(const Vector3 &axis, float angle);
EulerAngle ToEulerAngleXYZ() const;
Quaternion ToQuaternion() const;
static AxisAngle FromEulerAngleXYZ(const EulerAngle&);
};
}
/// Transitional datatype, not useful for internal representation of rotation
/// But has uses for conversion and manipulation.
class J3ML::LinearAlgebra::AxisAngle {
public:
Vector3 axis;
float angle;
public:
AxisAngle();
explicit AxisAngle(const Quaternion& q);
AxisAngle(const Vector3& axis, float angle);
public:
[[nodiscard]] Quaternion ToQuaternion() const;
[[nodiscard]] EulerAngleXYZ ToEulerAngleXYZ() const;
};

View File

@@ -5,48 +5,18 @@
#include <J3ML/LinearAlgebra/AxisAngle.hpp>
namespace J3ML::LinearAlgebra {
class EulerAngleXYZ;
}
class AxisAngle;
// Essential Reading:
// http://www.essentialmath.com/GDC2012/GDC2012_JMV_Rotations.pdf
class EulerAngle {
class J3ML::LinearAlgebra::EulerAngleXYZ {
public:
EulerAngle();
EulerAngle(float pitch, float yaw, float roll);
EulerAngle(const Vector3& vec) : pitch(vec.x), yaw(vec.y), roll(vec.z) {}
AxisAngle ToAxisAngle() const;
[[nodiscard]] Quaternion ToQuaternion() const;
explicit EulerAngle(const Quaternion& rhs);
explicit EulerAngle(const AxisAngle& rhs);
/// TODO: Implement separate upper and lower bounds
/// Preserves internal value of euler angles, normalizes and clamps the output.
/// This does not solve gimbal lock!!!
float GetPitch(float pitch_limit) const;
float GetYaw(float yaw_limit) const;
float GetRoll(float roll_limit) const;
bool operator==(const EulerAngle& a) const;
void clamp();
// TODO: Euler Angles do not represent a vector, length doesn't apply, nor is this information meaningful for this data type.
// If you need a meaningful representation of length in 3d space, use a vector!!
[[nodiscard]] float length() const {
return 0;
}
// TODO: Implement
Vector3 unitVector() const;
EulerAngle movementAngle() const;
public:
float pitch;
float yaw;
float roll;
float roll = 0; // X
float pitch = 0; // Y
float yaw = 0; // Z
public:
EulerAngleXYZ(float roll, float pitch, float yaw);
public:
explicit EulerAngleXYZ(const Quaternion& rhs);
explicit EulerAngleXYZ(const AxisAngle& rhs);
};
}

View File

@@ -7,7 +7,7 @@ namespace J3ML::LinearAlgebra
class Vector3; // A type representing a position in a 3-dimensional coordinate space.
class Vector4; // A type representing a position in a 4-dimensional coordinate space.
class Angle2D; // Uses x,y components to represent a 2D rotation.
class EulerAngle; // Uses pitch,yaw,roll components to represent a 3D orientation.
class EulerAngleXYZ; // Uses pitch,yaw,roll components to represent a 3D orientation.
class AxisAngle; //
class CoordinateFrame; //
class Matrix2x2;

View File

@@ -63,7 +63,7 @@ namespace J3ML::LinearAlgebra {
/// Constructs this matrix3x3 from the given quaternion.
explicit Matrix3x3(const Quaternion& orientation);
/// Constructs this matrix3x3 from the given euler angle.
explicit Matrix3x3(const EulerAngle& orientation);
explicit Matrix3x3(const EulerAngleXYZ& orientation);
/// Constructs this Matrix3x3 from a pointer to an array of floats.
explicit Matrix3x3(const float *data);
@@ -247,9 +247,6 @@ namespace J3ML::LinearAlgebra {
/// matrix, and there is scaling ,shearing, or mirroring in this matrix)
bool TryConvertToQuat(Quaternion& q) const;
/// Converts this rotation matrix to an Euler Angle.
[[nodiscard]] EulerAngle ToEulerAngle() const;
/// Returns the main diagonal.
/// The main diagonal consists of the elements at m[0][0], m[1][1], m[2][2]
[[nodiscard]] Vector3 Diagonal() const;

View File

@@ -71,8 +71,7 @@ namespace J3ML::LinearAlgebra {
/// Constructs this Matrix4x4 from the given quaternion.
explicit Matrix4x4(const Quaternion& orientation);
/// Constructs this Matrix4x4 from the given Euler Angle.
explicit Matrix4x4(const EulerAngle& orientation);
/// Constructs this float4x4 from the given quaternion and translation.
/// Logically, the translation occurs after the rotation has been performed.
@@ -567,8 +566,6 @@ namespace J3ML::LinearAlgebra {
[[nodiscard]] Quaternion ToQuat() const;
[[nodiscard]] EulerAngle ToEulerAngle() const;
/// Returns true if this Matrix4x4 is equal to the given Matrix4x4, up to given per-element epsilon.
bool Equals(const Matrix4x4& other, float epsilon = 1e-3f) const;

View File

@@ -42,7 +42,6 @@ namespace J3ML::LinearAlgebra
Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians);
explicit Quaternion(const Vector4& vector4);
explicit Quaternion(const EulerAngle& angle);
explicit Quaternion(const AxisAngle& angle);
/// Creates a LookAt quaternion.
@@ -143,9 +142,6 @@ namespace J3ML::LinearAlgebra
[[nodiscard]] float LengthSquared() const;
[[nodiscard]] float Length() const;
[[nodiscard]] EulerAngle ToEulerAngle() const;
[[nodiscard]] Matrix3x3 ToMatrix3x3() const;
[[nodiscard]] Matrix4x4 ToMatrix4x4() const;

View File

@@ -2,16 +2,16 @@
#include <J3ML/LinearAlgebra/Quaternion.hpp>
namespace J3ML::LinearAlgebra {
AxisAngle::AxisAngle() : axis(Vector3::Zero) {}
AxisAngle::AxisAngle() : axis(Vector3::Zero), angle(0) {}
AxisAngle::AxisAngle(const Vector3 &axis, float angle) : axis(axis), angle(angle) {}
AxisAngle::AxisAngle(const Vector3& axis, float angle) : axis(axis), angle(angle) {}
Quaternion AxisAngle::ToQuaternion() const {
return {
axis.x * std::sin(angle/2),
axis.y * std::sin(angle/2),
axis.z * std::sin(angle/2),
std::cos(angle/2)
axis.x * std::sin(angle / 2),
axis.y * std::sin(angle / 2),
axis.z * std::sin(angle / 2),
std::cos(angle / 2)
};
}
@@ -22,40 +22,7 @@ namespace J3ML::LinearAlgebra {
auto az = q.z / std::sin(std::acos(theta));
}
AxisAngle::AxisAngle(const EulerAngle &e) {
// Assuming the angles are in radians
float heading = e.pitch;
float attitude = e.yaw;
float bank = e.roll;
float c1 = std::cos(heading / 2.f);
float s1 = std::sin(heading / 2.f);
float c2 = std::cos(attitude / 2.f);
float s2 = std::sin(attitude / 2.f);
float c3 = std::cos(bank / 2.f);
float s3 = std::sin(bank / 2.f);
float w = c1*c2*c3 - s1*s2*s3;
float x = c1*c2*c3 + s1*s2*s3;
float y = s1*c2*c3 + c1*s2*s3;
float z = c1*s2*c3 - s1*c2*s3;
angle = 2.f * std::acos(w);
double norm = x*x + y*y + z*z;
if (norm < 0.001) { // when all euler angles are zero angle=0, so
// we can set axis to anything to avoid divide by zero
x = 1;
y = z = 0;
} else {
norm = std::sqrt(norm);
x /= norm;
y /= norm;
z /= norm;
}
axis = {x, y, z};
EulerAngleXYZ AxisAngle::ToEulerAngleXYZ() const {
return EulerAngleXYZ(*this);
}
}

View File

@@ -1,147 +1,35 @@
#include <J3ML/LinearAlgebra/EulerAngle.hpp>
#include <cmath>
#include <algorithm>
#include <iostream>
namespace J3ML::LinearAlgebra {
EulerAngle::EulerAngle(float pitch, float yaw, float roll): pitch(pitch), yaw(yaw), roll(roll)
{}
float EulerAngle::GetPitch(float pitch_limit) const
{ return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
float EulerAngle::GetYaw(float yaw_limit) const
{ return std::clamp(std::remainderf(yaw, 360.f), -yaw_limit, yaw_limit); }
float EulerAngle::GetRoll(float pitch_limit) const
{ return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
bool EulerAngle::operator==(const EulerAngle& a) const
{
return (pitch == a.pitch) && (yaw == a.yaw) && (roll == a.roll);
EulerAngleXYZ::EulerAngleXYZ(float roll, float pitch, float yaw) {
this->roll = roll;
this->pitch = pitch;
this->yaw = yaw;
}
void EulerAngle::clamp()
{
if (this->pitch > 89.0f)
this->pitch = 89.0f;
if (this->pitch <= -89.0f)
this->pitch = -89.0f;
//TODO: Make this entirely seamless by getting the amount they rotated passed -180 and +180 by.
if (this->yaw <= -180.0f)
this->yaw = 180.0f;
if (this->yaw >= 180.01f)
this->yaw = -179.9f;
if (this->roll >= 360.0f)
this->roll = 0.0;
if (this->roll <= -360.0f)
this->roll = 0.0;
EulerAngleXYZ::EulerAngleXYZ(const AxisAngle& rhs) {
}
EulerAngle EulerAngle::movementAngle() const
{
EulerAngle a;
a.pitch = (cos(Math::Radians(yaw)) * cos(Math::Radians(pitch)));
a.yaw = -sin(Math::Radians(pitch));
a.roll = (sin(Math::Radians(yaw)) * cos(Math::Radians(pitch)));
return a;
}
EulerAngleXYZ::EulerAngleXYZ(const Quaternion& q) {
float sy = 2 * q.x * q.z + 2 * q.y * q.w;
bool gimbal_lock = std::abs(sy) > 0.99999f;
EulerAngle::EulerAngle() : pitch(0), yaw(0), roll(0) {}
if (!gimbal_lock)
roll = Math::Degrees(std::atan2(-(2 * q.y * q.z - 2 * q.x * q.w),2 * q.w * q.w + 2 * q.z * q.z - 1));
else
roll = Math::Degrees(std::atan2(2 * q.y * q.z + 2 * q.x * q.w,2 * q.w * q.w + 2 * q.y * q.y - 1));
EulerAngle::EulerAngle(const AxisAngle &rhs) {
pitch = Math::Degrees(std::asin(sy));
float x = rhs.axis.x;
float y = rhs.axis.y;
float z = rhs.axis.z;
float angle = rhs.angle;
double s = std::sin(rhs.angle);
double c = std::cos(rhs.angle);
double t = 1-c;
// if axis is not already normalized then uncomment this
// double magnitude = std::sqrt(x*x + y*y + z*z);
// if (magnitude == 0) throw error;
// x /= magnitude;
// y /= magnitude;
// z /= magnitude;
if ((x*y*t + z*s) > 0.998) { // North pole singularity detected
pitch = 2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
yaw = Math::Pi / 2.f;
roll = 0;
return;
}
if ((x*y*t + z*s) < -0.998) { // South pole singularity detected
pitch = -2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
yaw = -Math::Pi / 2.f;
roll = 0;
return;
}
pitch = std::atan2(y * s-x * z * t, 1 - (y*y + z*z) * t);
yaw = std::asin(x * y * t + z * s);
roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
}
AxisAngle EulerAngle::ToAxisAngle() const {
auto c1 = std::cos(yaw / 2);
auto c2 = std::cos(pitch / 2);
auto c3 = std::cos(roll / 2);
auto s1 = std::sin(yaw / 2);
auto s2 = std::sin(pitch / 2);
auto s3 = std::sin(roll / 2);
auto angle = 2 * std::acos(c1*c2*c3 - s1*s2*s3);
auto x = s1*s2*c3 + c1*c2*s3;
auto y = s1*c2*c3 + c1*s2*s3;
auto z = c1*s2*c3 - s1*c2*s3;
// todo: normalize?
// sqrt(x^2 + y^2 + z^2) = sqrt((s1 s2 c3 +c1 c2 s3)^2+(s1 c2 c3 + c1 s2 s3)^2+(c1 s2 c3 - s1 c2 s3)^2)
return {{x,y,z}, angle};
}
Quaternion EulerAngle::ToQuaternion() const {
auto c1 = std::cos(yaw / 2);
auto c2 = std::cos(pitch / 2);
auto c3 = std::cos(roll / 2);
auto s1 = std::sin(yaw / 2);
auto s2 = std::sin(pitch / 2);
auto s3 = std::sin(roll / 2);
auto w = c1*c2*c3 - s1*s2*s3;
auto x = s1*s2*c3 + c1*c2*s3;
auto y = s1*c2*c3 + c1*s2*s3;
auto z = c1*s2*c3 - s1*c2*s3;
return {w,x,y,z};
}
EulerAngle::EulerAngle(const Quaternion &rhs) {
double test = rhs.x * rhs.y + rhs.z * rhs.w;
if (test > 0.499) { // Singularity at north pole
pitch = 2 * std::atan2(rhs.x, rhs.w);
yaw = Math::Pi / 2.f;
roll = 0;
return;
}
if (test < -0.499) { // Singularity at south pole
pitch = -2 * std::atan2(rhs.x, rhs.y);
yaw = - Math::Pi / 2.f;
roll = 0;
return;
}
float sqx = rhs.x * rhs.x;
float sqy = rhs.y * rhs.y;
float sqz = rhs.z * rhs.z;
if (!gimbal_lock)
yaw = Math::Degrees(std::atan2(-(2 * q.x * q.y - 2 * q.z * q.w),2 * q.w * q.w + 2 * q.x * q.x - 1));
else
yaw = 0;
}
}

View File

@@ -113,25 +113,6 @@ namespace J3ML::LinearAlgebra {
SetRotatePart(orientation);
}
Matrix3x3::Matrix3x3(const EulerAngle &orientation) {
auto sa = std::sin(orientation.pitch);
auto ca = std::cos(orientation.pitch);
auto sb = std::sin(orientation.roll);
auto cb = std::cos(orientation.roll);
auto sh = std::sin(orientation.yaw);
auto ch = std::cos(orientation.yaw);
At(0, 0) = ch*ca;
At(0, 1) = -ch*sa*cb + sh*sh;
At(0, 2) = ch*sa*sb + sh*cb;
At(1, 0) = sa;
At(1, 1) = ca*cb;
At(1, 2) = -ca*cb;
At(2, 0) = -sh*ca;
At(2, 1) = sh*sa*cb + ch*sb;
At(2, 2) = -sh*sa*sb + ch*cb;
}
float Matrix3x3::Determinant() const {
const float a = elems[0][0];
const float b = elems[0][1];
@@ -228,7 +209,7 @@ namespace J3ML::LinearAlgebra {
};
}
void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
void Matrix3x3::SetRotatePart(const Vector3& a, float angle) {
float s = std::sin(angle);
float c = std::cos(angle);
@@ -998,19 +979,6 @@ namespace J3ML::LinearAlgebra {
return false;
}
EulerAngle Matrix3x3::ToEulerAngle() const {
auto heading = std::atan2(-At(2, 0), At(0, 0));
auto attitude = std::asin(At(1, 0));
auto bank = std::atan2(-At(1,2), At(1,1));
if (At(1, 0) == 1 || At(1, 0) == -1) // North Pole || South Pole
{
heading = std::atan2(At(0, 2), At(2,2));
bank = 0;
}
return {attitude, heading, bank};
}
void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints, int stride) const {
assert(pointArray || numPoints == 0);
assert(stride >= (int)sizeof(Vector3));

View File

@@ -86,10 +86,6 @@ namespace J3ML::LinearAlgebra {
Set3x3Part(Matrix3x3(orientation));
}
Matrix4x4::Matrix4x4(const EulerAngle &orientation) {
Set3x3Part(Matrix3x3(orientation));
}
void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
elems[0][3] = translateX;
elems[1][3] = translateY;
@@ -777,19 +773,6 @@ namespace J3ML::LinearAlgebra {
};
}
EulerAngle Matrix4x4::ToEulerAngle() const {
auto heading = std::atan2(-At(2, 0), At(0, 0));
auto attitude = std::asin(At(1, 0));
auto bank = std::atan2(-At(1,2), At(1,1));
if (At(1, 0) == 1 || At(1, 0) == -1) // North Pole || South Pole
{
heading = std::atan2(At(0, 2), At(2,2));
bank = 0;
}
return {attitude, heading, bank};
}
bool Matrix4x4::InverseOrthogonalUniformScale() {
assert(!ContainsProjection());
assert(IsColOrthogonal(1e-3f));

View File

@@ -228,21 +228,6 @@ namespace J3ML::LinearAlgebra {
w = std::cos(angle.angle / 2);
}
Quaternion::Quaternion(const EulerAngle &angle) {
// Abbreviations for the various angular functions
double cr = std::cos(angle.roll * 0.5);
double sr = std::sin(angle.roll * 0.5);
double cp = std::cos(angle.pitch * 0.5);
double sp = std::sin(angle.pitch * 0.5);
double cy = std::cos(angle.yaw * 0.5);
double sy = std::sin(angle.yaw * 0.5);
w = cr * cp * cy + sr * sp * sy;
x = sr * cp * cy - cr * sp * sy;
y = cr * sp * cy + sr * cp * sy;
z = cr * cp * sy - sr * sp * cy;
}
void Quaternion::SetFrom(const AxisAngle &angle) {
double s = std::sin(angle.angle / 2);
x = angle.axis.x * s;
@@ -251,10 +236,6 @@ namespace J3ML::LinearAlgebra {
w = std::cos(angle.angle / 2);
}
EulerAngle Quaternion::ToEulerAngle() const {
return EulerAngle(*this);
}
Quaternion Quaternion::RandomRotation(RNG &rng) {
// Generate a random point on the 4D unitary hypersphere using the rejection sampling method.
for (int i = 0; i < 1000; ++i)

View File

@@ -0,0 +1,17 @@
#include <jtest/jtest.hpp>
#include <jtest/Unit.hpp>
jtest::Unit AxisAngleUnit {"AxisAngle"};
namespace AxisAngleTests {
inline void Define() {
using namespace jtest;
AxisAngleUnit += Test("Not Implemented", [] {
throw("Not Implemented");
});
}
inline void Run() {
AxisAngleUnit.RunAll();
}
}

View File

@@ -5,14 +5,21 @@
#include <jtest/jtest.hpp>
#include <jtest/Unit.hpp>
jtest::Unit EulerAngleUnit {"EulerAngle"};
jtest::Unit EulerAngleUnit {"EulerAngle_XYZ"};
namespace EulerAngleTests {
inline void Define() {
using namespace jtest;
EulerAngleUnit += Test("Not Implemented", [] {
throw("Not Implemented");
EulerAngleUnit += Test("From_Quaternion", [] {
EulerAngleXYZ expected_result(-170, 88, -160);
Quaternion q(0.1840604, 0.6952024, 0.1819093, 0.6706149);
EulerAngleXYZ from_quaternion(q);
jtest::check(Math::EqualAbs(Math::Radians(expected_result.roll), Math::Radians(from_quaternion.roll), 1e-5f));
jtest::check(Math::EqualAbs(Math::Radians(expected_result.pitch), Math::Radians(from_quaternion.pitch), 1e-5f));
jtest::check(Math::EqualAbs(Math::Radians(expected_result.yaw), Math::Radians(from_quaternion.yaw), 1e-5f));
});
}
inline void Run() {

View File

@@ -112,18 +112,15 @@ namespace Matrix4x4Tests {
Matrix4x4Unit += Test("DeterminantCorrectness", [] { });
Matrix4x4Unit += Test("MulMat3x3", [] {});
/*
Matrix4x4Unit += Test("AngleTypeRoundtripConversion", [] {
Matrix4x4 matrix;
EulerAngle a(Math::Radians(45), Math::Radians(45), Math::Radians(45));
Quaternion q(a);
matrix.SetRotatePart(q);
EulerAngleXYZ a(Math::Radians(45), Math::Radians(45), Math::Radians(45));
//matrix.SetRotatePartX(a.pitch);
//matrix.SetRotatePartY(a.yaw);
//matrix.SetRotatePartZ(a.roll);
EulerAngle fromMatrix = matrix.GetRotatePart().ToQuat().ToEulerAngle();
jtest::check(a == fromMatrix);
});
*/
}

View File

@@ -16,6 +16,7 @@
#include "Geometry/FrustumTests.hpp"
#include "LinearAlgebra/EulerAngleTests.hpp"
#include "LinearAlgebra/AxisAngleTests.hpp"
#include "LinearAlgebra/Matrix2x2Tests.hpp"
#include "LinearAlgebra/Matrix3x3Tests.hpp"
#include "LinearAlgebra/Matrix4x4Tests.hpp"
@@ -64,10 +65,11 @@ namespace LinearAlgebraTests
{
void Define()
{
EulerAngleTests::Define();
Vector2Tests::Define();
Vector3Tests::Define();
Vector4Tests::Define();
AxisAngleTests::Define();
EulerAngleTests::Define();
QuaternionTests::Define();
Matrix2x2Tests::Define();
Matrix3x3Tests::Define();
@@ -76,10 +78,11 @@ namespace LinearAlgebraTests
}
void Run()
{
EulerAngleTests::Run();
Vector2Tests::Run();
Vector3Tests::Run();
Vector4Tests::Run();
AxisAngleTests::Run();
EulerAngleTests::Run();
QuaternionTests::Run();
Matrix2x2Tests::Run();
Matrix3x3Tests::Run();