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

Implement Matrix3x3::ToEulerAngle and Matrix3x3::ctor(EulerAngle)

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

6
include/J3ML/LinearAlgebra/Matrix3x3.h
View File

@@ -5,6 +5,7 @@
#include <J3ML/LinearAlgebra/Vector2.h>
#include <J3ML/LinearAlgebra/Vector3.h>
#include <J3ML/LinearAlgebra/Quaternion.h>
#include <J3ML/LinearAlgebra/EulerAngle.h>
#include <J3ML/Algorithm/RNG.h>
using namespace J3ML::Algorithm;
@@ -61,6 +62,9 @@ namespace J3ML::LinearAlgebra {
Matrix3x3(const Vector3& col0, const Vector3& col1, const Vector3& col2);
/// 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);
/// 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.
@@ -243,6 +247,8 @@ 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]

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

@@ -113,6 +113,25 @@ 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];
@@ -979,6 +998,19 @@ 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));