josh/j3ml
1
0
Fork 1
Code Issues 12 Pull Requests Actions Packages Projects Releases 41 Wiki Activity

RoundTrip angle conversion.
All checks were successful
Run ReCI Build Test / Explore-Gitea-Actions (push) Successful in 1m23s
Details
Build Docs With Doxygen / Explore-Gitea-Actions (push) Successful in 31s
Details

Browse Source
This commit is contained in:
Redacted
2024-11-19 15:24:30 -05:00
parent aaea5ff53e
commit 79e617b780
9 changed files with 111 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
include/J3ML/LinearAlgebra/AxisAngle.hpp
View File

@@ -18,8 +18,7 @@ public:
public:
AxisAngle();
explicit AxisAngle(const Quaternion& q);
explicit AxisAngle(const EulerAngleXYZ& e);
AxisAngle(const Vector3& axis, float angle);
public:
[[nodiscard]] Quaternion ToQuaternion() const;
[[nodiscard]] EulerAngleXYZ ToEulerAngleXYZ() const;
};

1
include/J3ML/LinearAlgebra/EulerAngle.hpp
View File

@@ -19,4 +19,5 @@ public:
public:
explicit EulerAngleXYZ(const Quaternion& rhs);
explicit EulerAngleXYZ(const AxisAngle& rhs);
explicit EulerAngleXYZ(const Matrix3x3& rhs);
};

8
include/J3ML/LinearAlgebra/Matrix3x3.hpp
View File

@@ -63,8 +63,11 @@ 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 EulerAngleXYZ& orientation);
explicit Matrix3x3(const AxisAngle& orientation);
/// Constructs this Matrix3x3 from a pointer to an array of floats.
explicit Matrix3x3(const float *data);
/// Creates a new Matrix3x3 that rotates about one of the principal axes by the given angle.
@@ -153,6 +156,7 @@ namespace J3ML::LinearAlgebra {
/// Sets this matrix to perform a rotation about the given axis and angle.
void SetRotatePart(const Vector3& a, float angle);
void SetRotatePart(const AxisAngle& axisAngle);
/// Sets this matrix to perform the rotation expressed by the given quaternion.
void SetRotatePart(const Quaternion& quat);
@@ -239,10 +243,6 @@ namespace J3ML::LinearAlgebra {
inline float* ptr() { return &elems[0][0];}
[[nodiscard]] inline const float* ptr() const {return &elems[0][0];}
/// Convers this rotation matrix to a quaternion.
/// This function assumes that the matrix is orthonormal (no shear or scaling) and does not perform any mirroring (determinant > 0)
[[nodiscard]] Quaternion ToQuat() const;
/// Attempts to convert this matrix to a quaternion. Returns false if the conversion cannot succeed (this matrix was not a rotation
/// matrix, and there is scaling ,shearing, or mirroring in this matrix)
bool TryConvertToQuat(Quaternion& q) const;

14
src/J3ML/LinearAlgebra/AxisAngle.cpp
View File

@@ -6,14 +6,6 @@ namespace J3ML::LinearAlgebra {
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)
};
}
AxisAngle::AxisAngle(const Quaternion& rhs) {
float halfAngle = std::acos(rhs.w);
@@ -23,7 +15,9 @@ namespace J3ML::LinearAlgebra {
axis = { rhs.x*reciprocalSinAngle, rhs.y*reciprocalSinAngle, rhs.z*reciprocalSinAngle };
}
EulerAngleXYZ AxisAngle::ToEulerAngleXYZ() const {
return EulerAngleXYZ(*this);
AxisAngle::AxisAngle(const EulerAngleXYZ& e) {
auto a = AxisAngle(Quaternion(e));
axis = a.axis;
angle = a.angle;
}
}

14
src/J3ML/LinearAlgebra/EulerAngle.cpp
View File

@@ -1,7 +1,7 @@
#include <J3ML/LinearAlgebra/EulerAngle.hpp>
#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
#include <cmath>
#include <algorithm>
#include <iostream>
namespace J3ML::LinearAlgebra {
@@ -13,7 +13,7 @@ namespace J3ML::LinearAlgebra {
}
EulerAngleXYZ::EulerAngleXYZ(const AxisAngle& rhs) {
*this = EulerAngleXYZ(Quaternion(rhs));
}
EulerAngleXYZ::EulerAngleXYZ(const Quaternion& q) {
@@ -32,4 +32,14 @@ namespace J3ML::LinearAlgebra {
else
yaw = 0;
}
EulerAngleXYZ::EulerAngleXYZ(const Matrix3x3& rhs) {
auto m = rhs.Transposed();
auto sy = m.At(0, 2);
auto unlocked = std::abs(sy) < 0.99999f;
roll = Math::Degrees(unlocked ? std::atan2(-m.At(1, 2), m.At(2, 2)) : std::atan2(m.At(2, 1), m.At(1, 1)));
pitch = Math::Degrees(std::asin(sy));
yaw = Math::Degrees(unlocked ? std::atan2(-m.At(0, 1), m.At(0, 0)) : 0);
}
}

50
src/J3ML/LinearAlgebra/Matrix3x3.cpp
View File

@@ -109,8 +109,8 @@ namespace J3ML::LinearAlgebra {
//this->elems[2][2] = r3.z;
}
Matrix3x3::Matrix3x3(const Quaternion &orientation) {
SetRotatePart(orientation);
Matrix3x3::Matrix3x3(const Quaternion& orientation) {
*this = Matrix3x3(EulerAngleXYZ(orientation));
}
float Matrix3x3::Determinant() const {
@@ -188,27 +188,6 @@ namespace J3ML::LinearAlgebra {
};
}
Quaternion Matrix3x3::ToQuat() const {
auto m00 = At(0,0);
auto m01 = At(0, 1);
auto m02 = At(0, 2);
auto m10 = At(1,0);
auto m11 = At(1, 1);
auto m12 = At(1, 2);
auto m20 = At(2,0);
auto m21 = At(2, 1);
auto m22 = At(2, 2);
auto w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
float w4 = (4.f * w);
return {
(m21 - m12) / w4,
(m02 - m20) / w4,
(m10 - m01) / w4,
w
};
}
void Matrix3x3::SetRotatePart(const Vector3& a, float angle) {
float s = std::sin(angle);
float c = std::cos(angle);
@@ -352,9 +331,9 @@ namespace J3ML::LinearAlgebra {
return m;
}
Matrix3x3 Matrix3x3::FromScale(float sx, float sy, float sz) {
Matrix3x3 Matrix3x3::FromScale(float sin_roll, float sy, float sz) {
Matrix3x3 m;
m.At(0,0) = sx;
m.At(0,0) = sin_roll;
m.At(1,1) = sy;
m.At(2,2) = sz;
return m;
@@ -973,7 +952,7 @@ namespace J3ML::LinearAlgebra {
bool Matrix3x3::TryConvertToQuat(Quaternion &q) const {
if (IsColOrthogonal() && HasUnitaryScale() && !HasNegativeScale()) {
q = ToQuat();
q = Quaternion(*this);
return true;
}
return false;
@@ -1087,6 +1066,25 @@ namespace J3ML::LinearAlgebra {
return m;
}
Matrix3x3::Matrix3x3(const EulerAngleXYZ& e) {
float cos_roll = std::cos(Math::Radians(e.roll));
float sin_roll = std::sin(Math::Radians(e.roll));
float cos_pitch = std::cos(Math::Radians(e.pitch));
float sin_pitch = std::sin(Math::Radians(e.pitch));
float cos_yaw = std::cos(Math::Radians(e.yaw));
float sin_yaw = std::sin(Math::Radians(e.yaw));
Matrix3x3 m;
m.SetRow(0, Vector3(cos_pitch * cos_yaw, sin_roll * sin_pitch *cos_yaw + cos_roll * sin_yaw, -cos_roll * sin_pitch *cos_yaw + sin_roll * sin_yaw));
m.SetRow(1, Vector3(-cos_pitch * sin_yaw, -sin_roll * sin_pitch * sin_yaw + cos_roll *cos_yaw, cos_roll * sin_pitch * sin_yaw + sin_roll *cos_yaw));
m.SetRow(2, Vector3(sin_pitch, -sin_roll * cos_pitch, cos_roll * cos_pitch));
*this = m;
}
Matrix3x3::Matrix3x3(const AxisAngle& orientation) {
*this = Matrix3x3(Quaternion(orientation));
}
}

31
src/J3ML/LinearAlgebra/Quaternion.cpp
View File

@@ -15,9 +15,32 @@ namespace J3ML::LinearAlgebra {
return {-x, -y, -z, -w};
}
Quaternion::Quaternion(const Matrix3x3 &rotationMtrx) {}
Quaternion::Quaternion(const Matrix3x3& ro_mat) {
auto m = ro_mat.Transposed();
auto m00 = m.At(0,0);
auto m01 = m.At(0, 1);
auto m02 = m.At(0, 2);
auto m10 = m.At(1,0);
auto m11 = m.At(1, 1);
auto m12 = m.At(1, 2);
auto m20 = m.At(2,0);
auto m21 = m.At(2, 1);
auto m22 = m.At(2, 2);
Quaternion::Quaternion(const Matrix4x4 &rotationMtrx) {}
auto field_w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
float w4 = (4.f * field_w);
x = (m21 - m12) / w4;
y = (m02 - m20) / w4;
z = (m10 - m01) / w4;
w = field_w;
Normalize();
}
Quaternion::Quaternion(const Matrix4x4& ro_mat) {
auto q = Quaternion(ro_mat.GetRotatePart());
x = q.x; y = q.y; z = q.z; w = q.w;
}
Vector3 Quaternion::WorldX() const { return Transform(1.f, 0.f, 0.f); }
@@ -186,6 +209,7 @@ namespace J3ML::LinearAlgebra {
y = angle.axis.y * s;
z = angle.axis.z * s;
w = std::cos(angle.angle / 2);
Normalize();
}
Quaternion Quaternion::RandomRotation(RNG &rng) {
@@ -258,7 +282,7 @@ namespace J3ML::LinearAlgebra {
Quaternion Quaternion::LookAt(const Vector3 &localForward, const Vector3 &targetDirection, const Vector3 &localUp,
const Vector3 &worldUp) {
return Matrix3x3::LookAt(localForward, targetDirection, localUp, worldUp).ToQuat();
return Quaternion(Matrix3x3::LookAt(localForward, targetDirection, localUp, worldUp));
}
/*
@@ -343,6 +367,7 @@ namespace J3ML::LinearAlgebra {
y = -sin_roll * cos_pitch * sin_yaw + cos_roll * sin_pitch * cos_yaw;
z = cos_roll * cos_pitch * sin_yaw + sin_roll * sin_pitch * cos_yaw;
w = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_pitch * cos_yaw;
Normalize();
}
Quaternion::Quaternion(const Quaternion& rhs) {

36
tests/LinearAlgebra/Matrix3x3Tests.hpp
View File

@@ -11,6 +11,42 @@ namespace Matrix3x3Tests
using namespace jtest;
using namespace J3ML::LinearAlgebra;
Matrix3x3Unit += Test("AngleTypeRound-TripConversion", [] {
EulerAngleXYZ expected_result(8, 60, -27);
Matrix3x3 m(expected_result);
AxisAngle a(expected_result);
Quaternion q(a);
Matrix3x3 m2(q);
Quaternion q2(m2);
AxisAngle a2(q2);
EulerAngleXYZ round_trip(a2);
jtest::check(Math::EqualAbs(Math::Radians(expected_result.roll), Math::Radians(round_trip.roll), 1e-6f));
jtest::check(Math::EqualAbs(Math::Radians(expected_result.pitch), Math::Radians(round_trip.pitch), 1e-6f));
jtest::check(Math::EqualAbs(Math::Radians(expected_result.yaw), Math::Radians(round_trip.yaw), 1e-6f));
});
Matrix3x3Unit += Test("From_EulerAngleXYZ", []{
Matrix3x3 expected_result(Vector3(0.4455033, 0.2269952, 0.8660254),
Vector3(-0.3421816, 0.9370536, -0.0695866),
Vector3(-0.8273081, -0.2653369, 0.4951340)
);
EulerAngleXYZ e(8, 60, -27);
Matrix3x3 from_euler(e);
jtest::check(Math::EqualAbs(expected_result.At(0, 0), from_euler.At(0, 0), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(0, 1), from_euler.At(0, 1), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(0, 2), from_euler.At(0, 2), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(1, 0), from_euler.At(1, 0), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(1, 1), from_euler.At(1, 1), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(1, 2), from_euler.At(1, 2), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(2, 0), from_euler.At(2, 0), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(2, 1), from_euler.At(2, 1), 1e-6f));
jtest::check(Math::EqualAbs(expected_result.At(2, 2), from_euler.At(2, 2), 1e-6f));
});
Matrix3x3Unit += Test("Add_Unary", []
{
Matrix3x3 m(1,2,3, 4,5,6, 7,8,9);

11
tests/LinearAlgebra/Matrix4x4Tests.hpp
View File

@@ -111,17 +111,6 @@ namespace Matrix4x4Tests {
Matrix4x4Unit += Test("InverseOrthonormal", [] {});
Matrix4x4Unit += Test("DeterminantCorrectness", [] { });
Matrix4x4Unit += Test("MulMat3x3", [] {});
/*
Matrix4x4Unit += Test("AngleTypeRoundtripConversion", [] {
Matrix4x4 matrix;
EulerAngleXYZ a(Math::Radians(45), Math::Radians(45), Math::Radians(45));
//matrix.SetRotatePartX(a.pitch);
//matrix.SetRotatePartY(a.yaw);
//matrix.SetRotatePartZ(a.roll);
});
*/
}
inline void Run() {