Tribes2/engine
1
0
Fork 0
mirror of https://github.com/tribes2/engine.git synced 2026-01-20 03:34:48 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
engine/platformLinux/linuxMath_VC.c

1146 lines
25 KiB
C
Raw Normal View History

t2 engine svn checkout
2024-01-07 04:36:33 +00:00
/*
Sigh, we're going to get around the stupid gcc math bugs by compiling
this code with Visual C++ and then using the object file in our code.
HACK HACK HACK
*/
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define TRIBES2_NEW_MATH
#if defined(_MSC_VER)
#define __inline__ __inline
#else
#define __inline__ inline
#endif
typedef unsigned char U8;
typedef signed int S32;
typedef unsigned int U32;
typedef float F32;
typedef double F64;
/* Utility routines */
static __inline__ void* dMemcpy(void *dst, const void *src, unsigned size)
{
return memcpy(dst,src,size);
}
static __inline__ F32 getMin(F32 a, F32 b)
{
return a>b ? b : a;
}
static __inline__ F32 getMax(F32 a, F32 b)
{
return a>b ? a : b;
}
static __inline__ F32 mClampF(F32 val, F32 low, F32 high)
{
return getMax(getMin(val, high), low);
}
static __inline__ F32 mFabs(const F32 val)
{
return fabs(val);
}
static __inline__ F32 mSin(const F32 angle)
{
return sin(angle);
}
static __inline__ F32 mCos(const F32 angle)
{
return cos(angle);
}
#define mSinCos(angle, s, c) \
{ \
s = mSin(angle); \
c = mCos(angle); \
}
static __inline__ F32 mAcos(const F32 val)
{
#if defined(_MSC_VER)
extern double ACOS(double X);
return ACOS(val);
#else
return acos(val);
#endif
}
static __inline__ F32 mSqrt(const F32 val)
{
return sqrt(val);
}
static __inline__ F64 mSqrtD(const F64 val)
{
return sqrt(val);
}
static __inline__ F32 mPow(const F32 x, const F32 y)
{
#if defined(_MSC_VER)
extern double POW(double X, double Y);
return POW(x, y);
#else
return pow(x, y);
#endif
}
#define swap(x, y) { F32 t = y; y = x; x = t; }
static __inline__ F32 square( F32 d )
{
return ( d * d );
}
static __inline__ F32 cube( F32 d )
{
return ( d * d * d );
}
/* The actual math routines */
void m_matF_x_point3F_ASM(const F32 *m, const F32 *p, F32 *presult)
{
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];
}
void m_matF_x_vectorF_ASM(const F32 *m, const F32 *v, F32 *vresult)
{
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];
}
void m_quatF_set_matF_ASM( 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
}
F32 m_matF_determinant_ASM(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]) ;
}
void m_matF_inverse_ASM(F32 *m)
{
// using Cramers Rule find the Inverse
// Minv = (1/det(M)) * adjoint(M)
F32 det = m_matF_determinant_ASM( m );
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_ASM(m, temp, &temp[4]);
m[3] = temp[4];
m[7] = temp[5];
m[11]= temp[6];
}
void m_matF_affineInverse_ASM(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]);
}
void m_matF_x_matF_ASM(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];
}
#ifdef TRIBES2_NEW_MATH
// Code taken from Math/mSolver.cc
//--------------------------------------------------------------------------
#define EQN_EPSILON (1e-8)
static __inline__ int mIsZero(F32 val)
{
return((val > -EQN_EPSILON) && (val < EQN_EPSILON));
}
static __inline__ F32 mCbrt(F32 val)
{
if(val < 0.f)
return(-mPow(-val, (F32)(1.f/3.f)));
else
return(mPow(val, (F32)(1.f/3.f)));
}
static __inline__ U32 mSolveLinear(F32 a, F32 b, F32 * x)
{
if(mIsZero(a))
return(0);
x[0] = -b/a;
return(1);
}
U32 mSolveQuadratic_ASM(F32 a, F32 b, F32 c, F32 * x)
{
F32 desc;
// really linear?
if(mIsZero(a))
return(mSolveLinear(b, c, x));
// get the descriminant: (b^2 - 4ac)
desc = (b * b) - (4.f * a * c);
// solutions:
// desc < 0: two imaginary solutions
// desc > 0: two real solutions (b +- sqrt(desc)) / 2a
// desc = 0: one real solution (b / 2a)
if(mIsZero(desc))
{
x[0] = b / (2.f * a);
return(1);
}
else if(desc > 0.f)
{
F32 sqrdesc = mSqrt(desc);
F32 den = (2.f * a);
x[0] = (b + sqrdesc) / den;
x[1] = (b - sqrdesc) / den;
if(x[1] < x[0])
swap(x[0], x[1]);
return(2);
}
else
return(0);
}
//--------------------------------------------------------------------------
// from Graphics Gems I: pp 738-742
U32 mSolveCubic_ASM(F32 a, F32 b, F32 c, F32 d, F32 * x)
{
F32 A, B, C, D;
F32 A2, A3;
F32 p, q;
F32 p3, q2;
U32 num;
F32 sub;
U32 i;
S32 j, k;
if(mIsZero(a))
return(mSolveQuadratic_ASM(b, c, d, x));
// normal form: x^3 + Ax^2 + BX + C = 0
A = b / a;
B = c / a;
C = d / a;
// substitute x = y - A/3 to eliminate quadric term and depress
// the cubic equation to (x^3 + px + q = 0)
A2 = A * A;
A3 = A2 * A;
p = (1.f/3.f) * (((-1.f/3.f) * A2) + B);
q = (1.f/2.f) * (((2.f/27.f) * A3) - ((1.f/3.f) * A * B) + C);
// use Cardano's fomula to solve the depressed cubic
p3 = p * p * p;
q2 = q * q;
D = q2 + p3;
num = 0;
if(mIsZero(D)) // 1 or 2 solutions
{
if(mIsZero(q)) // 1 triple solution
{
x[0] = 0.f;
num = 1;
}
else // 1 single and 1 double
{
F32 u = mCbrt(-q);
x[0] = 2.f * u;
x[1] = -u;
num = 2;
}
}
else if(D < 0.f) // 3 solutions: casus irreducibilis
{
F32 phi = (1.f/3.f) * mAcos(-q / mSqrt(-p3));
F32 t = 2.f * mSqrt(-p);
x[0] = t * mCos(phi);
x[1] = -t * mCos(phi + (M_PI / 3.f));
x[2] = -t * mCos(phi - (M_PI / 3.f));
num = 3;
}
else // 1 solution
{
F32 sqrtD = mSqrt(D);
F32 u = mCbrt(sqrtD - q);
F32 v = -mCbrt(sqrtD + q);
x[0] = u + v;
num = 1;
}
// resubstitute
sub = (1.f/3.f) * A;
for(i = 0; i < num; i++)
x[i] -= sub;
// sort the roots
for(j = 0; j < (num - 1); j++)
for(k = j + 1; k < num; k++)
if(x[k] < x[j])
swap(x[k], x[j]);
return(num);
}
//--------------------------------------------------------------------------
// from Graphics Gems I: pp 738-742
U32 mSolveQuartic_ASM(F32 a, F32 b, F32 c, F32 d, F32 e, F32 * x)
{
F32 A, B, C, D;
F32 A2, A3, A4;
F32 p, q, r;
U32 num;
F32 sub;
U32 i;
S32 j, k;
if(mIsZero(a))
return(mSolveCubic_ASM(b, c, d, e, x));
// normal form: x^4 + ax^3 + bx^2 + cx + d = 0
A = b / a;
B = c / a;
C = d / a;
D = e / a;
// substitue x = y - A/4 to eliminate cubic term:
// x^4 + px^2 + qx + r = 0
A2 = A * A;
A3 = A2 * A;
A4 = A2 * A2;
p = ((-3.f/8.f) * A2) + B;
q = ((1.f/8.f) * A3) - ((1.f/2.f) * A * B) + C;
r = ((-3.f/256.f) * A4) + ((1.f/16.f) * A2 * B) - ((1.f/4.f) * A * C) + D;
num = 0;
if(mIsZero(r)) // no absolute term: y(y^3 + py + q) = 0
{
num = mSolveCubic_ASM(1.f, 0.f, p, q, x);
x[num++] = 0.f;
}
else
{
F32 q2;
F32 z;
F32 u, v;
// solve the resolvent cubic
q2 = q * q;
a = 1.f;
b = (-1.f/2.f) * p;
c = -r;
d = ((1.f/2.f) * r * p) - ((1.f/8.f) * q2);
mSolveCubic_ASM(a, b, c, d, x);
z = x[0];
// build 2 quadratic equations from the one solution
u = (z * z) - r;
v = (2.f * z) - p;
if(mIsZero(u))
u = 0.f;
else if(u > 0.f)
u = mSqrt(u);
else
return(0);
if(mIsZero(v))
v = 0.f;
else if(v > 0.f)
v = mSqrt(v);
else
return(0);
// solve the two quadratics
a = 1.f;
b = v;
c = z - u;
num = mSolveQuadratic_ASM(a, b, c, x);
a = 1.f;
b = -v;
c = z + u;
num += mSolveQuadratic_ASM(a, b, c, x + num);
}
// resubstitute
sub = (1.f/4.f) * A;
for(i = 0; i < num; i++)
x[i] -= sub;
// sort the roots
for(j = 0; j < (num - 1); j++)
for(k = j + 1; k < num; k++)
if(x[k] < x[j])
swap(x[k], x[j]);
return(num);
}
#else
/* Old cubic and quadratic math functions */
U32 mSolveCubic_ASM(F32 A, F32 B, F32 C, F32 D, F32* x)
{
int i;
U32 nreal;
F32 w, p, q, qp, dis, h, phi, phip;
F32 AP, BP, CP, DP, XP;
if( A != 0.0f ) {
// is it a cubic?
w = B / ( 3.0f * A );
CP = C / ( 3.0f * A );
p = cube( CP - square( w ) );
CP = ( C * w ) - D;
DP = CP / A;
CP = 2.0f * cube( w );
q = -0.5f *( CP - DP );
dis = square( q ) + p; // discriminant
if( dis < 0.0 ) {
// 3 real solutions
h = q / mSqrt( -p );
h = mClampF( h, -1.0f, 1.0f );
phi = mAcos( h );
p = 2.0f * mPow( -p, ( 1.0f / 6.0f ) );
for( i = 0; i < 3; i++ ) {
//x[i] = p*cos((phi+2*i*M_PI)/3.0) - w;
phip = ( 2.0f * i ) * M_PI;
phip = ( phip + phi ) / 3.0f;
x[i] = ( p * mCos( phip ) ) - w;
}
// sort results
if( x[1] < x[0] ) {
swap( x[0], x[1] );
}
if( x[2] < x[1] ) {
swap( x[1], x[2] );
}
if( x[1] < x[0] ) {
swap( x[0], x[1] );
}
nreal = 3;
} else {
// only one real solution
dis = mSqrt( dis );
//h = pow(fabs(q+dis), 1.f/3.f);
//p = pow(fabs(q-dis), 1.f/3.f);
qp = mFabs( q + dis );
h = mPow( qp, ( 1.0f / 3.0f ) );
qp = mFabs( q - dis );
p = mPow( qp, ( 1.0f / 3.0f ) );
//x[0] = ((q+dis>0.0)? h : -h) + ((q-dis>0.0)? p : -p) - w;
if( ( q + dis ) <= 0.0f ) {
h = -h;
}
if( ( q - dis ) <= 0.0f ) {
p = -p;
}
x[0] = ( h + p ) - w;
nreal = 1;
}
// Perform one step of a Newton iteration in order to minimize
// round-off errors
for (i = 0; i < nreal; i++)
{
//if ((h = C + x[i] * (2 * B + 3 * x[i] * A)) != 0.0) {
AP = x[i] * 3.0f * A;
BP = 2.0f * B;
XP = x[i] * ( BP + AP );
CP = C + XP;
h = CP;
if ( h != 0.0f ) {
//x[i] -= (D + x[i] * (C + x[i] * (B + x[i] * A)))/h;
AP = x[i] * A;
BP = B + AP;
XP = x[i] * BP;
CP = C + XP;
XP = x[i] * CP;
DP = D + XP;
DP = DP / h;
x[i] = x[i] - DP;
}
}
} else if( B != 0.0f ) {
// is it a quadratic?
p = 0.5f * ( C / B );
dis = square( p ) - ( D / B );
if( dis >= 0.0 ) {
// are there solutions?
dis = mSqrt( dis );
x[0] = -p - dis;
x[1] = -p + dis;
nreal = 2;
} else {
// no real solution
nreal = 0;
}
} else if( C != 0.0f ) {
// is it a linear?
x[0] = -D / C;
nreal = 1;
} else {
// not an equation
nreal = 0;
}
return nreal;
}
U32 mSolveQuartic_ASM( F32 A, F32 B, F32 C, F32 D, F32 E, F32* x )
{
int i;
U32 nreal;
F32 w, b0, b1, b2, d0, d1, d2, h, hw, t, z;
F32* px;
F32 w2;
F32 CA, DA, EA;
F32 CP, DP;
F32 zp;
F32 tp;
// check if it's a cubic.
if( A == 0.0f ) {
return mSolveCubic_ASM( B, C, D, E, x );
}
//w = B/(4*A); // offset
w = B / ( 4.0f * A );
w2 = square( w );
//b2 = -6*square(w) + C/A; // coeffs. of shifted polynomial
CA = C / A;
b2 = ( -6.0f * w2 ) + CA;
//b1 = (8*square(w) - 2*C/A)*w + D/A;
DA = D / A;
CP = 2.0f * CA;
CP = ( 8.0f * w2 ) - CP;
b1 = ( CP * w ) + DA;
//b0 = ((-3*square(w) + C/A)*w - D/A)*w + E/A;
EA = E / A;
CP = ( -3.0f * w2 ) + CA;
DP = ( CP * w ) - DA;
b0 = ( DP * w ) + EA;
// cubic resolvent
{
F32 cubeA = 1.0;
F32 cubeB = b2;
F32 cubeC = -4 * b0;
F32 bp = 4 * ( b0 * b2 );
F32 cubeD = square( b1 ) - bp;
// only lowermost root needed
mSolveCubic_ASM( cubeA, cubeB, cubeC, cubeD, x );
}
z = x[0];
nreal = 0;
px = x;
zp = 0.25f * square( z );
t = mSqrt( zp - b0 );
for( i = -1; i <= 1; i += 2 ) {
//d0 = -0.5*z + i*t;
d0 = ( -0.5 * z ) + ( i * t ); // coeffs. of quadratic factor
//d1 = (t!=0.0)? -i*0.5*b1/t : i*mSqrt(-z - b2);
if( t != 0.0f ) {
tp = -i * 0.5f;
tp = tp * b1;
tp = tp / t;
} else {
tp = mSqrt( -z - b2 );
tp = i * tp;
}
d1 = tp;
h = ( 0.25f * square( d1 ) ) - d0;
if( h >= 0.0f ) {
h = mSqrt( h );
nreal += 2;
//*px++ = -0.5*d1 - h - w;
//*px++ = -0.5*d1 + h - w;
d2 = -0.5f * d1;
hw = h - w;
*px = d2 - hw;
px++;
*px = d2 + hw;
px++;
}
}
// sort results in ascending order
if( nreal == 4 ) {
if( x[2] < x[0] ) {
swap( x[0], x[2] );
}
if( x[3] < x[1] ) {
swap( x[1], x[3] );
}
if( x[1] < x[0] ) {
swap( x[0], x[1] );
}
if( x[3] < x[2] ) {
swap( x[2], x[3] );
}
if( x[2] < x[1] ) {
swap( x[1], x[2] );
}
}
return nreal;
}
#endif /* TRIBES2_NEW_MATH */
/* Added lots more math functions optimized by Visual C++ */
//--------------------------------------
void m_point2F_normalize_ASM(F32 *p)
{
F32 factor = 1.0f / mSqrt(p[0]*p[0] + p[1]*p[1] );
p[0] *= factor;
p[1] *= factor;
}
//--------------------------------------
void m_point2F_normalize_f_ASM(F32 *p, F32 val)
{
F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] );
p[0] *= factor;
p[1] *= factor;
}
//--------------------------------------
void m_point2D_normalize_ASM(F64 *p)
{
F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] );
p[0] *= factor;
p[1] *= factor;
}
//--------------------------------------
void m_point2D_normalize_f_ASM(F64 *p, F64 val)
{
F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
p[0] *= factor;
p[1] *= factor;
}
//--------------------------------------
void m_point3F_normalize_ASM(F32 *p)
{
F32 squared = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
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;
}
}
//--------------------------------------
void m_point3F_normalize_f_ASM(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;
}
//--------------------------------------
void m_point3F_interpolate_ASM(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;
}
//--------------------------------------
void m_point3D_normalize_ASM(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;
}
//--------------------------------------
void m_point3D_interpolate_ASM(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;
}
void m_point3F_bulk_dot_ASM(const F32* refVector,
const F32* dotPoints,
const U32 numPoints,
const U32 pointStride,
F32* output)
{
U32 i;
for (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_ASM(const F32* refVector,
const F32* dotPoints,
const U32 numPoints,
const U32 pointStride,
const U32* pointIndices,
F32* output)
{
U32 i;
for (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]));
}
}
//--------------------------------------
void m_matF_identity_ASM(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;
}
//--------------------------------------
void m_matF_set_euler_ASM(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_ASM(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;
F32 r1, r2, r3, r4;
mSinCos( e[2], sz, cz );
r1 = cz;
r2 = sz;
r3 = 0.0f;
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;
F32 cy,sy;
F32 cz,sz;
F32 r1, r2, r3, r4;
mSinCos( e[0], sx, cx );
mSinCos( e[1], sy, cy );
mSinCos( e[2], sz, cz );
r1 = cy * cz;
r2 = cy * sz;
r3 = sy * cz;
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;
}
}
}
//--------------------------------------
void m_matF_set_euler_point_ASM(const F32 *e, const F32 *p, F32 *result)
{
m_matF_set_euler_ASM(e, result);
result[3] = p[0];
result[7] = p[1];
result[11]= p[2];
}
//--------------------------------------
void m_matF_transpose_ASM(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]);
}
//--------------------------------------
void m_matF_scale_ASM(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 __inline__ void mCross(const F32* a, const F32* b, F32 *res)
{
res[0] = (a[1] * b[2]) - (a[2] * b[1]);
res[1] = (a[2] * b[0]) - (a[0] * b[2]);
res[2] = (a[0] * b[1]) - (a[1] * b[0]);
}
//--------------------------------------
void m_matF_normalize_ASM(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_ASM( col0 );
m_point3F_normalize_ASM( col1 );
m_point3F_normalize_ASM( 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];
}
//--------------------------------------
void m_matF_x_point4F_ASM(const F32 *m, const F32 *p, F32 *presult)
{
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];
}
Reference in a new issue Copy permalink