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 0a152ad6c..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 @@ -210,428 +212,11 @@ EngineFieldTable::Field MatrixFEngineExport::getMatrixField() 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 //------------------------------------ -template -const Matrix Matrix::Identity = []() { - Matrix identity(true); - return identity; -}(); - -template -Matrix::Matrix(const EulerF& e) -{ - set(e); -} - -template -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)(1, 0) = 0.0f; (*this)(2, 0) = 0.0f; - (*this)(0, 1) = 0.0f; (*this)(1, 1) = cosPitch; (*this)(2, 1) = -sinPitch; - (*this)(0, 2) = 0.0f; (*this)(1, 2) = 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)(1, 0) = -sinRoll; (*this)(2, 0) = 0.0f; - (*this)(0, 1) = sinRoll; (*this)(1, 1) = cosRoll; (*this)(2, 1) = 0.0f; - (*this)(0, 2) = 0.0f; (*this)(1, 2) = 0.0f; (*this)(2, 2) = 0.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)(1, 0) = -cosPitch * sinRoll; (*this)(2, 0) = r3 + (r2 * sinPitch); - (*this)(0, 1) = r2 + (r3 * sinPitch); (*this)(1, 1) = cosPitch * cosRoll; (*this)(2, 1) = r4 - (r1 * sinPitch); - (*this)(0, 2) = -cosPitch * sinYaw; (*this)(1, 2) = 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 -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 -Matrix& Matrix::inverse() -{ - // TODO: insert return statement here - AssertFatal(rows == cols, "Can only perform inverse on square matrices."); - const U32 size = rows; - - // Create augmented matrix [this | I] - Matrix augmentedMatrix; - Matrix resultMatrix; - - 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; - - for (U32 k = i + 1; k < size; k++) { - // use std::abs until the templated math functions are in place. - if (std::abs(augmentedMatrix(k, i)) > std::abs(augmentedMatrix(pivotRow, i))) { - pivotRow = k; - } - } - - // Swap if needed. - if (i != pivotRow) { - for (U32 j = 0; j < 2 * size; j++) { - std::swap(augmentedMatrix(i, j), augmentedMatrix(pivotRow, j)); - } - } - - // Early out if pivot is 0, return identity matrix. - if (augmentedMatrix(i, i) == static_cast(0)) { - return Matrix(true); - } - - DATA_TYPE pivotVal = 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++) { - resultMatrix(i, j) = augmentedMatrix(i, j + size); - } - } - - return resultMatrix; -} - -template -void Matrix::invert() -{ - (*this) = inverse(); -} - -template -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 -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 -void Matrix::mul(Box3F& box) const -{ - AssertFatal(rows == 4 && cols == 4, "Multiplying Box3F with matrix requires 4x4"); - - // Create an array of all 8 corners of the box - Point3F corners[8] = { - Point3F(box.minExtents.x, box.minExtents.y, box.minExtents.z), - Point3F(box.minExtents.x, box.minExtents.y, box.maxExtents.z), - Point3F(box.minExtents.x, box.maxExtents.y, box.minExtents.z), - Point3F(box.minExtents.x, box.maxExtents.y, box.maxExtents.z), - Point3F(box.maxExtents.x, box.minExtents.y, box.minExtents.z), - Point3F(box.maxExtents.x, box.minExtents.y, box.maxExtents.z), - Point3F(box.maxExtents.x, box.maxExtents.y, box.minExtents.z), - Point3F(box.maxExtents.x, box.maxExtents.y, box.maxExtents.z), - }; - - for (U32 i = 0; i < 8; i++) { - corners[i] = (*this) * corners[i]; - } - - box.minExtents = corners[0]; - box.maxExtents = corners[0]; - for (U32 i = 1; i < 8; ++i) { - box.minExtents.x = mMin(box.minExtents.x, corners[i].x); - box.minExtents.y = mMin(box.minExtents.y, corners[i].y); - box.minExtents.z = mMin(box.minExtents.z, corners[i].z); - - box.maxExtents.x = mMax(box.maxExtents.x, corners[i].x); - box.maxExtents.y = mMax(box.maxExtents.y, corners[i].y); - box.maxExtents.z = mMax(box.maxExtents.z, corners[i].z); - } -} - -template -bool Matrix::isAffine() const -{ - if ((*this)(rows - 1, cols - 1) != 1.0f) { - return false; - } - - for (U32 col = 0; col < cols - 1; ++col) { - if ((*this)(rows - 1, 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 -Matrix& Matrix::affineInverse() -{ - AssertFatal(rows >= 4 && cols >= 4, "affineInverse requires at least 4x4"); - Matrix subMatrix; - - for (U32 i = 0; i < 3; i++) { - for (U32 j = 0; j < 3; j++) { - subMatrix(i, j) = (*this)(i, j); - } - } - - subMatrix.transpose(); - - Point3F pos = getPosition(); - (*this)(0, 3) = mDot(subMatrix.getColumn3F(0), pos); - (*this)(1, 3) = mDot(subMatrix.getColumn3F(1), pos); - (*this)(2, 3) = mDot(subMatrix.getColumn3F(2), pos); - - return *this; -} - -template -EulerF Matrix::toEuler() const -{ - AssertFatal(rows >= 3 && cols >= 3, "Euler rotations require at least a 3x3 matrix."); - // Extract rotation matrix components - const DATA_TYPE m00 = (*this)(0, 0); - const DATA_TYPE m01 = (*this)(0, 1); - const DATA_TYPE m02 = (*this)(0, 2); - const DATA_TYPE m10 = (*this)(1, 0); - const DATA_TYPE m11 = (*this)(1, 1); - const DATA_TYPE m21 = (*this)(2, 1); - const DATA_TYPE m22 = (*this)(2, 2); - - // like all others assume float for now. - EulerF r; - - r.x = mAsin(mClampF(m21, -1.0, 1.0)); - if (mCos(r.x) != 0.0f) { - r.y = mAtan2(-m02, m22); // yaw - r.z = mAtan2(-m10, m11); // roll - } - else { - r.y = 0.0f; - r.z = mAtan2(m01, m00); // this rolls when pitch is +90 degrees - } - - return r; -} - -template -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()); -} +#endif // !USE_TEMPLATE_MATRIX diff --git a/Engine/source/math/mMatrix.h b/Engine/source/math/mMatrix.h index fe715298e..503ede5a0 100644 --- a/Engine/source/math/mMatrix.h +++ b/Engine/source/math/mMatrix.h @@ -39,6 +39,7 @@ #include "console/engineTypeInfo.h" #endif +#ifndef USE_TEMPLATE_MATRIX /// 4x4 Matrix Class /// @@ -620,6 +621,8 @@ 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 //------------------------------------ @@ -652,7 +655,7 @@ public: explicit Matrix(const EulerF& e); /// Make this an identity matrix. Matrix& identity(); - + void reverseProjection(); void normalize(); Matrix& set(const EulerF& e); @@ -660,7 +663,7 @@ public: Matrix(const EulerF& e, const Point3F p); Matrix& set(const EulerF& e, const Point3F p); - Matrix& inverse(); + Matrix inverse(); Matrix& transpose(); void invert(); @@ -673,13 +676,18 @@ public: 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); } ///< M * a -> M Matrix& mul(const Matrix& a) - { return *this * a; } + { + *this = *this * a; return *this; + } ///< a * M -> M Matrix& mulL(const Matrix& a) { return *this = a * *this; } @@ -732,13 +740,23 @@ public: 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; + } // col * rows + row - // static U32 idx(U32 i, U32 j) { return (i * rows + j); } + static U32 idx(U32 i, U32 j) { return (i * rows + j); } bool isAffine() const; bool isIdentity() const; /// Take inverse of matrix assuming it is affine (rotation, /// scale, sheer, translation only). - Matrix& affineInverse(); + Matrix affineInverse(); Point3F getScale() const; @@ -758,6 +776,10 @@ public: 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; } @@ -766,6 +788,24 @@ public: 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 invertTo(Matrix* matrix) { + Matrix invMatrix = this->inverse(); + + for (U32 i = 0; i < rows; ++i) { + for (U32 j = 0; j < cols; ++j) { + (*matrix)(j, i) = invMatrix(i, j); + } + } + } + void dumpMatrix(const char* caption = NULL) const; // Static identity matrix static const Matrix Identity; @@ -1093,6 +1133,30 @@ inline void Matrix::getRow(S32 row, Point3F* cptr) const 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::setRow(S32 row, const Point4F& cptr) { if(cols >= 2) @@ -1121,11 +1185,514 @@ inline void Matrix::setRow(S32 row, const Point3F& cptr) } +template +inline void Matrix::displace(const Point3F& delta) +{ + (*this)(0, 3) += delta.x; + (*this)(1, 3) += delta.y; + (*this)(2, 3) += delta.z; +} + + +template +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 +Matrix::Matrix(const EulerF& e) +{ + set(e); +} + +template +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)(1, 0) = 0.0f; (*this)(2, 0) = 0.0f; + (*this)(0, 1) = 0.0f; (*this)(1, 1) = cosPitch; (*this)(2, 1) = -sinPitch; + (*this)(0, 2) = 0.0f; (*this)(1, 2) = 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)(1, 0) = -sinRoll; (*this)(2, 0) = 0.0f; + (*this)(0, 1) = sinRoll; (*this)(1, 1) = cosRoll; (*this)(2, 1) = 0.0f; + (*this)(0, 2) = 0.0f; (*this)(1, 2) = 0.0f; (*this)(2, 2) = 0.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)(1, 0) = -cosPitch * sinRoll; (*this)(2, 0) = r3 + (r2 * sinPitch); + (*this)(0, 1) = r2 + (r3 * sinPitch); (*this)(1, 1) = cosPitch * cosRoll; (*this)(2, 1) = r4 - (r1 * sinPitch); + (*this)(0, 2) = -cosPitch * sinYaw; (*this)(1, 2) = 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 +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 +Matrix Matrix::inverse() +{ + // TODO: insert return statement here + AssertFatal(rows == cols, "Can only perform inverse on square matrices."); + const U32 size = rows; + + // Create augmented matrix [this | I] + Matrix augmentedMatrix; + Matrix resultMatrix; + + 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; + + for (U32 k = i + 1; k < size; k++) { + // use std::abs until the templated math functions are in place. + if (std::abs(augmentedMatrix(k, i)) > std::abs(augmentedMatrix(pivotRow, i))) { + pivotRow = k; + } + } + + // Swap if needed. + if (i != pivotRow) { + for (U32 j = 0; j < 2 * size; j++) { + std::swap(augmentedMatrix(i, j), augmentedMatrix(pivotRow, j)); + } + } + + // Early out if pivot is 0, return identity matrix. + if (augmentedMatrix(i, i) == static_cast(0)) { + return Matrix(true); + } + + DATA_TYPE pivotVal = 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++) { + resultMatrix(i, j) = augmentedMatrix(i, j + size); + } + } + + return resultMatrix; +} + +template +inline bool Matrix::fullInverse() +{ + Matrix inv = this->inverse(); + + if (inv.isIdentity()) + return false; + + *this = inv; + 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"); + + // Create an array of all 8 corners of the box + Point3F corners[8] = { + Point3F(box.minExtents.x, box.minExtents.y, box.minExtents.z), + Point3F(box.minExtents.x, box.minExtents.y, box.maxExtents.z), + Point3F(box.minExtents.x, box.maxExtents.y, box.minExtents.z), + Point3F(box.minExtents.x, box.maxExtents.y, box.maxExtents.z), + Point3F(box.maxExtents.x, box.minExtents.y, box.minExtents.z), + Point3F(box.maxExtents.x, box.minExtents.y, box.maxExtents.z), + Point3F(box.maxExtents.x, box.maxExtents.y, box.minExtents.z), + Point3F(box.maxExtents.x, box.maxExtents.y, box.maxExtents.z), + }; + + for (U32 i = 0; i < 8; i++) { + corners[i] = (*this) * corners[i]; + } + + box.minExtents = corners[0]; + box.maxExtents = corners[0]; + for (U32 i = 1; i < 8; ++i) { + box.minExtents.x = mMin(box.minExtents.x, corners[i].x); + box.minExtents.y = mMin(box.minExtents.y, corners[i].y); + box.minExtents.z = mMin(box.minExtents.z, corners[i].z); + + box.maxExtents.x = mMax(box.maxExtents.x, corners[i].x); + box.maxExtents.y = mMax(box.maxExtents.y, corners[i].y); + box.maxExtents.z = mMax(box.maxExtents.z, corners[i].z); + } +} + +template +inline bool Matrix::isAffine() const +{ + if ((*this)(rows - 1, cols - 1) != 1.0f) { + return false; + } + + for (U32 col = 0; col < cols - 1; ++col) { + if ((*this)(rows - 1, 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 subMatrix; + + for (U32 i = 0; i < 3; i++) { + for (U32 j = 0; j < 3; j++) { + subMatrix(i, j) = (*this)(i, j); + } + } + + subMatrix.transpose(); + + Point3F pos = getPosition(); + (*this)(0, 3) = mDot(subMatrix.getColumn3F(0), pos); + (*this)(1, 3) = mDot(subMatrix.getColumn3F(1), pos); + (*this)(2, 3) = mDot(subMatrix.getColumn3F(2), pos); + + return *this; +} + +template +inline EulerF Matrix::toEuler() const +{ + AssertFatal(rows >= 3 && cols >= 3, "Euler rotations require at least a 3x3 matrix."); + // Extract rotation matrix components + const DATA_TYPE m00 = (*this)(0, 0); + const DATA_TYPE m01 = (*this)(0, 1); + const DATA_TYPE m02 = (*this)(0, 2); + const DATA_TYPE m10 = (*this)(1, 0); + const DATA_TYPE m11 = (*this)(1, 1); + const DATA_TYPE m21 = (*this)(2, 1); + const DATA_TYPE m22 = (*this)(2, 2); + + // like all others assume float for now. + EulerF r; + + r.x = mAsin(mClampF(m21, -1.0, 1.0)); + if (mCos(r.x) != 0.0f) { + r.y = mAtan2(-m02, m22); // yaw + r.z = mAtan2(-m10, m11); // roll + } + else { + r.y = 0.0f; + r.z = mAtan2(m01, m00); // 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 +//------------------------------------ + +template +inline void mTransformPlane( + const Matrix& mat, + const Point3F& scale, + const PlaneF& plane, + PlaneF* result +) { + AssertFatal(Rows == 4 && Cols == 4, "Matrix must be 4x4"); + + // Create a non-const copy of the matrix + Matrix matCopy = mat; + + // Create the inverse scale matrix + Matrix invScale = Matrix::Identity; + invScale(0, 0) = 1.0f / scale.x; + invScale(1, 1) = 1.0f / scale.y; + invScale(2, 2) = 1.0f / scale.z; + + // Compute the inverse transpose of the matrix + Matrix invTrMatrix = matCopy.transpose().affineInverse() * invScale; + + // Transform the plane normal + Point3F norm(plane.x, plane.y, plane.z); + norm = invTrMatrix * norm; + float normLength = std::sqrt(norm.x * norm.x + norm.y * norm.y + norm.z * norm.z); + norm.x /= normLength; + norm.y /= normLength; + norm.z /= normLength; + + // Transform the plane point + Point3F point = norm * (-plane.d); + Matrix temp = mat; + point.x *= scale.x; + point.y *= scale.y; + point.z *= scale.z; + point = temp * 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 Matrix4F; +typedef Matrix MatrixF; class MatrixTemplateExport { @@ -1141,6 +1708,6 @@ inline EngineFieldTable::Field MatrixTemplateExport::getMatrixField() 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 751f3e31b..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,13 +109,15 @@ IMPLEMENT_STRUCT( MatrixF, MatrixFEngineExport::getMatrixField(), END_IMPLEMENT_STRUCT; -IMPLEMENT_STRUCT(Matrix4F, - Matrix4F, MathTypes, - "") +#else +IMPLEMENT_STRUCT(MatrixF, +MatrixF, MathTypes, +"") - MatrixTemplateExport::getMatrixField(), +MatrixTemplateExport::getMatrixField(), - END_IMPLEMENT_STRUCT; +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/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