mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-02-13 11:43:49 +00:00
backup
initial implemenation of templated classes : Matrix class first.
This commit is contained in:
marauder2k7
parent
c0dec83a21
commit
dd25f1c58a
2 changed files with 653 additions and 0 deletions
|
|
@ -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());
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue