//-----------------------------------------------------------------------------
// Torque Game Engine
// Copyright (C) GarageGames.com, Inc.
//-----------------------------------------------------------------------------
#include "math/mMath.h"
#include "math/mMatrix.h"
#include "console/console.h"
#include "core/frameAllocator.h"
// idx(i,j) is index to element in column i, row j
void MatrixF::transposeTo(F32 *matrix) const
{
matrix[idx(0,0)] = m[idx(0,0)];
matrix[idx(0,1)] = m[idx(1,0)];
matrix[idx(0,2)] = m[idx(2,0)];
matrix[idx(0,3)] = m[idx(3,0)];
matrix[idx(1,0)] = m[idx(0,1)];
matrix[idx(1,1)] = m[idx(1,1)];
matrix[idx(1,2)] = m[idx(2,1)];
matrix[idx(1,3)] = m[idx(3,1)];
matrix[idx(2,0)] = m[idx(0,2)];
matrix[idx(2,1)] = m[idx(1,2)];
matrix[idx(2,2)] = m[idx(2,2)];
matrix[idx(2,3)] = m[idx(3,2)];
matrix[idx(3,0)] = m[idx(0,3)];
matrix[idx(3,1)] = m[idx(1,3)];
matrix[idx(3,2)] = m[idx(2,3)];
matrix[idx(3,3)] = m[idx(3,3)];
}
bool MatrixF::isAffine() const
{
// An affine transform is defined by the following structure
//
// [ X X X P ]
// [ X X X P ]
// [ X X X P ]
// [ 0 0 0 1 ]
//
// Where X is an orthonormal 3x3 submatrix and P is an arbitrary translation
// We'll check in the following order:
// 1: [3][3] must be 1
// 2: Shear portion must be zero
// 3: Dot products of rows and columns must be zero
// 4: Length of rows and columns must be 1
//
if (m[idx(3,3)] != 1.0f)
return false;
if (m[idx(0,3)] != 0.0f ||
m[idx(1,3)] != 0.0f ||
m[idx(2,3)] != 0.0f)
return false;
Point3F one, two, three;
getColumn(0, &one);
getColumn(1, &two);
getColumn(2, &three);
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);
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;
// We're ok.
return true;
}
// Perform inverse on full 4x4 matrix. Used in special cases only, so not at all optimized.
bool MatrixF::fullInverse()
{
Point4F a,b,c,d;
getRow(0,&a);
getRow(1,&b);
getRow(2,&c);
getRow(3,&d);
// det = a0*b1*c2*d3 - a0*b1*c3*d2 - a0*c1*b2*d3 + a0*c1*b3*d2 + a0*d1*b2*c3 - a0*d1*b3*c2 -
// b0*a1*c2*d3 + b0*a1*c3*d2 + b0*c1*a2*d3 - b0*c1*a3*d2 - b0*d1*a2*c3 + b0*d1*a3*c2 +
// c0*a1*b2*d3 - c0*a1*b3*d2 - c0*b1*a2*d3 + c0*b1*a3*d2 + c0*d1*a2*b3 - c0*d1*a3*b2 -
// d0*a1*b2*c3 + d0*a1*b3*c2 + d0*b1*a2*c3 - d0*b1*a3*c2 - d0*c1*a2*b3 + d0*c1*a3*b2
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;
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);
return true;
}
EulerF MatrixF::toEuler() const
{
const F32 * mat = m;
EulerF r;
r.x = mAsin(mat[MatrixF::idx(2,1)]);
if(mCos(r.x) != 0.f)
{
r.y = mAtan(-mat[MatrixF::idx(2,0)], mat[MatrixF::idx(2,2)]);
r.z = mAtan(-mat[MatrixF::idx(0,1)], mat[MatrixF::idx(1,1)]);
}
else
{
r.y = 0.f;
r.z = mAtan(mat[MatrixF::idx(1,0)], mat[MatrixF::idx(0,0)]);
}
return r;
}
void MatrixF::dumpMatrix(const char *caption /* =NULL */) const
{
U32 size = dStrlen(caption);
FrameTemp<char> spacer(size+1);
char *spacerRef = spacer;
dMemset(spacerRef, ' ', size);
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)]);
}