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

Implement Matrix4x4::ToEulerAngle Matrix4x4::ctor(EulerAngle) Matrix4x4::ctor(Quaternion) Matrix4x4::ToQuat()

Browse Source
This commit is contained in:
josh
2024-07-01 14:23:03 -04:00
parent 552715f443
commit ea40c40725
2 changed files with 44 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
include/J3ML/LinearAlgebra/Matrix4x4.h
View File

@@ -73,6 +73,8 @@ 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.
@@ -548,6 +550,9 @@ namespace J3ML::LinearAlgebra {
/// Returns the sum of the diagonal elements of this matrix.
float Trace() const;
[[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;

39
src/J3ML/LinearAlgebra/Matrix4x4.cpp
View File

@@ -82,7 +82,12 @@ namespace J3ML::LinearAlgebra {
}
Matrix4x4::Matrix4x4(const Quaternion &orientation) {
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
Set3x3Part(Matrix3x3(orientation));
}
Matrix4x4::Matrix4x4(const EulerAngle &orientation) {
Set3x3Part(Matrix3x3(orientation));
}
void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
@@ -738,6 +743,40 @@ namespace J3ML::LinearAlgebra {
return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
}
Quaternion Matrix4x4::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
};
}
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));