diff --git a/Engine/source/math/mMatrix.cpp b/Engine/source/math/mMatrix.cpp index d8205838f..119c3c0a4 100644 --- a/Engine/source/math/mMatrix.cpp +++ b/Engine/source/math/mMatrix.cpp @@ -209,3 +209,377 @@ EngineFieldTable::Field MatrixFEngineExport::getMatrixField() typedef MatrixF ThisType; return _FIELD_AS(F32, m, m, 16, ""); } + +//------------------------------------ +// Templatized matrix class to replace MATRIXF above +// row-major for now, since torque says it uses that +// but in future could cut down on transpose calls if +// we switch to column major. +//------------------------------------ + +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 a new empty matrix. + if (augmentedMatrix(i, i) == static_cast(0)) { + return Matrix(); + } + + 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 (std::size_t k = 0; k < size; k++) { + if (k != i) { + DATA_TYPE factor = augmentedMatrix(k, i); + for (std::size_t 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 +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 +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()); +} diff --git a/Engine/source/math/mMatrix.h b/Engine/source/math/mMatrix.h index 18981eeea..9c53fa1c2 100644 --- a/Engine/source/math/mMatrix.h +++ b/Engine/source/math/mMatrix.h @@ -620,4 +620,283 @@ inline void mTransformPlane(const MatrixF& mat, const Point3F& scale, const Plan m_matF_x_scale_x_planeF(mat, &scale.x, &plane.x, &result->x); } +//------------------------------------ +// Templatized matrix class to replace MATRIXF above +// row-major for now, since torque says it uses that +// but in future could cut down on transpose calls if +// we switch to column major. +//------------------------------------ + +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); + /// Make this an identity matrix. + Matrix& identity(); + + 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)); } + + // ------ Getters ------ + bool isAffine() const; + 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; } + + DATA_TYPE* getData() { + return data; + } + + const DATA_TYPE* getData() const { + return data; + } + + void dumpMatrix(const char* caption = NULL) const; + // Static identity matrix + static const Matrix Identity; + + // ------ Operators ------ + + 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[col * rows + row]; + } + + const DATA_TYPE& operator()(U32 row, U32 col) const { + if (row >= rows || col >= cols) + AssertFatal(false, "Matrix indices out of range"); + + return data[col * rows + row]; + } + +}; + +//-------------------------------------------- +// INLINE FUNCTIONS +//-------------------------------------------- +template +inline Matrix& Matrix::transpose() +{ + // square matrices can just swap, non square requires a temp mat. + if (rows == cols) { + for (U32 i = 0; i < rows; i++) { + for (U32 j = 0; j < cols; j++) { + std::swap((*this)(j, i), (*this)(i, j)); + } + } + } + else { + Matrix result; + for (U32 i = 0; i < rows; i++) { + for (U32 j = 0; j < cols; j++) { + result(j, i) = (*this)(i, j); + } + } + std::copy(std::begin(result.data), std::end(result.data), std::begin(data)); + } + + 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 Matrix& Matrix::scale(const Point3F& s) +{ + // torques scale applies directly, does not create another matrix to multiply with the translation matrix. + AssertFatal(rows >= 3 && cols >= 3, "Scale can only be applied 3x3 or more"); + for (U32 i = 0; i < 3; i++) { + for (U32 j = 0; j < 3; j++) { + DATA_TYPE scale = (i == 0) ? s.x : (i == 1) ? s.y : s.z; + (*this)(i, j) *= scale; + } + } + + return (*this); +} + +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 >= 3 && cols >= 3, "Scale can only be applied 3x3 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::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; +} + +//-------------------------------------------- +// INLINE FUNCTIONS END +//-------------------------------------------- + +typedef Matrix Matrix4F; + +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 //_MMATRIX_H_