|
@@ -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<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+const Matrix<DATA_TYPE, rows, cols> Matrix<DATA_TYPE, rows, cols>::Identity = []() {
|
|
|
+ Matrix<DATA_TYPE, rows, cols> identity(true);
|
|
|
+ return identity;
|
|
|
+}();
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>::Matrix(const EulerF& e)
|
|
|
+{
|
|
|
+ set(e);
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e)
|
|
|
+{
|
|
|
+ // when the template refactor is done, euler will be able to be setup in different ways
|
|
|
+ AssertFatal(rows >= 3 && cols >= 3, "EulerF can only initialize 3x3 or more");
|
|
|
+ static_assert(std::is_same<DATA_TYPE, float>::value, "Can only initialize eulers with floats for now");
|
|
|
+
|
|
|
+ F32 cosPitch, sinPitch;
|
|
|
+ mSinCos(e.x, sinPitch, cosPitch);
|
|
|
+
|
|
|
+ F32 cosYaw, sinYaw;
|
|
|
+ mSinCos(e.y, sinYaw, cosYaw);
|
|
|
+
|
|
|
+ F32 cosRoll, sinRoll;
|
|
|
+ mSinCos(e.z, sinRoll, cosRoll);
|
|
|
+
|
|
|
+ enum {
|
|
|
+ AXIS_X = (1 << 0),
|
|
|
+ AXIS_Y = (1 << 1),
|
|
|
+ AXIS_Z = (1 << 2)
|
|
|
+ };
|
|
|
+
|
|
|
+ U32 axis = 0;
|
|
|
+ if (e.x != 0.0f) axis |= AXIS_X;
|
|
|
+ if (e.y != 0.0f) axis |= AXIS_Y;
|
|
|
+ if (e.z != 0.0f) axis |= AXIS_Z;
|
|
|
+
|
|
|
+ switch (axis) {
|
|
|
+ case 0:
|
|
|
+ (*this) = Matrix<DATA_TYPE, rows, cols>(true);
|
|
|
+ break;
|
|
|
+ case AXIS_X:
|
|
|
+ (*this)(0, 0) = 1.0f; (*this)(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<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>::Matrix(const EulerF& e, const Point3F p)
|
|
|
+{
|
|
|
+ set(e, p);
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e, const Point3F p)
|
|
|
+{
|
|
|
+ AssertFatal(rows >= 3 && cols >= 4, "Euler and Point can only initialize 3x4 or more");
|
|
|
+ // call set euler, this already sets the last row if it exists.
|
|
|
+ set(e);
|
|
|
+
|
|
|
+ // does this need to multiply with the result of the euler? or are we just setting position.
|
|
|
+ (*this)(0, 3) = p.x;
|
|
|
+ (*this)(1, 3) = p.y;
|
|
|
+ (*this)(2, 3) = p.z;
|
|
|
+
|
|
|
+ return (*this);
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::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<DATA_TYPE, size, 2 * size> augmentedMatrix;
|
|
|
+ Matrix<DATA_TYPE, size, size> 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<DATA_TYPE>(1) : static_cast<DATA_TYPE>(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<DATA_TYPE>(0)) {
|
|
|
+ return Matrix<DATA_TYPE, rows, cols>();
|
|
|
+ }
|
|
|
+
|
|
|
+ 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<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+void Matrix<DATA_TYPE, rows, cols>::invert()
|
|
|
+{
|
|
|
+ (*this) = inverse();
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setCrossProduct(const Point3F& p)
|
|
|
+{
|
|
|
+ AssertFatal(rows == 4 && cols == 4, "Cross product only supported on 4x4 for now");
|
|
|
+
|
|
|
+ (*this)(0, 0) = 0;
|
|
|
+ (*this)(0, 1) = -p.z;
|
|
|
+ (*this)(0, 2) = p.y;
|
|
|
+ (*this)(0, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(1, 0) = p.z;
|
|
|
+ (*this)(1, 1) = 0;
|
|
|
+ (*this)(1, 2) = -p.x;
|
|
|
+ (*this)(1, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(2, 0) = -p.y;
|
|
|
+ (*this)(2, 1) = p.x;
|
|
|
+ (*this)(2, 2) = 0;
|
|
|
+ (*this)(2, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(3, 0) = 0;
|
|
|
+ (*this)(3, 1) = 0;
|
|
|
+ (*this)(3, 2) = 0;
|
|
|
+ (*this)(3, 3) = 1;
|
|
|
+
|
|
|
+ return (*this);
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setTensorProduct(const Point3F& p, const Point3F& q)
|
|
|
+{
|
|
|
+ AssertFatal(rows == 4 && cols == 4, "Tensor product only supported on 4x4 for now");
|
|
|
+
|
|
|
+ (*this)(0, 0) = p.x * q.x;
|
|
|
+ (*this)(0, 1) = p.x * q.y;
|
|
|
+ (*this)(0, 2) = p.x * q.z;
|
|
|
+ (*this)(0, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(1, 0) = p.y * q.x;
|
|
|
+ (*this)(1, 1) = p.y * q.y;
|
|
|
+ (*this)(1, 2) = p.y * q.z;
|
|
|
+ (*this)(1, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(2, 0) = p.z * q.x;
|
|
|
+ (*this)(2, 1) = p.z * q.y;
|
|
|
+ (*this)(2, 2) = p.z * q.z;
|
|
|
+ (*this)(2, 3) = 0;
|
|
|
+
|
|
|
+ (*this)(3, 0) = 0;
|
|
|
+ (*this)(3, 1) = 0;
|
|
|
+ (*this)(3, 2) = 0;
|
|
|
+ (*this)(3, 3) = 1;
|
|
|
+
|
|
|
+ return (*this);
|
|
|
+}
|
|
|
+
|
|
|
+template<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+bool Matrix<DATA_TYPE, rows, cols>::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<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+EulerF Matrix<DATA_TYPE, rows, cols>::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<typename DATA_TYPE, U32 rows, U32 cols>
|
|
|
+void Matrix<DATA_TYPE, rows, cols>::dumpMatrix(const char* caption) const
|
|
|
+{
|
|
|
+ U32 size = (caption == NULL) ? 0 : dStrlen(caption);
|
|
|
+ FrameTemp<char> spacer(size + 1);
|
|
|
+ char* spacerRef = spacer;
|
|
|
+
|
|
|
+ // is_floating_point should return true for floats and doubles.
|
|
|
+ const char* formatSpec = std::is_floating_point_v<DATA_TYPE> ? " %-8.4f" : " %d";
|
|
|
+
|
|
|
+ dMemset(spacerRef, ' ', size);
|
|
|
+ // null terminate.
|
|
|
+ spacerRef[size] = '\0';
|
|
|
+
|
|
|
+ /*Con::printf("%s = | %-8.4f %-8.4f %-8.4f %-8.4f |", caption, m[idx(0, 0)], m[idx(0, 1)], m[idx(0, 2)], m[idx(0, 3)]);
|
|
|
+ Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(1, 0)], m[idx(1, 1)], m[idx(1, 2)], m[idx(1, 3)]);
|
|
|
+ Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(2, 0)], m[idx(2, 1)], m[idx(2, 2)], m[idx(2, 3)]);
|
|
|
+ Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(3, 0)], m[idx(3, 1)], m[idx(3, 2)], m[idx(3, 3)]);*/
|
|
|
+
|
|
|
+ StringBuilder str;
|
|
|
+ str.format("%s = |", caption);
|
|
|
+ for (U32 i = 0; i < rows; i++) {
|
|
|
+ if (i > 0) {
|
|
|
+ str.append(spacerRef);
|
|
|
+ }
|
|
|
+
|
|
|
+ for (U32 j = 0; j < cols; j++) {
|
|
|
+ str.format(formatSpec, (*this)(i, j));
|
|
|
+ }
|
|
|
+ str.append(" |\n");
|
|
|
+ }
|
|
|
+
|
|
|
+ Con::printf("%s", str.end().c_str());
|
|
|
+}
|