forked from a1batross/Paranoia2_original
1223 lines
27 KiB
C++
1223 lines
27 KiB
C++
/***
|
|
*
|
|
* Copyright (c) 1996-2002, Valve LLC. All rights reserved.
|
|
*
|
|
* This product contains software technology licensed from Id
|
|
* Software, Inc. ("Id Technology"). Id Technology (c) 1996 Id Software, Inc.
|
|
* All Rights Reserved.
|
|
*
|
|
****/
|
|
|
|
// mathlib.c -- math primitives
|
|
#include "cmdlib.h"
|
|
#include "mathlib.h"
|
|
#include "bspfile.h"
|
|
|
|
vec3_t vec3_origin = { 0, 0, 0 };
|
|
|
|
#define IA 16807
|
|
#define IM 2147483647
|
|
#define IQ 127773
|
|
#define IR 2836
|
|
#define NTAB 32
|
|
#define NDIV (1+(IM-1)/NTAB)
|
|
#define MAX_RANDOM_RANGE 0x7FFFFFFFUL
|
|
|
|
// fran1 -- return a random floating-point number on the interval [0,1)
|
|
//
|
|
#define AM (1.0/IM)
|
|
#define EPS 1.2e-7
|
|
#define RNMX (1.0-EPS)
|
|
|
|
static int m_idum = 0;
|
|
static int m_iy = 0;
|
|
static int m_iv[NTAB];
|
|
|
|
static int GenerateRandomNumber( void )
|
|
{
|
|
int j, k;
|
|
|
|
if( m_idum <= 0 || !m_iy )
|
|
{
|
|
if( -(m_idum) < 1 )
|
|
m_idum = 1;
|
|
else m_idum = -(m_idum);
|
|
|
|
for( j = NTAB + 7; j >= 0; j-- )
|
|
{
|
|
k = (m_idum) / IQ;
|
|
m_idum = IA * (m_idum - k * IQ) - IR * k;
|
|
|
|
if( m_idum < 0 )
|
|
m_idum += IM;
|
|
|
|
if( j < NTAB )
|
|
m_iv[j] = m_idum;
|
|
}
|
|
|
|
m_iy = m_iv[0];
|
|
}
|
|
|
|
k = (m_idum) / IQ;
|
|
m_idum = IA * (m_idum - k * IQ) - IR * k;
|
|
|
|
if( m_idum < 0 )
|
|
m_idum += IM;
|
|
j = m_iy / NDIV;
|
|
|
|
// We're seeing some strange memory corruption in the contents of s_pUniformStream.
|
|
// Perhaps it's being caused by something writing past the end of this array?
|
|
// Bounds-check in release to see if that's the case.
|
|
if( j >= NTAB || j < 0 )
|
|
{
|
|
MsgDev( D_WARN, "GenerateRandomNumber had an array overrun: tried to write to element %d of 0..31.\n", j );
|
|
j = ( j % NTAB ) & 0x7fffffff;
|
|
}
|
|
|
|
m_iy = m_iv[j];
|
|
m_iv[j] = m_idum;
|
|
|
|
return m_iy;
|
|
}
|
|
|
|
float RandomFloat( float flLow, float flHigh )
|
|
{
|
|
// float in [0,1)
|
|
float fl = AM * GenerateRandomNumber();
|
|
if( fl > RNMX ) fl = RNMX;
|
|
|
|
return (fl * ( flHigh - flLow ) ) + flLow; // float in [low,high)
|
|
}
|
|
|
|
const matrix3x4 matrix3x4_identity =
|
|
{
|
|
{ 1.0f, 0.0f, 0.0f }, // PITCH
|
|
{ 0.0f, 1.0f, 0.0f }, // YAW
|
|
{ 0.0f, 0.0f, 1.0f }, // ROLL
|
|
{ 0.0f, 0.0f, 0.0f }, // ORG
|
|
};
|
|
|
|
half :: half( const float x )
|
|
{
|
|
union
|
|
{
|
|
float floatI;
|
|
unsigned int i;
|
|
};
|
|
|
|
floatI = x;
|
|
int e = ((i >> 23) & 0xFF) - 112;
|
|
int m = i & 0x007FFFFF;
|
|
|
|
sh = (i >> 16) & 0x8000;
|
|
if( e <= 0 )
|
|
{
|
|
// Denorm
|
|
m = ((m | 0x00800000) >> (1 - e)) + 0x1000;
|
|
sh |= (m >> 13);
|
|
}
|
|
else if( e == 143 )
|
|
{
|
|
sh |= 0x7C00;
|
|
if (m != 0){
|
|
// NAN
|
|
m >>= 13;
|
|
sh |= m | (m == 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m += 0x1000;
|
|
|
|
if( m & 0x00800000 )
|
|
{
|
|
// Mantissa overflow
|
|
m = 0;
|
|
e++;
|
|
}
|
|
|
|
if( e >= 31 )
|
|
{
|
|
// Exponent overflow
|
|
sh |= 0x7C00;
|
|
}
|
|
else
|
|
{
|
|
sh |= (e << 10) | (m >> 13);
|
|
}
|
|
}
|
|
}
|
|
|
|
half :: operator float() const
|
|
{
|
|
union
|
|
{
|
|
unsigned int s;
|
|
float result;
|
|
};
|
|
|
|
s = (sh & 0x8000) << 16;
|
|
unsigned int e = (sh >> 10) & 0x1F;
|
|
unsigned int m = sh & 0x03FF;
|
|
|
|
if( e == 0 )
|
|
{
|
|
// +/- 0
|
|
if( m == 0 ) return result;
|
|
|
|
// Denorm
|
|
while(( m & 0x0400 ) == 0 )
|
|
{
|
|
m += m;
|
|
e--;
|
|
}
|
|
e++;
|
|
m &= ~0x0400;
|
|
}
|
|
else if( e == 31 )
|
|
{
|
|
// INF / NAN
|
|
s |= 0x7F800000 | (m << 13);
|
|
return result;
|
|
}
|
|
|
|
s |= ((e + 112) << 23) | (m << 13);
|
|
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
SinCos
|
|
=================
|
|
*/
|
|
void SinCos( float radians, float *sine, float *cosine )
|
|
{
|
|
_asm
|
|
{
|
|
fld dword ptr [radians]
|
|
fsincos
|
|
|
|
mov edx, dword ptr [cosine]
|
|
mov eax, dword ptr [sine]
|
|
|
|
fstp dword ptr [edx]
|
|
fstp dword ptr [eax]
|
|
}
|
|
}
|
|
|
|
/*
|
|
==============
|
|
ColorNormalize
|
|
|
|
==============
|
|
*/
|
|
float ColorNormalize( const vec3_t in, vec3_t out )
|
|
{
|
|
float scale;
|
|
|
|
if(( scale = VectorMax( in )) == 0 )
|
|
return 0;
|
|
|
|
VectorScale( in, (1.0 / scale), out );
|
|
|
|
return scale;
|
|
}
|
|
|
|
/*
|
|
==============
|
|
FloatToHalf
|
|
|
|
==============
|
|
*/
|
|
unsigned short FloatToHalf( float v )
|
|
{
|
|
unsigned int i = *((unsigned int *)&v);
|
|
unsigned int e = (i >> 23) & 0x00ff;
|
|
unsigned int m = i & 0x007fffff;
|
|
unsigned short h;
|
|
|
|
if( e <= 127 - 15 )
|
|
h = ((m | 0x00800000) >> (127 - 14 - e)) >> 13;
|
|
else h = (i >> 13) & 0x3fff;
|
|
|
|
h |= (i >> 16) & 0xc000;
|
|
|
|
return h;
|
|
}
|
|
|
|
/*
|
|
==============
|
|
HalfToFloat
|
|
|
|
==============
|
|
*/
|
|
float HalfToFloat( unsigned short h )
|
|
{
|
|
unsigned int f = (h << 16) & 0x80000000;
|
|
unsigned int em = h & 0x7fff;
|
|
|
|
if( em > 0x03ff )
|
|
{
|
|
f |= (em << 13) + ((127 - 15) << 23);
|
|
}
|
|
else
|
|
{
|
|
unsigned int m = em & 0x03ff;
|
|
|
|
if( m != 0 )
|
|
{
|
|
unsigned int e = (em >> 10) & 0x1f;
|
|
|
|
while(( m & 0x0400 ) == 0 )
|
|
{
|
|
m <<= 1;
|
|
e--;
|
|
}
|
|
|
|
m &= 0x3ff;
|
|
f |= ((e + (127 - 14)) << 23) | (m << 13);
|
|
}
|
|
}
|
|
|
|
return *((float *)&f);
|
|
}
|
|
|
|
/*
|
|
==============
|
|
VectorCompareEpsilon
|
|
|
|
==============
|
|
*/
|
|
bool VectorCompareEpsilon( const vec3_t vec1, const vec3_t vec2, vec_t epsilon )
|
|
{
|
|
vec_t ax, ay, az;
|
|
|
|
ax = fabs( vec1[0] - vec2[0] );
|
|
ay = fabs( vec1[1] - vec2[1] );
|
|
az = fabs( vec1[2] - vec2[2] );
|
|
|
|
if(( ax <= epsilon ) && ( ay <= epsilon ) && ( az <= epsilon ))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
bool VectorCompareEpsilon2( const vec3_t vec1, const float vec2[3], vec_t epsilon )
|
|
{
|
|
vec_t ax, ay, az;
|
|
|
|
ax = fabs( vec1[0] - vec2[0] );
|
|
ay = fabs( vec1[1] - vec2[1] );
|
|
az = fabs( vec1[2] - vec2[2] );
|
|
|
|
if(( ax <= epsilon ) && ( ay <= epsilon ) && ( az <= epsilon ))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
==================
|
|
VertexHashKey
|
|
==================
|
|
*/
|
|
uint VertexHashKey( const vec3_t point, uint hashSize )
|
|
{
|
|
uint hashKey = 0;
|
|
|
|
hashKey ^= int( fabs( point[0] ));
|
|
hashKey ^= int( fabs( point[1] ));
|
|
hashKey ^= int( fabs( point[2] ));
|
|
|
|
hashKey &= (hashSize - 1);
|
|
|
|
return hashKey;
|
|
}
|
|
|
|
/*
|
|
================
|
|
VectorIsOnAxis
|
|
================
|
|
*/
|
|
bool VectorIsOnAxis( const vec3_t v )
|
|
{
|
|
int count = 0;
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( v[i] == 0.0 )
|
|
count++;
|
|
}
|
|
|
|
// the zero vector will be on axis.
|
|
return (count > 1) ? true : false;
|
|
}
|
|
|
|
vec_t VectorNormalizeLength2( const vec3_t v, vec3_t out )
|
|
{
|
|
vec_t length, ilength;
|
|
|
|
length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
|
length = sqrt( length );
|
|
|
|
if( length )
|
|
{
|
|
ilength = 1.0 / length;
|
|
out[0] = v[0] * ilength;
|
|
out[1] = v[1] * ilength;
|
|
out[2] = v[2] * ilength;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
void VectorVectors( const vec3_t forward, vec3_t right, vec3_t up )
|
|
{
|
|
VectorSet( right, forward[2], -forward[0], forward[1] );
|
|
vec_t d = DotProduct( forward, right );
|
|
VectorMA( right, -d, forward, right );
|
|
VectorNormalize( right );
|
|
CrossProduct( right, forward, up );
|
|
VectorNormalize( up );
|
|
}
|
|
|
|
/*
|
|
=================
|
|
PlaneTypeForNormal
|
|
=================
|
|
*/
|
|
int PlaneTypeForNormal( const vec3_t normal )
|
|
{
|
|
if( normal[0] == 1 )
|
|
return PLANE_X;
|
|
if( normal[1] == 1 )
|
|
return PLANE_Y;
|
|
if( normal[2] == 1 )
|
|
return PLANE_Z;
|
|
return PLANE_NONAXIAL;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
SignbitsForPlane
|
|
|
|
fast box on planeside test
|
|
=================
|
|
*/
|
|
int SignbitsForPlane( const vec3_t normal )
|
|
{
|
|
for( int bits = 0, i = 0; i < 3; i++ )
|
|
if( normal[i] < 0.0f )
|
|
bits |= 1<<i;
|
|
return bits;
|
|
}
|
|
|
|
//
|
|
// bounds operations
|
|
//
|
|
/*
|
|
=================
|
|
ClearBounds
|
|
=================
|
|
*/
|
|
void ClearBounds( vec3_t mins, vec3_t maxs )
|
|
{
|
|
// make bogus range
|
|
mins[0] = mins[1] = mins[2] = 999999.0;
|
|
maxs[0] = maxs[1] = maxs[2] = -999999.0;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
AddPointToBounds
|
|
=================
|
|
*/
|
|
void AddPointToBounds( const vec3_t v, vec3_t mins, vec3_t maxs )
|
|
{
|
|
vec_t val;
|
|
int i;
|
|
|
|
for( i = 0; i < 3; i++ )
|
|
{
|
|
val = v[i];
|
|
if( val < mins[i] ) mins[i] = val;
|
|
if( val > maxs[i] ) maxs[i] = val;
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
ExpandBounds
|
|
=================
|
|
*/
|
|
void ExpandBounds( vec3_t mins, vec3_t maxs, vec_t offset )
|
|
{
|
|
mins[0] -= offset;
|
|
mins[1] -= offset;
|
|
mins[2] -= offset;
|
|
maxs[0] += offset;
|
|
maxs[1] += offset;
|
|
maxs[2] += offset;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
RadiusFromBounds
|
|
=================
|
|
*/
|
|
vec_t RadiusFromBounds( const vec3_t mins, const vec3_t maxs )
|
|
{
|
|
vec3_t corner;
|
|
int i;
|
|
|
|
for( i = 0; i < 3; i++ )
|
|
{
|
|
corner[i] = fabs( mins[i] ) > fabs( maxs[i] ) ? fabs( mins[i] ) : fabs( maxs[i] );
|
|
}
|
|
return VectorLength( corner );
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsIsCleared
|
|
=================
|
|
*/
|
|
bool BoundsIsCleared( const vec3_t mins, const vec3_t maxs )
|
|
{
|
|
if( mins[0] <= maxs[0] || mins[1] <= maxs[1] || mins[2] <= maxs[2] )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsIntersect
|
|
=================
|
|
*/
|
|
bool BoundsIntersect( const vec3_t mins1, const vec3_t maxs1, const vec3_t mins2, const vec3_t maxs2, vec_t epsilon )
|
|
{
|
|
if( mins1[0] > maxs2[0] + epsilon || mins1[1] > maxs2[1] + epsilon || mins1[2] > maxs2[2] + epsilon )
|
|
return false;
|
|
if( maxs1[0] < mins2[0] - epsilon || maxs1[1] < mins2[1] - epsilon || maxs1[2] < mins2[2] - epsilon )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsAndSphereIntersect
|
|
=================
|
|
*/
|
|
bool BoundsAndSphereIntersect( const vec3_t mins, const vec3_t maxs, const vec3_t origin, vec_t radius )
|
|
{
|
|
if( mins[0] > origin[0] + radius || mins[1] > origin[1] + radius || mins[2] > origin[2] + radius )
|
|
return false;
|
|
if( maxs[0] < origin[0] - radius || maxs[1] < origin[1] - radius || maxs[2] < origin[2] - radius )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
SphereIntersect
|
|
=================
|
|
*/
|
|
bool SphereIntersect( const vec3_t vSphereCenter, vec_t fSphereRadiusSquared, const vec3_t vLinePt, const vec3_t vLineDir )
|
|
{
|
|
vec_t a, b, c, insideSqr;
|
|
vec3_t p;
|
|
|
|
// translate sphere to origin.
|
|
VectorSubtract( vLinePt, vSphereCenter, p );
|
|
|
|
a = DotProduct( vLineDir, vLineDir );
|
|
b = 2.0 * DotProduct( p, vLineDir );
|
|
c = DotProduct( p, p ) - fSphereRadiusSquared;
|
|
|
|
insideSqr = b * b - 4.0 * a * c;
|
|
if( insideSqr <= 0.000001 )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
void CalcST( const vec3_t p0, const vec3_t p1, const vec3_t p2, const vec2_t t0, const vec2_t t1, const vec2_t t2, vec3_t s, vec3_t t, bool areaweight )
|
|
{
|
|
vec3_t edge01, edge02, cross;
|
|
|
|
// Compute the partial derivatives of X, Y, and Z with respect to S and T.
|
|
VectorClear( s );
|
|
VectorClear( t );
|
|
|
|
// x, s, t
|
|
VectorSet( edge01, p1[0] - p0[0], t1[0] - t0[0], t1[1] - t0[1] );
|
|
VectorSet( edge02, p2[0] - p0[0], t2[0] - t0[0], t2[1] - t0[1] );
|
|
CrossProduct( edge01, edge02, cross );
|
|
|
|
if( fabs( cross[0] ) > SMALL_FLOAT )
|
|
{
|
|
s[0] += -cross[1] / cross[0];
|
|
t[0] += -cross[2] / cross[0];
|
|
}
|
|
|
|
// y, s, t
|
|
VectorSet( edge01, p1[1] - p0[1], t1[0] - t0[0], t1[1] - t0[1] );
|
|
VectorSet( edge02, p2[1] - p0[1], t2[0] - t0[0], t2[1] - t0[1] );
|
|
CrossProduct( edge01, edge02, cross );
|
|
|
|
if( fabs( cross[0] ) > SMALL_FLOAT )
|
|
{
|
|
s[1] += -cross[1] / cross[0];
|
|
t[1] += -cross[2] / cross[0];
|
|
}
|
|
|
|
// z, s, t
|
|
VectorSet( edge01, p1[2] - p0[2], t1[0] - t0[0], t1[1] - t0[1] );
|
|
VectorSet( edge02, p2[2] - p0[2], t2[0] - t0[0], t2[1] - t0[1] );
|
|
CrossProduct( edge01, edge02, cross );
|
|
|
|
if( fabs( cross[0] ) > SMALL_FLOAT )
|
|
{
|
|
s[2] += -cross[1] / cross[0];
|
|
t[2] += -cross[2] / cross[0];
|
|
}
|
|
|
|
if( !areaweight )
|
|
{
|
|
// Normalize s and t
|
|
VectorNormalize2( s );
|
|
VectorNormalize2( t );
|
|
}
|
|
}
|
|
|
|
/*
|
|
=============
|
|
COM_NormalizeAngles
|
|
|
|
=============
|
|
*/
|
|
void COM_NormalizeAngles( vec3_t angles )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( angles[i] > 180.0f )
|
|
angles[i] -= 360.0f;
|
|
else if( angles[i] < -180.0f )
|
|
angles[i] += 360.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
====================
|
|
AngleQuaternion
|
|
====================
|
|
*/
|
|
void AngleQuaternion( const vec3_t angles, vec4_t quat )
|
|
{
|
|
float sr, sp, sy, cr, cp, cy;
|
|
|
|
SinCos( angles[ROLL] * 0.5f, &sy, &cy );
|
|
SinCos( angles[YAW] * 0.5f, &sp, &cp );
|
|
SinCos( angles[PITCH] * 0.5f, &sr, &cr );
|
|
|
|
quat[0] = sr * cp * cy - cr * sp * sy; // X
|
|
quat[1] = cr * sp * cy + sr * cp * sy; // Y
|
|
quat[2] = cr * cp * sy - sr * sp * cy; // Z
|
|
quat[3] = cr * cp * cy + sr * sp * sy; // W
|
|
}
|
|
|
|
/*
|
|
====================
|
|
AngleMatrix
|
|
====================
|
|
*/
|
|
void Matrix3x4_CreateFromEntityScale3f( matrix3x4 out, const vec3_t angles, const vec3_t origin, const vec3_t scale )
|
|
{
|
|
float sr, sp, sy, cr, cp, cy;
|
|
|
|
SinCos( DEG2RAD( angles[YAW] ), &sy, &cy );
|
|
SinCos( DEG2RAD( angles[PITCH] ), &sp, &cp );
|
|
SinCos( DEG2RAD( angles[ROLL] ), &sr, &cr );
|
|
|
|
out[0][0] = (cp*cy) * scale[0];
|
|
out[1][0] = (sr*sp*cy+cr*-sy) * scale[1];
|
|
out[2][0] = (cr*sp*cy+-sr*-sy) * scale[2];
|
|
out[3][0] = origin[0];
|
|
out[0][1] = (cp*sy) * scale[0];
|
|
out[1][1] = (sr*sp*sy+cr*cy) * scale[1];
|
|
out[2][1] = (cr*sp*sy+-sr*cy) * scale[2];
|
|
out[3][1] = origin[1];
|
|
out[0][2] = (-sp) * scale[0];
|
|
out[1][2] = (sr*cp) * scale[1];
|
|
out[2][2] = (cr*cp) * scale[2];
|
|
out[3][2] = origin[2];
|
|
}
|
|
|
|
/*
|
|
================
|
|
Matrix3x4_MatrixToEntityScale3f
|
|
================
|
|
*/
|
|
void Matrix3x4_MatrixToEntityScale3f( const matrix3x4 in, vec3_t origin, vec3_t angles, vec3_t scale )
|
|
{
|
|
float xyDist = sqrt( in[0][0] * in[0][0] + in[0][1] * in[0][1] );
|
|
|
|
if( xyDist > 0.001f )
|
|
{
|
|
// enough here to get angles?
|
|
angles[0] = RAD2DEG( atan2( -in[0][2], xyDist ));
|
|
angles[1] = RAD2DEG( atan2( in[0][1], in[0][0] ));
|
|
angles[2] = RAD2DEG( atan2( in[1][2], in[2][2] ));
|
|
}
|
|
else
|
|
{
|
|
// forward is mostly Z, gimbal lock
|
|
angles[0] = RAD2DEG( atan2( -in[0][2], xyDist ));
|
|
angles[1] = RAD2DEG( atan2( -in[1][0], in[1][1] ));
|
|
angles[2] = 0.0f;
|
|
}
|
|
|
|
origin[0] = in[3][0];
|
|
origin[1] = in[3][1];
|
|
origin[2] = in[3][2];
|
|
|
|
scale[0] = VectorLength( in[0] );
|
|
scale[1] = VectorLength( in[1] );
|
|
scale[2] = VectorLength( in[2] );
|
|
}
|
|
|
|
/*
|
|
================
|
|
QuaternionMatrix
|
|
================
|
|
*/
|
|
void Matrix3x4_FromOriginQuat( matrix3x4 out, const vec4_t quaternion, const vec3_t origin )
|
|
{
|
|
out[0][0] = 1.0f - 2.0f * quaternion[1] * quaternion[1] - 2.0f * quaternion[2] * quaternion[2];
|
|
out[0][1] = 2.0f * quaternion[0] * quaternion[1] + 2.0f * quaternion[3] * quaternion[2];
|
|
out[0][2] = 2.0f * quaternion[0] * quaternion[2] - 2.0f * quaternion[3] * quaternion[1];
|
|
|
|
out[1][0] = 2.0f * quaternion[0] * quaternion[1] - 2.0f * quaternion[3] * quaternion[2];
|
|
out[1][1] = 1.0f - 2.0f * quaternion[0] * quaternion[0] - 2.0f * quaternion[2] * quaternion[2];
|
|
out[1][2] = 2.0f * quaternion[1] * quaternion[2] + 2.0f * quaternion[3] * quaternion[0];
|
|
|
|
out[2][0] = 2.0f * quaternion[0] * quaternion[2] + 2.0f * quaternion[3] * quaternion[1];
|
|
out[2][1] = 2.0f * quaternion[1] * quaternion[2] - 2.0f * quaternion[3] * quaternion[0];
|
|
out[2][2] = 1.0f - 2.0f * quaternion[0] * quaternion[0] - 2.0f * quaternion[1] * quaternion[1];
|
|
|
|
out[3][0] = origin[0];
|
|
out[3][1] = origin[1];
|
|
out[3][2] = origin[2];
|
|
}
|
|
|
|
/*
|
|
================
|
|
ConcatTransforms
|
|
================
|
|
*/
|
|
void Matrix3x4_ConcatTransforms( matrix3x4 out, const matrix3x4 in1, const matrix3x4 in2 )
|
|
{
|
|
out[0][0] = in1[0][0] * in2[0][0] + in1[1][0] * in2[0][1] + in1[2][0] * in2[0][2];
|
|
out[1][0] = in1[0][0] * in2[1][0] + in1[1][0] * in2[1][1] + in1[2][0] * in2[1][2];
|
|
out[2][0] = in1[0][0] * in2[2][0] + in1[1][0] * in2[2][1] + in1[2][0] * in2[2][2];
|
|
out[3][0] = in1[0][0] * in2[3][0] + in1[1][0] * in2[3][1] + in1[2][0] * in2[3][2] + in1[3][0];
|
|
out[0][1] = in1[0][1] * in2[0][0] + in1[1][1] * in2[0][1] + in1[2][1] * in2[0][2];
|
|
out[1][1] = in1[0][1] * in2[1][0] + in1[1][1] * in2[1][1] + in1[2][1] * in2[1][2];
|
|
out[2][1] = in1[0][1] * in2[2][0] + in1[1][1] * in2[2][1] + in1[2][1] * in2[2][2];
|
|
out[3][1] = in1[0][1] * in2[3][0] + in1[1][1] * in2[3][1] + in1[2][1] * in2[3][2] + in1[3][1];
|
|
out[0][2] = in1[0][2] * in2[0][0] + in1[1][2] * in2[0][1] + in1[2][2] * in2[0][2];
|
|
out[1][2] = in1[0][2] * in2[1][0] + in1[1][2] * in2[1][1] + in1[2][2] * in2[1][2];
|
|
out[2][2] = in1[0][2] * in2[2][0] + in1[1][2] * in2[2][1] + in1[2][2] * in2[2][2];
|
|
out[3][2] = in1[0][2] * in2[3][0] + in1[1][2] * in2[3][1] + in1[2][2] * in2[3][2] + in1[3][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
TransformStandardPlane
|
|
====================
|
|
*/
|
|
void Matrix3x4_TransformStandardPlane( const matrix3x4 in, const vec3_t normal, float d, vec3_t out, float *dist )
|
|
{
|
|
#if 0
|
|
float scale = sqrt( in[0][0] * in[0][0] + in[0][1] * in[0][1] + in[0][2] * in[0][2] );
|
|
float iscale = 1.0f / scale;
|
|
|
|
out[0] = (normal[0] * in[0][0] + normal[1] * in[1][0] + normal[2] * in[2][0]) * iscale;
|
|
out[1] = (normal[0] * in[0][1] + normal[1] * in[1][1] + normal[2] * in[2][1]) * iscale;
|
|
out[2] = (normal[0] * in[0][2] + normal[1] * in[1][2] + normal[2] * in[2][2]) * iscale;
|
|
*dist = d * scale + ( out[0] * in[3][0] + out[1] * in[3][1] + out[2] * in[3][2] );
|
|
#else
|
|
// g-cont. because matrix is already inverted
|
|
out[0] = (normal[0] * in[0][0] + normal[1] * in[1][0] + normal[2] * in[2][0]);
|
|
out[1] = (normal[0] * in[0][1] + normal[1] * in[1][1] + normal[2] * in[2][1]);
|
|
out[2] = (normal[0] * in[0][2] + normal[1] * in[1][2] + normal[2] * in[2][2]);
|
|
*dist = -((normal[0] * in[3][0] + normal[1] * in[3][1] + normal[2] * in[3][2]) - d);
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
====================
|
|
VectorTransform
|
|
====================
|
|
*/
|
|
void Matrix3x4_VectorTransform( const matrix3x4 in, const vec3_t v, vec3_t out )
|
|
{
|
|
vec3_t iv;
|
|
|
|
// in case v == out
|
|
iv[0] = v[0];
|
|
iv[1] = v[1];
|
|
iv[2] = v[2];
|
|
|
|
out[0] = iv[0] * in[0][0] + iv[1] * in[1][0] + iv[2] * in[2][0] + in[3][0];
|
|
out[1] = iv[0] * in[0][1] + iv[1] * in[1][1] + iv[2] * in[2][1] + in[3][1];
|
|
out[2] = iv[0] * in[0][2] + iv[1] * in[1][2] + iv[2] * in[2][2] + in[3][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
VectorTransform
|
|
====================
|
|
*/
|
|
void Matrix3x4_Vector4Transform( const matrix3x4 in, const vec4_t v, vec4_t out )
|
|
{
|
|
vec4_t iv;
|
|
|
|
// in case v == out
|
|
iv[0] = v[0];
|
|
iv[1] = v[1];
|
|
iv[2] = v[2];
|
|
iv[3] = v[3];
|
|
|
|
out[0] = iv[0] * in[0][0] + iv[1] * in[1][0] + iv[2] * in[2][0] + iv[3] * in[3][0];
|
|
out[1] = iv[0] * in[0][1] + iv[1] * in[1][1] + iv[2] * in[2][1] + iv[3] * in[3][1];
|
|
out[2] = iv[0] * in[0][2] + iv[1] * in[1][2] + iv[2] * in[2][2] + iv[3] * in[3][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
VectorITransform
|
|
====================
|
|
*/
|
|
void Matrix3x4_VectorITransform( const matrix3x4 in, const vec3_t v, vec3_t out )
|
|
{
|
|
vec3_t iv;
|
|
|
|
iv[0] = v[0] - in[3][0];
|
|
iv[1] = v[1] - in[3][1];
|
|
iv[2] = v[2] - in[3][2];
|
|
|
|
out[0] = iv[0] * in[0][0] + iv[1] * in[0][1] + iv[2] * in[0][2];
|
|
out[1] = iv[0] * in[1][0] + iv[1] * in[1][1] + iv[2] * in[1][2];
|
|
out[2] = iv[0] * in[2][0] + iv[1] * in[2][1] + iv[2] * in[2][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
VectorRotate
|
|
====================
|
|
*/
|
|
void Matrix3x4_VectorRotate( const matrix3x4 in, const vec3_t v, vec3_t out )
|
|
{
|
|
vec3_t iv;
|
|
|
|
// in case v == out
|
|
iv[0] = v[0];
|
|
iv[1] = v[1];
|
|
iv[2] = v[2];
|
|
|
|
out[0] = iv[0] * in[0][0] + iv[1] * in[1][0] + iv[2] * in[2][0];
|
|
out[1] = iv[0] * in[0][1] + iv[1] * in[1][1] + iv[2] * in[2][1];
|
|
out[2] = iv[0] * in[0][2] + iv[1] * in[1][2] + iv[2] * in[2][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
VectorIRotate
|
|
====================
|
|
*/
|
|
void Matrix3x4_VectorIRotate( const matrix3x4 in, const vec3_t v, vec3_t out )
|
|
{
|
|
vec3_t iv;
|
|
|
|
// in case v == out
|
|
iv[0] = v[0];
|
|
iv[1] = v[1];
|
|
iv[2] = v[2];
|
|
|
|
out[0] = iv[0] * in[0][0] + iv[1] * in[0][1] + iv[2] * in[0][2];
|
|
out[1] = iv[0] * in[1][0] + iv[1] * in[1][1] + iv[2] * in[1][2];
|
|
out[2] = iv[0] * in[2][0] + iv[1] * in[2][1] + iv[2] * in[2][2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
CalcSign
|
|
====================
|
|
*/
|
|
vec_t Matrix3x4_CalcSign( const matrix3x4 in )
|
|
{
|
|
vec3_t out;
|
|
|
|
out[0] = in[0][1] * in[1][2] - in[0][2] * in[1][1];
|
|
out[1] = in[0][2] * in[1][0] - in[0][0] * in[1][2];
|
|
out[2] = in[0][0] * in[1][1] - in[0][1] * in[1][0];
|
|
|
|
return ( out[0] * in[2][0] + out[1] * in[2][1] + out[2] * in[2][2] );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
Invert_Full
|
|
====================
|
|
*/
|
|
bool Matrix3x4_Invert_Full( matrix3x4 out, const matrix3x4 in )
|
|
{
|
|
double texplanes[2][4];
|
|
double faceplane[4];
|
|
double texaxis[2][3];
|
|
double normalaxis[3];
|
|
double det, sqrlen1, sqrlen2, sqrlen3;
|
|
double dot_epsilon = NORMAL_EPSILON * NORMAL_EPSILON;
|
|
double texorg[3];
|
|
|
|
for( int i = 0; i < 4; i++ )
|
|
{
|
|
texplanes[0][i] = in[i][0];
|
|
texplanes[1][i] = in[i][1];
|
|
faceplane[i] = in[i][2];
|
|
}
|
|
|
|
sqrlen1 = DotProduct( texplanes[0], texplanes[0] );
|
|
sqrlen2 = DotProduct( texplanes[1], texplanes[1] );
|
|
sqrlen3 = DotProduct( faceplane, faceplane );
|
|
|
|
if( sqrlen1 <= dot_epsilon || sqrlen2 <= dot_epsilon || sqrlen3 <= dot_epsilon )
|
|
{
|
|
// s gradient, t gradient or face normal is too close to 0
|
|
return false;
|
|
}
|
|
|
|
CrossProduct( texplanes[0], texplanes[1], normalaxis );
|
|
det = DotProduct( normalaxis, faceplane );
|
|
|
|
if( det * det <= sqrlen1 * sqrlen2 * sqrlen3 * dot_epsilon )
|
|
{
|
|
// s gradient, t gradient and face normal are coplanar
|
|
return false;
|
|
}
|
|
|
|
VectorScale( normalaxis, 1.0 / det, normalaxis );
|
|
CrossProduct( texplanes[1], faceplane, texaxis[0] );
|
|
VectorScale( texaxis[0], 1.0 / det, texaxis[0] );
|
|
|
|
CrossProduct( faceplane, texplanes[0], texaxis[1] );
|
|
VectorScale( texaxis[1], 1.0 / det, texaxis[1] );
|
|
|
|
VectorScale( normalaxis, -faceplane[3], texorg );
|
|
VectorMA( texorg, -texplanes[0][3], texaxis[0], texorg );
|
|
VectorMA( texorg, -texplanes[1][3], texaxis[1], texorg );
|
|
|
|
VectorCopy( texaxis[0], out[0] );
|
|
VectorCopy( texaxis[1], out[1] );
|
|
VectorCopy( normalaxis, out[2] );
|
|
VectorCopy( texorg, out[3] );
|
|
|
|
return true;
|
|
}
|
|
|
|
const matrix4x4 matrix4x4_identity =
|
|
{
|
|
{ 1.0f, 0.0f, 0.0f, 0.0f }, // PITCH
|
|
{ 0.0f, 1.0f, 0.0f, 0.0f }, // YAW
|
|
{ 0.0f, 0.0f, 1.0f, 0.0f }, // ROLL
|
|
{ 0.0f, 0.0f, 0.0f, 1.0f }, // ORIGIN
|
|
};
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Matrix4x4 operations
|
|
|
|
========================================================================
|
|
*/
|
|
void Matrix4x4_Concat( matrix4x4 out, const matrix4x4 in1, const matrix4x4 in2 )
|
|
{
|
|
out[0][0] = in1[0][0] * in2[0][0] + in1[1][0] * in2[0][1] + in1[2][0] * in2[0][2] + in1[3][0] * in2[0][3];
|
|
out[1][0] = in1[0][0] * in2[1][0] + in1[1][0] * in2[1][1] + in1[2][0] * in2[1][2] + in1[3][0] * in2[1][3];
|
|
out[2][0] = in1[0][0] * in2[2][0] + in1[1][0] * in2[2][1] + in1[2][0] * in2[2][2] + in1[3][0] * in2[2][3];
|
|
out[3][0] = in1[0][0] * in2[3][0] + in1[1][0] * in2[3][1] + in1[2][0] * in2[3][2] + in1[3][0] * in2[3][3];
|
|
out[0][1] = in1[0][1] * in2[0][0] + in1[1][1] * in2[0][1] + in1[2][1] * in2[0][2] + in1[3][1] * in2[0][3];
|
|
out[1][1] = in1[0][1] * in2[1][0] + in1[1][1] * in2[1][1] + in1[2][1] * in2[1][2] + in1[3][1] * in2[1][3];
|
|
out[2][1] = in1[0][1] * in2[2][0] + in1[1][1] * in2[2][1] + in1[2][1] * in2[2][2] + in1[3][1] * in2[2][3];
|
|
out[3][1] = in1[0][1] * in2[3][0] + in1[1][1] * in2[3][1] + in1[2][1] * in2[3][2] + in1[3][1] * in2[3][3];
|
|
out[0][2] = in1[0][2] * in2[0][0] + in1[1][2] * in2[0][1] + in1[2][2] * in2[0][2] + in1[3][2] * in2[0][3];
|
|
out[1][2] = in1[0][2] * in2[1][0] + in1[1][2] * in2[1][1] + in1[2][2] * in2[1][2] + in1[3][2] * in2[1][3];
|
|
out[2][2] = in1[0][2] * in2[2][0] + in1[1][2] * in2[2][1] + in1[2][2] * in2[2][2] + in1[3][2] * in2[2][3];
|
|
out[3][2] = in1[0][2] * in2[3][0] + in1[1][2] * in2[3][1] + in1[2][2] * in2[3][2] + in1[3][2] * in2[3][3];
|
|
out[0][3] = in1[0][3] * in2[0][0] + in1[1][3] * in2[0][1] + in1[2][3] * in2[0][2] + in1[3][3] * in2[0][3];
|
|
out[1][3] = in1[0][3] * in2[1][0] + in1[1][3] * in2[1][1] + in1[2][3] * in2[1][2] + in1[3][3] * in2[1][3];
|
|
out[2][3] = in1[0][3] * in2[2][0] + in1[1][3] * in2[2][1] + in1[2][3] * in2[2][2] + in1[3][3] * in2[2][3];
|
|
out[3][3] = in1[0][3] * in2[3][0] + in1[1][3] * in2[3][1] + in1[2][3] * in2[3][2] + in1[3][3] * in2[3][3];
|
|
}
|
|
|
|
bool Matrix4x4_Invert_Full( matrix4x4 out, const matrix4x4 in1 )
|
|
{
|
|
float *temp;
|
|
float *r[4];
|
|
float rtemp[4][8];
|
|
float m[4];
|
|
float s;
|
|
|
|
r[0] = rtemp[0];
|
|
r[1] = rtemp[1];
|
|
r[2] = rtemp[2];
|
|
r[3] = rtemp[3];
|
|
|
|
r[0][0] = in1[0][0];
|
|
r[0][1] = in1[1][0];
|
|
r[0][2] = in1[2][0];
|
|
r[0][3] = in1[3][0];
|
|
r[0][4] = 1.0f;
|
|
r[0][5] = 0.0f;
|
|
r[0][6] = 0.0f;
|
|
r[0][7] = 0.0f;
|
|
|
|
r[1][0] = in1[0][1];
|
|
r[1][1] = in1[1][1];
|
|
r[1][2] = in1[2][1];
|
|
r[1][3] = in1[3][1];
|
|
r[1][5] = 1.0f;
|
|
r[1][4] = 0.0f;
|
|
r[1][6] = 0.0f;
|
|
r[1][7] = 0.0f;
|
|
|
|
r[2][0] = in1[0][2];
|
|
r[2][1] = in1[1][2];
|
|
r[2][2] = in1[2][2];
|
|
r[2][3] = in1[3][2];
|
|
r[2][6] = 1.0f;
|
|
r[2][4] = 0.0f;
|
|
r[2][5] = 0.0f;
|
|
r[2][7] = 0.0f;
|
|
|
|
r[3][0] = in1[0][3];
|
|
r[3][1] = in1[1][3];
|
|
r[3][2] = in1[2][3];
|
|
r[3][3] = in1[3][3];
|
|
r[3][4] = 0.0f;
|
|
r[3][5] = 0.0f;
|
|
r[3][6] = 0.0f;
|
|
r[3][7] = 1.0f;
|
|
|
|
if( fabs( r[3][0] ) > fabs( r[2][0] ))
|
|
{
|
|
temp = r[3];
|
|
r[3] = r[2];
|
|
r[2] = temp;
|
|
}
|
|
|
|
if( fabs( r[2][0] ) > fabs( r[1][0] ))
|
|
{
|
|
temp = r[2];
|
|
r[2] = r[1];
|
|
r[1] = temp;
|
|
}
|
|
|
|
if( fabs( r[1][0] ) > fabs( r[0][0] ))
|
|
{
|
|
temp = r[1];
|
|
r[1] = r[0];
|
|
r[0] = temp;
|
|
}
|
|
|
|
if( r[0][0] )
|
|
{
|
|
m[1] = r[1][0] / r[0][0];
|
|
m[2] = r[2][0] / r[0][0];
|
|
m[3] = r[3][0] / r[0][0];
|
|
|
|
s = r[0][1];
|
|
r[1][1] -= m[1] * s;
|
|
r[2][1] -= m[2] * s;
|
|
r[3][1] -= m[3] * s;
|
|
|
|
s = r[0][2];
|
|
r[1][2] -= m[1] * s;
|
|
r[2][2] -= m[2] * s;
|
|
r[3][2] -= m[3] * s;
|
|
|
|
s = r[0][3];
|
|
r[1][3] -= m[1] * s;
|
|
r[2][3] -= m[2] * s;
|
|
r[3][3] -= m[3] * s;
|
|
|
|
s = r[0][4];
|
|
if( s )
|
|
{
|
|
r[1][4] -= m[1] * s;
|
|
r[2][4] -= m[2] * s;
|
|
r[3][4] -= m[3] * s;
|
|
}
|
|
|
|
s = r[0][5];
|
|
if( s )
|
|
{
|
|
r[1][5] -= m[1] * s;
|
|
r[2][5] -= m[2] * s;
|
|
r[3][5] -= m[3] * s;
|
|
}
|
|
|
|
s = r[0][6];
|
|
if( s )
|
|
{
|
|
r[1][6] -= m[1] * s;
|
|
r[2][6] -= m[2] * s;
|
|
r[3][6] -= m[3] * s;
|
|
}
|
|
|
|
s = r[0][7];
|
|
if( s )
|
|
{
|
|
r[1][7] -= m[1] * s;
|
|
r[2][7] -= m[2] * s;
|
|
r[3][7] -= m[3] * s;
|
|
}
|
|
|
|
if( fabs( r[3][1] ) > fabs( r[2][1] ))
|
|
{
|
|
temp = r[3];
|
|
r[3] = r[2];
|
|
r[2] = temp;
|
|
}
|
|
|
|
if( fabs( r[2][1] ) > fabs( r[1][1] ))
|
|
{
|
|
temp = r[2];
|
|
r[2] = r[1];
|
|
r[1] = temp;
|
|
}
|
|
|
|
if( r[1][1] )
|
|
{
|
|
m[2] = r[2][1] / r[1][1];
|
|
m[3] = r[3][1] / r[1][1];
|
|
r[2][2] -= m[2] * r[1][2];
|
|
r[3][2] -= m[3] * r[1][2];
|
|
r[2][3] -= m[2] * r[1][3];
|
|
r[3][3] -= m[3] * r[1][3];
|
|
|
|
s = r[1][4];
|
|
if( s )
|
|
{
|
|
r[2][4] -= m[2] * s;
|
|
r[3][4] -= m[3] * s;
|
|
}
|
|
|
|
s = r[1][5];
|
|
if( s )
|
|
{
|
|
r[2][5] -= m[2] * s;
|
|
r[3][5] -= m[3] * s;
|
|
}
|
|
|
|
s = r[1][6];
|
|
if( s )
|
|
{
|
|
r[2][6] -= m[2] * s;
|
|
r[3][6] -= m[3] * s;
|
|
}
|
|
|
|
s = r[1][7];
|
|
if( s )
|
|
{
|
|
r[2][7] -= m[2] * s;
|
|
r[3][7] -= m[3] * s;
|
|
}
|
|
|
|
if( fabs( r[3][2] ) > fabs( r[2][2] ))
|
|
{
|
|
temp = r[3];
|
|
r[3] = r[2];
|
|
r[2] = temp;
|
|
}
|
|
|
|
if( r[2][2] )
|
|
{
|
|
m[3] = r[3][2] / r[2][2];
|
|
r[3][3] -= m[3] * r[2][3];
|
|
r[3][4] -= m[3] * r[2][4];
|
|
r[3][5] -= m[3] * r[2][5];
|
|
r[3][6] -= m[3] * r[2][6];
|
|
r[3][7] -= m[3] * r[2][7];
|
|
|
|
if( r[3][3] )
|
|
{
|
|
s = 1.0f / r[3][3];
|
|
r[3][4] *= s;
|
|
r[3][5] *= s;
|
|
r[3][6] *= s;
|
|
r[3][7] *= s;
|
|
|
|
m[2] = r[2][3];
|
|
s = 1.0f / r[2][2];
|
|
r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
|
|
r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
|
|
r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
|
|
r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
|
|
|
|
m[1] = r[1][3];
|
|
r[1][4] -= r[3][4] * m[1];
|
|
r[1][5] -= r[3][5] * m[1];
|
|
r[1][6] -= r[3][6] * m[1];
|
|
r[1][7] -= r[3][7] * m[1];
|
|
|
|
m[0] = r[0][3];
|
|
r[0][4] -= r[3][4] * m[0];
|
|
r[0][5] -= r[3][5] * m[0];
|
|
r[0][6] -= r[3][6] * m[0];
|
|
r[0][7] -= r[3][7] * m[0];
|
|
|
|
m[1] = r[1][2];
|
|
s = 1.0f / r[1][1];
|
|
r[1][4] = s * (r[1][4] - r[2][4] * m[1]);
|
|
r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
|
|
r[1][6] = s * (r[1][6] - r[2][6] * m[1]);
|
|
r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
|
|
|
|
m[0] = r[0][2];
|
|
r[0][4] -= r[2][4] * m[0];
|
|
r[0][5] -= r[2][5] * m[0];
|
|
r[0][6] -= r[2][6] * m[0];
|
|
r[0][7] -= r[2][7] * m[0];
|
|
|
|
m[0] = r[0][1];
|
|
s = 1.0f / r[0][0];
|
|
r[0][4] = s * (r[0][4] - r[1][4] * m[0]);
|
|
r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
|
|
r[0][6] = s * (r[0][6] - r[1][6] * m[0]);
|
|
r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
|
|
|
|
out[0][0] = r[0][4];
|
|
out[0][1] = r[1][4];
|
|
out[0][2] = r[2][4];
|
|
out[0][3] = r[3][4];
|
|
out[1][0] = r[0][5];
|
|
out[1][1] = r[1][5];
|
|
out[1][2] = r[2][5];
|
|
out[1][3] = r[3][5];
|
|
out[2][0] = r[0][6];
|
|
out[2][1] = r[1][6];
|
|
out[2][2] = r[2][6];
|
|
out[2][3] = r[3][6];
|
|
out[3][0] = r[0][7];
|
|
out[3][1] = r[1][7];
|
|
out[3][2] = r[2][7];
|
|
out[3][3] = r[3][7];
|
|
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
} |