TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-03-19 04:10:54 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 3
Torque3D/Engine/source/math/mMatrix.h
marauder2k7 0ba8d948fb neon implementation 
removed some x86 intrinsic functions that were in the mat44_impl file
reinstated some mMath_C functions and mMathFn ptrs trying to diagnose an issue.
Had to come up with a different way to initialize the scalar table if the isa tables are not initialized yet. Mac did not like the static initialization.
Had to change neon over to using explicit masks for shifting, cross product was failing during bakes and matrix calculations
2026-03-05 18:55:38 +00:00

2052 lines
57 KiB
C++
Raw Blame History

//-----------------------------------------------------------------------------
// Copyright (c) 2012 GarageGames, LLC
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//-----------------------------------------------------------------------------
#ifndef _MMATRIX_H_
#define _MMATRIX_H_
#include <algorithm>
#ifndef _MPLANE_H_
#include "math/mPlane.h"
#endif
#ifndef _MBOX_H_
#include "math/mBox.h"
#endif
#ifndef _MPOINT4_H_
#include "math/mPoint4.h"
#endif
#ifndef _ENGINETYPEINFO_H_
#include "console/engineTypeInfo.h"
#endif
#ifndef _FRAMEALLOCATOR_H_
#include "core/frameAllocator.h"
#endif
#ifndef _STRINGFUNCTIONS_H_
#include "core/strings/stringFunctions.h"
#endif
#ifndef _CONSOLE_H_
#include "console/console.h"
#endif
#ifndef _MATH_BACKEND_H_
#include "math/public/math_backend.h"
#endif
#ifndef USE_TEMPLATE_MATRIX
/// 4x4 Matrix Class
///
/// This runs at F32 precision.
using namespace math_backend::mat44::dispatch;
class MatrixF
{
friend class MatrixFEngineExport;
private:
F32 m[16]; ///< Note: Torque uses row-major matrices
public:
/// Create an uninitialized matrix.
///
/// @param identity If true, initialize to the identity matrix.
explicit MatrixF(bool identity=false);
/// Create a matrix to rotate about origin by e.
/// @see set
explicit MatrixF( const EulerF &e);
/// Create a matrix to rotate about p by e.
/// @see set
MatrixF( const EulerF &e, const Point3F& p);
/// Get the index in m to element in column i, row j
///
/// This is necessary as we have m as a one dimensional array.
///
/// @param i Column desired.
/// @param j Row desired.
static U32 idx(U32 i, U32 j) { return (i + j*4); }
/// Initialize matrix to rotate about origin by e.
MatrixF& set( const EulerF &e);
/// Initialize matrix to rotate about p by e.
MatrixF& set( const EulerF &e, const Point3F& p);
/// Initialize matrix with a cross product of p.
MatrixF& setCrossProduct( const Point3F &p);
/// Initialize matrix with a tensor product of p.
MatrixF& setTensorProduct( const Point3F &p, const Point3F& q);
operator F32*() { return (m); } ///< Allow people to get at m.
operator const F32*() const { return (F32*)(m); } ///< Allow people to get at m.
bool isAffine() const; ///< Check to see if this is an affine matrix.
bool isIdentity() const; ///< Checks for identity matrix.
/// Make this an identity matrix.
MatrixF& identity();
/// Invert m.
MatrixF& inverse();
/// Copy the inversion of this into out matrix.
void invertTo( MatrixF *out );
/// Take inverse of matrix assuming it is affine (rotation,
/// scale, sheer, translation only).
MatrixF& affineInverse();
/// Swap rows and columns.
MatrixF& transpose();
/// M * Matrix(p) -> M
MatrixF& scale( const Point3F &s );
MatrixF& scale( F32 s ) { return scale( Point3F( s, s, s ) ); }
/// Return scale assuming scale was applied via mat.scale(s).
Point3F getScale() const;
EulerF toEuler() const;
F32 determinant() const {
return GetMat44().determinant(*this);
}
/// Compute the inverse of the matrix.
///
/// Computes inverse of full 4x4 matrix. Returns false and performs no inverse if
/// the determinant is 0.
///
/// Note: In most cases you want to use the normal inverse function. This method should
/// be used if the matrix has something other than (0,0,0,1) in the bottom row.
bool fullInverse();
/// Reverse depth for projection matrix
/// Simplifies reversal matrix mult to 4 subtractions
void reverseProjection();
/// Swaps rows and columns into matrix.
void transposeTo(F32 *matrix) const;
/// Normalize the matrix.
void normalize();
/// Copy the requested column into a Point4F.
void getColumn(S32 col, Point4F *cptr) const;
Point4F getColumn4F(S32 col) const { Point4F ret; getColumn(col,&ret); return ret; }
/// Copy the requested column into a Point3F.
///
/// This drops the bottom-most row.
void getColumn(S32 col, Point3F *cptr) const;
Point3F getColumn3F(S32 col) const { Point3F ret; getColumn(col,&ret); return ret; }
/// Set the specified column from a Point4F.
void setColumn(S32 col, const Point4F& cptr);
/// Set the specified column from a Point3F.
///
/// The bottom-most row is not set.
void setColumn(S32 col, const Point3F& cptr);
/// Copy the specified row into a Point4F.
void getRow(S32 row, Point4F *cptr) const;
Point4F getRow4F(S32 row) const { Point4F ret; getRow(row,&ret); return ret; }
/// Copy the specified row into a Point3F.
///
/// Right-most item is dropped.
void getRow(S32 row, Point3F *cptr) const;
Point3F getRow3F(S32 row) const { Point3F ret; getRow(row,&ret); return ret; }
/// Set the specified row from a Point4F.
void setRow(S32 row, const Point4F& cptr);
/// Set the specified row from a Point3F.
///
/// The right-most item is not set.
void setRow(S32 row, const Point3F& cptr);
/// Get the position of the matrix.
///
/// This is the 4th column of the matrix.
Point3F getPosition() const;
/// Set the position of the matrix.
///
/// This is the 4th column of the matrix.
void setPosition( const Point3F &pos ) { setColumn( 3, pos ); }
/// Add the passed delta to the matrix position.
void displace( const Point3F &delta );
/// Get the x axis of the matrix.
///
/// This is the 1st column of the matrix and is
/// normally considered the right vector.
VectorF getRightVector() const;
/// Get the y axis of the matrix.
///
/// This is the 2nd column of the matrix and is
/// normally considered the forward vector.
VectorF getForwardVector() const;
/// Get the z axis of the matrix.
///
/// This is the 3rd column of the matrix and is
/// normally considered the up vector.
VectorF getUpVector() const;
MatrixF& mul(const MatrixF &a); ///< M * a -> M
MatrixF& mulL(const MatrixF &a); ///< a * M -> M
MatrixF& mul(const MatrixF &a, const MatrixF &b); ///< a * b -> M
// Scalar multiplies
MatrixF& mul(const F32 a); ///< M * a -> M
MatrixF& mul(const MatrixF &a, const F32 b); ///< a * b -> M
void mul( Point4F& p ) const; ///< M * p -> p (full [4x4] * [1x4])
void mulP( Point3F& p ) const; ///< M * p -> p (assume w = 1.0f)
void mulP( const Point3F &p, Point3F *d) const; ///< M * p -> d (assume w = 1.0f)
void mulV( VectorF& p ) const; ///< M * v -> v (assume w = 0.0f)
void mulV( const VectorF &p, Point3F *d) const; ///< M * v -> d (assume w = 0.0f)
void mul(Box3F& b) const; ///< Axial box -> Axial Box
MatrixF& add( const MatrixF& m );
/// <summary>
/// Turns this matrix into a view matrix that looks at target.
/// </summary>
/// <param name="eye">The eye position.</param>
/// <param name="target">The target position/direction.</param>
/// <param name="up">The up direction.</param>
void LookAt(const VectorF& eye, const VectorF& target, const VectorF& up);
/// Convenience function to allow people to treat this like an array.
F32& operator ()(S32 row, S32 col) { return m[idx(col,row)]; }
F32 operator ()(S32 row, S32 col) const { return m[idx(col,row)]; }
void dumpMatrix(const char *caption=NULL) const;
// Math operator overloads
//------------------------------------
friend MatrixF operator * ( const MatrixF &m1, const MatrixF &m2 );
MatrixF& operator *= ( const MatrixF &m );
MatrixF &operator = (const MatrixF &m);
bool isNaN();
// Static identity matrix
const static MatrixF Identity;
};
class MatrixFEngineExport
{
public:
static EngineFieldTable::Field getMatrixField();
};
//--------------------------------------
// Inline Functions
inline MatrixF::MatrixF(bool _identity)
{
if (_identity)
identity();
else
std::fill_n(m, 16, 0);
}
inline MatrixF::MatrixF( const EulerF &e )
{
set(e);
}
inline MatrixF::MatrixF( const EulerF &e, const Point3F& p )
{
set(e,p);
}
inline MatrixF& MatrixF::set( const EulerF &e)
{
m_matF_set_euler( e, *this );
return (*this);
}
inline MatrixF& MatrixF::set( const EulerF &e, const Point3F& p)
{
m_matF_set_euler_point( e, p, *this );
return (*this);
}
inline MatrixF& MatrixF::setCrossProduct( const Point3F &p)
{
m[1] = -(m[4] = p.z);
m[8] = -(m[2] = p.y);
m[6] = -(m[9] = p.x);
m[0] = m[3] = m[5] = m[7] = m[10] = m[11] =
m[12] = m[13] = m[14] = 0.0f;
m[15] = 1;
return (*this);
}
inline MatrixF& MatrixF::setTensorProduct( const Point3F &p, const Point3F &q)
{
m[0] = p.x * q.x;
m[1] = p.x * q.y;
m[2] = p.x * q.z;
m[4] = p.y * q.x;
m[5] = p.y * q.y;
m[6] = p.y * q.z;
m[8] = p.z * q.x;
m[9] = p.z * q.y;
m[10] = p.z * q.z;
m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
m[15] = 1.0f;
return (*this);
}
inline bool MatrixF::isIdentity() const
{
return
m[0] == 1.0f &&
m[1] == 0.0f &&
m[2] == 0.0f &&
m[3] == 0.0f &&
m[4] == 0.0f &&
m[5] == 1.0f &&
m[6] == 0.0f &&
m[7] == 0.0f &&
m[8] == 0.0f &&
m[9] == 0.0f &&
m[10] == 1.0f &&
m[11] == 0.0f &&
m[12] == 0.0f &&
m[13] == 0.0f &&
m[14] == 0.0f &&
m[15] == 1.0f;
}
inline MatrixF& MatrixF::identity()
{
m[0] = 1.0f;
m[1] = 0.0f;
m[2] = 0.0f;
m[3] = 0.0f;
m[4] = 0.0f;
m[5] = 1.0f;
m[6] = 0.0f;
m[7] = 0.0f;
m[8] = 0.0f;
m[9] = 0.0f;
m[10] = 1.0f;
m[11] = 0.0f;
m[12] = 0.0f;
m[13] = 0.0f;
m[14] = 0.0f;
m[15] = 1.0f;
return (*this);
}
inline MatrixF& MatrixF::inverse()
{
GetMat44().inverse(m);
return (*this);
}
inline void MatrixF::invertTo( MatrixF *out )
{
out = this;
GetMat44().inverse(*out);
}
inline MatrixF& MatrixF::affineInverse()
{
GetMat44().affine_inverse(m);
return (*this);
}
inline MatrixF& MatrixF::transpose()
{
GetMat44().transpose(m);
return (*this);
}
inline MatrixF& MatrixF::scale(const Point3F& p)
{
GetMat44().scale(m, p);
return *this;
}
inline Point3F MatrixF::getScale() const
{
Point3F scale;
GetMat44().get_scale(m, scale);
return scale;
}
inline void MatrixF::normalize()
{
GetMat44().normalize(m);
}
inline MatrixF& MatrixF::mul( const MatrixF &a )
{ // M * a -> M
AssertFatal(&a != this, "MatrixF::mul - a.mul(a) is invalid!");
MatrixF tempThis(*this);
GetMat44().mul_mat44(tempThis, a, *this);
return (*this);
}
inline MatrixF& MatrixF::mulL( const MatrixF &a )
{ // a * M -> M
AssertFatal(&a != this, "MatrixF::mulL - a.mul(a) is invalid!");
MatrixF tempThis(*this);
GetMat44().mul_mat44(a, tempThis, *this);
return (*this);
}
inline MatrixF& MatrixF::mul( const MatrixF &a, const MatrixF &b )
{ // a * b -> M
AssertFatal((&a != this) && (&b != this), "MatrixF::mul - a.mul(a, b) a.mul(b, a) a.mul(a, a) is invalid!");
GetMat44().mul_mat44(a, b, *this);
return (*this);
}
inline MatrixF& MatrixF::mul(const F32 a)
{
for (U32 i = 0; i < 16; i++)
m[i] *= a;
return *this;
}
inline MatrixF& MatrixF::mul(const MatrixF &a, const F32 b)
{
*this = a;
mul(b);
return *this;
}
inline void MatrixF::mul( Point4F& p ) const
{
Point4F temp;
GetMat44().mul_float4(*this, p, temp);
p = temp;
}
inline void MatrixF::mulP( Point3F& p) const
{
// M * p -> d
Point3F d;
GetMat44().mul_pos3(*this, p, d);
p = d;
}
inline void MatrixF::mulP( const Point3F &p, Point3F *d) const
{
// M * p -> d
GetMat44().mul_pos3(*this, p, *d);
}
inline void MatrixF::mulV( VectorF& v) const
{
// M * v -> v
VectorF temp;
GetMat44().mul_vec3(*this, v, temp);
v = temp;
}
inline void MatrixF::mulV( const VectorF &v, Point3F *d) const
{
// M * v -> d
GetMat44().mul_vec3(*this, v, *d);
}
inline void MatrixF::mul(Box3F& b) const
{
m_matF_x_box3F(*this, &b.minExtents.x, &b.maxExtents.x);
}
inline MatrixF& MatrixF::add( const MatrixF& a )
{
for( U32 i = 0; i < 16; ++ i )
m[ i ] += a.m[ i ];
return *this;
}
inline void MatrixF::LookAt(const VectorF& eye, const VectorF& target, const VectorF& up)
{
// Calculate the forward vector (camera direction).
VectorF zAxis = target; // Camera looks towards the target
zAxis.normalize();
// Calculate the right vector.
VectorF xAxis = mCross(up, zAxis);
xAxis.normalize();
// Recalculate the up vector.
VectorF yAxis = mCross(zAxis, xAxis);
// Set the rotation part of the matrix (camera axes).
setColumn(0, xAxis); // Right
setColumn(1, zAxis); // Forward
setColumn(2, yAxis); // Up
// Set the translation part (camera position).
setPosition(eye);
}
inline void MatrixF::getColumn(S32 col, Point4F *cptr) const
{
cptr->x = m[col];
cptr->y = m[col+4];
cptr->z = m[col+8];
cptr->w = m[col+12];
}
inline void MatrixF::getColumn(S32 col, Point3F *cptr) const
{
cptr->x = m[col];
cptr->y = m[col+4];
cptr->z = m[col+8];
}
inline void MatrixF::setColumn(S32 col, const Point4F &cptr)
{
m[col] = cptr.x;
m[col+4] = cptr.y;
m[col+8] = cptr.z;
m[col+12]= cptr.w;
}
inline void MatrixF::setColumn(S32 col, const Point3F &cptr)
{
m[col] = cptr.x;
m[col+4] = cptr.y;
m[col+8] = cptr.z;
}
inline void MatrixF::getRow(S32 col, Point4F *cptr) const
{
col *= 4;
cptr->x = m[col++];
cptr->y = m[col++];
cptr->z = m[col++];
cptr->w = m[col];
}
inline void MatrixF::getRow(S32 col, Point3F *cptr) const
{
col *= 4;
cptr->x = m[col++];
cptr->y = m[col++];
cptr->z = m[col];
}
inline void MatrixF::setRow(S32 col, const Point4F &cptr)
{
col *= 4;
m[col++] = cptr.x;
m[col++] = cptr.y;
m[col++] = cptr.z;
m[col] = cptr.w;
}
inline void MatrixF::setRow(S32 col, const Point3F &cptr)
{
col *= 4;
m[col++] = cptr.x;
m[col++] = cptr.y;
m[col] = cptr.z;
}
inline Point3F MatrixF::getPosition() const
{
return Point3F( m[3], m[3+4], m[3+8] );
}
inline void MatrixF::displace( const Point3F &delta )
{
m[3] += delta.x;
m[3+4] += delta.y;
m[3+8] += delta.z;
}
inline VectorF MatrixF::getForwardVector() const
{
VectorF vec;
getColumn( 1, &vec );
return vec;
}
inline VectorF MatrixF::getRightVector() const
{
VectorF vec;
getColumn( 0, &vec );
return vec;
}
inline VectorF MatrixF::getUpVector() const
{
VectorF vec;
getColumn( 2, &vec );
return vec;
}
//------------------------------------
// Math operator overloads
//------------------------------------
inline MatrixF operator * ( const MatrixF &m1, const MatrixF &m2 )
{
// temp = m1 * m2
MatrixF temp;
GetMat44().mul_mat44(m1, m2, temp);
return temp;
}
inline MatrixF& MatrixF::operator *= ( const MatrixF &m1 )
{
MatrixF tempThis(*this);
GetMat44().mul_mat44(tempThis, m1, *this);
return (*this);
}
inline MatrixF &MatrixF::operator = (const MatrixF &m1)
{
for (U32 i=0;i<16;i++)
this->m[i] = m1.m[i];
return (*this);
}
inline bool MatrixF::isNaN()
{
bool isaNaN = false;
for (U32 i = 0; i < 16; i++)
if (mIsNaN_F(m[i]))
isaNaN = true;
return isaNaN;
}
//------------------------------------
// Non-member methods
//------------------------------------
inline void mTransformPlane(const MatrixF& mat, const Point3F& scale, const PlaneF& plane, PlaneF * result)
{
m_matF_x_scale_x_planeF(mat, &scale.x, &plane.x, &result->x);
}
#else // !USE_TEMPLATE_MATRIX
//------------------------------------
// Templatized matrix class to replace MATRIXF above
//------------------------------------
template<typename DATA_TYPE, U32 rows, U32 cols>
class Matrix {
friend class MatrixTemplateExport;
private:
DATA_TYPE data[rows * cols];
public:
static_assert(rows >= 2 && cols >= 2, "Matrix must have at least 2 rows and 2 cols.");
// ------ Setters and initializers ------
explicit Matrix(bool identity = false) {
std::fill(data, data + (rows * cols), DATA_TYPE(0));
if (identity) {
for (U32 i = 0; i < rows; i++) {
for (U32 j = 0; j < cols; j++) {
// others already get filled with 0
if (j == i)
(*this)(i, j) = static_cast<DATA_TYPE>(1);
}
}
}
}
explicit Matrix(const EulerF& e) {
set(e);
}
~Matrix() = default;
/// Make this an identity matrix.
Matrix<DATA_TYPE, rows, cols>& identity();
void reverseProjection();
void normalize();
Matrix<DATA_TYPE, rows, cols>& set(const EulerF& e);
Matrix(const EulerF& e, const Point3F p);
Matrix<DATA_TYPE, rows, cols>& set(const EulerF& e, const Point3F p);
Matrix<DATA_TYPE, rows, cols>& inverse();
Matrix<DATA_TYPE, rows, cols>& transpose();
void invert();
Matrix<DATA_TYPE, rows, cols>& setCrossProduct(const Point3F& p);
Matrix<DATA_TYPE, rows, cols>& setTensorProduct(const Point3F& p, const Point3F& q);
/// M * Matrix(p) -> M
Matrix<DATA_TYPE, rows, cols>& scale(const Point3F& s);
Matrix<DATA_TYPE, rows, cols>& scale(DATA_TYPE s) { return scale(Point3F(s, s, s)); }
void setColumn(S32 col, const Point4F& cptr);
void setColumn(S32 col, const Point3F& cptr);
void setRow(S32 row, const Point4F& cptr);
void setRow(S32 row, const Point3F& cptr);
void displace(const Point3F& delta);
bool fullInverse();
void setPosition(const Point3F& pos) { setColumn(3, pos); }
DATA_TYPE determinant() const {
AssertFatal(rows == cols, "Determinant is only defined for square matrices.");
// For simplicity, only implement for 3x3 matrices
AssertFatal(rows >= 3 && cols >= 3, "Determinant only for 3x3 or more"); // Ensure the matrix is 3x3
return (*this)(0, 0) * ((*this)(1, 1) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 1)) +
(*this)(1, 0) * ((*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2)) +
(*this)(2, 0) * ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1));
}
///< M * a -> M
Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a)
{ return *this = *this * a; }
///< a * M -> M
Matrix<DATA_TYPE, rows, cols>& mulL(const Matrix<DATA_TYPE, rows, cols>& a)
{ return *this = a * *this; }
///< a * b -> M
Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a, const Matrix<DATA_TYPE, rows, cols>& b)
{ return *this = a * b; }
///< M * a -> M
Matrix<DATA_TYPE, rows, cols>& mul(const F32 a)
{ return *this = *this * a; }
///< a * b -> M
Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a, const F32 b)
{ return *this = a * b; }
Matrix<DATA_TYPE, rows, cols>& add(const Matrix<DATA_TYPE, rows, cols>& a)
{
return *this = *this += a;
}
///< M * p -> p (full [4x4] * [1x4])
void mul(Point4F& p) const { p = *this * p; }
///< M * p -> p (assume w = 1.0f)
void mulP(Point3F& p) const {
Point3F result;
result.x = (*this)(0, 0) * p.x + (*this)(0, 1) * p.y + (*this)(0, 2) * p.z + (*this)(0, 3);
result.y = (*this)(1, 0) * p.x + (*this)(1, 1) * p.y + (*this)(1, 2) * p.z + (*this)(1, 3);
result.z = (*this)(2, 0) * p.x + (*this)(2, 1) * p.y + (*this)(2, 2) * p.z + (*this)(2, 3);
p = result;
}
///< M * p -> d (assume w = 1.0f)
void mulP(const Point3F& p, Point3F* d) const { *d = *this * p; }
///< M * v -> v (assume w = 0.0f)
void mulV(VectorF& v) const
{
AssertFatal(rows == 4 && cols == 4, "Multiplying VectorF with matrix requires 4x4");
VectorF result(
(*this)(0, 0) * v.x + (*this)(0, 1) * v.y + (*this)(0, 2) * v.z,
(*this)(1, 0) * v.x + (*this)(1, 1) * v.y + (*this)(1, 2) * v.z,
(*this)(2, 0) * v.x + (*this)(2, 1) * v.y + (*this)(2, 2) * v.z
);
v = result;
}
///< M * v -> d (assume w = 0.0f)
void mulV(const VectorF& v, Point3F* d) const
{
AssertFatal(rows == 4 && cols == 4, "Multiplying VectorF with matrix requires 4x4");
VectorF result(
(*this)(0, 0) * v.x + (*this)(0, 1) * v.y + (*this)(0, 2) * v.z,
(*this)(1, 0) * v.x + (*this)(1, 1) * v.y + (*this)(1, 2) * v.z,
(*this)(2, 0) * v.x + (*this)(2, 1) * v.y + (*this)(2, 2) * v.z
);
d->x = result.x;
d->y = result.y;
d->z = result.z;
}
///< Axial box -> Axial Box (too big a function to be inline)
void mul(Box3F& box) const;
// ------ Getters ------
bool isNaN() {
for (U32 i = 0; i < rows; i++) {
for (U32 j = 0; j < cols; j++) {
if (mIsNaN_F((*this)(i, j)))
return true;
}
}
return false;
}
// row + col * cols
static U32 idx(U32 i, U32 j) { return (i + j * cols); }
bool isAffine() const;
bool isIdentity() const;
/// Take inverse of matrix assuming it is affine (rotation,
/// scale, sheer, translation only).
Matrix<DATA_TYPE, rows, cols>& affineInverse();
Point3F getScale() const;
EulerF toEuler() const;
Point3F getPosition() const;
void getColumn(S32 col, Point4F* cptr) const;
Point4F getColumn4F(S32 col) const { Point4F ret; getColumn(col, &ret); return ret; }
void getColumn(S32 col, Point3F* cptr) const;
Point3F getColumn3F(S32 col) const { Point3F ret; getColumn(col, &ret); return ret; }
void getRow(S32 row, Point4F* cptr) const;
Point4F getRow4F(S32 row) const { Point4F ret; getRow(row, &ret); return ret; }
void getRow(S32 row, Point3F* cptr) const;
Point3F getRow3F(S32 row) const { Point3F ret; getRow(row, &ret); return ret; }
VectorF getRightVector() const;
VectorF getForwardVector() const;
VectorF getUpVector() const;
DATA_TYPE* getData() {
return data;
}
const DATA_TYPE* getData() const {
return data;
}
void transposeTo(Matrix<DATA_TYPE, cols, rows>& matrix) const {
for (U32 i = 0; i < rows; ++i) {
for (U32 j = 0; j < cols; ++j) {
matrix(j, i) = (*this)(i, j);
}
}
}
void swap(DATA_TYPE& a, DATA_TYPE& b) {
DATA_TYPE temp = a;
a = b;
b = temp;
}
void invertTo(Matrix<DATA_TYPE, cols, rows>* matrix) const;
void dumpMatrix(const char* caption = NULL) const;
// Static identity matrix
static const Matrix Identity;
// ------ Operators ------
friend Matrix<DATA_TYPE, rows, cols> operator*(const Matrix<DATA_TYPE, rows, cols>& m1, const Matrix<DATA_TYPE, rows, cols>& m2) {
Matrix<DATA_TYPE, rows, cols> result;
for (U32 i = 0; i < rows; ++i) {
for (U32 j = 0; j < cols; ++j)
{
result(i, j) = static_cast<DATA_TYPE>(0);
for (U32 k = 0; k < cols; ++k)
{
result(i, j) += m1(i, k) * m2(k, j);
}
}
}
return result;
}
Matrix<DATA_TYPE, rows, cols> operator *= (const Matrix<DATA_TYPE, rows, cols>& other) {
*this = *this * other;
return *this;
}
Matrix<DATA_TYPE, rows, cols> operator+(const Matrix<DATA_TYPE, rows, cols>& m2) {
Matrix<DATA_TYPE, rows, cols> result;
for (U32 i = 0; i < rows; ++i)
{
for (U32 j = 0; j < cols; ++j)
{
result(i, j) = 0; // Initialize result element to 0
result(i, j) = (*this)(i, j) + m2(i, j);
}
}
return result;
}
Matrix<DATA_TYPE, rows, cols> operator+=(const Matrix<DATA_TYPE, rows, cols>& m2) {
for (U32 i = 0; i < rows; ++i)
{
for (U32 j = 0; j < cols; ++j)
{
(*this)(i, j) += m2(i, j);
}
}
return (*this);
}
Matrix<DATA_TYPE, rows, cols> operator * (const DATA_TYPE scalar) const {
Matrix<DATA_TYPE, rows, cols> result;
for (U32 i = 0; i < rows; i++)
{
for (U32 j = 0; j < cols; j++)
{
result(i, j) = (*this)(i, j) * scalar;
}
}
return result;
}
Matrix<DATA_TYPE, rows, cols>& operator *= (const DATA_TYPE scalar) {
for (U32 i = 0; i < rows; i++)
{
for (U32 j = 0; j < cols; j++)
{
(*this)(i, j) *= scalar;
}
}
return *this;
}
Point3F operator*(const Point3F& point) const {
AssertFatal(rows == 4 && cols == 4, "Multiplying point3 with matrix requires 4x4");
Point3F result;
result.x = (*this)(0, 0) * point.x + (*this)(0, 1) * point.y + (*this)(0, 2) * point.z + (*this)(0, 3);
result.y = (*this)(1, 0) * point.x + (*this)(1, 1) * point.y + (*this)(1, 2) * point.z + (*this)(1, 3);
result.z = (*this)(2, 0) * point.x + (*this)(2, 1) * point.y + (*this)(2, 2) * point.z + (*this)(2, 3);
return result;
}
Point4F operator*(const Point4F& point) const {
AssertFatal(rows == 4 && cols == 4, "Multiplying point4 with matrix requires 4x4");
return Point4F(
(*this)(0, 0) * point.x + (*this)(0, 1) * point.y + (*this)(0, 2) * point.z + (*this)(0, 3) * point.w,
(*this)(1, 0) * point.x + (*this)(1, 1) * point.y + (*this)(1, 2) * point.z + (*this)(1, 3) * point.w,
(*this)(2, 0) * point.x + (*this)(2, 1) * point.y + (*this)(2, 2) * point.z + (*this)(2, 3) * point.w,
(*this)(3, 0) * point.x + (*this)(3, 1) * point.y + (*this)(3, 2) * point.z + (*this)(3, 3) * point.w
);
}
Matrix<DATA_TYPE, rows, cols>& operator = (const Matrix<DATA_TYPE, rows, cols>& other) {
if (this != &other) {
std::copy(other.data, other.data + rows * cols, this->data);
}
return *this;
}
bool operator == (const Matrix<DATA_TYPE, rows, cols>& other) const {
for (U32 i = 0; i < rows; i++)
{
for (U32 j = 0; j < cols; j++)
{
if ((*this)(i, j) != other(i, j))
return false;
}
}
return true;
}
bool operator != (const Matrix<DATA_TYPE, rows, cols>& other) const {
return !(*this == other);
}
operator DATA_TYPE* () { return (data); }
operator const DATA_TYPE* () const { return (DATA_TYPE*)(data); }
DATA_TYPE& operator () (U32 row, U32 col) {
if (row >= rows || col >= cols)
AssertFatal(false, "Matrix indices out of range");
return data[idx(col,row)];
}
DATA_TYPE operator () (U32 row, U32 col) const {
if (row >= rows || col >= cols)
AssertFatal(false, "Matrix indices out of range");
return data[idx(col, row)];
}
};
//--------------------------------------------
// INLINE FUNCTIONS
//--------------------------------------------
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::transpose()
{
AssertFatal(rows == cols, "Transpose can only be performed on square matrices.");
swap((*this)(0, 1), (*this)(1, 0));
swap((*this)(0, 2), (*this)(2, 0));
swap((*this)(0, 3), (*this)(3, 0));
swap((*this)(1, 2), (*this)(2, 1));
swap((*this)(1, 3), (*this)(3, 1));
swap((*this)(2, 3), (*this)(3, 2));
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::identity()
{
for (U32 i = 0; i < rows; i++)
{
for (U32 j = 0; j < cols; j++)
{
if (j == i)
(*this)(i, j) = static_cast<DATA_TYPE>(1);
else
(*this)(i, j) = static_cast<DATA_TYPE>(0);
}
}
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::normalize()
{
AssertFatal(rows >= 3 && cols >= 3, "Normalize can only be applied 3x3 or more");
Point3F col0, col1, col2;
getColumn(0, &col0);
getColumn(1, &col1);
mCross(col0, col1, &col2);
mCross(col2, col0, &col1);
col0.normalize();
col1.normalize();
col2.normalize();
setColumn(0, col0);
setColumn(1, col1);
setColumn(2, col2);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::scale(const Point3F& s)
{
// torques scale applies directly, does not create another matrix to multiply with the translation matrix.
AssertFatal(rows >= 4 && cols >= 4, "Scale can only be applied 4x4 or more");
(*this)(0, 0) *= s.x; (*this)(0, 1) *= s.y; (*this)(0, 2) *= s.z;
(*this)(1, 0) *= s.x; (*this)(1, 1) *= s.y; (*this)(1, 2) *= s.z;
(*this)(2, 0) *= s.x; (*this)(2, 1) *= s.y; (*this)(2, 2) *= s.z;
(*this)(3, 0) *= s.x; (*this)(3, 1) *= s.y; (*this)(3, 2) *= s.z;
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline bool Matrix<DATA_TYPE, rows, cols>::isIdentity() const {
for (U32 i = 0; i < rows; i++)
{
for (U32 j = 0; j < cols; j++)
{
if (j == i)
{
if((*this)(i, j) != static_cast<DATA_TYPE>(1))
{
return false;
}
}
else
{
if((*this)(i, j) != static_cast<DATA_TYPE>(0))
{
return false;
}
}
}
}
return true;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Point3F Matrix<DATA_TYPE, rows, cols>::getScale() const
{
// this function assumes the matrix has scale applied through the scale(const Point3F& s) function.
// for now assume float since we have point3F.
AssertFatal(rows >= 4 && cols >= 4, "Scale can only be applied 4x4 or more");
Point3F scale;
scale.x = mSqrt((*this)(0, 0) * (*this)(0, 0) + (*this)(1, 0) * (*this)(1, 0) + (*this)(2, 0) * (*this)(2, 0));
scale.y = mSqrt((*this)(0, 1) * (*this)(0, 1) + (*this)(1, 1) * (*this)(1, 1) + (*this)(2, 1) * (*this)(2, 1));
scale.z = mSqrt((*this)(0, 2) * (*this)(0, 2) + (*this)(1, 2) * (*this)(1, 2) + (*this)(2, 2) * (*this)(2, 2));
return scale;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Point3F Matrix<DATA_TYPE, rows, cols>::getPosition() const
{
Point3F pos;
getColumn(3, &pos);
return pos;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::getColumn(S32 col, Point4F* cptr) const
{
if (rows >= 2)
{
cptr->x = (*this)(0, col);
cptr->y = (*this)(1, col);
}
if (rows >= 3)
cptr->z = (*this)(2, col);
else
cptr->z = 0.0f;
if (rows >= 4)
cptr->w = (*this)(3, col);
else
cptr->w = 0.0f;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::getColumn(S32 col, Point3F* cptr) const
{
if (rows >= 2)
{
cptr->x = (*this)(0, col);
cptr->y = (*this)(1, col);
}
if (rows >= 3)
cptr->z = (*this)(2, col);
else
cptr->z = 0.0f;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::setColumn(S32 col, const Point4F &cptr) {
if(rows >= 2)
{
(*this)(0, col) = cptr.x;
(*this)(1, col) = cptr.y;
}
if(rows >= 3)
(*this)(2, col) = cptr.z;
if(rows >= 4)
(*this)(3, col) = cptr.w;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::setColumn(S32 col, const Point3F &cptr) {
if(rows >= 2)
{
(*this)(0, col) = cptr.x;
(*this)(1, col) = cptr.y;
}
if(rows >= 3)
(*this)(2, col) = cptr.z;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::getRow(S32 row, Point4F* cptr) const
{
if (cols >= 2)
{
cptr->x = (*this)(row, 0);
cptr->y = (*this)(row, 1);
}
if (cols >= 3)
cptr->z = (*this)(row, 2);
else
cptr->z = 0.0f;
if (cols >= 4)
cptr->w = (*this)(row, 3);
else
cptr->w = 0.0f;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::getRow(S32 row, Point3F* cptr) const
{
if (cols >= 2)
{
cptr->x = (*this)(row, 0);
cptr->y = (*this)(row, 1);
}
if (cols >= 3)
cptr->z = (*this)(row, 2);
else
cptr->z = 0.0f;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline VectorF Matrix<DATA_TYPE, rows, cols>::getRightVector() const
{
VectorF vec;
getColumn(0, &vec);
return vec;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline VectorF Matrix<DATA_TYPE, rows, cols>::getForwardVector() const
{
VectorF vec;
getColumn(1, &vec);
return vec;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline VectorF Matrix<DATA_TYPE, rows, cols>::getUpVector() const
{
VectorF vec;
getColumn(2, &vec);
return vec;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::invertTo(Matrix<DATA_TYPE, cols, rows>* matrix) const
{
Matrix<DATA_TYPE, rows, cols> invMatrix;
for (U32 i = 0; i < rows; ++i)
{
for (U32 j = 0; j < cols; ++j)
{
invMatrix(i, j) = (*this)(i, j);
}
}
invMatrix.inverse();
for (U32 i = 0; i < rows; ++i)
{
for (U32 j = 0; j < cols; ++j)
{
(*matrix)(i, j) = invMatrix(i, j);
}
}
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::setRow(S32 row, const Point4F& cptr) {
if(cols >= 2)
{
(*this)(row, 0) = cptr.x;
(*this)(row, 1) = cptr.y;
}
if(cols >= 3)
(*this)(row, 2) = cptr.z;
if(cols >= 4)
(*this)(row, 3) = cptr.w;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::setRow(S32 row, const Point3F& cptr) {
if(cols >= 2)
{
(*this)(row, 0) = cptr.x;
(*this)(row, 1) = cptr.y;
}
if(cols >= 3)
(*this)(row, 2) = cptr.z;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::displace(const Point3F& delta)
{
(*this)(0, 3) += delta.x;
(*this)(1, 3) += delta.y;
(*this)(2, 3) += delta.z;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::reverseProjection()
{
AssertFatal(rows == 4 && cols == 4, "reverseProjection requires a 4x4 matrix.");
(*this)(2, 0) = (*this)(3, 0) - (*this)(2, 0);
(*this)(2, 1) = (*this)(3, 1) - (*this)(2, 1);
(*this)(2, 2) = (*this)(3, 2) - (*this)(2, 2);
(*this)(2, 3) = (*this)(3, 3) - (*this)(2, 3);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
const Matrix<DATA_TYPE, rows, cols> Matrix<DATA_TYPE, rows, cols>::Identity = []() {
Matrix<DATA_TYPE, rows, cols> identity(true);
return identity;
}();
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e)
{
// when the template refactor is done, euler will be able to be setup in different ways
AssertFatal(rows >= 3 && cols >= 3, "EulerF can only initialize 3x3 or more");
static_assert(std::is_same<DATA_TYPE, float>::value, "Can only initialize eulers with floats for now");
F32 cosPitch, sinPitch;
mSinCos(e.x, sinPitch, cosPitch);
F32 cosYaw, sinYaw;
mSinCos(e.y, sinYaw, cosYaw);
F32 cosRoll, sinRoll;
mSinCos(e.z, sinRoll, cosRoll);
enum {
AXIS_X = (1 << 0),
AXIS_Y = (1 << 1),
AXIS_Z = (1 << 2)
};
U32 axis = 0;
if (e.x != 0.0f) axis |= AXIS_X;
if (e.y != 0.0f) axis |= AXIS_Y;
if (e.z != 0.0f) axis |= AXIS_Z;
switch (axis) {
case 0:
(*this) = Matrix<DATA_TYPE, rows, cols>(true);
break;
case AXIS_X:
(*this)(0, 0) = 1.0f; (*this)(0, 1) = 0.0f; (*this)(0, 2) = 0.0f;
(*this)(1, 0) = 0.0f; (*this)(1, 1) = cosPitch; (*this)(1, 2) = sinPitch;
(*this)(2, 0) = 0.0f; (*this)(2, 1) = -sinPitch; (*this)(2, 2) = cosPitch;
break;
case AXIS_Y:
(*this)(0, 0) = cosYaw; (*this)(1, 0) = 0.0f; (*this)(2, 0) = sinYaw;
(*this)(0, 1) = 0.0f; (*this)(1, 1) = 1.0f; (*this)(2, 1) = 0.0f;
(*this)(0, 2) = -sinYaw; (*this)(1, 2) = 0.0f; (*this)(2, 2) = cosYaw;
break;
case AXIS_Z:
(*this)(0, 0) = cosRoll; (*this)(0, 1) = sinRoll; (*this)(0, 2) = 0.0f;
(*this)(1, 0) = -sinRoll; (*this)(1, 1) = cosRoll; (*this)(1, 2) = 0.0f;
(*this)(2, 0) = 0.0f; (*this)(2, 1) = 0.0f; (*this)(2, 2) = 1.0f;
break;
default:
F32 r1 = cosYaw * cosRoll;
F32 r2 = cosYaw * sinRoll;
F32 r3 = sinYaw * cosRoll;
F32 r4 = sinYaw * sinRoll;
// the matrix looks like this:
// r1 - (r4 * sin(x)) r2 + (r3 * sin(x)) -cos(x) * sin(y)
// -cos(x) * sin(z) cos(x) * cos(z) sin(x)
// r3 + (r2 * sin(x)) r4 - (r1 * sin(x)) cos(x) * cos(y)
//
// where:
// r1 = cos(y) * cos(z)
// r2 = cos(y) * sin(z)
// r3 = sin(y) * cos(z)
// r4 = sin(y) * sin(z)
// init the euler 3x3 rotation matrix.
(*this)(0, 0) = r1 - (r4 * sinPitch); (*this)(0, 1) = r2 + (r3 * sinPitch); (*this)(0, 2) = -cosPitch * sinYaw;
(*this)(1, 0) = -cosPitch * sinRoll; (*this)(1, 1) = cosPitch * cosRoll; (*this)(1, 2) = sinPitch;
(*this)(2, 0) = r3 + (r2 * sinPitch); (*this)(2, 1) = r4 - (r1 * sinPitch); (*this)(2, 2) = cosPitch * cosYaw;
break;
}
if (rows == 4)
{
(*this)(3, 0) = 0.0f;
(*this)(3, 1) = 0.0f;
(*this)(3, 2) = 0.0f;
}
if (cols == 4)
{
(*this)(0, 3) = 0.0f;
(*this)(1, 3) = 0.0f;
(*this)(2, 3) = 0.0f;
}
if (rows == 4 && cols == 4)
{
(*this)(3, 3) = 1.0f;
}
return(*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
Matrix<DATA_TYPE, rows, cols>::Matrix(const EulerF& e, const Point3F p)
{
set(e, p);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e, const Point3F p)
{
AssertFatal(rows >= 3 && cols >= 4, "Euler and Point can only initialize 3x4 or more");
// call set euler, this already sets the last row if it exists.
set(e);
// does this need to multiply with the result of the euler? or are we just setting position.
(*this)(0, 3) = p.x;
(*this)(1, 3) = p.y;
(*this)(2, 3) = p.z;
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::inverse()
{
#if 1
// NOTE: Gauss-Jordan elimination is yielding unpredictable results due to precission handling and
// numbers near 0.0
//
AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
const U32 size = rows - 1;
const DATA_TYPE pivot_eps = static_cast<DATA_TYPE>(1e-20); // Smaller epsilon to handle numerical precision
// Create augmented matrix [this | I]
Matrix<DATA_TYPE, size, rows + size> augmentedMatrix;
for (U32 i = 0; i < size; i++)
{
for (U32 j = 0; j < size; j++)
{
augmentedMatrix(i, j) = (*this)(i, j);
augmentedMatrix(i, j + size) = (i == j) ? static_cast<DATA_TYPE>(1) : static_cast<DATA_TYPE>(0);
}
}
// Apply gauss-joran elimination
for (U32 i = 0; i < size; i++)
{
U32 pivotRow = i;
DATA_TYPE pivotValue = std::abs(augmentedMatrix(i, i));
for (U32 k = i + 1; k < size; k++)
{
DATA_TYPE curValue = std::abs(augmentedMatrix(k, i));
if (curValue > pivotValue) {
pivotRow = k;
pivotValue = curValue;
}
}
// Swap if needed.
if (i != pivotRow)
{
for (U32 j = 0; j < 2 * size; j++)
{
DATA_TYPE temp = augmentedMatrix(i, j);
augmentedMatrix(i, j) = augmentedMatrix(pivotRow, j);
augmentedMatrix(pivotRow, j) = temp;
}
}
// Early out if pivot is 0, return identity matrix.
if (std::abs(augmentedMatrix(i, i)) < pivot_eps)
{
return *this;
}
DATA_TYPE pivotVal = static_cast<DATA_TYPE>(1.0) / augmentedMatrix(i, i);
// scale the pivot
for (U32 j = 0; j < 2 * size; j++)
{
augmentedMatrix(i, j) *= pivotVal;
}
// Eliminate the current column in all other rows
for (U32 k = 0; k < size; k++)
{
if (k != i)
{
DATA_TYPE factor = augmentedMatrix(k, i);
for (U32 j = 0; j < 2 * size; j++)
{
augmentedMatrix(k, j) -= factor * augmentedMatrix(i, j);
}
}
}
}
for (U32 i = 0; i < size; i++)
{
for (U32 j = 0; j < size; j++)
{
(*this)(i, j) = augmentedMatrix(i, j + size);
}
}
#else
AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
AssertFatal(rows >= 3 && cols >= 3, "Must be at least a 3x3 matrix");
DATA_TYPE det = determinant();
// Check if the determinant is non-zero
if (std::abs(det) < static_cast<DATA_TYPE>(1e-10)) {
this->identity(); // Return the identity matrix if the determinant is zero
return *this;
}
DATA_TYPE invDet = DATA_TYPE(1) / det;
Matrix<DATA_TYPE, rows, cols> temp;
// Calculate the inverse of the 3x3 upper-left submatrix using Cramer's rule
temp(0, 0) = ((*this)(1, 1) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 1)) * invDet;
temp(0, 1) = ((*this)(2, 1) * (*this)(0, 2) - (*this)(2, 2) * (*this)(0, 1)) * invDet;
temp(0, 2) = ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1)) * invDet;
temp(1, 0) = ((*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2)) * invDet;
temp(1, 1) = ((*this)(2, 2) * (*this)(0, 0) - (*this)(2, 0) * (*this)(0, 2)) * invDet;
temp(1, 2) = ((*this)(0, 2) * (*this)(1, 0) - (*this)(0, 0) * (*this)(1, 2)) * invDet;
temp(2, 0) = ((*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0)) * invDet;
temp(2, 1) = ((*this)(2, 0) * (*this)(0, 1) - (*this)(2, 1) * (*this)(0, 0)) * invDet;
temp(2, 2) = ((*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1, 0)) * invDet;
// Copy the 3x3 inverse back into this matrix
for (U32 i = 0; i < 3; ++i)
{
for (U32 j = 0; j < 3; ++j)
{
(*this)(i, j) = temp(i, j);
}
}
#endif
Point3F pos = -this->getPosition();
mulV(pos);
this->setPosition(pos);
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline bool Matrix<DATA_TYPE, rows, cols>::fullInverse()
{
#if 1
// NOTE: Gauss-Jordan elimination is yielding unpredictable results due to precission handling and
// numbers near 0.0
AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
const U32 size = rows;
const DATA_TYPE pivot_eps = static_cast<DATA_TYPE>(1e-20); // Smaller epsilon to handle numerical precision
// Create augmented matrix [this | I]
Matrix<DATA_TYPE, size, rows + size> augmentedMatrix;
for (U32 i = 0; i < size; i++)
{
for (U32 j = 0; j < size; j++)
{
augmentedMatrix(i, j) = (*this)(i, j);
augmentedMatrix(i, j + size) = (i == j) ? static_cast<DATA_TYPE>(1) : static_cast<DATA_TYPE>(0);
}
}
// Apply gauss-joran elimination
for (U32 i = 0; i < size; i++)
{
U32 pivotRow = i;
DATA_TYPE pivotValue = std::abs(augmentedMatrix(i, i));
for (U32 k = i + 1; k < size; k++)
{
DATA_TYPE curValue = std::abs(augmentedMatrix(k, i));
if (curValue > pivotValue) {
pivotRow = k;
pivotValue = curValue;
}
}
// Swap if needed.
if (i != pivotRow)
{
for (U32 j = 0; j < 2 * size; j++)
{
DATA_TYPE temp = augmentedMatrix(i, j);
augmentedMatrix(i, j) = augmentedMatrix(pivotRow, j);
augmentedMatrix(pivotRow, j) = temp;
}
}
// Early out if pivot is 0, return identity matrix.
if (std::abs(augmentedMatrix(i, i)) < pivot_eps)
{
return false;
}
DATA_TYPE pivotVal = static_cast<DATA_TYPE>(1.0) / augmentedMatrix(i, i);
// scale the pivot
for (U32 j = 0; j < 2 * size; j++)
{
augmentedMatrix(i, j) *= pivotVal;
}
// Eliminate the current column in all other rows
for (U32 k = 0; k < size; k++)
{
if (k != i)
{
DATA_TYPE factor = augmentedMatrix(k, i);
for (U32 j = 0; j < 2 * size; j++)
{
augmentedMatrix(k, j) -= factor * augmentedMatrix(i, j);
}
}
}
}
for (U32 i = 0; i < size; i++)
{
for (U32 j = 0; j < size; j++)
{
(*this)(i, j) = augmentedMatrix(i, j + size);
}
}
#else
AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
AssertFatal(rows >= 4 && cols >= 4, "Can only perform fullInverse on minimum 4x4 matrix");
Point4F a, b, c, d;
getRow(0, &a);
getRow(1, &b);
getRow(2, &c);
getRow(3, &d);
F32 det = a.x * b.y * c.z * d.w - a.x * b.y * c.w * d.z - a.x * c.y * b.z * d.w + a.x * c.y * b.w * d.z + a.x * d.y * b.z * c.w - a.x * d.y * b.w * c.z
- b.x * a.y * c.z * d.w + b.x * a.y * c.w * d.z + b.x * c.y * a.z * d.w - b.x * c.y * a.w * d.z - b.x * d.y * a.z * c.w + b.x * d.y * a.w * c.z
+ c.x * a.y * b.z * d.w - c.x * a.y * b.w * d.z - c.x * b.y * a.z * d.w + c.x * b.y * a.w * d.z + c.x * d.y * a.z * b.w - c.x * d.y * a.w * b.z
- d.x * a.y * b.z * c.w + d.x * a.y * b.w * c.z + d.x * b.y * a.z * c.w - d.x * b.y * a.w * c.z - d.x * c.y * a.z * b.w + d.x * c.y * a.w * b.z;
if (mFabs(det) < 0.00001f)
return false;
Point4F aa, bb, cc, dd;
aa.x = b.y * c.z * d.w - b.y * c.w * d.z - c.y * b.z * d.w + c.y * b.w * d.z + d.y * b.z * c.w - d.y * b.w * c.z;
aa.y = -a.y * c.z * d.w + a.y * c.w * d.z + c.y * a.z * d.w - c.y * a.w * d.z - d.y * a.z * c.w + d.y * a.w * c.z;
aa.z = a.y * b.z * d.w - a.y * b.w * d.z - b.y * a.z * d.w + b.y * a.w * d.z + d.y * a.z * b.w - d.y * a.w * b.z;
aa.w = -a.y * b.z * c.w + a.y * b.w * c.z + b.y * a.z * c.w - b.y * a.w * c.z - c.y * a.z * b.w + c.y * a.w * b.z;
bb.x = -b.x * c.z * d.w + b.x * c.w * d.z + c.x * b.z * d.w - c.x * b.w * d.z - d.x * b.z * c.w + d.x * b.w * c.z;
bb.y = a.x * c.z * d.w - a.x * c.w * d.z - c.x * a.z * d.w + c.x * a.w * d.z + d.x * a.z * c.w - d.x * a.w * c.z;
bb.z = -a.x * b.z * d.w + a.x * b.w * d.z + b.x * a.z * d.w - b.x * a.w * d.z - d.x * a.z * b.w + d.x * a.w * b.z;
bb.w = a.x * b.z * c.w - a.x * b.w * c.z - b.x * a.z * c.w + b.x * a.w * c.z + c.x * a.z * b.w - c.x * a.w * b.z;
cc.x = b.x * c.y * d.w - b.x * c.w * d.y - c.x * b.y * d.w + c.x * b.w * d.y + d.x * b.y * c.w - d.x * b.w * c.y;
cc.y = -a.x * c.y * d.w + a.x * c.w * d.y + c.x * a.y * d.w - c.x * a.w * d.y - d.x * a.y * c.w + d.x * a.w * c.y;
cc.z = a.x * b.y * d.w - a.x * b.w * d.y - b.x * a.y * d.w + b.x * a.w * d.y + d.x * a.y * b.w - d.x * a.w * b.y;
cc.w = -a.x * b.y * c.w + a.x * b.w * c.y + b.x * a.y * c.w - b.x * a.w * c.y - c.x * a.y * b.w + c.x * a.w * b.y;
dd.x = -b.x * c.y * d.z + b.x * c.z * d.y + c.x * b.y * d.z - c.x * b.z * d.y - d.x * b.y * c.z + d.x * b.z * c.y;
dd.y = a.x * c.y * d.z - a.x * c.z * d.y - c.x * a.y * d.z + c.x * a.z * d.y + d.x * a.y * c.z - d.x * a.z * c.y;
dd.z = -a.x * b.y * d.z + a.x * b.z * d.y + b.x * a.y * d.z - b.x * a.z * d.y - d.x * a.y * b.z + d.x * a.z * b.y;
dd.w = a.x * b.y * c.z - a.x * b.z * c.y - b.x * a.y * c.z + b.x * a.z * c.y + c.x * a.y * b.z - c.x * a.z * b.y;
setRow(0, aa);
setRow(1, bb);
setRow(2, cc);
setRow(3, dd);
mul(1.0f / det);
#endif
return true;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::invert()
{
(*this) = inverse();
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setCrossProduct(const Point3F& p)
{
AssertFatal(rows == 4 && cols == 4, "Cross product only supported on 4x4 for now");
(*this)(0, 0) = 0;
(*this)(0, 1) = -p.z;
(*this)(0, 2) = p.y;
(*this)(0, 3) = 0;
(*this)(1, 0) = p.z;
(*this)(1, 1) = 0;
(*this)(1, 2) = -p.x;
(*this)(1, 3) = 0;
(*this)(2, 0) = -p.y;
(*this)(2, 1) = p.x;
(*this)(2, 2) = 0;
(*this)(2, 3) = 0;
(*this)(3, 0) = 0;
(*this)(3, 1) = 0;
(*this)(3, 2) = 0;
(*this)(3, 3) = 1;
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setTensorProduct(const Point3F& p, const Point3F& q)
{
AssertFatal(rows == 4 && cols == 4, "Tensor product only supported on 4x4 for now");
(*this)(0, 0) = p.x * q.x;
(*this)(0, 1) = p.x * q.y;
(*this)(0, 2) = p.x * q.z;
(*this)(0, 3) = 0;
(*this)(1, 0) = p.y * q.x;
(*this)(1, 1) = p.y * q.y;
(*this)(1, 2) = p.y * q.z;
(*this)(1, 3) = 0;
(*this)(2, 0) = p.z * q.x;
(*this)(2, 1) = p.z * q.y;
(*this)(2, 2) = p.z * q.z;
(*this)(2, 3) = 0;
(*this)(3, 0) = 0;
(*this)(3, 1) = 0;
(*this)(3, 2) = 0;
(*this)(3, 3) = 1;
return (*this);
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::mul(Box3F& box) const
{
AssertFatal(rows == 4 && cols == 4, "Multiplying Box3F with matrix requires 4x4");
// Extract the min and max extents
const Point3F& originalMin = box.minExtents;
const Point3F& originalMax = box.maxExtents;
// Array to store transformed corners
Point3F transformedCorners[8];
// Compute all 8 corners of the box
Point3F corners[8] = {
{originalMin.x, originalMin.y, originalMin.z},
{originalMax.x, originalMin.y, originalMin.z},
{originalMin.x, originalMax.y, originalMin.z},
{originalMax.x, originalMax.y, originalMin.z},
{originalMin.x, originalMin.y, originalMax.z},
{originalMax.x, originalMin.y, originalMax.z},
{originalMin.x, originalMax.y, originalMax.z},
{originalMax.x, originalMax.y, originalMax.z}
};
// Transform each corner
for (U32 i = 0; i < 8; ++i)
{
const Point3F& corner = corners[i];
transformedCorners[i].x = (*this)(0, 0) * corner.x + (*this)(0, 1) * corner.y + (*this)(0, 2) * corner.z + (*this)(0, 3);
transformedCorners[i].y = (*this)(1, 0) * corner.x + (*this)(1, 1) * corner.y + (*this)(1, 2) * corner.z + (*this)(1, 3);
transformedCorners[i].z = (*this)(2, 0) * corner.x + (*this)(2, 1) * corner.y + (*this)(2, 2) * corner.z + (*this)(2, 3);
}
// Initialize min and max extents to the transformed values
Point3F newMin = transformedCorners[0];
Point3F newMax = transformedCorners[0];
// Compute the new min and max extents from the transformed corners
for (U32 i = 1; i < 8; ++i)
{
const Point3F& corner = transformedCorners[i];
if (corner.x < newMin.x) newMin.x = corner.x;
if (corner.y < newMin.y) newMin.y = corner.y;
if (corner.z < newMin.z) newMin.z = corner.z;
if (corner.x > newMax.x) newMax.x = corner.x;
if (corner.y > newMax.y) newMax.y = corner.y;
if (corner.z > newMax.z) newMax.z = corner.z;
}
// Update the box with the new min and max extents
box.minExtents = newMin;
box.maxExtents = newMax;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline bool Matrix<DATA_TYPE, rows, cols>::isAffine() const
{
if ((*this)(3, 3) != 1.0f)
{
return false;
}
for (U32 col = 0; col < cols - 1; ++col)
{
if ((*this)(3, col) != 0.0f)
{
return false;
}
}
Point3F one, two, three;
getColumn(0, &one);
getColumn(1, &two);
getColumn(2, &three);
// check columns
{
if (mDot(one, two) > 0.0001f ||
mDot(one, three) > 0.0001f ||
mDot(two, three) > 0.0001f)
return false;
if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
mFabs(1.0f - two.lenSquared()) > 0.0001f ||
mFabs(1.0f - three.lenSquared()) > 0.0001f)
return false;
}
getRow(0, &one);
getRow(1, &two);
getRow(2, &three);
// check rows
{
if (mDot(one, two) > 0.0001f ||
mDot(one, three) > 0.0001f ||
mDot(two, three) > 0.0001f)
return false;
if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
mFabs(1.0f - two.lenSquared()) > 0.0001f ||
mFabs(1.0f - three.lenSquared()) > 0.0001f)
return false;
}
return true;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::affineInverse()
{
AssertFatal(rows >= 4 && cols >= 4, "affineInverse requires at least 4x4");
Matrix<DATA_TYPE, rows, cols> temp = *this;
// Transpose rotation part
(*this)(0, 1) = temp(1, 0);
(*this)(0, 2) = temp(2, 0);
(*this)(1, 0) = temp(0, 1);
(*this)(1, 2) = temp(2, 1);
(*this)(2, 0) = temp(0, 2);
(*this)(2, 1) = temp(1, 2);
// Adjust translation part
(*this)(0, 3) = -(temp(0, 0) * temp(0, 3) + temp(1, 0) * temp(1, 3) + temp(2, 0) * temp(2, 3));
(*this)(1, 3) = -(temp(0, 1) * temp(0, 3) + temp(1, 1) * temp(1, 3) + temp(2, 1) * temp(2, 3));
(*this)(2, 3) = -(temp(0, 2) * temp(0, 3) + temp(1, 2) * temp(1, 3) + temp(2, 2) * temp(2, 3));
return *this;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline EulerF Matrix<DATA_TYPE, rows, cols>::toEuler() const
{
AssertFatal(rows >= 3 && cols >= 3, "Euler rotations require at least a 3x3 matrix.");
// like all others assume float for now.
EulerF r;
r.x = mAsin(mClampF((*this)(1,2), -1.0, 1.0));
if (mCos(r.x) != 0.0f)
{
r.y = mAtan2(-(*this)(0, 2), (*this)(2, 2)); // yaw
r.z = mAtan2(-(*this)(1, 0), (*this)(1, 1)); // roll
}
else
{
r.y = 0.0f;
r.z = mAtan2((*this)(0, 1), (*this)(0, 0)); // this rolls when pitch is +90 degrees
}
return r;
}
template<typename DATA_TYPE, U32 rows, U32 cols>
inline void Matrix<DATA_TYPE, rows, cols>::dumpMatrix(const char* caption) const
{
U32 size = (caption == NULL) ? 0 : dStrlen(caption);
FrameTemp<char> spacer(size + 1);
char* spacerRef = spacer;
// is_floating_point should return true for floats and doubles.
const char* formatSpec = std::is_floating_point_v<DATA_TYPE> ? " %-8.4f" : " %d";
dMemset(spacerRef, ' ', size);
// null terminate.
spacerRef[size] = '\0';
/*Con::printf("%s = | %-8.4f %-8.4f %-8.4f %-8.4f |", caption, m[idx(0, 0)], m[idx(0, 1)], m[idx(0, 2)], m[idx(0, 3)]);
Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(1, 0)], m[idx(1, 1)], m[idx(1, 2)], m[idx(1, 3)]);
Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(2, 0)], m[idx(2, 1)], m[idx(2, 2)], m[idx(2, 3)]);
Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(3, 0)], m[idx(3, 1)], m[idx(3, 2)], m[idx(3, 3)]);*/
StringBuilder str;
str.format("%s = |", caption);
for (U32 i = 0; i < rows; i++)
{
if (i > 0)
{
str.append(spacerRef);
}
for (U32 j = 0; j < cols; j++)
{
str.format(formatSpec, (*this)(i, j));
}
str.append(" |\n");
}
Con::printf("%s", str.end().c_str());
}
//------------------------------------
// Non-member methods
//------------------------------------
inline void mTransformPlane(
const MatrixF& mat,
const Point3F& scale,
const PlaneF& plane,
PlaneF* result
) {
// Create the inverse scale matrix
MatrixF invScale(true);
invScale(0, 0) = 1.0f / scale.x;
invScale(1, 1) = 1.0f / scale.y;
invScale(2, 2) = 1.0f / scale.z;
const Point3F shear(mat(0, 3), mat(1, 3), mat(2, 3));
const Point3F row0 = mat.getRow3F(0);
const Point3F row1 = mat.getRow3F(1);
const Point3F row2 = mat.getRow3F(2);
const F32 A = -mDot(row0, shear);
const F32 B = -mDot(row1, shear);
const F32 C = -mDot(row2, shear);
// Compute the inverse transpose of the matrix
MatrixF invTrMatrix(true);
invTrMatrix(0, 0) = mat(0, 0);
invTrMatrix(0, 1) = mat(0, 1);
invTrMatrix(0, 2) = mat(0, 2);
invTrMatrix(1, 0) = mat(1, 0);
invTrMatrix(1, 1) = mat(1, 1);
invTrMatrix(1, 2) = mat(1, 2);
invTrMatrix(2, 0) = mat(2, 0);
invTrMatrix(2, 1) = mat(2, 1);
invTrMatrix(2, 2) = mat(2, 2);
invTrMatrix(3, 0) = A;
invTrMatrix(3, 1) = B;
invTrMatrix(3, 2) = C;
invTrMatrix.mul(invScale);
// Transform the plane normal
Point3F norm(plane.x, plane.y, plane.z);
invTrMatrix.mulP(norm);
norm.normalize();
// Transform the plane point
Point3F point = norm * -plane.d;
MatrixF temp = mat;
point.x *= scale.x;
point.y *= scale.y;
point.z *= scale.z;
temp.mulP(point);
// Recompute the plane distance
PlaneF resultPlane(point, norm);
result->x = resultPlane.x;
result->y = resultPlane.y;
result->z = resultPlane.z;
result->d = resultPlane.d;
}
//--------------------------------------------
// INLINE FUNCTIONS END
//--------------------------------------------
typedef Matrix<F32, 4, 4> MatrixF;
class MatrixTemplateExport
{
public:
template <typename T, U32 rows, U32 cols>
static EngineFieldTable::Field getMatrixField();
};
template<typename T, U32 rows, U32 cols>
inline EngineFieldTable::Field MatrixTemplateExport::getMatrixField()
{
typedef Matrix<T, rows, cols> ThisType;
return _FIELD_AS(T, data, data, rows * cols, "");
}
#endif // !USE_TEMPLATE_MATRIX
#endif //_MMATRIX_H_
Reference in a new issue View git blame Copy permalink