diff --git a/Engine/source/T3D/gameBase/gameConnection.h b/Engine/source/T3D/gameBase/gameConnection.h index c9e32e14b..61558be3d 100644 --- a/Engine/source/T3D/gameBase/gameConnection.h +++ b/Engine/source/T3D/gameBase/gameConnection.h @@ -52,8 +52,12 @@ enum GameConnectionConstants class IDisplayDevice; class SFXProfile; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; -class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class Point3F; class MoveManager; class MoveList; diff --git a/Engine/source/T3D/physics/physicsCollision.h b/Engine/source/T3D/physics/physicsCollision.h index d6e51f843..0f4e5a095 100644 --- a/Engine/source/T3D/physics/physicsCollision.h +++ b/Engine/source/T3D/physics/physicsCollision.h @@ -28,7 +28,12 @@ #endif class Point3F; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class PlaneF; @@ -84,4 +89,4 @@ public: const MatrixF &localXfm ) = 0; }; -#endif // _T3D_PHYSICS_PHYSICSCOLLISION_H_ \ No newline at end of file +#endif // _T3D_PHYSICS_PHYSICSCOLLISION_H_ diff --git a/Engine/source/T3D/physics/physicsObject.h b/Engine/source/T3D/physics/physicsObject.h index 12697ce96..4bc7a38f3 100644 --- a/Engine/source/T3D/physics/physicsObject.h +++ b/Engine/source/T3D/physics/physicsObject.h @@ -34,7 +34,12 @@ #endif class PhysicsWorld; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class Point3F; class Box3F; @@ -88,4 +93,4 @@ protected: U32 mQueuedEvent; }; -#endif // _T3D_PHYSICS_PHYSICSOBJECT_H_ \ No newline at end of file +#endif // _T3D_PHYSICS_PHYSICSOBJECT_H_ diff --git a/Engine/source/console/propertyParsing.h b/Engine/source/console/propertyParsing.h index 90272f9da..f782df2b4 100644 --- a/Engine/source/console/propertyParsing.h +++ b/Engine/source/console/propertyParsing.h @@ -35,7 +35,12 @@ class RectI; class RectF; class Box3I; class Box3F; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class AngAxisF; class QuatF; class String; diff --git a/Engine/source/core/stream/bitStream.h b/Engine/source/core/stream/bitStream.h index dc2107519..0fe5895d6 100644 --- a/Engine/source/core/stream/bitStream.h +++ b/Engine/source/core/stream/bitStream.h @@ -45,7 +45,12 @@ // class Point3F; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class HuffmanProcessor; class BitVector; class QuatF; diff --git a/Engine/source/gfx/gfxShader.h b/Engine/source/gfx/gfxShader.h index b7f23a9e3..edb20a127 100644 --- a/Engine/source/gfx/gfxShader.h +++ b/Engine/source/gfx/gfxShader.h @@ -60,7 +60,12 @@ class Point2I; class Point2F; class LinearColorF; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class GFXShader; class GFXVertexFormat; diff --git a/Engine/source/math/mAngAxis.h b/Engine/source/math/mAngAxis.h index a5adca3a7..02845902b 100644 --- a/Engine/source/math/mAngAxis.h +++ b/Engine/source/math/mAngAxis.h @@ -27,7 +27,13 @@ #include "math/mPoint3.h" #endif +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif + class QuatF; //---------------------------------------------------------------------------- diff --git a/Engine/source/math/mBox.h b/Engine/source/math/mBox.h index b81775bb0..1cff4ccc9 100644 --- a/Engine/source/math/mBox.h +++ b/Engine/source/math/mBox.h @@ -36,7 +36,12 @@ #endif +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class SphereF; diff --git a/Engine/source/math/mMatrix.cpp b/Engine/source/math/mMatrix.cpp index d8205838f..727caab42 100644 --- a/Engine/source/math/mMatrix.cpp +++ b/Engine/source/math/mMatrix.cpp @@ -29,6 +29,8 @@ #include "console/enginePrimitives.h" #include "console/engineTypes.h" +#ifndef USE_TEMPLATE_MATRIX + const MatrixF MatrixF::Identity( true ); // idx(i,j) is index to element in column i, row j @@ -209,3 +211,12 @@ EngineFieldTable::Field MatrixFEngineExport::getMatrixField() typedef MatrixF ThisType; return _FIELD_AS(F32, m, m, 16, ""); } + +#else // !USE_TEMPLATE_MATRIX + +//------------------------------------ +// Templatized matrix class to replace MATRIXF above +// due to templated class, all functions need to be inline +//------------------------------------ + +#endif // !USE_TEMPLATE_MATRIX diff --git a/Engine/source/math/mMatrix.h b/Engine/source/math/mMatrix.h index 18981eeea..47787acc5 100644 --- a/Engine/source/math/mMatrix.h +++ b/Engine/source/math/mMatrix.h @@ -39,6 +39,19 @@ #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 USE_TEMPLATE_MATRIX /// 4x4 Matrix Class /// @@ -114,6 +127,10 @@ public: EulerF toEuler() const; + F32 determinant() const { + return m_matF_determinant(*this); + } + /// Compute the inverse of the matrix. /// /// Computes inverse of full 4x4 matrix. Returns false and performs no inverse if @@ -620,4 +637,1386 @@ inline void mTransformPlane(const MatrixF& mat, const Point3F& scale, const Plan 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 +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(1); + } + } + } + } + + explicit Matrix(const EulerF& e) { + set(e); + } + + ~Matrix() = default; + + /// Make this an identity matrix. + Matrix& identity(); + void reverseProjection(); + void normalize(); + + Matrix& set(const EulerF& e); + + Matrix(const EulerF& e, const Point3F p); + Matrix& set(const EulerF& e, const Point3F p); + + Matrix& inverse(); + Matrix& transpose(); + void invert(); + + Matrix& setCrossProduct(const Point3F& p); + Matrix& setTensorProduct(const Point3F& p, const Point3F& q); + + /// M * Matrix(p) -> M + Matrix& scale(const Point3F& s); + Matrix& 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& mul(const Matrix& a) + { return *this = *this * a; } + ///< a * M -> M + Matrix& mulL(const Matrix& a) + { return *this = a * *this; } + ///< a * b -> M + Matrix& mul(const Matrix& a, const Matrix& b) + { return *this = a * b; } + ///< M * a -> M + Matrix& mul(const F32 a) + { return *this = *this * a; } + ///< a * b -> M + Matrix& mul(const Matrix& a, const F32 b) + { return *this = a * b; } + Matrix& add(const Matrix& 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& 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& 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* matrix) const; + + void dumpMatrix(const char* caption = NULL) const; + // Static identity matrix + static const Matrix Identity; + + // ------ Operators ------ + friend Matrix operator*(const Matrix& m1, const Matrix& m2) { + Matrix result; + + for (U32 i = 0; i < rows; ++i) { + for (U32 j = 0; j < cols; ++j) + { + result(i, j) = static_cast(0); + for (U32 k = 0; k < cols; ++k) + { + result(i, j) += m1(i, k) * m2(k, j); + } + } + } + + return result; + } + + Matrix operator *= (const Matrix& other) { + *this = *this * other; + return *this; + } + + Matrix operator+(const Matrix& m2) { + Matrix 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 operator+=(const Matrix& m2) { + for (U32 i = 0; i < rows; ++i) + { + for (U32 j = 0; j < cols; ++j) + { + (*this)(i, j) += m2(i, j); + } + } + + return (*this); + } + + Matrix operator * (const DATA_TYPE scalar) const { + Matrix 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& 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& operator = (const Matrix& other) { + if (this != &other) { + std::copy(other.data, other.data + rows * cols, this->data); + } + + return *this; + } + + bool operator == (const Matrix& 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& 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 +inline Matrix& Matrix::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 +inline Matrix& Matrix::identity() +{ + for (U32 i = 0; i < rows; i++) + { + for (U32 j = 0; j < cols; j++) + { + if (j == i) + (*this)(i, j) = static_cast(1); + else + (*this)(i, j) = static_cast(0); + } + } + + return (*this); +} + +template +inline void Matrix::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 +inline Matrix& Matrix::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 +inline bool Matrix::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(1)) + { + return false; + } + } + else + { + if((*this)(i, j) != static_cast(0)) + { + return false; + } + } + } + } + + return true; +} + +template +inline Point3F Matrix::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 +inline Point3F Matrix::getPosition() const +{ + Point3F pos; + getColumn(3, &pos); + return pos; +} + +template +inline void Matrix::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 +inline void Matrix::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 +inline void Matrix::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 +inline void Matrix::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 +inline void Matrix::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 +inline void Matrix::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 +inline VectorF Matrix::getRightVector() const +{ + VectorF vec; + getColumn(0, &vec); + return vec; +} + +template +inline VectorF Matrix::getForwardVector() const +{ + VectorF vec; + getColumn(1, &vec); + return vec; +} + +template +inline VectorF Matrix::getUpVector() const +{ + VectorF vec; + getColumn(2, &vec); + return vec; +} + +template +inline void Matrix::invertTo(Matrix* matrix) const +{ + Matrix 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 +inline void Matrix::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 +inline void Matrix::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 +inline void Matrix::displace(const Point3F& delta) +{ + (*this)(0, 3) += delta.x; + (*this)(1, 3) += delta.y; + (*this)(2, 3) += delta.z; +} + + +template +inline void Matrix::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 +const Matrix Matrix::Identity = []() { + Matrix identity(true); + return identity; +}(); + +template +inline Matrix& Matrix::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::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(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 +Matrix::Matrix(const EulerF& e, const Point3F p) +{ + set(e, p); +} + +template +inline Matrix& Matrix::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 +inline Matrix& Matrix::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(1e-20); // Smaller epsilon to handle numerical precision + + // Create augmented matrix [this | I] + Matrix 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(1) : static_cast(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(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(1e-10)) { + this->identity(); // Return the identity matrix if the determinant is zero + return *this; + } + + DATA_TYPE invDet = DATA_TYPE(1) / det; + + Matrix 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 +inline bool Matrix::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(1e-20); // Smaller epsilon to handle numerical precision + + // Create augmented matrix [this | I] + Matrix 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(1) : static_cast(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(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 +inline void Matrix::invert() +{ + (*this) = inverse(); +} + +template +inline Matrix& Matrix::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 +inline Matrix& Matrix::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 +inline void Matrix::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 +inline bool Matrix::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 +inline Matrix& Matrix::affineInverse() +{ + AssertFatal(rows >= 4 && cols >= 4, "affineInverse requires at least 4x4"); + Matrix 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 +inline EulerF Matrix::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 +inline void Matrix::dumpMatrix(const char* caption) const +{ + U32 size = (caption == NULL) ? 0 : dStrlen(caption); + FrameTemp spacer(size + 1); + char* spacerRef = spacer; + + // is_floating_point should return true for floats and doubles. + const char* formatSpec = std::is_floating_point_v ? " %-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 MatrixF; + +class MatrixTemplateExport +{ +public: + template + static EngineFieldTable::Field getMatrixField(); +}; + +template +inline EngineFieldTable::Field MatrixTemplateExport::getMatrixField() +{ + typedef Matrix ThisType; + return _FIELD_AS(T, data, data, rows * cols, ""); +} + +#endif // !USE_TEMPLATE_MATRIX + #endif //_MMATRIX_H_ diff --git a/Engine/source/math/mOrientedBox.h b/Engine/source/math/mOrientedBox.h index 422dd8f27..9af32874e 100644 --- a/Engine/source/math/mOrientedBox.h +++ b/Engine/source/math/mOrientedBox.h @@ -32,7 +32,12 @@ #endif +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class Box3F; diff --git a/Engine/source/math/mQuat.h b/Engine/source/math/mQuat.h index d713b405d..1af1a5a95 100644 --- a/Engine/source/math/mQuat.h +++ b/Engine/source/math/mQuat.h @@ -27,7 +27,12 @@ #include "math/mPoint3.h" #endif +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class AngAxisF; //---------------------------------------------------------------------------- diff --git a/Engine/source/math/mathTypes.cpp b/Engine/source/math/mathTypes.cpp index 5beb3f9b0..ef7616340 100644 --- a/Engine/source/math/mathTypes.cpp +++ b/Engine/source/math/mathTypes.cpp @@ -101,6 +101,7 @@ IMPLEMENT_STRUCT( RectF, FIELD( extent, extent, 1, "The width and height of the Rect.") END_IMPLEMENT_STRUCT; +#ifndef USE_TEMPLATE_MATRIX IMPLEMENT_STRUCT( MatrixF, MatrixF, MathTypes, "" ) @@ -108,6 +109,15 @@ IMPLEMENT_STRUCT( MatrixF, MatrixFEngineExport::getMatrixField(), END_IMPLEMENT_STRUCT; +#else +IMPLEMENT_STRUCT(MatrixF, +MatrixF, MathTypes, +"") + +MatrixTemplateExport::getMatrixField(), + +END_IMPLEMENT_STRUCT; +#endif IMPLEMENT_STRUCT( AngAxisF, AngAxisF, MathTypes, "" ) diff --git a/Engine/source/math/mathTypes.h b/Engine/source/math/mathTypes.h index 043d59e1f..08818f685 100644 --- a/Engine/source/math/mathTypes.h +++ b/Engine/source/math/mathTypes.h @@ -38,7 +38,12 @@ class Point3F; class Point4F; class RectI; class RectF; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class Box3F; class EaseF; class AngAxisF; diff --git a/Engine/source/math/test/mMatrixTest.cpp b/Engine/source/math/test/mMatrixTest.cpp deleted file mode 100644 index b5276ce22..000000000 --- a/Engine/source/math/test/mMatrixTest.cpp +++ /dev/null @@ -1,93 +0,0 @@ -//----------------------------------------------------------------------------- -// Copyright (c) 2014 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. -//----------------------------------------------------------------------------- - -#ifdef TORQUE_TESTS_ENABLED -#include "testing/unitTesting.h" -#include "platform/platform.h" -#include "math/mMatrix.h" -#include "math/mRandom.h" - -extern void default_matF_x_matF_C(const F32 *a, const F32 *b, F32 *mresult); -extern void mInstallLibrary_ASM(); - -// If we're x86 and not Mac, then include these. There's probably a better way to do this. -#if defined(WIN32) && defined(TORQUE_CPU_X86) -void Athlon_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result); -void SSE_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result); -#endif - -#if defined( __VEC__ ) -extern void vec_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result); -#endif - -TEST(MatrixF, MultiplyImplmentations) -{ - F32 m1[16], m2[16], mrC[16]; - - // I am not positive that the best way to do this is to use random numbers - // but I think that using some kind of standard matrix may not always catch - // all problems. - for (S32 i = 0; i < 16; i++) - { - m1[i] = gRandGen.randF(); - m2[i] = gRandGen.randF(); - } - - // C will be the baseline - default_matF_x_matF_C(m1, m2, mrC); - -#if defined(WIN32) && defined(TORQUE_CPU_X86) - // Check the CPU info - U32 cpuProperties = Platform::SystemInfo.processor.properties; - bool same; - - // Test SSE if it is available - F32 mrSSE[16]; - if (cpuProperties & CPU_PROP_SSE) - { - SSE_MatrixF_x_MatrixF(m1, m2, mrSSE); - - same = true; - for (S32 i = 0; i < 16; i++) - same &= mIsEqual(mrC[i], mrSSE[i]); - - EXPECT_TRUE(same) << "Matrix multiplication verification failed. (C vs. SSE)"; - } - - same = true; -#endif - - // If Altivec exists, test it! -#if defined(__VEC__) - bool same = false; - F32 mrVEC[16]; - - vec_MatrixF_x_MatrixF(m1, m2, mrVEC); - - for (S32 i = 0; i < 16; i++) - same &= isEqual(mrC[i], mrVEC[i]); - - EXPECT_TRUE(same) << "Matrix multiplication verification failed. (C vs. Altivec)"; -#endif -} - -#endif \ No newline at end of file diff --git a/Engine/source/math/util/matrixSetDelegateMethods.h b/Engine/source/math/util/matrixSetDelegateMethods.h index f5d237472..c2b375814 100644 --- a/Engine/source/math/util/matrixSetDelegateMethods.h +++ b/Engine/source/math/util/matrixSetDelegateMethods.h @@ -22,7 +22,7 @@ #ifndef _MATRIXSETDELEGATES_H_ #define _MATRIXSETDELEGATES_H_ - // Access to the direct value +#ifndef USE_TEMPLATE_MATRIX #define MATRIX_SET_GET_VALUE_FN(xfm) _transform_##xfm #define MATRIX_SET_GET_VALUE(xfm) inline const MatrixF &MATRIX_SET_GET_VALUE_FN(xfm)() { return mTransform[xfm]; } #define MATRIX_SET_BIND_VALUE(xfm) mEvalDelegate[xfm].bind(this, &MatrixSet::MATRIX_SET_GET_VALUE_FN(xfm)) @@ -44,5 +44,28 @@ return mTransform[matC]; \ } +#else + // Access to the direct value +#define MATRIX_SET_GET_VALUE_FN(xfm) _transform_##xfm +#define MATRIX_SET_GET_VALUE(xfm) inline const MatrixF &MATRIX_SET_GET_VALUE_FN(xfm)() { return mTransform[xfm]; } +#define MATRIX_SET_BIND_VALUE(xfm) mEvalDelegate[xfm].bind(this, &MatrixSet::MATRIX_SET_GET_VALUE_FN(xfm)) +#define MATRIX_SET_IS_INVERSE_OF_FN(inv_xfm, src_xfm) _##inv_xfm##_is_inverse_of_##src_xfm +#define MATRIX_SET_IS_INVERSE_OF(inv_xfm, src_xfm) inline const MatrixF &MATRIX_SET_IS_INVERSE_OF_FN(inv_xfm, src_xfm)() \ + { \ + mEvalDelegate[src_xfm]().invertTo(&mTransform[inv_xfm]); \ + MATRIX_SET_BIND_VALUE(inv_xfm); \ + return mTransform[inv_xfm]; \ + } + + +#define MATRIX_SET_MULT_ASSIGN_FN(matA, matB, matC) _##matC##_is_##matA##_x_##matB +#define MATRIX_SET_MULT_ASSIGN(matA, matB, matC) inline const MatrixF &MATRIX_SET_MULT_ASSIGN_FN(matA, matB, matC)() \ + { \ + mTransform[matC].mul(mEvalDelegate[matA](),mEvalDelegate[matB]()); \ + MATRIX_SET_BIND_VALUE(matC); \ + return mTransform[matC]; \ + } + +#endif #endif // _MATRIXSETDELEGATES_H_ diff --git a/Engine/source/scene/sgUtil.h b/Engine/source/scene/sgUtil.h index be83c7c57..4a5b982eb 100644 --- a/Engine/source/scene/sgUtil.h +++ b/Engine/source/scene/sgUtil.h @@ -32,7 +32,12 @@ class Frustum; class RectI; +#ifndef USE_TEMPLATE_MATRIX class MatrixF; +#else +template class Matrix; +typedef Matrix MatrixF; +#endif class PlaneF; struct SGWinding diff --git a/Engine/source/testing/mathMatrixTest.cpp b/Engine/source/testing/mathMatrixTest.cpp new file mode 100644 index 000000000..57a8b49d3 --- /dev/null +++ b/Engine/source/testing/mathMatrixTest.cpp @@ -0,0 +1,1014 @@ +#include "testing/unitTesting.h" + +#include "platform/platform.h" +#include "console/simBase.h" +#include "console/consoleTypes.h" +#include "console/scriptObjects.h" +#include "console/simBase.h" +#include "console/engineAPI.h" +#include "math/mMath.h" +#include "math/util/frustum.h" +#include "math/mathUtils.h" + +TEST(MatrixTest, TestIdentityInit) +{ + MatrixF test(true); + + EXPECT_NEAR(test(0, 0), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 1), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestIdentitySet) +{ + MatrixF test(false); + test.identity(); + + EXPECT_NEAR(test(0, 0), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 1), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestIsIdentity) +{ + MatrixF test(true); + EXPECT_TRUE(test.isIdentity()); +} + +TEST(MatrixTest, TestEulerInit) +{ + MatrixF test(EulerF(1.0f, 0.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + + +TEST(MatrixTest, TestEulerSet) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestToEuler) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f)); + + EulerF euler = test.toEuler(); + + EXPECT_NEAR(euler.x, 1.0f, POINT_EPSILON); EXPECT_NEAR(euler.y, 0.0f, POINT_EPSILON); EXPECT_NEAR(euler.z, 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestEulerPointInit) +{ + MatrixF test(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + + +TEST(MatrixTest, TestEulerPointSet) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestGetColumn4) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column; + + test.getColumn(0, &column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON); + + test.getColumn(1, &column); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON); + + test.getColumn(2, &column); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON); + + test.getColumn(3, &column); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); + +} + +TEST(MatrixTest, TestGetRow4) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column; + + test.getRow(0, &column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON); + + test.getRow(1, &column); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON); + + test.getRow(2, &column); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON); + + test.getRow(3, &column); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); +} + +TEST(MatrixTest, TestGetColumn3) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column; + + test.getColumn(0, &column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + + test.getColumn(1, &column); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + + test.getColumn(2, &column); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + test.getColumn(3, &column); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); + +} + +TEST(MatrixTest, TestGetRow3) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column; + + test.getRow(0, &column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + + test.getRow(1, &column); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + + test.getRow(2, &column); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + test.getRow(3, &column); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); +} + +TEST(MatrixTest, TestGetColumn4F) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column; + + column = test.getColumn4F(0); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON); + + column = test.getColumn4F(1); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON); + + column = test.getColumn4F(2); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON); + + column = test.getColumn4F(3); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); + +} + +TEST(MatrixTest, TestGetRow4F) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column; + + column = test.getRow4F(0); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON); + + column = test.getRow4F(1); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON); + + column = test.getRow4F(2); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON); + + column = test.getRow4F(3); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); +} + +TEST(MatrixTest, TestGetColumn3F) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column; + + column = test.getColumn3F(0); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + + column = test.getColumn3F(1); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + + column = test.getColumn3F(2); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + column = test.getColumn3F(3); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); + +} + +TEST(MatrixTest, TestGetRow3F) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column; + + column = test.getRow3F(0); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + + column = test.getRow3F(1); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + + column = test.getRow3F(2); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + column = test.getRow3F(3); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); +} + +TEST(MatrixTest, TestSetColumn4) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column(1.0f, 1.0f, 1.0f, 1.0f); + + test.setColumn(0, column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON); + + test.setColumn(1, column); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON); + + test.setColumn(2, column); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON); + + test.setColumn(3, column); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); +} + +TEST(MatrixTest, TestSetRow4) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point4F column(1.0f, 1.0f, 1.0f, 1.0f); + + test.setRow(0, column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON); + + test.setRow(1, column); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON); + + test.setRow(2, column); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON); + + test.setRow(3, column); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); + EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON); +} + +TEST(MatrixTest, TestSetColumn3) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column(1.0f, 1.0f, 1.0f); + + test.setColumn(0, column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON); + + test.setColumn(1, column); + + EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON); + + test.setColumn(2, column); + + EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + test.setColumn(3, column); + + EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON); +} + +TEST(MatrixTest, TestSetRow3) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column(1.0f, 1.0f, 1.0f); + + test.setRow(0, column); + + EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON); + + test.setRow(1, column); + + EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON); + + test.setRow(2, column); + + EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON); + + test.setRow(3, column); + + EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON); + EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON); + EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON); +} + +TEST(MatrixTest, TestDisplace) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + Point3F column(1.0f, 1.0f, 1.0f); + + test.displace(column); + + EXPECT_NEAR(test(0, 3), 6.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 3), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 3), 2.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestGetVectorFunctions) +{ + MatrixF test(true); + + test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f)); + + VectorF vector; + + vector = test.getRightVector(); + + EXPECT_NEAR(vector.x, test(0, 0), POINT_EPSILON); + EXPECT_NEAR(vector.y, test(1, 0), POINT_EPSILON); + EXPECT_NEAR(vector.z, test(2, 0), POINT_EPSILON); + + vector = test.getForwardVector(); + + EXPECT_NEAR(vector.x, test(0, 1), POINT_EPSILON); + EXPECT_NEAR(vector.y, test(1, 1), POINT_EPSILON); + EXPECT_NEAR(vector.z, test(2, 1), POINT_EPSILON); + + vector = test.getUpVector(); + + EXPECT_NEAR(vector.x, test(0, 2), POINT_EPSILON); + EXPECT_NEAR(vector.y, test(1, 2), POINT_EPSILON); + EXPECT_NEAR(vector.z, test(2, 2), POINT_EPSILON); +} + +TEST(MatrixTest, TestSetCrossProduct) +{ + MatrixF test(true); + + test.setCrossProduct(Point3F(5.0f, 0.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -1.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 1.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.0, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -5.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 5.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestSetTensorProduct) +{ + MatrixF test(true); + + test.setTensorProduct(Point3F(5.0f, 2.0f, 1.0f), Point3F(5.0f, 2.0f, 1.0f)); + + EXPECT_NEAR(test(0, 0), 25.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 10.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 5.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 10.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 4.0, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 2.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 5.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 2.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulFunction) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mul(test2); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulOperator) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test = test * test2; + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulLFunction) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 4.3845f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -0.8479f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 3.1714, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulArgMatrixFunction) +{ + MatrixF testResult(true); + + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + testResult.mul(test2, test); + + EXPECT_NEAR(testResult(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 3), 4.3845f, POINT_EPSILON); + EXPECT_NEAR(testResult(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 3), -0.8479f, POINT_EPSILON); + EXPECT_NEAR(testResult(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 3), 3.1714, POINT_EPSILON); + EXPECT_NEAR(testResult(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulArgMultipleRotationMatrix) +{ + MatrixF testResult(true); + + MatrixF xRot(EulerF(-1.54f, 0.0f, 0.0f)); + MatrixF zRot(EulerF(0.0f, 0.0f, -1.57f)); + + testResult.mul(zRot, xRot); + + EXPECT_NEAR(testResult(0, 0), 0.0008f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 1), -0.0308f, POINT_EPSILON);EXPECT_NEAR(testResult(0, 2), 0.9995f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(testResult(1, 0), 1.0f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 2), -0.0008f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(testResult(2, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 1), 0.9995f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 2), 0.0308f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(testResult(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(testResult(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulScalarFunction) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.mul(2.0f); + + EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 8.7689f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -1.6958f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 6.3427f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 2.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulMatScalarFunction) +{ + MatrixF testTran(true); + testTran.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + testTran.mulL(test2); + + MatrixF test(true); + test.mul(testTran, 2.0f); + + EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 8.7689f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -1.6958f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 6.3427f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 2.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulPoint4) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + Point4F testPoint(0.5f, 1.0f, 2.0f, 1.0f); + test.mul(testPoint); + + EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON); EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON); EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON); EXPECT_NEAR(testPoint.w, 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulPoint3) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + Point3F testPoint(0.5f, 1.0f, 2.0f); + test.mulP(testPoint); + + EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON); EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON); EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulPoint3ToPoint3) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + Point3F point(0.5f, 1.0f, 2.0f); + Point3F testPoint; + test.mulP(point, &testPoint); + + EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON); EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON); EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulVector) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + VectorF testPoint(0.5f, 1.0f, 2.0f); + test.mulV(testPoint); + + EXPECT_NEAR(testPoint.x, 1.1116f, POINT_EPSILON); EXPECT_NEAR(testPoint.y, 1.7475f, POINT_EPSILON); EXPECT_NEAR(testPoint.z, 0.9799f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulVectorToPoint3) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + VectorF vec(0.5f, 1.0f, 2.0f); + Point3F testPoint; + test.mulV(vec, &testPoint); + + EXPECT_NEAR(testPoint.x, 1.1116f, POINT_EPSILON); EXPECT_NEAR(testPoint.y, 1.7475f, POINT_EPSILON); EXPECT_NEAR(testPoint.z, 0.9799f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMulBox) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + Box3F testBox(1.0f); + + test.mul(testBox); + + EXPECT_NEAR(testBox.minExtents.x, 4.5f, POINT_EPSILON); EXPECT_NEAR(testBox.minExtents.y, 1.5f, POINT_EPSILON); EXPECT_NEAR(testBox.minExtents.z, 0.5f, POINT_EPSILON); + EXPECT_NEAR(testBox.maxExtents.x, 5.5f, POINT_EPSILON); EXPECT_NEAR(testBox.maxExtents.y, 2.5f, POINT_EPSILON); EXPECT_NEAR(testBox.maxExtents.z, 1.5f, POINT_EPSILON); +} + +TEST(MatrixTest, TestMatrixAdd) +{ + MatrixF test(true); + MatrixF test2(true); + for (U32 i = 0; i < 4; i++) + { + for (U32 j = 0; j < 4; j++) + { + test(i, j) = 1.0f; + test2(i, j) = 1.0f; + } + } + + test.add(test2); + + for (U32 i = 0; i < 4; i++) + { + for (U32 j = 0; j < 4; j++) + { + EXPECT_NEAR(test(i,j), 2.0f, POINT_EPSILON); + } + } + +} + +TEST(MatrixTest, TestFrustumProjectionMatrix) +{ + MatrixF test(true); + Frustum testFrustum(false, -0.119894862f, 0.119894862f, 0.0767327100f, -0.0767327100f, 0.1f, 1000.0f); + + testFrustum.getProjectionMatrix(&test); + + EXPECT_NEAR(test(0, 0), 0.8341f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 1.3032f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.0001f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.1f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON); + + test.reverseProjection(); + + EXPECT_NEAR(test(0, 0), 0.8341f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 1.3032f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 1.0001f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), -0.1f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON); + + test.inverse(); + + EXPECT_NEAR(test(0, 0), 1.1989f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 0.9999f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.1f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.7673f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON); + +} + +TEST(MatrixTest, TestUnProjectStack) +{ + MatrixF saveModel(true); + MatrixF saveProjection(true); + RectI renderRect(0 ,0, 1600, 1024); + + Frustum testFrustum(false, -0.119894862f, 0.119894862f, 0.0767327100f, -0.0767327100f, 0.1f, 1000.0f); + + testFrustum.getProjectionMatrix(&saveProjection); + + Point3F dest; + MathUtils::mProjectScreenToWorld(Point3F(626.0f, 600.0f, 1.0f), &dest, renderRect, saveModel, saveProjection, 0.1f, 10.0f); + EXPECT_NEAR(dest.x, -0.2868f, POINT_EPSILON); EXPECT_NEAR(dest.y, 1.1998f, POINT_EPSILON); EXPECT_NEAR(dest.z, -0.1450f, POINT_EPSILON); +} + +TEST(MatrixTest, TestInverse) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.inverse(); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestInvertTo) +{ + MatrixF test1(true); + test1.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test1.mulL(test2); + + MatrixF test(true); + + test1.invertTo(&test); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); + + // make sure our original matrix is unchanged + EXPECT_NEAR(test1(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test1(0, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test1(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test1(0, 3), 4.3845f, POINT_EPSILON); + EXPECT_NEAR(test1(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test1(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test1(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test1(1, 3), -0.8479f, POINT_EPSILON); + EXPECT_NEAR(test1(2, 0), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test1(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test1(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test1(2, 3), 3.1714, POINT_EPSILON); + EXPECT_NEAR(test1(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test1(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test1(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test1(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestFullInverse) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + EXPECT_TRUE(test.fullInverse()); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestIsAffine) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + EXPECT_TRUE(test.isAffine()); +} + +TEST(MatrixTest, TestScale) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.scale(2.0f); + + EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 4.3845f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -0.8479f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON); EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 3.1714f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestGetScale) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.scale(2.0f); + + Point3F scale; + scale = test.getScale(); + + EXPECT_NEAR(scale.x, 2.0f, POINT_EPSILON); EXPECT_NEAR(scale.y, 2.0f, POINT_EPSILON); EXPECT_NEAR(scale.z, 2.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestAffineInverse) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.affineInverse(); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestTranspose) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + test.transpose(); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 4.3845f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), -0.8479f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 3.1714, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestTransposeTo) +{ + MatrixF test1(true); + test1.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test1.mulL(test2); + + MatrixF test(true); + + test1.transposeTo(test); + + EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON); EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON); EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON); EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON); + EXPECT_NEAR(test(3, 0), 4.3845f, POINT_EPSILON); EXPECT_NEAR(test(3, 1), -0.8479f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 3.1714, POINT_EPSILON); EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON); +} + +TEST(MatrixTest, TestTransformPlane) +{ + MatrixF test(true); + test.setPosition(Point3F(5.0f, 2.0f, 1.0f)); + MatrixF test2(EulerF(1.0f, 0.0f, 1.0f)); + + test.mulL(test2); + + PlaneF plane(Point3F(0.0f, 0.0f, 0.0f), Point3F(0.0f, 0.0f, 1.0f)); + + PlaneF res; + mTransformPlane(test, Point3F(1.0f, 1.0f, 1.0f), plane, &res); + + EXPECT_NEAR(res.x, 0.0f, POINT_EPSILON); + EXPECT_NEAR(res.y, 0.8414f, POINT_EPSILON); + EXPECT_NEAR(res.z, 0.5403f, POINT_EPSILON); + EXPECT_NEAR(res.d, -1.0f, POINT_EPSILON); + +} diff --git a/Tools/CMake/torqueConfig.h.in b/Tools/CMake/torqueConfig.h.in index 39b9a7c03..717c05eb1 100644 --- a/Tools/CMake/torqueConfig.h.in +++ b/Tools/CMake/torqueConfig.h.in @@ -32,7 +32,7 @@ /// What's the name of your application? Used in a variety of places. #define TORQUE_APP_NAME "@TORQUE_APP_NAME@" - +#cmakedefine USE_TEMPLATE_MATRIX /// What version of the application specific source code is this? /// /// Version number is major * 1000 + minor * 100 + revision * 10. diff --git a/Tools/CMake/torque_configs.cmake b/Tools/CMake/torque_configs.cmake index 739a7b28b..11832520a 100644 --- a/Tools/CMake/torque_configs.cmake +++ b/Tools/CMake/torque_configs.cmake @@ -74,7 +74,7 @@ advanced_option(TORQUE_TOOLS "Enable or disable the tools" ON) advanced_option(TORQUE_TOOLS_EXT_COMMANDS "Enable or disable some extended functionality like shell commands or free write access" OFF) advanced_option(TORQUE_ENABLE_PROFILER "Enable or disable the profiler" OFF) advanced_option(TORQUE_SHOW_LEGACY_FILE_FIELDS "If on, shows legacy direct file path fields in the inspector." OFF) - +advanced_option(USE_TEMPLATE_MATRIX "Set to true to use the new templated matrix class(still in beta)." OFF) #testing advanced_option(TORQUE_TESTING "Unit test build" OFF)