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

Implement several missing Matrix3x3 member functions
Some checks failed
Run tests / Explore-Gitea-Actions (push) Has been cancelled
Details
Build Docs With Doxygen / Explore-Gitea-Actions (push) Has been cancelled
Details

Browse Source
This commit is contained in:
josh
2024-05-29 13:16:08 -04:00
parent 5e253e0b2c
commit b4cba4cac3
3 changed files with 282 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,7 +5,7 @@
#include <J3ML/LinearAlgebra/Vector2.h>
#include <J3ML/LinearAlgebra/Vector3.h>
#include <J3ML/LinearAlgebra/Quaternion.h>
#include <J3ML/LinearAlgebra/Matrices.inl>
#include <cstring>
namespace J3ML::LinearAlgebra {
@@ -55,10 +55,11 @@ namespace J3ML::LinearAlgebra {
static const Matrix3x3 NaN;
public: /// Constructors
/// Creates a new Matrix3x3 with uninitalized member values.
Matrix3x3() {}
Matrix3x3() = default;
Matrix3x3(const Matrix3x3& rhs) { Set(rhs); }
Matrix3x3(float val);
/// Creates a new Matrix3x3 by setting all matrix elements equal to val.
explicit Matrix3x3(float val);
/// Creates a new Matrix3x3 by explicitly specifying all the matrix elements.
/// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
/// E.g. The element m10 denotes the scalar at second (idx 1) row, first (idx 0) column.
@@ -152,10 +153,7 @@ namespace J3ML::LinearAlgebra {
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)
{
SetMatrixRotatePart(*this, quat);
}
void SetRotatePart(const Quaternion& quat);
/// Returns the given row.
/** @param row The zero-based index [0, 2] of the row to get. */
@@ -172,16 +170,16 @@ namespace J3ML::LinearAlgebra {
/// Returns the given column.
/** @param col The zero-based index [0, 2] of the column to get. */
Vector3 GetColumn(int index) const;
Vector3 Column(int index) const;
Vector3 Col(int index) const;
[[nodiscard]] Vector3 GetColumn(int index) const;
[[nodiscard]] Vector3 Column(int index) const;
[[nodiscard]] Vector3 Col(int index) const;
/// This method also allows assignment to the retrieved column.
//Vector3& Col(int index);
/// Returns only the first three elements of the given column.
Vector3 GetColumn3(int index) const;
Vector3 Column3(int index) const;
Vector3 Col3(int index) const;
[[nodiscard]] Vector3 GetColumn3(int index) const;
[[nodiscard]] Vector3 Column3(int index) const;
[[nodiscard]] Vector3 Col3(int index) const;
/// This method also allows assignment to the retrieved column.
//Vector3& Col3(int index);
@@ -210,12 +208,14 @@ namespace J3ML::LinearAlgebra {
/// Sets this matrix to equal the identity.
void SetIdentity();
/// Swaps two columns.
void SwapColumns(int col1, int col2);
/// Swaps two rows.
void SwapRows(int row1, int row2);
float &At(int row, int col);
float At(int x, int y) const;
[[nodiscard]] float At(int x, int y) const;
/// Sets this to be a copy of the matrix rhs.
void Set(const Matrix3x3 &rhs);
@@ -231,10 +231,16 @@ namespace J3ML::LinearAlgebra {
/// Orthonormalizes the basis formed by the column vectors of this matrix.
void Orthonormalize(int c0, int c1, int c2);
/// Accesses this structure as a float array.
/// @return A pointer to the upper-left element. The data is contiguous in memory.
/// ptr[0] gives the element [0][0], ptr[1] is [0][1]. ptr[2] is [0][2]
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)
Quaternion ToQuat() const;
[[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;
@@ -242,16 +248,16 @@ namespace J3ML::LinearAlgebra {
/// Returns the main diagonal.
/// The main diagonal consists of the elements at m[0][0], m[1][1], m[2][2]
Vector3 Diagonal() const;
[[nodiscard]] Vector3 Diagonal() const;
/// Returns the local +X/+Y/+Z axis in world space.
/// This is the same as transforming the vector{1,0,0} by this matrix.
Vector3 WorldX() const;
[[nodiscard]] Vector3 WorldX() const;
/// Returns the local +Y axis in world space.
/// This is the same as transforming the vector{0,1,0} by this matrix.
Vector3 WorldY() const;
[[nodiscard]] Vector3 WorldY() const;
/// Returns the local +Z axis in world space.
/// This is the same as transforming the vector{0,0,1} by this matrix.
Vector3 WorldZ() const;
[[nodiscard]] Vector3 WorldZ() const;
/// Computes the determinant of this matrix.
// If the determinant is nonzero, this matrix is invertible.
@@ -260,24 +266,24 @@ namespace J3ML::LinearAlgebra {
// "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
// If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
// @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
float Determinant() const;
[[nodiscard]] float Determinant() const;
/// Computes the determinant of a symmetric matrix.
/** This function can be used to compute the determinant of a matrix in the case the matrix is known beforehand
to be symmetric. This function is slightly faster than Determinant().
* @return
*/
float DeterminantSymmetric() const;
[[nodiscard]] float DeterminantSymmetric() const;
// Returns an inverted copy of this matrix.
Matrix3x3 Inverted() const;
[[nodiscard]] Matrix3x3 Inverted() const;
// Returns a transposed copy of this matrix.
Matrix3x3 Transposed() const;
[[nodiscard]] Matrix3x3 Transposed() const;
/// Returns the inverse transpose of this matrix.
/// Use the matrix to transform covariant vectors (normal vectors).
Matrix3x3 InverseTransposed() const;
[[nodiscard]] Matrix3x3 InverseTransposed() const;
/// Computes the inverse transpose of this matrix.
/// Use the matrix to transform covariant vectors (normal vectors).
@@ -316,6 +322,11 @@ namespace J3ML::LinearAlgebra {
This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.*/
bool InverseOrthogonalUniformScale();
/// Inverts a rotation matrix.
/// If a matrix is of form M = R*S, where R is a rotation matrix and S is either identity or a mirroring matrix, then
/// the matrix M is orthonormal and this function can be used to compute the inverse.
/// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverse()
/// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
void InverseOrthonormal();
/// Inverts a symmetric matrix.
@@ -331,27 +342,34 @@ namespace J3ML::LinearAlgebra {
void Transpose();
/// Removes the scaling performed by this matrix. That is, decomposes this matrix M into a form M = M' * S, where
/// M' has unitary column vectors and S is a diagonal matrix. Then replaces this matrix with M'
/// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
/// @note This function does not remove reflection (-1 scale along some axis).
void RemoveScale();
// Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)
Vector2 Transform(const Vector2& rhs) const;
Vector3 Transform(const Vector3& rhs) const;
[[nodiscard]] Vector2 Transform(const Vector2& rhs) const;
[[nodiscard]] Vector3 Transform(const Vector3& rhs) const;
/// Performs a batch transformation of the given array.
void BatchTransform(Vector3 *pointArray, int numPoints) const;
/// Performs a batch transformation of the given array.
void BatchTransform(Vector3 *pointArray, int numPoints, int stride) const;
/// Performs a batch transformation of the given array.
/// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
void BatchTransform(Vector4 *vectorArray, int numVectors) const;
/// Performs a batch transformation of the given array.
/// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
void BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const;
/// Returns the sum of the diagonal elements of this matrix.
float Trace() const;
[[nodiscard]] float Trace() const;
Matrix3x3 ScaleBy(const Vector3& rhs);
Vector3 GetScale() const;
[[nodiscard]] Vector3 GetScale() const;
/// Scales the given row by a scalar value.
void ScaleRow(int row, float scalar);
@@ -367,79 +385,84 @@ namespace J3ML::LinearAlgebra {
/// This function ignores the W component of the given input vector. This component is assumed to be either 0 or 1.
Vector4 operator * (const Vector4& rhs) const;
Matrix3x3 operator * (const Quaternion& rhs) const
{
return *this * Matrix3x3(rhs);
}
/// Multiplies the two matrices.
Matrix3x3 operator * (const Matrix3x3& rhs) const;
Matrix3x3 Mul(const Matrix3x3& rhs) const;
[[nodiscard]] Matrix3x3 Mul(const Matrix3x3& rhs) const;
/// Multiplies the two matrices.
Matrix4x4 operator * (const Matrix4x4& rhs) const;
Matrix4x4 Mul(const Matrix4x4& rhs) const;
[[nodiscard]] Matrix4x4 Mul(const Matrix4x4& rhs) const;
Vector2 Mul(const Vector2& rhs) const;
Vector3 Mul(const Vector3& rhs) const;
Vector4 Mul(const Vector4& rhs) const;
[[nodiscard]] Vector2 Mul(const Vector2& rhs) const;
[[nodiscard]] Vector3 Mul(const Vector3& rhs) const;
[[nodiscard]] Vector4 Mul(const Vector4& rhs) const;
/// Converts the quaternion to a M3x3 and multiplies the two matrices together.
Matrix3x3 operator *(const Quaternion& rhs) const;
Quaternion Mul(const Quaternion& rhs) const;
[[nodiscard]] Matrix3x3 Mul(const Quaternion& rhs) const;
// Returns true if the column vectors of this matrix are all perpendicular to each other.
bool IsColOrthogonal(float epsilon = 1e-3f) const;
bool IsColOrthogonal3(float epsilon = 1e-3f) const { return IsColOrthogonal(epsilon);}
[[nodiscard]] bool IsColOrthogonal(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsColOrthogonal3(float epsilon = 1e-3f) const { return IsColOrthogonal(epsilon);}
// Returns true if the row vectors of this matrix are all perpendicular to each other.
bool IsRowOrthogonal(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsRowOrthogonal(float epsilon = 1e-3f) const;
bool HasUniformScale(float epsilon = 1e-3f) const;
[[nodiscard]] bool HasUniformScale(float epsilon = 1e-3f) const;
Vector3 ExtractScale() const;
[[nodiscard]] Vector3 ExtractScale() const;
/// Tests if this matrix does not contain any NaNs or infs
/// @return Returns true if the entries of this M3x3 are all finite.
bool IsFinite() const;
[[nodiscard]] bool IsFinite() const;
/// Tests if this is the identity matrix.
/// @return Returns true if this matrix is the identity matrix, up to the given epsilon.
bool IsIdentity(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsIdentity(float epsilon = 1e-3f) const;
/// Tests if this matrix is in lower triangular form.
/// @return Returns true if this matrix is in lower triangular form, up to the given epsilon.
bool IsLowerTriangular(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsLowerTriangular(float epsilon = 1e-3f) const;
/// Tests if this matrix is in upper triangular form.
/// @return Returns true if this matrix is in upper triangular form, up to the given epsilon.
bool IsUpperTriangular(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsUpperTriangular(float epsilon = 1e-3f) const;
/// Tests if this matrix has an inverse.
/// @return Returns true if this matrix can be inverted, up to the given epsilon.
bool IsInvertible(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const;
/// Tests if this matrix is symmetric (M == M^T).
/// The test compares the elements for equality. Up to the given epsilon. A matrix is symmetric if it is its own transpose.
bool IsSymmetric(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsSymmetric(float epsilon = 1e-3f) const;
/// Tests if this matrix is skew-symmetric (M == -M^T).
/// The test compares the elements of this matrix up to the given epsilon. A matrix M is skew-symmetric if the identity M=-M^T holds.
bool IsSkewSymmetric(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsSkewSymmetric(float epsilon = 1e-3f) const;
/// Returns true if this matrix does not perform any scaling,
/** A matrix does not do any scaling if the column vectors of this matrix are normalized in length,
compared to the given epsilon. Note that this matrix may still perform reflection,
i.e. it has a -1 scale along some axis.
@note This function only examines the upper 3-by-3 part of this matrix.
@note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0,0,0,1] */
bool HasUnitaryScale(float epsilon = 1e-3f) const;
[[nodiscard]] bool HasUnitaryScale(float epsilon = 1e-3f) const;
/// Returns true if this matrix performs a reflection along some plane.
/** In 3D space, an even number of reflections corresponds to a rotation about some axis, so a matrix consisting of
an odd number of consecutive mirror operations can only reflect about one axis. A matrix that contains reflection reverses
the handedness of the coordinate system. This function tests if this matrix does perform mirroring.
This occurs if this matrix has a negative determinant.*/
bool HasNegativeScale() const;
[[nodiscard]] bool HasNegativeScale() const;
/// Returns true if the column and row vectors of this matrix form an orthonormal set.
/// @note In math terms, there does not exist such a thing as an 'orthonormal matrix'. In math terms, a matrix
/// is orthogonal if the column and row vectors are orthogonal *unit* vectors.
/// In terms of this library however, a matrix is orthogonal if its column and row vectors are orthogonal. (no need to be unitary),
/// and a matrix is orthonormal if the column and row vectors are orthonormal.
bool IsOrthonormal(float epsilon = 1e-3f) const;
[[nodiscard]] bool IsOrthonormal(float epsilon = 1e-3f) const;
protected: /// Member values
float elems[3][3];
float elems[3][3]{};
};
}

1
include/J3ML/LinearAlgebra/Quaternion.h
View File

@@ -5,7 +5,6 @@
#include <J3ML/LinearAlgebra/Matrix4x4.h>
#include <J3ML/LinearAlgebra/Vector4.h>
#include <J3ML/LinearAlgebra/AxisAngle.h>
//#include <J3ML/LinearAlgebra/AxisAngle.h>
#include <cmath>

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

@@ -1,4 +1,5 @@
#include <J3ML/LinearAlgebra/Matrix3x3.h>
#include <J3ML/LinearAlgebra/Matrices.inl>
#include <cmath>
namespace J3ML::LinearAlgebra {
@@ -843,6 +844,212 @@ namespace J3ML::LinearAlgebra {
At(2, 0) = _20; At(2, 1) = _21; At(2, 2) = _22;
}
void Matrix3x3::SetRotatePart(const Quaternion &quat) {
SetMatrixRotatePart(*this, quat);
}
void Matrix3x3::RemoveScale() {
float x = Row(0).Normalize();
float y = Row(1).Normalize();
float z = Row(2).Normalize();
assert(x != 0 && "Matrix3x3::RemoveScale failed!");
assert(y != 0 && "Matrix3x3::RemoveScale failed!");
assert(z != 0 && "Matrix3x3::RemoveScale failed!");
}
float Matrix3x3::Trace() const {
return elems[0][0] + elems[1][1] + elems[2][2];
}
bool Matrix3x3::InverseSymmetric() {
assert(IsSymmetric());
const float a = elems[0][0];
const float b = elems[0][1];
const float c = elems[0][2];
const float d = elems[1][1];
const float e = elems[1][2];
const float f = elems[2][2];
/* If the matrix is symmetric, it is of form
a b c
b d e
c e f
*/
// A direct cofactor expansion gives
// det = a * (df - ee) + b * (ce - bf) + c * (be-dc)
const float df_ee = d*f - e*e;
const float ce_bf = c*e - b*f;
const float be_dc = b*e - d*c;
float det = a * df_ee + b * ce_bf + c * be_dc;
if (Math::EqualAbs(det, 0.f))
return false;
det = 1.f / det;
// The inverse of a symmetric matrix will also be symmetric, so we can avoid some computations altogether.
elems[0][0] = det * df_ee;
elems[1][0] = elems[0][1] = det * ce_bf;
elems[0][2] = elems[2][0] = det * be_dc;
elems[1][1] = det * (a*f - c*c);
elems[1][2] = elems[2][1] = det * (c*b - a*e);
elems[2][2] = det * (a*d - b*b);
return true;
}
void Matrix3x3::InverseOrthonormal() {
Matrix3x3 orig = *this;
assert(IsOrthonormal());
Transpose();
assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
}
bool Matrix3x3::InverseOrthogonalUniformScale() {
Matrix3x3 orig = *this;
assert(IsColOrthogonal());
assert(HasUniformScale());
float scale = Vector3(At(0, 0), At(1, 0), At(2, 0)).LengthSquared();
if (scale < 1e-8f)
return false;
scale = 1.f / scale;
Swap(At(0, 1), At(1, 0));
Swap(At(0, 2), At(2, 0));
Swap(At(1, 2), At(2, 1));
At(0, 0) *= scale; At(0, 1) *= scale; At(0, 2) *= scale;
At(1, 0) *= scale; At(1, 1) *= scale; At(1, 2) *= scale;
At(2, 0) *= scale; At(2, 1) *= scale; At(2, 2) *= scale;
assert(IsFinite());
assert(IsColOrthogonal());
assert(HasUniformScale());
assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
return true;
}
bool Matrix3x3::InverseColOrthogonal() {
Matrix3x3 orig = *this;
assert(IsColOrthogonal());
float s1 = Vector3(At(0, 0), At(1, 0), At(2, 0)).LengthSquared();
float s2 = Vector3(At(0, 1), At(1, 1), At(2, 1)).LengthSquared();
float s3 = Vector3(At(0, 2), At(1, 2), At(2, 2)).LengthSquared();
if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
return false;
s1 = 1.f / s1;
s2 = 1.f / s2;
s3 = 1.f / s3;
Swap(At(0, 1), At(1, 0));
Swap(At(0, 2), At(2, 0));
Swap(At(1, 2), At(2, 1));
At(0, 0) *= s1; At(0, 1) *= s1; At(0, 2) *= s1;
At(1, 0) *= s2; At(1, 1) *= s2; At(1, 2) *= s2;
At(2, 0) *= s3; At(2, 1) *= s3; At(2, 2) *= s3;
assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
assert(IsRowOrthogonal());
return true;
}
bool Matrix3x3::TryConvertToQuat(Quaternion &q) const {
if (IsColOrthogonal() && HasUnitaryScale() && !HasNegativeScale()) {
q = ToQuat();
return true;
}
return false;
}
void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints, int stride) const {
assert(pointArray || numPoints == 0);
assert(stride >= (int)sizeof(Vector3));
u8 *data = reinterpret_cast<u8*>(pointArray);
for (int i = 0; i < numPoints; ++i)
{
Vector3 *vtx = reinterpret_cast<Vector3*>(data + stride*i);
*vtx = *this * *vtx;
}
}
void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints) const {
assert(pointArray || numPoints == 0);
for (int i = 0; i < numPoints; ++i)
{
pointArray[i] = *this * pointArray[i];
}
}
void Matrix3x3::BatchTransform(Vector4 *vectorArray, int numVectors) const {
assert(vectorArray || numVectors == 0);
for(int i = 0; i < numVectors; ++i)
vectorArray[i] = *this * vectorArray[i];
}
void Matrix3x3::BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const {
assert(vectorArray || numVectors == 0);
assert(stride >= (int)sizeof(Vector4));
u8 *data = reinterpret_cast<u8*>(vectorArray);
for(int i = 0; i < numVectors; ++i)
{
Vector4 *vtx = reinterpret_cast<Vector4*>(data + stride*i);
*vtx = *this * *vtx;
}
}
Vector4 Matrix3x3::operator*(const Vector4 &rhs) const {
return Vector4();
}
void Matrix3x3::SetRow(int row, const float *data) {
assert(data);
SetRow(row, data[0], data[1], data[2]);
}
void Matrix3x3::SetRow(int row, float x, float y, float z) {
assert(row > 0);
assert(row < Rows);
assert(std::isfinite(x));
assert(std::isfinite(y));
assert(std::isfinite(z));
At(row, 0) = x;
At(row, 1) = y;
At(row, 2) = z;
}
void Matrix3x3::SetColumn(int column, const float *data) {
assert(data);
SetColumn(column, data[0], data[1], data[2]);
}
void Matrix3x3::SetColumn(int column, float x, float y, float z) {
assert(column >= 0);
assert(column < Cols);
assert(std::isfinite(x));
assert(std::isfinite(y));
assert(std::isfinite(z));
At(0, column) = x;
At(1, column) = y;
At(2, column) = z;
}
Matrix3x3 Matrix3x3::Mul(const Quaternion &rhs) const {
return *this * rhs;
}
}