824 lines
25 KiB
C++
Executable File
824 lines
25 KiB
C++
Executable File
//-----------------------------------------------------------------------------
|
|
// Torque Game Engine
|
|
// Copyright (C) GarageGames.com, Inc.
|
|
//-----------------------------------------------------------------------------
|
|
|
|
#include "math/mMath.h"
|
|
#include <math.h> // Caution!!! Possible platform specific include
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
// C version of Math Library
|
|
|
|
// asm externals
|
|
extern "C" {
|
|
|
|
S32 m_mulDivS32_ASM( S32 a, S32 b, S32 c );
|
|
U32 m_mulDivU32_ASM( U32 a, U32 b, U32 c );
|
|
F32 m_fmod_ASM(F32 val, F32 modulo);
|
|
F32 m_fmodD_ASM(F64 val, F64 modulo);
|
|
void m_sincos_ASM( F32 angle, F32 *s, F32 *c );
|
|
void m_sincosD_ASM( F64 angle, F64 *s, F64 *c );
|
|
|
|
}
|
|
//--------------------------------------
|
|
|
|
static S32 m_mulDivS32_C(S32 a, S32 b, S32 c)
|
|
{
|
|
// S64/U64 support in most 32-bit compilers generate
|
|
// horrible code, the C version are here just for porting
|
|
// assembly implementation is recommended
|
|
return (S32) ((S64)a*(S64)b) / (S64)c;
|
|
}
|
|
|
|
static U32 m_mulDivU32_C(S32 a, S32 b, U32 c)
|
|
{
|
|
return (U32) ((S64)a*(S64)b) / (U64)c;
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static F32 m_catmullrom_C(F32 t, F32 p0, F32 p1, F32 p2, F32 p3)
|
|
{
|
|
return 0.5f * ((3.0f*p1 - 3.0f*p2 + p3 - p0)*t*t*t
|
|
+ (2.0f*p0 - 5.0f*p1 + 4.0f*p2 - p3)*t*t
|
|
+ (p2-p0)*t
|
|
+ 2.0f*p1);
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_point2F_normalize_C(F32 *p)
|
|
{
|
|
F32 factor = 1.0f / mSqrt(p[0]*p[0] + p[1]*p[1] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point2F_normalize_f_C(F32 *p, F32 val)
|
|
{
|
|
F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point2D_normalize_C(F64 *p)
|
|
{
|
|
F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point2D_normalize_f_C(F64 *p, F64 val)
|
|
{
|
|
F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point3D_normalize_f_C(F64 *p, F64 val)
|
|
{
|
|
F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
p[2] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point3F_normalize_C(F32 *p)
|
|
{
|
|
F32 squared = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
|
|
// This can happen in Container::castRay -> ForceFieldBare::castRay
|
|
//AssertFatal(squared != 0.0, "Error, zero length vector normalized!");
|
|
if (squared != 0.0) {
|
|
F32 factor = 1.0f / mSqrt(squared);
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
p[2] *= factor;
|
|
} else {
|
|
p[0] = 0;
|
|
p[1] = 0;
|
|
p[2] = 1;
|
|
}
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point3F_normalize_f_C(F32 *p, F32 val)
|
|
{
|
|
F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
p[2] *= factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point3F_interpolate_C(const F32 *from, const F32 *to, F32 factor, F32 *result )
|
|
{
|
|
F32 inverse = 1.0f - factor;
|
|
result[0] = from[0] * inverse + to[0] * factor;
|
|
result[1] = from[1] * inverse + to[1] * factor;
|
|
result[2] = from[2] * inverse + to[2] * factor;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_point3D_normalize_C(F64 *p)
|
|
{
|
|
F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
|
|
p[0] *= factor;
|
|
p[1] *= factor;
|
|
p[2] *= factor;
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_point3D_interpolate_C(const F64 *from, const F64 *to, F64 factor, F64 *result )
|
|
{
|
|
F64 inverse = 1.0f - factor;
|
|
result[0] = from[0] * inverse + to[0] * factor;
|
|
result[1] = from[1] * inverse + to[1] * factor;
|
|
result[2] = from[2] * inverse + to[2] * factor;
|
|
}
|
|
|
|
|
|
static void m_quatF_set_matF_C( F32 x, F32 y, F32 z, F32 w, F32* m )
|
|
{
|
|
#define qidx(r,c) (r*4 + c)
|
|
F32 xs = x * 2.0f;
|
|
F32 ys = y * 2.0f;
|
|
F32 zs = z * 2.0f;
|
|
F32 wx = w * xs;
|
|
F32 wy = w * ys;
|
|
F32 wz = w * zs;
|
|
F32 xx = x * xs;
|
|
F32 xy = x * ys;
|
|
F32 xz = x * zs;
|
|
F32 yy = y * ys;
|
|
F32 yz = y * zs;
|
|
F32 zz = z * zs;
|
|
m[qidx(0,0)] = 1.0f - (yy + zz);
|
|
m[qidx(1,0)] = xy - wz;
|
|
m[qidx(2,0)] = xz + wy;
|
|
m[qidx(3,0)] = 0.0f;
|
|
m[qidx(0,1)] = xy + wz;
|
|
m[qidx(1,1)] = 1.0f - (xx + zz);
|
|
m[qidx(2,1)] = yz - wx;
|
|
m[qidx(3,1)] = 0.0f;
|
|
m[qidx(0,2)] = xz - wy;
|
|
m[qidx(1,2)] = yz + wx;
|
|
m[qidx(2,2)] = 1.0f - (xx + yy);
|
|
m[qidx(3,2)] = 0.0f;
|
|
|
|
m[qidx(0,3)] = 0.0f;
|
|
m[qidx(1,3)] = 0.0f;
|
|
m[qidx(2,3)] = 0.0f;
|
|
m[qidx(3,3)] = 1.0f;
|
|
#undef qidx
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_matF_set_euler_C(const F32 *e, F32 *result)
|
|
{
|
|
enum {
|
|
AXIS_X = (1<<0),
|
|
AXIS_Y = (1<<1),
|
|
AXIS_Z = (1<<2)
|
|
};
|
|
|
|
U32 axis = 0;
|
|
if (e[0] != 0.0f) axis |= AXIS_X;
|
|
if (e[1] != 0.0f) axis |= AXIS_Y;
|
|
if (e[2] != 0.0f) axis |= AXIS_Z;
|
|
|
|
switch (axis)
|
|
{
|
|
case 0:
|
|
m_matF_identity(result);
|
|
break;
|
|
|
|
case AXIS_X:
|
|
{
|
|
F32 cx,sx;
|
|
mSinCos( e[0], sx, cx );
|
|
|
|
result[0] = 1.0f;
|
|
result[1] = 0.0f;
|
|
result[2] = 0.0f;
|
|
result[3] = 0.0f;
|
|
|
|
result[4] = 0.0f;
|
|
result[5] = cx;
|
|
result[6] = sx;
|
|
result[7] = 0.0f;
|
|
|
|
result[8] = 0.0f;
|
|
result[9] = -sx;
|
|
result[10]= cx;
|
|
result[11]= 0.0f;
|
|
|
|
result[12]= 0.0f;
|
|
result[13]= 0.0f;
|
|
result[14]= 0.0f;
|
|
result[15]= 1.0f;
|
|
break;
|
|
}
|
|
|
|
case AXIS_Y:
|
|
{
|
|
F32 cy,sy;
|
|
mSinCos( e[1], sy, cy );
|
|
|
|
result[0] = cy;
|
|
result[1] = 0.0f;
|
|
result[2] = -sy;
|
|
result[3] = 0.0f;
|
|
|
|
result[4] = 0.0f;
|
|
result[5] = 1.0f;
|
|
result[6] = 0.0f;
|
|
result[7] = 0.0f;
|
|
|
|
result[8] = sy;
|
|
result[9] = 0.0f;
|
|
result[10]= cy;
|
|
result[11]= 0.0f;
|
|
|
|
result[12]= 0.0f;
|
|
result[13]= 0.0f;
|
|
result[14]= 0.0f;
|
|
result[15]= 1.0f;
|
|
break;
|
|
}
|
|
|
|
case AXIS_Z:
|
|
{
|
|
// 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)
|
|
F32 cz,sz;
|
|
mSinCos( e[2], sz, cz );
|
|
F32 r1 = cz;
|
|
F32 r2 = sz;
|
|
F32 r3 = 0.0f;
|
|
F32 r4 = 0.0f;
|
|
|
|
result[0] = cz;
|
|
result[1] = sz;
|
|
result[2] = 0.0f;
|
|
result[3] = 0.0f;
|
|
|
|
result[4] = -sz;
|
|
result[5] = cz;
|
|
result[6] = 0.0f;
|
|
result[7] = 0.0f;
|
|
|
|
result[8] = 0.0f;
|
|
result[9] = 0.0f;
|
|
result[10]= 1.0f;
|
|
result[11]= 0.0f;
|
|
|
|
result[12]= 0.0f;
|
|
result[13]= 0.0f;
|
|
result[14]= 0.0f;
|
|
result[15]= 1.0f;
|
|
break;
|
|
}
|
|
|
|
default:
|
|
// 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)
|
|
F32 cx,sx;
|
|
mSinCos( e[0], sx, cx );
|
|
F32 cy,sy;
|
|
mSinCos( e[1], sy, cy );
|
|
F32 cz,sz;
|
|
mSinCos( e[2], sz, cz );
|
|
F32 r1 = cy * cz;
|
|
F32 r2 = cy * sz;
|
|
F32 r3 = sy * cz;
|
|
F32 r4 = sy * sz;
|
|
|
|
result[0] = r1 - (r4 * sx);
|
|
result[1] = r2 + (r3 * sx);
|
|
result[2] = -cx * sy;
|
|
result[3] = 0.0f;
|
|
|
|
result[4] = -cx * sz;
|
|
result[5] = cx * cz;
|
|
result[6] = sx;
|
|
result[7] = 0.0f;
|
|
|
|
result[8] = r3 + (r2 * sx);
|
|
result[9] = r4 - (r1 * sx);
|
|
result[10]= cx * cy;
|
|
result[11]= 0.0f;
|
|
|
|
result[12]= 0.0f;
|
|
result[13]= 0.0f;
|
|
result[14]= 0.0f;
|
|
result[15]= 1.0f;
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_matF_set_euler_point_C(const F32 *e, const F32 *p, F32 *result)
|
|
{
|
|
m_matF_set_euler(e, result);
|
|
result[3] = p[0];
|
|
result[7] = p[1];
|
|
result[11]= p[2];
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_identity_C(F32 *m)
|
|
{
|
|
*m++ = 1.0f;
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
|
|
*m++ = 0.0f;
|
|
*m++ = 1.0f;
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
*m++ = 1.0f;
|
|
*m++ = 0.0f;
|
|
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
*m++ = 0.0f;
|
|
*m = 1.0f;
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_matF_inverse_C(F32 *m)
|
|
{
|
|
// using Cramers Rule find the Inverse
|
|
// Minv = (1/det(M)) * adjoint(M)
|
|
F32 det = m_matF_determinant( m );
|
|
AssertFatal( det != 0.0f, "MatrixF::inverse: non-singular matrix, no inverse.");
|
|
|
|
F32 invDet = 1.0f/det;
|
|
F32 temp[16];
|
|
|
|
temp[0] = (m[5] * m[10]- m[6] * m[9]) * invDet;
|
|
temp[1] = (m[9] * m[2] - m[10]* m[1]) * invDet;
|
|
temp[2] = (m[1] * m[6] - m[2] * m[5]) * invDet;
|
|
|
|
temp[4] = (m[6] * m[8] - m[4] * m[10])* invDet;
|
|
temp[5] = (m[10]* m[0] - m[8] * m[2]) * invDet;
|
|
temp[6] = (m[2] * m[4] - m[0] * m[6]) * invDet;
|
|
|
|
temp[8] = (m[4] * m[9] - m[5] * m[8]) * invDet;
|
|
temp[9] = (m[8] * m[1] - m[9] * m[0]) * invDet;
|
|
temp[10]= (m[0] * m[5] - m[1] * m[4]) * invDet;
|
|
|
|
m[0] = temp[0];
|
|
m[1] = temp[1];
|
|
m[2] = temp[2];
|
|
|
|
m[4] = temp[4];
|
|
m[5] = temp[5];
|
|
m[6] = temp[6];
|
|
|
|
m[8] = temp[8];
|
|
m[9] = temp[9];
|
|
m[10] = temp[10];
|
|
|
|
// invert the translation
|
|
temp[0] = -m[3];
|
|
temp[1] = -m[7];
|
|
temp[2] = -m[11];
|
|
m_matF_x_vectorF(m, temp, &temp[4]);
|
|
m[3] = temp[4];
|
|
m[7] = temp[5];
|
|
m[11]= temp[6];
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_affineInverse_C(F32 *m)
|
|
{
|
|
// Matrix class checks to make sure this is an affine transform before calling
|
|
// this function, so we can proceed assuming it is...
|
|
F32 temp[16];
|
|
dMemcpy(temp, m, 16 * sizeof(F32));
|
|
|
|
// Transpose rotation
|
|
m[1] = temp[4];
|
|
m[4] = temp[1];
|
|
m[2] = temp[8];
|
|
m[8] = temp[2];
|
|
m[6] = temp[9];
|
|
m[9] = temp[6];
|
|
|
|
m[3] = -(temp[0]*temp[3] + temp[4]*temp[7] + temp[8]*temp[11]);
|
|
m[7] = -(temp[1]*temp[3] + temp[5]*temp[7] + temp[9]*temp[11]);
|
|
m[11] = -(temp[2]*temp[3] + temp[6]*temp[7] + temp[10]*temp[11]);
|
|
}
|
|
|
|
inline void swap(F32 &a, F32 &b)
|
|
{
|
|
F32 temp = a;
|
|
a = b;
|
|
b = temp;
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_transpose_C(F32 *m)
|
|
{
|
|
swap(m[1], m[4]);
|
|
swap(m[2], m[8]);
|
|
swap(m[3], m[12]);
|
|
swap(m[6], m[9]);
|
|
swap(m[7], m[13]);
|
|
swap(m[11],m[14]);
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_scale_C(F32 *m,const F32 *p)
|
|
{
|
|
// Note, doesn't allow scaling w...
|
|
|
|
m[0] *= p[0]; m[1] *= p[1]; m[2] *= p[2];
|
|
m[4] *= p[0]; m[5] *= p[1]; m[6] *= p[2];
|
|
m[8] *= p[0]; m[9] *= p[1]; m[10] *= p[2];
|
|
m[12] *= p[0]; m[13] *= p[1]; m[14] *= p[2];
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_normalize_C(F32 *m)
|
|
{
|
|
F32 col0[3], col1[3], col2[3];
|
|
// extract columns 0 and 1
|
|
col0[0] = m[0];
|
|
col0[1] = m[4];
|
|
col0[2] = m[8];
|
|
|
|
col1[0] = m[1];
|
|
col1[1] = m[5];
|
|
col1[2] = m[9];
|
|
|
|
// assure their relationsips to one another
|
|
mCross(col0, col1, col2);
|
|
mCross(col2, col0, col1);
|
|
|
|
// assure their lengh is 1.0f
|
|
m_point3F_normalize( col0 );
|
|
m_point3F_normalize( col1 );
|
|
m_point3F_normalize( col2 );
|
|
|
|
// store the normalized columns
|
|
m[0] = col0[0];
|
|
m[4] = col0[1];
|
|
m[8] = col0[2];
|
|
|
|
m[1] = col1[0];
|
|
m[5] = col1[1];
|
|
m[9] = col1[2];
|
|
|
|
m[2] = col2[0];
|
|
m[6] = col2[1];
|
|
m[10]= col2[2];
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
static F32 m_matF_determinant_C(const F32 *m)
|
|
{
|
|
return m[0] * (m[5] * m[10] - m[6] * m[9]) +
|
|
m[4] * (m[2] * m[9] - m[1] * m[10]) +
|
|
m[8] * (m[1] * m[6] - m[2] * m[5]) ;
|
|
}
|
|
|
|
|
|
//--------------------------------------
|
|
// Removed static in order to write benchmarking code (that compares against
|
|
// specialized SSE/AMD versions) elsewhere.
|
|
void default_matF_x_matF_C(const F32 *a, const F32 *b, F32 *mresult)
|
|
{
|
|
mresult[0] = a[0]*b[0] + a[1]*b[4] + a[2]*b[8] + a[3]*b[12];
|
|
mresult[1] = a[0]*b[1] + a[1]*b[5] + a[2]*b[9] + a[3]*b[13];
|
|
mresult[2] = a[0]*b[2] + a[1]*b[6] + a[2]*b[10] + a[3]*b[14];
|
|
mresult[3] = a[0]*b[3] + a[1]*b[7] + a[2]*b[11] + a[3]*b[15];
|
|
|
|
mresult[4] = a[4]*b[0] + a[5]*b[4] + a[6]*b[8] + a[7]*b[12];
|
|
mresult[5] = a[4]*b[1] + a[5]*b[5] + a[6]*b[9] + a[7]*b[13];
|
|
mresult[6] = a[4]*b[2] + a[5]*b[6] + a[6]*b[10] + a[7]*b[14];
|
|
mresult[7] = a[4]*b[3] + a[5]*b[7] + a[6]*b[11] + a[7]*b[15];
|
|
|
|
mresult[8] = a[8]*b[0] + a[9]*b[4] + a[10]*b[8] + a[11]*b[12];
|
|
mresult[9] = a[8]*b[1] + a[9]*b[5] + a[10]*b[9] + a[11]*b[13];
|
|
mresult[10]= a[8]*b[2] + a[9]*b[6] + a[10]*b[10]+ a[11]*b[14];
|
|
mresult[11]= a[8]*b[3] + a[9]*b[7] + a[10]*b[11]+ a[11]*b[15];
|
|
|
|
mresult[12]= a[12]*b[0]+ a[13]*b[4]+ a[14]*b[8] + a[15]*b[12];
|
|
mresult[13]= a[12]*b[1]+ a[13]*b[5]+ a[14]*b[9] + a[15]*b[13];
|
|
mresult[14]= a[12]*b[2]+ a[13]*b[6]+ a[14]*b[10]+ a[15]*b[14];
|
|
mresult[15]= a[12]*b[3]+ a[13]*b[7]+ a[14]*b[11]+ a[15]*b[15];
|
|
}
|
|
|
|
|
|
// //--------------------------------------
|
|
// static void m_matF_x_point3F_C(const F32 *m, const F32 *p, F32 *presult)
|
|
// {
|
|
// AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
|
|
// presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2] + m[3];
|
|
// presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2] + m[7];
|
|
// presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11];
|
|
// }
|
|
|
|
|
|
// //--------------------------------------
|
|
// static void m_matF_x_vectorF_C(const F32 *m, const F32 *v, F32 *vresult)
|
|
// {
|
|
// AssertFatal(v != vresult, "Error, aliasing matrix mul pointers not allowed here!");
|
|
// vresult[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
|
|
// vresult[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2];
|
|
// vresult[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2];
|
|
// }
|
|
|
|
|
|
//--------------------------------------
|
|
static void m_matF_x_point4F_C(const F32 *m, const F32 *p, F32 *presult)
|
|
{
|
|
AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
|
|
presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2] + m[3]*p[3];
|
|
presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2] + m[7]*p[3];
|
|
presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11]*p[3];
|
|
presult[3] = m[12]*p[0]+ m[13]*p[1]+ m[14]*p[2] + m[15]*p[3];
|
|
}
|
|
|
|
//--------------------------------------
|
|
static void m_matF_x_scale_x_planeF_C(const F32* m, const F32* s, const F32* p, F32* presult)
|
|
{
|
|
// We take in a matrix, a scale factor, and a plane equation. We want to output
|
|
// the resultant normal
|
|
// We have T = m*s
|
|
// To multiply the normal, we want Inv(Tr(m*s))
|
|
// Inv(Tr(ms)) = Inv(Tr(s) * Tr(m))
|
|
// = Inv(Tr(m)) * Inv(Tr(s))
|
|
//
|
|
// Inv(Tr(s)) = Inv(s) = [ 1/x 0 0 0]
|
|
// [ 0 1/y 0 0]
|
|
// [ 0 0 1/z 0]
|
|
// [ 0 0 0 1]
|
|
//
|
|
// Since m is an affine matrix,
|
|
// Tr(m) = [ [ ] 0 ]
|
|
// [ [ R ] 0 ]
|
|
// [ [ ] 0 ]
|
|
// [ [ x y z ] 1 ]
|
|
//
|
|
// Inv(Tr(m)) = [ [ -1 ] 0 ]
|
|
// [ [ R ] 0 ]
|
|
// [ [ ] 0 ]
|
|
// [ [ A B C ] 1 ]
|
|
// Where:
|
|
//
|
|
// P = (x, y, z)
|
|
// A = -(Row(0, r) * P);
|
|
// B = -(Row(1, r) * P);
|
|
// C = -(Row(2, r) * P);
|
|
|
|
MatrixF invScale(true);
|
|
F32* pScaleElems = invScale;
|
|
pScaleElems[MatrixF::idx(0, 0)] = 1.0 / s[0];
|
|
pScaleElems[MatrixF::idx(1, 1)] = 1.0 / s[1];
|
|
pScaleElems[MatrixF::idx(2, 2)] = 1.0 / s[2];
|
|
|
|
Point3F shear;
|
|
shear.x = m[MatrixF::idx(3, 0)];
|
|
shear.y = m[MatrixF::idx(3, 1)];
|
|
shear.z = m[MatrixF::idx(3, 2)];
|
|
|
|
Point3F row0(m[MatrixF::idx(0, 0)], m[MatrixF::idx(0, 1)], m[MatrixF::idx(0, 2)]);
|
|
Point3F row1(m[MatrixF::idx(1, 0)], m[MatrixF::idx(1, 1)], m[MatrixF::idx(1, 2)]);
|
|
Point3F row2(m[MatrixF::idx(2, 0)], m[MatrixF::idx(2, 1)], m[MatrixF::idx(2, 2)]);
|
|
|
|
F32 A = -mDot(row0, shear);
|
|
F32 B = -mDot(row1, shear);
|
|
F32 C = -mDot(row2, shear);
|
|
|
|
MatrixF invTrMatrix(true);
|
|
F32* destMat = invTrMatrix;
|
|
destMat[MatrixF::idx(0, 0)] = m[MatrixF::idx(0, 0)];
|
|
destMat[MatrixF::idx(1, 0)] = m[MatrixF::idx(1, 0)];
|
|
destMat[MatrixF::idx(2, 0)] = m[MatrixF::idx(2, 0)];
|
|
destMat[MatrixF::idx(0, 1)] = m[MatrixF::idx(0, 1)];
|
|
destMat[MatrixF::idx(1, 1)] = m[MatrixF::idx(1, 1)];
|
|
destMat[MatrixF::idx(2, 1)] = m[MatrixF::idx(2, 1)];
|
|
destMat[MatrixF::idx(0, 2)] = m[MatrixF::idx(0, 2)];
|
|
destMat[MatrixF::idx(1, 2)] = m[MatrixF::idx(1, 2)];
|
|
destMat[MatrixF::idx(2, 2)] = m[MatrixF::idx(2, 2)];
|
|
destMat[MatrixF::idx(0, 3)] = A;
|
|
destMat[MatrixF::idx(1, 3)] = B;
|
|
destMat[MatrixF::idx(2, 3)] = C;
|
|
invTrMatrix.mul(invScale);
|
|
|
|
Point3F norm(p[0], p[1], p[2]);
|
|
Point3F point = norm * -p[3];
|
|
invTrMatrix.mulP(norm);
|
|
norm.normalize();
|
|
|
|
MatrixF temp;
|
|
dMemcpy(temp, m, sizeof(F32) * 16);
|
|
point.x *= s[0];
|
|
point.y *= s[1];
|
|
point.z *= s[2];
|
|
temp.mulP(point);
|
|
|
|
PlaneF resultPlane(point, norm);
|
|
presult[0] = resultPlane.x;
|
|
presult[1] = resultPlane.y;
|
|
presult[2] = resultPlane.z;
|
|
presult[3] = resultPlane.d;
|
|
}
|
|
|
|
static void m_matF_x_box3F_C(const F32 *m, F32* min, F32* max)
|
|
{
|
|
// Algorithm for axis aligned bounding box adapted from
|
|
// Graphic Gems I, pp 548-550
|
|
//
|
|
F32 originalMin[3];
|
|
F32 originalMax[3];
|
|
originalMin[0] = min[0];
|
|
originalMin[1] = min[1];
|
|
originalMin[2] = min[2];
|
|
originalMax[0] = max[0];
|
|
originalMax[1] = max[1];
|
|
originalMax[2] = max[2];
|
|
|
|
min[0] = max[0] = m[3];
|
|
min[1] = max[1] = m[7];
|
|
min[2] = max[2] = m[11];
|
|
|
|
const F32 * row = &m[0];
|
|
for (U32 i = 0; i < 3; i++)
|
|
{
|
|
#define Do_One_Row(j) { \
|
|
F32 a = (row[j] * originalMin[j]); \
|
|
F32 b = (row[j] * originalMax[j]); \
|
|
if (a < b) { *min += a; *max += b; } \
|
|
else { *min += b; *max += a; } }
|
|
|
|
// Simpler addressing (avoiding things like [ecx+edi*4]) might be worthwhile (LH):
|
|
Do_One_Row(0);
|
|
Do_One_Row(1);
|
|
Do_One_Row(2);
|
|
row += 4;
|
|
min++;
|
|
max++;
|
|
}
|
|
}
|
|
|
|
|
|
void m_point3F_bulk_dot_C(const F32* refVector,
|
|
const F32* dotPoints,
|
|
const U32 numPoints,
|
|
const U32 pointStride,
|
|
F32* output)
|
|
{
|
|
for (U32 i = 0; i < numPoints; i++)
|
|
{
|
|
F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * i));
|
|
output[i] = ((refVector[0] * pPoint[0]) +
|
|
(refVector[1] * pPoint[1]) +
|
|
(refVector[2] * pPoint[2]));
|
|
}
|
|
}
|
|
|
|
void m_point3F_bulk_dot_indexed_C(const F32* refVector,
|
|
const F32* dotPoints,
|
|
const U32 numPoints,
|
|
const U32 pointStride,
|
|
const U32* pointIndices,
|
|
F32* output)
|
|
{
|
|
for (U32 i = 0; i < numPoints; i++)
|
|
{
|
|
F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * pointIndices[i]));
|
|
output[i] = ((refVector[0] * pPoint[0]) +
|
|
(refVector[1] * pPoint[1]) +
|
|
(refVector[2] * pPoint[2]));
|
|
}
|
|
}
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Math function pointer declarations
|
|
|
|
S32 (*m_mulDivS32)(S32 a, S32 b, S32 c) = m_mulDivS32_C;
|
|
U32 (*m_mulDivU32)(S32 a, S32 b, U32 c) = m_mulDivU32_C;
|
|
|
|
F32 (*m_catmullrom)(F32 t, F32 p0, F32 p1, F32 p2, F32 p3) = m_catmullrom_C;
|
|
|
|
void (*m_point2F_normalize)(F32 *p) = m_point2F_normalize_C;
|
|
void (*m_point2F_normalize_f)(F32 *p, F32 val) = m_point2F_normalize_f_C;
|
|
void (*m_point2D_normalize)(F64 *p) = m_point2D_normalize_C;
|
|
void (*m_point2D_normalize_f)(F64 *p, F64 val) = m_point2D_normalize_f_C;
|
|
void (*m_point3D_normalize_f)(F64 *p, F64 val) = m_point3D_normalize_f_C;
|
|
void (*m_point3F_normalize)(F32 *p) = m_point3F_normalize_C;
|
|
void (*m_point3F_normalize_f)(F32 *p, F32 len) = m_point3F_normalize_f_C;
|
|
void (*m_point3F_interpolate)(const F32 *from, const F32 *to, F32 factor, F32 *result) = m_point3F_interpolate_C;
|
|
|
|
void (*m_point3D_normalize)(F64 *p) = m_point3D_normalize_C;
|
|
void (*m_point3D_interpolate)(const F64 *from, const F64 *to, F64 factor, F64 *result) = m_point3D_interpolate_C;
|
|
|
|
void (*m_point3F_bulk_dot)(const F32* refVector,
|
|
const F32* dotPoints,
|
|
const U32 numPoints,
|
|
const U32 pointStride,
|
|
F32* output) = m_point3F_bulk_dot_C;
|
|
void (*m_point3F_bulk_dot_indexed)(const F32* refVector,
|
|
const F32* dotPoints,
|
|
const U32 numPoints,
|
|
const U32 pointStride,
|
|
const U32* pointIndices,
|
|
F32* output) = m_point3F_bulk_dot_indexed_C;
|
|
|
|
void (*m_quatF_set_matF)( F32 x, F32 y, F32 z, F32 w, F32* m ) = m_quatF_set_matF_C;
|
|
|
|
void (*m_matF_set_euler)(const F32 *e, F32 *result) = m_matF_set_euler_C;
|
|
void (*m_matF_set_euler_point)(const F32 *e, const F32 *p, F32 *result) = m_matF_set_euler_point_C;
|
|
void (*m_matF_identity)(F32 *m) = m_matF_identity_C;
|
|
void (*m_matF_inverse)(F32 *m) = m_matF_inverse_C;
|
|
void (*m_matF_affineInverse)(F32 *m) = m_matF_affineInverse_C;
|
|
void (*m_matF_transpose)(F32 *m) = m_matF_transpose_C;
|
|
void (*m_matF_scale)(F32 *m,const F32* p) = m_matF_scale_C;
|
|
void (*m_matF_normalize)(F32 *m) = m_matF_normalize_C;
|
|
F32 (*m_matF_determinant)(const F32 *m) = m_matF_determinant_C;
|
|
void (*m_matF_x_matF)(const F32 *a, const F32 *b, F32 *mresult) = default_matF_x_matF_C;
|
|
// void (*m_matF_x_point3F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point3F_C;
|
|
// void (*m_matF_x_vectorF)(const F32 *m, const F32 *v, F32 *vresult) = m_matF_x_vectorF_C;
|
|
void (*m_matF_x_point4F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point4F_C;
|
|
void (*m_matF_x_scale_x_planeF)(const F32 *m, const F32* s, const F32 *p, F32 *presult) = m_matF_x_scale_x_planeF_C;
|
|
void (*m_matF_x_box3F)(const F32 *m, F32 *min, F32 *max) = m_matF_x_box3F_C;
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
void mInstallLibrary_C()
|
|
{
|
|
m_mulDivS32 = m_mulDivS32_C;
|
|
m_mulDivU32 = m_mulDivU32_C;
|
|
|
|
m_catmullrom = m_catmullrom_C;
|
|
|
|
m_point2F_normalize = m_point2F_normalize_C;
|
|
m_point2F_normalize_f = m_point2F_normalize_f_C;
|
|
m_point2D_normalize = m_point2D_normalize_C;
|
|
m_point2D_normalize_f = m_point2D_normalize_f_C;
|
|
m_point3F_normalize = m_point3F_normalize_C;
|
|
m_point3F_normalize_f = m_point3F_normalize_f_C;
|
|
m_point3F_interpolate = m_point3F_interpolate_C;
|
|
|
|
m_point3D_normalize = m_point3D_normalize_C;
|
|
m_point3D_interpolate = m_point3D_interpolate_C;
|
|
|
|
m_point3F_bulk_dot = m_point3F_bulk_dot_C;
|
|
m_point3F_bulk_dot_indexed = m_point3F_bulk_dot_indexed_C;
|
|
|
|
m_quatF_set_matF = m_quatF_set_matF_C;
|
|
|
|
m_matF_set_euler = m_matF_set_euler_C;
|
|
m_matF_set_euler_point = m_matF_set_euler_point_C;
|
|
m_matF_identity = m_matF_identity_C;
|
|
m_matF_inverse = m_matF_inverse_C;
|
|
m_matF_affineInverse = m_matF_affineInverse_C;
|
|
m_matF_transpose = m_matF_transpose_C;
|
|
m_matF_scale = m_matF_scale_C;
|
|
m_matF_normalize = m_matF_normalize_C;
|
|
m_matF_determinant = m_matF_determinant_C;
|
|
m_matF_x_matF = default_matF_x_matF_C;
|
|
// m_matF_x_point3F = m_matF_x_point3F_C;
|
|
// m_matF_x_vectorF = m_matF_x_vectorF_C;
|
|
m_matF_x_point4F = m_matF_x_point4F_C;
|
|
m_matF_x_scale_x_planeF = m_matF_x_scale_x_planeF_C;
|
|
m_matF_x_box3F = m_matF_x_box3F_C;
|
|
}
|
|
|