From ab4b4cbf969da48cefdfaf29e23bb50d4b2c2a69 Mon Sep 17 00:00:00 2001 From: marauder2k7 Date: Wed, 31 Jul 2024 17:32:00 +0100 Subject: [PATCH] cramer for inverse added #if block around inverse methods to track down shadow bug uses old inverse method as default for now. --- Engine/source/math/mMatrix.h | 351 ++++++++++++++++++----- Engine/source/testing/mathMatrixTest.cpp | 67 +++++ 2 files changed, 349 insertions(+), 69 deletions(-) diff --git a/Engine/source/math/mMatrix.h b/Engine/source/math/mMatrix.h index 8226bb12c..aed956f6d 100644 --- a/Engine/source/math/mMatrix.h +++ b/Engine/source/math/mMatrix.h @@ -127,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 @@ -702,11 +706,9 @@ public: 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 - DATA_TYPE det = - data[0] * (data[4] * data[8] - data[5] * data[7]) - - data[1] * (data[3] * data[8] - data[5] * data[6]) + - data[2] * (data[3] * data[7] - data[4] * data[6]); - return det; + 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 @@ -823,6 +825,12 @@ public: } } + void swap(DATA_TYPE& a, DATA_TYPE& b) { + DATA_TYPE temp = a; + a = b; + b = temp; + } + void invertTo(Matrix* matrix) const; void invertTo(Matrix* matrix); @@ -834,17 +842,25 @@ public: 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) = 0; // Initialize result element to 0 - for (U32 k = 0; k < cols; ++k) - { - result(i, j) += m1(i, k) * m2(k, j); - } - } - } + result(0, 0) = m1(0, 0) * m2(0, 0) + m1(0, 1) * m2(1, 0) + m1(0, 2) * m2(2, 0) + m1(0, 3) * m2(3, 0); + result(0, 1) = m1(0, 0) * m2(0, 1) + m1(0, 1) * m2(1, 1) + m1(0, 2) * m2(2, 1) + m1(0, 3) * m2(3, 1); + result(0, 2) = m1(0, 0) * m2(0, 2) + m1(0, 1) * m2(1, 2) + m1(0, 2) * m2(2, 2) + m1(0, 3) * m2(3, 2); + result(0, 3) = m1(0, 0) * m2(0, 3) + m1(0, 1) * m2(1, 3) + m1(0, 2) * m2(2, 3) + m1(0, 3) * m2(3, 3); + + result(1, 0) = m1(1, 0) * m2(0, 0) + m1(1, 1) * m2(1, 0) + m1(1, 2) * m2(2, 0) + m1(1, 3) * m2(3, 0); + result(1, 1) = m1(1, 0) * m2(0, 1) + m1(1, 1) * m2(1, 1) + m1(1, 2) * m2(2, 1) + m1(1, 3) * m2(3, 1); + result(1, 2) = m1(1, 0) * m2(0, 2) + m1(1, 1) * m2(1, 2) + m1(1, 2) * m2(2, 2) + m1(1, 3) * m2(3, 2); + result(1, 3) = m1(1, 0) * m2(0, 3) + m1(1, 1) * m2(1, 3) + m1(1, 2) * m2(2, 3) + m1(1, 3) * m2(3, 3); + + result(2, 0) = m1(2, 0) * m2(0, 0) + m1(2, 1) * m2(1, 0) + m1(2, 2) * m2(2, 0) + m1(2, 3) * m2(3, 0); + result(2, 1) = m1(2, 0) * m2(0, 1) + m1(2, 1) * m2(1, 1) + m1(2, 2) * m2(2, 1) + m1(2, 3) * m2(3, 1); + result(2, 2) = m1(2, 0) * m2(0, 2) + m1(2, 1) * m2(1, 2) + m1(2, 2) * m2(2, 2) + m1(2, 3) * m2(3, 2); + result(2, 3) = m1(2, 0) * m2(0, 3) + m1(2, 1) * m2(1, 3) + m1(2, 2) * m2(2, 3) + m1(2, 3) * m2(3, 3); + + result(3, 0) = m1(3, 0) * m2(0, 0) + m1(3, 1) * m2(1, 0) + m1(3, 2) * m2(2, 0) + m1(3, 3) * m2(3, 0); + result(3, 1) = m1(3, 0) * m2(0, 1) + m1(3, 1) * m2(1, 1) + m1(3, 2) * m2(2, 1) + m1(3, 3) * m2(3, 1); + result(3, 2) = m1(3, 0) * m2(0, 2) + m1(3, 1) * m2(1, 2) + m1(3, 2) * m2(2, 2) + m1(3, 3) * m2(3, 2); + result(3, 3) = m1(3, 0) * m2(0, 3) + m1(3, 1) * m2(1, 3) + m1(3, 2) * m2(2, 3) + m1(3, 3) * m2(3, 3); return result; } @@ -907,13 +923,14 @@ public: Point3F operator*(const Point3F& point) const { AssertFatal(rows == 4 && cols == 4, "Multiplying point3 with matrix requires 4x4"); - return Point3F( - (*this)(0, 0) * point.x + (*this)(0, 1) * point.y + (*this)(0, 2) * point.z + (*this)(0, 3), - (*this)(1, 0) * point.x + (*this)(1, 1) * point.y + (*this)(1, 2) * point.z + (*this)(1, 3), - (*this)(2, 0) * point.x + (*this)(2, 1) * point.y + (*this)(2, 2) * point.z + (*this)(2, 3) - ); - } + 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( @@ -964,7 +981,6 @@ public: return data[idx(col, row)]; } - }; //-------------------------------------------- @@ -975,11 +991,13 @@ inline Matrix& Matrix::transpose() { AssertFatal(rows == cols, "Transpose can only be performed on square matrices."); - for (U32 i = 0; i < rows; ++i) { - for (U32 j = i + 1; j < cols; ++j) { - std::swap((*this)(i, j), (*this)(j, i)); - } - } + 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); } @@ -1331,9 +1349,9 @@ inline Matrix& Matrix::set(const E (*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; + (*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; @@ -1341,9 +1359,9 @@ inline Matrix& Matrix::set(const E (*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) = 1.0f; + (*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; @@ -1363,9 +1381,9 @@ inline Matrix& Matrix::set(const E // 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; + (*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; } @@ -1415,9 +1433,13 @@ inline Matrix& Matrix::set(const E template inline Matrix& Matrix::inverse() { - // TODO: insert return statement here +#if 0 + // 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; @@ -1436,11 +1458,14 @@ inline Matrix& Matrix::inverse() { U32 pivotRow = i; + DATA_TYPE pivotValue = std::abs(augmentedMatrix(i, 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))) { + DATA_TYPE curValue = std::abs(augmentedMatrix(k, i)); + if (curValue > pivotValue) { pivotRow = k; + pivotValue = curValue; } } @@ -1449,18 +1474,20 @@ inline Matrix& Matrix::inverse() { for (U32 j = 0; j < 2 * size; j++) { - std::swap(augmentedMatrix(i, j), augmentedMatrix(pivotRow, 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 (augmentedMatrix(i, i) == static_cast(0)) + if (std::abs(augmentedMatrix(i, i)) < pivot_eps) { this->identity(); return *this; } - DATA_TYPE pivotVal = 1.0f / augmentedMatrix(i, i); + DATA_TYPE pivotVal = static_cast(1.0) / augmentedMatrix(i, i); // scale the pivot for (U32 j = 0; j < 2 * size; j++) @@ -1489,6 +1516,44 @@ inline Matrix& Matrix::inverse() (*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); @@ -1500,13 +1565,136 @@ inline Matrix& Matrix::inverse() template inline bool Matrix::fullInverse() { - Matrix inv = this->inverse(); +#if 0 + // 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 - if (inv.isIdentity()) + // 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; - *this = inv; + 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 @@ -1576,39 +1764,67 @@ inline void Matrix::mul(Box3F& box) const { AssertFatal(rows == 4 && cols == 4, "Multiplying Box3F with matrix requires 4x4"); - // Save original min and max - Point3F originalMin = box.minExtents; - Point3F originalMax = box.maxExtents; + // Extract the min and max extents + const Point3F& originalMin = box.minExtents; + const Point3F& originalMax = box.maxExtents; - // Initialize min and max with the translation part of the matrix - box.minExtents.x = box.maxExtents.x = (*this)(0, 3); - box.minExtents.y = box.maxExtents.y = (*this)(1, 3); - box.minExtents.z = box.maxExtents.z = (*this)(2, 3); + // Array to store transformed corners + Point3F transformedCorners[8]; - for (U32 i = 0; i < 3; ++i) { - #define Do_One_Row(j) { \ - DATA_TYPE a = ((*this)(i, j) * originalMin[j]); \ - DATA_TYPE b = ((*this)(i, j) * originalMax[j]); \ - if (a < b) { box.minExtents[i] += a; box.maxExtents[i] += b; } \ - else { box.minExtents[i] += b; box.maxExtents[i] += a; } } + // 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} + }; - Do_One_Row(0); - Do_One_Row(1); - Do_One_Row(2); + // 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)(rows - 1, cols - 1) != 1.0f) + if ((*this)(3, 3) != 1.0f) { return false; } for (U32 col = 0; col < cols - 1; ++col) { - if ((*this)(rows - 1, col) != 0.0f) + if ((*this)(3, col) != 0.0f) { return false; } @@ -1744,11 +1960,8 @@ inline void mTransformPlane( const PlaneF& plane, PlaneF* result ) { - // Create a non-const copy of the matrix - MatrixF matCopy = mat; - // Create the inverse scale matrix - MatrixF invScale = MatrixF::Identity; + MatrixF invScale(true); invScale(0, 0) = 1.0f / scale.x; invScale(1, 1) = 1.0f / scale.y; invScale(2, 2) = 1.0f / scale.z; @@ -1764,7 +1977,7 @@ inline void mTransformPlane( const F32 C = -mDot(row2, shear); // Compute the inverse transpose of the matrix - MatrixF invTrMatrix = MatrixF::Identity; + MatrixF invTrMatrix(true); invTrMatrix(0, 0) = mat(0, 0); invTrMatrix(0, 1) = mat(0, 1); invTrMatrix(0, 2) = mat(0, 2); diff --git a/Engine/source/testing/mathMatrixTest.cpp b/Engine/source/testing/mathMatrixTest.cpp index 9237bc8b3..99ee7be62 100644 --- a/Engine/source/testing/mathMatrixTest.cpp +++ b/Engine/source/testing/mathMatrixTest.cpp @@ -801,6 +801,73 @@ TEST(MatrixTest, TestMatrixAdd) } +TEST(MatrixTest, TestCalcPlaneCulls) +{ + Point3F lightDir(-0.346188605f, -0.742403805f, -0.573576510f); + const F32 shadowDistance = 100.0f; + // frustum transform + MatrixF test(true); + + test(0, 0) = -0.8930f; test(0, 1) = 0.3043f; test(0, 2) = 0.3314f; test(0, 3) = -8.3111f; + test(1, 0) = -0.4499f; test(1, 1) = -0.6039f; test(1, 2) = -0.6578f; test(1, 3) = 8.4487f; + test(2, 0) = -0.0f; test(2, 1) = -0.7366f; test(2, 2) = 0.6763f; test(2, 3) = 12.5414f; + test(0, 0) = 0.00f; test(0, 1) = 0.0f; test(0, 2) = 0.0f; test(0, 3) = 1.0f; + + Box3F viewBB(-shadowDistance, -shadowDistance, -shadowDistance, + shadowDistance, shadowDistance, shadowDistance); + + Frustum testFrustum(false, -0.119894862f, 0.119894862f, 0.0767327100f, -0.0767327100f, 0.1f, 1000.0f, test); + testFrustum.getTransform().mul(viewBB); + + testFrustum.cropNearFar(0.1f, shadowDistance); + + PlaneF lightFarPlane, lightNearPlane; + + Point3F viewDir = testFrustum.getTransform().getForwardVector(); + EXPECT_NEAR(viewDir.x, 0.0f, 0.001f); EXPECT_NEAR(viewDir.y, -0.6039f, 0.001f); EXPECT_NEAR(viewDir.z, -0.7365f, 0.001f); + + viewDir.normalize(); + const Point3F viewPosition = testFrustum.getPosition(); + EXPECT_NEAR(viewPosition.x, 1.0f, 0.001f); EXPECT_NEAR(viewPosition.y, 8.4486f, 0.001f); EXPECT_NEAR(viewPosition.z, 12.5414f, 0.001f); + + const F32 viewDistance = testFrustum.getBounds().len(); + EXPECT_NEAR(viewDistance, 243.6571f, 0.001f); + + lightNearPlane = PlaneF(viewPosition + (viewDistance * -lightDir), lightDir); + + const Point3F lightFarPlanePos = viewPosition + (viewDistance * lightDir); + lightFarPlane = PlaneF(lightFarPlanePos, -lightDir); + + MatrixF invLightFarPlaneMat(true); + + MatrixF lightFarPlaneMat = MathUtils::createOrientFromDir(-lightDir); + lightFarPlaneMat.setPosition(lightFarPlanePos); + lightFarPlaneMat.invertTo(&invLightFarPlaneMat); + + Vector projVertices; + + //project all frustum vertices into plane + // all vertices are 2d and local to far plane + projVertices.setSize(8); + for (int i = 0; i < 8; ++i) // + { + const Point3F& point = testFrustum.getPoints()[i]; + + Point3F localPoint(lightFarPlane.project(point)); + invLightFarPlaneMat.mulP(localPoint); + projVertices[i] = Point2F(localPoint.x, localPoint.z); + } + + EXPECT_NEAR(projVertices[0].x, 0.0240f, 0.001f); EXPECT_NEAR(projVertices[0].y, 0.0117f, 0.001f); + EXPECT_NEAR(projVertices[1].x, 0.0696f, 0.001f); EXPECT_NEAR(projVertices[1].y, 0.0678f, 0.001f); + EXPECT_NEAR(projVertices[2].x, -0.0186f, 0.001f); EXPECT_NEAR(projVertices[2].y, -0.1257f, 0.001f); + EXPECT_NEAR(projVertices[3].x, 0.0269f, 0.001f); EXPECT_NEAR(projVertices[3].y, -0.0696f, 0.001f); + EXPECT_NEAR(projVertices[4].x, 24.0571f, 0.001f); EXPECT_NEAR(projVertices[4].y, 11.7618f, 0.001f); + EXPECT_NEAR(projVertices[5].x, 69.6498f, 0.001f); EXPECT_NEAR(projVertices[5].y, 67.8426f, 0.001f); + EXPECT_NEAR(projVertices[6].x, -18.6059f, 0.001f); EXPECT_NEAR(projVertices[6].y, -125.7341f, 0.001f); + EXPECT_NEAR(projVertices[7].x, 26.9866f, 0.001f); EXPECT_NEAR(projVertices[7].y, -69.6534f, 0.001f); +} + TEST(MatrixTest, TestFrustumProjectionMatrix) { MatrixF test(true);