You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
4303 lines
111 KiB
4303 lines
111 KiB
//========= Copyright Valve Corporation, All rights reserved. ============// |
|
// |
|
// Purpose: Math primitives. |
|
// |
|
//===========================================================================// |
|
|
|
/// FIXME: As soon as all references to mathlib.c are gone, include it in here |
|
|
|
#include <math.h> |
|
#include <float.h> // Needed for FLT_EPSILON |
|
|
|
#include "tier0/basetypes.h" |
|
#include <memory.h> |
|
#include "tier0/dbg.h" |
|
|
|
#include "tier0/vprof.h" |
|
//#define _VPROF_MATHLIB |
|
|
|
#pragma warning(disable:4244) // "conversion from 'const int' to 'float', possible loss of data" |
|
#pragma warning(disable:4730) // "mixing _m64 and floating point expressions may result in incorrect code" |
|
|
|
#include "mathlib/mathlib.h" |
|
#include "mathlib/vector.h" |
|
#if !defined( _X360 ) |
|
#include "mathlib/amd3dx.h" |
|
#ifndef OSX |
|
#include "3dnow.h" |
|
#endif |
|
#include "sse.h" |
|
#endif |
|
|
|
#include "mathlib/ssemath.h" |
|
#include "mathlib/ssequaternion.h" |
|
|
|
// memdbgon must be the last include file in a .cpp file!!! |
|
#include "tier0/memdbgon.h" |
|
|
|
bool s_bMathlibInitialized = false; |
|
|
|
#ifdef PARANOID |
|
// User must provide an implementation of Sys_Error() |
|
void Sys_Error (char *error, ...); |
|
#endif |
|
|
|
const Vector vec3_origin(0,0,0); |
|
const QAngle vec3_angle(0,0,0); |
|
const Vector vec3_invalid( FLT_MAX, FLT_MAX, FLT_MAX ); |
|
const int nanmask = 255<<23; |
|
|
|
//----------------------------------------------------------------------------- |
|
// Standard C implementations of optimized routines: |
|
//----------------------------------------------------------------------------- |
|
float _sqrtf(float _X) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return sqrtf(_X); |
|
} |
|
|
|
float _rsqrtf(float x) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
return 1.f / _sqrtf( x ); |
|
} |
|
|
|
float FASTCALL _VectorNormalize (Vector& vec) |
|
{ |
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "_VectorNormalize", "Mathlib" ); |
|
#endif |
|
Assert( s_bMathlibInitialized ); |
|
float radius = sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z); |
|
|
|
// FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. |
|
float iradius = 1.f / ( radius + FLT_EPSILON ); |
|
|
|
vec.x *= iradius; |
|
vec.y *= iradius; |
|
vec.z *= iradius; |
|
|
|
return radius; |
|
} |
|
|
|
// TODO: Add fast C VectorNormalizeFast. |
|
// Perhaps use approximate rsqrt trick, if the accuracy isn't too bad. |
|
void FASTCALL _VectorNormalizeFast (Vector& vec) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
// FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. |
|
float iradius = 1.f / ( sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z) + FLT_EPSILON ); |
|
|
|
vec.x *= iradius; |
|
vec.y *= iradius; |
|
vec.z *= iradius; |
|
|
|
} |
|
|
|
float _InvRSquared(const float* v) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float r2 = DotProduct(v, v); |
|
return r2 < 1.f ? 1.f : 1/r2; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Function pointers selecting the appropriate implementation |
|
//----------------------------------------------------------------------------- |
|
float (*pfSqrt)(float x) = _sqrtf; |
|
float (*pfRSqrt)(float x) = _rsqrtf; |
|
float (*pfRSqrtFast)(float x) = _rsqrtf; |
|
float (FASTCALL *pfVectorNormalize)(Vector& v) = _VectorNormalize; |
|
void (FASTCALL *pfVectorNormalizeFast)(Vector& v) = _VectorNormalizeFast; |
|
float (*pfInvRSquared)(const float* v) = _InvRSquared; |
|
void (*pfFastSinCos)(float x, float* s, float* c) = SinCos; |
|
float (*pfFastCos)(float x) = cosf; |
|
|
|
float SinCosTable[SIN_TABLE_SIZE]; |
|
void InitSinCosTable() |
|
{ |
|
for( int i = 0; i < SIN_TABLE_SIZE; i++ ) |
|
{ |
|
SinCosTable[i] = sin(i * 2.0 * M_PI / SIN_TABLE_SIZE); |
|
} |
|
} |
|
|
|
qboolean VectorsEqual( const float *v1, const float *v2 ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return ( ( v1[0] == v2[0] ) && |
|
( v1[1] == v2[1] ) && |
|
( v1[2] == v2[2] ) ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Generates Euler angles given a left-handed orientation matrix. The |
|
// columns of the matrix contain the forward, left, and up vectors. |
|
// Input : matrix - Left-handed orientation matrix. |
|
// angles[PITCH, YAW, ROLL]. Receives right-handed counterclockwise |
|
// rotations in degrees around Y, Z, and X respectively. |
|
//----------------------------------------------------------------------------- |
|
|
|
void MatrixAngles( const matrix3x4_t& matrix, RadianEuler &angles, Vector &position ) |
|
{ |
|
MatrixGetColumn( matrix, 3, position ); |
|
MatrixAngles( matrix, angles ); |
|
} |
|
|
|
void MatrixAngles( const matrix3x4_t &matrix, Quaternion &q, Vector &pos ) |
|
{ |
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "MatrixQuaternion", "Mathlib" ); |
|
#endif |
|
float trace; |
|
trace = matrix[0][0] + matrix[1][1] + matrix[2][2] + 1.0f; |
|
if( trace > 1.0f + FLT_EPSILON ) |
|
{ |
|
// VPROF_INCREMENT_COUNTER("MatrixQuaternion A",1); |
|
q.x = ( matrix[2][1] - matrix[1][2] ); |
|
q.y = ( matrix[0][2] - matrix[2][0] ); |
|
q.z = ( matrix[1][0] - matrix[0][1] ); |
|
q.w = trace; |
|
} |
|
else if ( matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2] ) |
|
{ |
|
// VPROF_INCREMENT_COUNTER("MatrixQuaternion B",1); |
|
trace = 1.0f + matrix[0][0] - matrix[1][1] - matrix[2][2]; |
|
q.x = trace; |
|
q.y = (matrix[1][0] + matrix[0][1] ); |
|
q.z = (matrix[0][2] + matrix[2][0] ); |
|
q.w = (matrix[2][1] - matrix[1][2] ); |
|
} |
|
else if (matrix[1][1] > matrix[2][2]) |
|
{ |
|
// VPROF_INCREMENT_COUNTER("MatrixQuaternion C",1); |
|
trace = 1.0f + matrix[1][1] - matrix[0][0] - matrix[2][2]; |
|
q.x = (matrix[0][1] + matrix[1][0] ); |
|
q.y = trace; |
|
q.z = (matrix[2][1] + matrix[1][2] ); |
|
q.w = (matrix[0][2] - matrix[2][0] ); |
|
} |
|
else |
|
{ |
|
// VPROF_INCREMENT_COUNTER("MatrixQuaternion D",1); |
|
trace = 1.0f + matrix[2][2] - matrix[0][0] - matrix[1][1]; |
|
q.x = (matrix[0][2] + matrix[2][0] ); |
|
q.y = (matrix[2][1] + matrix[1][2] ); |
|
q.z = trace; |
|
q.w = (matrix[1][0] - matrix[0][1] ); |
|
} |
|
|
|
QuaternionNormalize( q ); |
|
|
|
#if 0 |
|
// check against the angle version |
|
RadianEuler ang; |
|
MatrixAngles( matrix, ang ); |
|
Quaternion test; |
|
AngleQuaternion( ang, test ); |
|
float d = QuaternionDotProduct( q, test ); |
|
Assert( fabs(d) > 0.99 && fabs(d) < 1.01 ); |
|
#endif |
|
|
|
MatrixGetColumn( matrix, 3, pos ); |
|
} |
|
|
|
void MatrixAngles( const matrix3x4_t& matrix, float *angles ) |
|
{ |
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "MatrixAngles", "Mathlib" ); |
|
#endif |
|
Assert( s_bMathlibInitialized ); |
|
float forward[3]; |
|
float left[3]; |
|
float up[3]; |
|
|
|
// |
|
// Extract the basis vectors from the matrix. Since we only need the Z |
|
// component of the up vector, we don't get X and Y. |
|
// |
|
forward[0] = matrix[0][0]; |
|
forward[1] = matrix[1][0]; |
|
forward[2] = matrix[2][0]; |
|
left[0] = matrix[0][1]; |
|
left[1] = matrix[1][1]; |
|
left[2] = matrix[2][1]; |
|
up[2] = matrix[2][2]; |
|
|
|
float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); |
|
|
|
// enough here to get angles? |
|
if ( xyDist > 0.001f ) |
|
{ |
|
// (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis |
|
angles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); |
|
|
|
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); |
|
angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); |
|
|
|
// (roll) z = ATAN( left.z, up.z ); |
|
angles[2] = RAD2DEG( atan2f( left[2], up[2] ) ); |
|
} |
|
else // forward is mostly Z, gimbal lock- |
|
{ |
|
// (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw |
|
angles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); |
|
|
|
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); |
|
angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); |
|
|
|
// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) |
|
angles[2] = 0; |
|
} |
|
} |
|
|
|
|
|
// transform in1 by the matrix in2 |
|
void VectorTransform (const float *in1, const matrix3x4_t& in2, float *out) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( in1 != out ); |
|
out[0] = DotProduct(in1, in2[0]) + in2[0][3]; |
|
out[1] = DotProduct(in1, in2[1]) + in2[1][3]; |
|
out[2] = DotProduct(in1, in2[2]) + in2[2][3]; |
|
} |
|
|
|
|
|
// assuming the matrix is orthonormal, transform in1 by the transpose (also the inverse in this case) of in2. |
|
void VectorITransform (const float *in1, const matrix3x4_t& in2, float *out) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float in1t[3]; |
|
|
|
in1t[0] = in1[0] - in2[0][3]; |
|
in1t[1] = in1[1] - in2[1][3]; |
|
in1t[2] = in1[2] - in2[2][3]; |
|
|
|
out[0] = in1t[0] * in2[0][0] + in1t[1] * in2[1][0] + in1t[2] * in2[2][0]; |
|
out[1] = in1t[0] * in2[0][1] + in1t[1] * in2[1][1] + in1t[2] * in2[2][1]; |
|
out[2] = in1t[0] * in2[0][2] + in1t[1] * in2[1][2] + in1t[2] * in2[2][2]; |
|
} |
|
|
|
|
|
// assume in2 is a rotation and rotate the input vector |
|
void VectorRotate( const float *in1, const matrix3x4_t& in2, float *out ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( in1 != out ); |
|
out[0] = DotProduct( in1, in2[0] ); |
|
out[1] = DotProduct( in1, in2[1] ); |
|
out[2] = DotProduct( in1, in2[2] ); |
|
} |
|
|
|
// assume in2 is a rotation and rotate the input vector |
|
void VectorRotate( const Vector &in1, const QAngle &in2, Vector &out ) |
|
{ |
|
matrix3x4_t matRotate; |
|
AngleMatrix( in2, matRotate ); |
|
VectorRotate( in1, matRotate, out ); |
|
} |
|
|
|
// assume in2 is a rotation and rotate the input vector |
|
void VectorRotate( const Vector &in1, const Quaternion &in2, Vector &out ) |
|
{ |
|
matrix3x4_t matRotate; |
|
QuaternionMatrix( in2, matRotate ); |
|
VectorRotate( in1, matRotate, out ); |
|
} |
|
|
|
|
|
// rotate by the inverse of the matrix |
|
void VectorIRotate( const float *in1, const matrix3x4_t& in2, float *out ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( in1 != out ); |
|
out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0]; |
|
out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1]; |
|
out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2]; |
|
} |
|
|
|
#ifndef VECTOR_NO_SLOW_OPERATIONS |
|
// transform a set of angles in the output space of parentMatrix to the input space |
|
QAngle TransformAnglesToLocalSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) |
|
{ |
|
matrix3x4_t angToWorld, worldToParent, localMatrix; |
|
MatrixInvert( parentMatrix, worldToParent ); |
|
AngleMatrix( angles, angToWorld ); |
|
ConcatTransforms( worldToParent, angToWorld, localMatrix ); |
|
|
|
QAngle out; |
|
MatrixAngles( localMatrix, out ); |
|
return out; |
|
} |
|
|
|
// transform a set of angles in the input space of parentMatrix to the output space |
|
QAngle TransformAnglesToWorldSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) |
|
{ |
|
matrix3x4_t angToParent, angToWorld; |
|
AngleMatrix( angles, angToParent ); |
|
ConcatTransforms( parentMatrix, angToParent, angToWorld ); |
|
QAngle out; |
|
MatrixAngles( angToWorld, out ); |
|
return out; |
|
} |
|
|
|
#endif // VECTOR_NO_SLOW_OPERATIONS |
|
|
|
void MatrixInitialize( matrix3x4_t &mat, const Vector &vecOrigin, const Vector &vecXAxis, const Vector &vecYAxis, const Vector &vecZAxis ) |
|
{ |
|
MatrixSetColumn( vecXAxis, 0, mat ); |
|
MatrixSetColumn( vecYAxis, 1, mat ); |
|
MatrixSetColumn( vecZAxis, 2, mat ); |
|
MatrixSetColumn( vecOrigin, 3, mat ); |
|
} |
|
|
|
void MatrixCopy( const matrix3x4_t& in, matrix3x4_t& out ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
memcpy( out.Base(), in.Base(), sizeof( float ) * 3 * 4 ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Matrix equality test |
|
//----------------------------------------------------------------------------- |
|
bool MatricesAreEqual( const matrix3x4_t &src1, const matrix3x4_t &src2, float flTolerance ) |
|
{ |
|
for ( int i = 0; i < 3; ++i ) |
|
{ |
|
for ( int j = 0; j < 4; ++j ) |
|
{ |
|
if ( fabs( src1[i][j] - src2[i][j] ) > flTolerance ) |
|
return false; |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
// NOTE: This is just the transpose not a general inverse |
|
void MatrixInvert( const matrix3x4_t& in, matrix3x4_t& out ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
if ( &in == &out ) |
|
{ |
|
V_swap(out[0][1],out[1][0]); |
|
V_swap(out[0][2],out[2][0]); |
|
V_swap(out[1][2],out[2][1]); |
|
} |
|
else |
|
{ |
|
// transpose the matrix |
|
out[0][0] = in[0][0]; |
|
out[0][1] = in[1][0]; |
|
out[0][2] = in[2][0]; |
|
|
|
out[1][0] = in[0][1]; |
|
out[1][1] = in[1][1]; |
|
out[1][2] = in[2][1]; |
|
|
|
out[2][0] = in[0][2]; |
|
out[2][1] = in[1][2]; |
|
out[2][2] = in[2][2]; |
|
} |
|
|
|
// now fix up the translation to be in the other space |
|
float tmp[3]; |
|
tmp[0] = in[0][3]; |
|
tmp[1] = in[1][3]; |
|
tmp[2] = in[2][3]; |
|
|
|
out[0][3] = -DotProduct( tmp, out[0] ); |
|
out[1][3] = -DotProduct( tmp, out[1] ); |
|
out[2][3] = -DotProduct( tmp, out[2] ); |
|
} |
|
|
|
void MatrixGetColumn( const matrix3x4_t& in, int column, Vector &out ) |
|
{ |
|
out.x = in[0][column]; |
|
out.y = in[1][column]; |
|
out.z = in[2][column]; |
|
} |
|
|
|
void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out ) |
|
{ |
|
out[0][column] = in.x; |
|
out[1][column] = in.y; |
|
out[2][column] = in.z; |
|
} |
|
|
|
void MatrixScaleBy ( const float flScale, matrix3x4_t &out ) |
|
{ |
|
out[0][0] *= flScale; |
|
out[1][0] *= flScale; |
|
out[2][0] *= flScale; |
|
out[0][1] *= flScale; |
|
out[1][1] *= flScale; |
|
out[2][1] *= flScale; |
|
out[0][2] *= flScale; |
|
out[1][2] *= flScale; |
|
out[2][2] *= flScale; |
|
} |
|
|
|
void MatrixScaleByZero ( matrix3x4_t &out ) |
|
{ |
|
out[0][0] = 0.0f; |
|
out[1][0] = 0.0f; |
|
out[2][0] = 0.0f; |
|
out[0][1] = 0.0f; |
|
out[1][1] = 0.0f; |
|
out[2][1] = 0.0f; |
|
out[0][2] = 0.0f; |
|
out[1][2] = 0.0f; |
|
out[2][2] = 0.0f; |
|
} |
|
|
|
|
|
|
|
int VectorCompare (const float *v1, const float *v2) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
int i; |
|
|
|
for (i=0 ; i<3 ; i++) |
|
if (v1[i] != v2[i]) |
|
return 0; |
|
|
|
return 1; |
|
} |
|
|
|
void CrossProduct (const float* v1, const float* v2, float* cross) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( v1 != cross ); |
|
Assert( v2 != cross ); |
|
cross[0] = v1[1]*v2[2] - v1[2]*v2[1]; |
|
cross[1] = v1[2]*v2[0] - v1[0]*v2[2]; |
|
cross[2] = v1[0]*v2[1] - v1[1]*v2[0]; |
|
} |
|
|
|
int Q_log2(int val) |
|
{ |
|
int answer=0; |
|
while (val>>=1) |
|
answer++; |
|
return answer; |
|
} |
|
|
|
// Matrix is right-handed x=forward, y=left, z=up. We a left-handed convention for vectors in the game code (forward, right, up) |
|
void MatrixVectors( const matrix3x4_t &matrix, Vector* pForward, Vector *pRight, Vector *pUp ) |
|
{ |
|
MatrixGetColumn( matrix, 0, *pForward ); |
|
MatrixGetColumn( matrix, 1, *pRight ); |
|
MatrixGetColumn( matrix, 2, *pUp ); |
|
*pRight *= -1.0f; |
|
} |
|
|
|
|
|
void VectorVectors( const Vector &forward, Vector &right, Vector &up ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector tmp; |
|
|
|
if (forward[0] == 0 && forward[1] == 0) |
|
{ |
|
// pitch 90 degrees up/down from identity |
|
right[0] = 0; |
|
right[1] = -1; |
|
right[2] = 0; |
|
up[0] = -forward[2]; |
|
up[1] = 0; |
|
up[2] = 0; |
|
} |
|
else |
|
{ |
|
tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0; |
|
CrossProduct( forward, tmp, right ); |
|
VectorNormalize( right ); |
|
CrossProduct( right, forward, up ); |
|
VectorNormalize( up ); |
|
} |
|
} |
|
|
|
void VectorMatrix( const Vector &forward, matrix3x4_t& matrix) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector right, up; |
|
VectorVectors(forward, right, up); |
|
|
|
MatrixSetColumn( forward, 0, matrix ); |
|
MatrixSetColumn( -right, 1, matrix ); |
|
MatrixSetColumn( up, 2, matrix ); |
|
} |
|
|
|
|
|
void VectorAngles( const float *forward, float *angles ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float tmp, yaw, pitch; |
|
|
|
if (forward[1] == 0 && forward[0] == 0) |
|
{ |
|
yaw = 0; |
|
if (forward[2] > 0) |
|
pitch = 270; |
|
else |
|
pitch = 90; |
|
} |
|
else |
|
{ |
|
yaw = (atan2(forward[1], forward[0]) * 180 / M_PI); |
|
if (yaw < 0) |
|
yaw += 360; |
|
|
|
tmp = sqrt (forward[0]*forward[0] + forward[1]*forward[1]); |
|
pitch = (atan2(-forward[2], tmp) * 180 / M_PI); |
|
if (pitch < 0) |
|
pitch += 360; |
|
} |
|
|
|
angles[0] = pitch; |
|
angles[1] = yaw; |
|
angles[2] = 0; |
|
} |
|
|
|
|
|
/* |
|
================ |
|
R_ConcatRotations |
|
================ |
|
*/ |
|
void ConcatRotations (const float in1[3][3], const float in2[3][3], float out[3][3]) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( in1 != out ); |
|
Assert( in2 != out ); |
|
out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + |
|
in1[0][2] * in2[2][0]; |
|
out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + |
|
in1[0][2] * in2[2][1]; |
|
out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + |
|
in1[0][2] * in2[2][2]; |
|
out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + |
|
in1[1][2] * in2[2][0]; |
|
out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + |
|
in1[1][2] * in2[2][1]; |
|
out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + |
|
in1[1][2] * in2[2][2]; |
|
out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + |
|
in1[2][2] * in2[2][0]; |
|
out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + |
|
in1[2][2] * in2[2][1]; |
|
out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + |
|
in1[2][2] * in2[2][2]; |
|
} |
|
|
|
void ConcatTransforms_Aligned( const matrix3x4_t &m0, const matrix3x4_t &m1, matrix3x4_t &out ) |
|
{ |
|
Assert( (((size_t)&m0) % 16) == 0 ); |
|
Assert( (((size_t)&m1) % 16) == 0 ); |
|
Assert( (((size_t)&out) % 16) == 0 ); |
|
|
|
fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); |
|
fltx4 rowA0 = LoadAlignedSIMD( m0.m_flMatVal[0] ); |
|
fltx4 rowA1 = LoadAlignedSIMD( m0.m_flMatVal[1] ); |
|
fltx4 rowA2 = LoadAlignedSIMD( m0.m_flMatVal[2] ); |
|
|
|
fltx4 rowB0 = LoadAlignedSIMD( m1.m_flMatVal[0] ); |
|
fltx4 rowB1 = LoadAlignedSIMD( m1.m_flMatVal[1] ); |
|
fltx4 rowB2 = LoadAlignedSIMD( m1.m_flMatVal[2] ); |
|
|
|
// now we have the rows of m0 and the columns of m1 |
|
// first output row |
|
fltx4 A0 = SplatXSIMD(rowA0); |
|
fltx4 A1 = SplatYSIMD(rowA0); |
|
fltx4 A2 = SplatZSIMD(rowA0); |
|
fltx4 mul00 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul01 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul02 = MulSIMD( A2, rowB2 ); |
|
fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); |
|
|
|
// second output row |
|
A0 = SplatXSIMD(rowA1); |
|
A1 = SplatYSIMD(rowA1); |
|
A2 = SplatZSIMD(rowA1); |
|
fltx4 mul10 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul11 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul12 = MulSIMD( A2, rowB2 ); |
|
fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); |
|
|
|
// third output row |
|
A0 = SplatXSIMD(rowA2); |
|
A1 = SplatYSIMD(rowA2); |
|
A2 = SplatZSIMD(rowA2); |
|
fltx4 mul20 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul21 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul22 = MulSIMD( A2, rowB2 ); |
|
fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); |
|
|
|
// add in translation vector |
|
A0 = AndSIMD(rowA0,lastMask); |
|
A1 = AndSIMD(rowA1,lastMask); |
|
A2 = AndSIMD(rowA2,lastMask); |
|
out0 = AddSIMD(out0, A0); |
|
out1 = AddSIMD(out1, A1); |
|
out2 = AddSIMD(out2, A2); |
|
|
|
StoreAlignedSIMD( out.m_flMatVal[0], out0 ); |
|
StoreAlignedSIMD( out.m_flMatVal[1], out1 ); |
|
StoreAlignedSIMD( out.m_flMatVal[2], out2 ); |
|
} |
|
|
|
/* |
|
================ |
|
R_ConcatTransforms |
|
================ |
|
*/ |
|
|
|
void ConcatTransforms (const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out) |
|
{ |
|
#if 0 |
|
// test for ones that'll be 2x faster |
|
if ( (((size_t)&in1) % 16) == 0 && (((size_t)&in2) % 16) == 0 && (((size_t)&out) % 16) == 0 ) |
|
{ |
|
ConcatTransforms_Aligned( in1, in2, out ); |
|
return; |
|
} |
|
#endif |
|
|
|
fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); |
|
fltx4 rowA0 = LoadUnalignedSIMD( in1.m_flMatVal[0] ); |
|
fltx4 rowA1 = LoadUnalignedSIMD( in1.m_flMatVal[1] ); |
|
fltx4 rowA2 = LoadUnalignedSIMD( in1.m_flMatVal[2] ); |
|
|
|
fltx4 rowB0 = LoadUnalignedSIMD( in2.m_flMatVal[0] ); |
|
fltx4 rowB1 = LoadUnalignedSIMD( in2.m_flMatVal[1] ); |
|
fltx4 rowB2 = LoadUnalignedSIMD( in2.m_flMatVal[2] ); |
|
|
|
// now we have the rows of m0 and the columns of m1 |
|
// first output row |
|
fltx4 A0 = SplatXSIMD(rowA0); |
|
fltx4 A1 = SplatYSIMD(rowA0); |
|
fltx4 A2 = SplatZSIMD(rowA0); |
|
fltx4 mul00 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul01 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul02 = MulSIMD( A2, rowB2 ); |
|
fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); |
|
|
|
// second output row |
|
A0 = SplatXSIMD(rowA1); |
|
A1 = SplatYSIMD(rowA1); |
|
A2 = SplatZSIMD(rowA1); |
|
fltx4 mul10 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul11 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul12 = MulSIMD( A2, rowB2 ); |
|
fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); |
|
|
|
// third output row |
|
A0 = SplatXSIMD(rowA2); |
|
A1 = SplatYSIMD(rowA2); |
|
A2 = SplatZSIMD(rowA2); |
|
fltx4 mul20 = MulSIMD( A0, rowB0 ); |
|
fltx4 mul21 = MulSIMD( A1, rowB1 ); |
|
fltx4 mul22 = MulSIMD( A2, rowB2 ); |
|
fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); |
|
|
|
// add in translation vector |
|
A0 = AndSIMD(rowA0,lastMask); |
|
A1 = AndSIMD(rowA1,lastMask); |
|
A2 = AndSIMD(rowA2,lastMask); |
|
out0 = AddSIMD(out0, A0); |
|
out1 = AddSIMD(out1, A1); |
|
out2 = AddSIMD(out2, A2); |
|
|
|
// write to output |
|
StoreUnalignedSIMD( out.m_flMatVal[0], out0 ); |
|
StoreUnalignedSIMD( out.m_flMatVal[1], out1 ); |
|
StoreUnalignedSIMD( out.m_flMatVal[2], out2 ); |
|
} |
|
|
|
|
|
/* |
|
=================== |
|
FloorDivMod |
|
|
|
Returns mathematically correct (floor-based) quotient and remainder for |
|
numer and denom, both of which should contain no fractional part. The |
|
quotient must fit in 32 bits. |
|
==================== |
|
*/ |
|
|
|
void FloorDivMod (double numer, double denom, int *quotient, |
|
int *rem) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
int q, r; |
|
double x; |
|
|
|
#ifdef PARANOID |
|
if (denom <= 0.0) |
|
Sys_Error ("FloorDivMod: bad denominator %d\n", denom); |
|
|
|
// if ((floor(numer) != numer) || (floor(denom) != denom)) |
|
// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n", |
|
// numer, denom); |
|
#endif |
|
|
|
if (numer >= 0.0) |
|
{ |
|
|
|
x = floor(numer / denom); |
|
q = (int)x; |
|
r = Floor2Int(numer - (x * denom)); |
|
} |
|
else |
|
{ |
|
// |
|
// perform operations with positive values, and fix mod to make floor-based |
|
// |
|
x = floor(-numer / denom); |
|
q = -(int)x; |
|
r = Floor2Int(-numer - (x * denom)); |
|
if (r != 0) |
|
{ |
|
q--; |
|
r = (int)denom - r; |
|
} |
|
} |
|
|
|
*quotient = q; |
|
*rem = r; |
|
} |
|
|
|
|
|
/* |
|
=================== |
|
GreatestCommonDivisor |
|
==================== |
|
*/ |
|
int GreatestCommonDivisor (int i1, int i2) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
if (i1 > i2) |
|
{ |
|
if (i2 == 0) |
|
return (i1); |
|
return GreatestCommonDivisor (i2, i1 % i2); |
|
} |
|
else |
|
{ |
|
if (i1 == 0) |
|
return (i2); |
|
return GreatestCommonDivisor (i1, i2 % i1); |
|
} |
|
} |
|
|
|
|
|
bool IsDenormal( const float &val ) |
|
{ |
|
const int x = *reinterpret_cast <const int *> (&val); // needs 32-bit int |
|
const int abs_mantissa = x & 0x007FFFFF; |
|
const int biased_exponent = x & 0x7F800000; |
|
|
|
return ( biased_exponent == 0 && abs_mantissa != 0 ); |
|
} |
|
|
|
int SignbitsForPlane (cplane_t *out) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
int bits, j; |
|
|
|
// for fast box on planeside test |
|
|
|
bits = 0; |
|
for (j=0 ; j<3 ; j++) |
|
{ |
|
if (out->normal[j] < 0) |
|
bits |= 1<<j; |
|
} |
|
return bits; |
|
} |
|
|
|
/* |
|
================== |
|
BoxOnPlaneSide |
|
|
|
Returns 1, 2, or 1 + 2 |
|
================== |
|
*/ |
|
int __cdecl BoxOnPlaneSide (const float *emins, const float *emaxs, const cplane_t *p) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float dist1, dist2; |
|
int sides; |
|
|
|
// fast axial cases |
|
if (p->type < 3) |
|
{ |
|
if (p->dist <= emins[p->type]) |
|
return 1; |
|
if (p->dist >= emaxs[p->type]) |
|
return 2; |
|
return 3; |
|
} |
|
|
|
// general case |
|
switch (p->signbits) |
|
{ |
|
case 0: |
|
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]; |
|
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]; |
|
break; |
|
case 1: |
|
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]; |
|
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]; |
|
break; |
|
case 2: |
|
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]; |
|
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]; |
|
break; |
|
case 3: |
|
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]; |
|
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]; |
|
break; |
|
case 4: |
|
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]; |
|
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]; |
|
break; |
|
case 5: |
|
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]; |
|
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]; |
|
break; |
|
case 6: |
|
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]; |
|
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]; |
|
break; |
|
case 7: |
|
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]; |
|
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]; |
|
break; |
|
default: |
|
dist1 = dist2 = 0; // shut up compiler |
|
Assert( 0 ); |
|
break; |
|
} |
|
|
|
sides = 0; |
|
if (dist1 >= p->dist) |
|
sides = 1; |
|
if (dist2 < p->dist) |
|
sides |= 2; |
|
|
|
Assert( sides != 0 ); |
|
|
|
return sides; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Euler QAngle -> Basis Vectors |
|
//----------------------------------------------------------------------------- |
|
|
|
void AngleVectors (const QAngle &angles, Vector *forward) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( forward ); |
|
|
|
float sp, sy, cp, cy; |
|
|
|
SinCos( DEG2RAD( angles[YAW] ), &sy, &cy ); |
|
SinCos( DEG2RAD( angles[PITCH] ), &sp, &cp ); |
|
|
|
forward->x = cp*cy; |
|
forward->y = cp*sy; |
|
forward->z = -sp; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Euler QAngle -> Basis Vectors. Each vector is optional |
|
//----------------------------------------------------------------------------- |
|
void AngleVectors( const QAngle &angles, Vector *forward, Vector *right, Vector *up ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float sr, sp, sy, cr, cp, cy; |
|
|
|
#ifdef _X360 |
|
fltx4 radians, scale, sine, cosine; |
|
radians = LoadUnaligned3SIMD( angles.Base() ); |
|
scale = ReplicateX4( M_PI_F / 180.f ); |
|
radians = MulSIMD( radians, scale ); |
|
SinCos3SIMD( sine, cosine, radians ); |
|
sp = SubFloat( sine, 0 ); sy = SubFloat( sine, 1 ); sr = SubFloat( sine, 2 ); |
|
cp = SubFloat( cosine, 0 ); cy = SubFloat( cosine, 1 ); cr = SubFloat( cosine, 2 ); |
|
#else |
|
SinCos( DEG2RAD( angles[YAW] ), &sy, &cy ); |
|
SinCos( DEG2RAD( angles[PITCH] ), &sp, &cp ); |
|
SinCos( DEG2RAD( angles[ROLL] ), &sr, &cr ); |
|
#endif |
|
|
|
if (forward) |
|
{ |
|
forward->x = cp*cy; |
|
forward->y = cp*sy; |
|
forward->z = -sp; |
|
} |
|
|
|
if (right) |
|
{ |
|
right->x = (-1*sr*sp*cy+-1*cr*-sy); |
|
right->y = (-1*sr*sp*sy+-1*cr*cy); |
|
right->z = -1*sr*cp; |
|
} |
|
|
|
if (up) |
|
{ |
|
up->x = (cr*sp*cy+-sr*-sy); |
|
up->y = (cr*sp*sy+-sr*cy); |
|
up->z = cr*cp; |
|
} |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Euler QAngle -> Basis Vectors transposed |
|
//----------------------------------------------------------------------------- |
|
|
|
void AngleVectorsTranspose (const QAngle &angles, Vector *forward, Vector *right, Vector *up) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
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 ); |
|
|
|
if (forward) |
|
{ |
|
forward->x = cp*cy; |
|
forward->y = (sr*sp*cy+cr*-sy); |
|
forward->z = (cr*sp*cy+-sr*-sy); |
|
} |
|
|
|
if (right) |
|
{ |
|
right->x = cp*sy; |
|
right->y = (sr*sp*sy+cr*cy); |
|
right->z = (cr*sp*sy+-sr*cy); |
|
} |
|
|
|
if (up) |
|
{ |
|
up->x = -sp; |
|
up->y = sr*cp; |
|
up->z = cr*cp; |
|
} |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Forward direction vector -> Euler angles |
|
//----------------------------------------------------------------------------- |
|
|
|
void VectorAngles( const Vector& forward, QAngle &angles ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float tmp, yaw, pitch; |
|
|
|
if (forward[1] == 0 && forward[0] == 0) |
|
{ |
|
yaw = 0; |
|
if (forward[2] > 0) |
|
pitch = 270; |
|
else |
|
pitch = 90; |
|
} |
|
else |
|
{ |
|
yaw = (atan2(forward[1], forward[0]) * 180 / M_PI); |
|
if (yaw < 0) |
|
yaw += 360; |
|
|
|
tmp = FastSqrt (forward[0]*forward[0] + forward[1]*forward[1]); |
|
pitch = (atan2(-forward[2], tmp) * 180 / M_PI); |
|
if (pitch < 0) |
|
pitch += 360; |
|
} |
|
|
|
angles[0] = pitch; |
|
angles[1] = yaw; |
|
angles[2] = 0; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Forward direction vector with a reference up vector -> Euler angles |
|
//----------------------------------------------------------------------------- |
|
|
|
void VectorAngles( const Vector &forward, const Vector &pseudoup, QAngle &angles ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
Vector left; |
|
|
|
CrossProduct( pseudoup, forward, left ); |
|
VectorNormalizeFast( left ); |
|
|
|
float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); |
|
|
|
// enough here to get angles? |
|
if ( xyDist > 0.001f ) |
|
{ |
|
// (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis |
|
angles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); |
|
|
|
// The engine does pitch inverted from this, but we always end up negating it in the DLL |
|
// UNDONE: Fix the engine to make it consistent |
|
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); |
|
angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); |
|
|
|
float up_z = (left[1] * forward[0]) - (left[0] * forward[1]); |
|
|
|
// (roll) z = ATAN( left.z, up.z ); |
|
angles[2] = RAD2DEG( atan2f( left[2], up_z ) ); |
|
} |
|
else // forward is mostly Z, gimbal lock- |
|
{ |
|
// (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw |
|
angles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); //This was originally copied from the "void MatrixAngles( const matrix3x4_t& matrix, float *angles )" code, and it's 180 degrees off, negated the values and it all works now (Dave Kircher) |
|
|
|
// The engine does pitch inverted from this, but we always end up negating it in the DLL |
|
// UNDONE: Fix the engine to make it consistent |
|
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); |
|
angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); |
|
|
|
// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) |
|
angles[2] = 0; |
|
} |
|
} |
|
|
|
void SetIdentityMatrix( matrix3x4_t& matrix ) |
|
{ |
|
memset( matrix.Base(), 0, sizeof(float)*3*4 ); |
|
matrix[0][0] = 1.0; |
|
matrix[1][1] = 1.0; |
|
matrix[2][2] = 1.0; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Builds a scale matrix |
|
//----------------------------------------------------------------------------- |
|
void SetScaleMatrix( float x, float y, float z, matrix3x4_t &dst ) |
|
{ |
|
dst[0][0] = x; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f; |
|
dst[1][0] = 0.0f; dst[1][1] = y; dst[1][2] = 0.0f; dst[1][3] = 0.0f; |
|
dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = z; dst[2][3] = 0.0f; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis. |
|
// |
|
// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ | |
|
// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ | |
|
// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ | |
|
// |
|
// Input : mat - |
|
// vAxisOrRot - |
|
// angle - |
|
//----------------------------------------------------------------------------- |
|
void MatrixBuildRotationAboutAxis( const Vector &vAxisOfRot, float angleDegrees, matrix3x4_t &dst ) |
|
{ |
|
float radians; |
|
float axisXSquared; |
|
float axisYSquared; |
|
float axisZSquared; |
|
float fSin; |
|
float fCos; |
|
|
|
radians = angleDegrees * ( M_PI / 180.0 ); |
|
fSin = sin( radians ); |
|
fCos = cos( radians ); |
|
|
|
axisXSquared = vAxisOfRot[0] * vAxisOfRot[0]; |
|
axisYSquared = vAxisOfRot[1] * vAxisOfRot[1]; |
|
axisZSquared = vAxisOfRot[2] * vAxisOfRot[2]; |
|
|
|
// Column 0: |
|
dst[0][0] = axisXSquared + (1 - axisXSquared) * fCos; |
|
dst[1][0] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) + vAxisOfRot[2] * fSin; |
|
dst[2][0] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) - vAxisOfRot[1] * fSin; |
|
|
|
// Column 1: |
|
dst[0][1] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) - vAxisOfRot[2] * fSin; |
|
dst[1][1] = axisYSquared + (1 - axisYSquared) * fCos; |
|
dst[2][1] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) + vAxisOfRot[0] * fSin; |
|
|
|
// Column 2: |
|
dst[0][2] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) + vAxisOfRot[1] * fSin; |
|
dst[1][2] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) - vAxisOfRot[0] * fSin; |
|
dst[2][2] = axisZSquared + (1 - axisZSquared) * fCos; |
|
|
|
// Column 3: |
|
dst[0][3] = 0; |
|
dst[1][3] = 0; |
|
dst[2][3] = 0; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Computes the transpose |
|
//----------------------------------------------------------------------------- |
|
void MatrixTranspose( matrix3x4_t& mat ) |
|
{ |
|
vec_t tmp; |
|
tmp = mat[0][1]; mat[0][1] = mat[1][0]; mat[1][0] = tmp; |
|
tmp = mat[0][2]; mat[0][2] = mat[2][0]; mat[2][0] = tmp; |
|
tmp = mat[1][2]; mat[1][2] = mat[2][1]; mat[2][1] = tmp; |
|
} |
|
|
|
void MatrixTranspose( const matrix3x4_t& src, matrix3x4_t& dst ) |
|
{ |
|
dst[0][0] = src[0][0]; dst[0][1] = src[1][0]; dst[0][2] = src[2][0]; dst[0][3] = 0.0f; |
|
dst[1][0] = src[0][1]; dst[1][1] = src[1][1]; dst[1][2] = src[2][1]; dst[1][3] = 0.0f; |
|
dst[2][0] = src[0][2]; dst[2][1] = src[1][2]; dst[2][2] = src[2][2]; dst[2][3] = 0.0f; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: converts engine euler angles into a matrix |
|
// Input : vec3_t angles - PITCH, YAW, ROLL |
|
// Output : *matrix - left-handed column matrix |
|
// the basis vectors for the rotations will be in the columns as follows: |
|
// matrix[][0] is forward |
|
// matrix[][1] is left |
|
// matrix[][2] is up |
|
//----------------------------------------------------------------------------- |
|
void AngleMatrix( RadianEuler const &angles, const Vector &position, matrix3x4_t& matrix ) |
|
{ |
|
AngleMatrix( angles, matrix ); |
|
MatrixSetColumn( position, 3, matrix ); |
|
} |
|
|
|
void AngleMatrix( const RadianEuler& angles, matrix3x4_t& matrix ) |
|
{ |
|
QAngle quakeEuler( RAD2DEG( angles.y ), RAD2DEG( angles.z ), RAD2DEG( angles.x ) ); |
|
|
|
AngleMatrix( quakeEuler, matrix ); |
|
} |
|
|
|
|
|
void AngleMatrix( const QAngle &angles, const Vector &position, matrix3x4_t& matrix ) |
|
{ |
|
AngleMatrix( angles, matrix ); |
|
MatrixSetColumn( position, 3, matrix ); |
|
} |
|
|
|
void AngleMatrix( const QAngle &angles, matrix3x4_t& matrix ) |
|
{ |
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "AngleMatrix", "Mathlib" ); |
|
#endif |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float sr, sp, sy, cr, cp, cy; |
|
|
|
#ifdef _X360 |
|
fltx4 radians, scale, sine, cosine; |
|
radians = LoadUnaligned3SIMD( angles.Base() ); |
|
scale = ReplicateX4( M_PI_F / 180.f ); |
|
radians = MulSIMD( radians, scale ); |
|
SinCos3SIMD( sine, cosine, radians ); |
|
|
|
sp = SubFloat( sine, 0 ); sy = SubFloat( sine, 1 ); sr = SubFloat( sine, 2 ); |
|
cp = SubFloat( cosine, 0 ); cy = SubFloat( cosine, 1 ); cr = SubFloat( cosine, 2 ); |
|
#else |
|
SinCos( DEG2RAD( angles[YAW] ), &sy, &cy ); |
|
SinCos( DEG2RAD( angles[PITCH] ), &sp, &cp ); |
|
SinCos( DEG2RAD( angles[ROLL] ), &sr, &cr ); |
|
#endif |
|
|
|
// matrix = (YAW * PITCH) * ROLL |
|
matrix[0][0] = cp*cy; |
|
matrix[1][0] = cp*sy; |
|
matrix[2][0] = -sp; |
|
|
|
float crcy = cr*cy; |
|
float crsy = cr*sy; |
|
float srcy = sr*cy; |
|
float srsy = sr*sy; |
|
matrix[0][1] = sp*srcy-crsy; |
|
matrix[1][1] = sp*srsy+crcy; |
|
matrix[2][1] = sr*cp; |
|
|
|
matrix[0][2] = (sp*crcy+srsy); |
|
matrix[1][2] = (sp*crsy-srcy); |
|
matrix[2][2] = cr*cp; |
|
|
|
matrix[0][3] = 0.0f; |
|
matrix[1][3] = 0.0f; |
|
matrix[2][3] = 0.0f; |
|
} |
|
|
|
void AngleIMatrix( const RadianEuler& angles, matrix3x4_t& matrix ) |
|
{ |
|
QAngle quakeEuler( RAD2DEG( angles.y ), RAD2DEG( angles.z ), RAD2DEG( angles.x ) ); |
|
|
|
AngleIMatrix( quakeEuler, matrix ); |
|
} |
|
|
|
void AngleIMatrix (const QAngle& angles, matrix3x4_t& matrix ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
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 ); |
|
|
|
// matrix = (YAW * PITCH) * ROLL |
|
matrix[0][0] = cp*cy; |
|
matrix[0][1] = cp*sy; |
|
matrix[0][2] = -sp; |
|
matrix[1][0] = sr*sp*cy+cr*-sy; |
|
matrix[1][1] = sr*sp*sy+cr*cy; |
|
matrix[1][2] = sr*cp; |
|
matrix[2][0] = (cr*sp*cy+-sr*-sy); |
|
matrix[2][1] = (cr*sp*sy+-sr*cy); |
|
matrix[2][2] = cr*cp; |
|
matrix[0][3] = 0.f; |
|
matrix[1][3] = 0.f; |
|
matrix[2][3] = 0.f; |
|
} |
|
|
|
void AngleIMatrix (const QAngle &angles, const Vector &position, matrix3x4_t &mat ) |
|
{ |
|
AngleIMatrix( angles, mat ); |
|
|
|
Vector vecTranslation; |
|
VectorRotate( position, mat, vecTranslation ); |
|
vecTranslation *= -1.0f; |
|
MatrixSetColumn( vecTranslation, 3, mat ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Bounding box construction methods |
|
//----------------------------------------------------------------------------- |
|
|
|
void ClearBounds (Vector& mins, Vector& maxs) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
mins[0] = mins[1] = mins[2] = 99999; |
|
maxs[0] = maxs[1] = maxs[2] = -99999; |
|
} |
|
|
|
void AddPointToBounds (const Vector& v, Vector& mins, Vector& maxs) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
int i; |
|
vec_t val; |
|
|
|
for (i=0 ; i<3 ; i++) |
|
{ |
|
val = v[i]; |
|
if (val < mins[i]) |
|
mins[i] = val; |
|
if (val > maxs[i]) |
|
maxs[i] = val; |
|
} |
|
} |
|
|
|
// solve a x^2 + b x + c = 0 |
|
bool SolveQuadratic( float a, float b, float c, float &root1, float &root2 ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
if (a == 0) |
|
{ |
|
if (b != 0) |
|
{ |
|
// no x^2 component, it's a linear system |
|
root1 = root2 = -c / b; |
|
return true; |
|
} |
|
if (c == 0) |
|
{ |
|
// all zero's |
|
root1 = root2 = 0; |
|
return true; |
|
} |
|
return false; |
|
} |
|
|
|
float tmp = b * b - 4.0f * a * c; |
|
|
|
if (tmp < 0) |
|
{ |
|
// imaginary number, bah, no solution. |
|
return false; |
|
} |
|
|
|
tmp = sqrt( tmp ); |
|
root1 = (-b + tmp) / (2.0f * a); |
|
root2 = (-b - tmp) / (2.0f * a); |
|
return true; |
|
} |
|
|
|
// solves for "a, b, c" where "a x^2 + b x + c = y", return true if solution exists |
|
bool SolveInverseQuadratic( float x1, float y1, float x2, float y2, float x3, float y3, float &a, float &b, float &c ) |
|
{ |
|
float det = (x1 - x2)*(x1 - x3)*(x2 - x3); |
|
|
|
// FIXME: check with some sort of epsilon |
|
if (det == 0.0) |
|
return false; |
|
|
|
a = (x3*(-y1 + y2) + x2*(y1 - y3) + x1*(-y2 + y3)) / det; |
|
|
|
b = (x3*x3*(y1 - y2) + x1*x1*(y2 - y3) + x2*x2*(-y1 + y3)) / det; |
|
|
|
c = (x1*x3*(-x1 + x3)*y2 + x2*x2*(x3*y1 - x1*y3) + x2*(-(x3*x3*y1) + x1*x1*y3)) / det; |
|
|
|
return true; |
|
} |
|
|
|
bool SolveInverseQuadraticMonotonic( float x1, float y1, float x2, float y2, float x3, float y3, |
|
float &a, float &b, float &c ) |
|
{ |
|
// use SolveInverseQuadratic, but if the sigm of the derivative at the start point is the wrong |
|
// sign, displace the mid point |
|
|
|
// first, sort parameters |
|
if (x1>x2) |
|
{ |
|
V_swap(x1,x2); |
|
V_swap(y1,y2); |
|
} |
|
if (x2>x3) |
|
{ |
|
V_swap(x2,x3); |
|
V_swap(y2,y3); |
|
} |
|
if (x1>x2) |
|
{ |
|
V_swap(x1,x2); |
|
V_swap(y1,y2); |
|
} |
|
// this code is not fast. what it does is when the curve would be non-monotonic, slowly shifts |
|
// the center point closer to the linear line between the endpoints. Should anyone need htis |
|
// function to be actually fast, it would be fairly easy to change it to be so. |
|
for(float blend_to_linear_factor=0.0;blend_to_linear_factor<=1.0;blend_to_linear_factor+=0.05) |
|
{ |
|
float tempy2=(1-blend_to_linear_factor)*y2+blend_to_linear_factor*FLerp(y1,y3,x1,x3,x2); |
|
if (!SolveInverseQuadratic(x1,y1,x2,tempy2,x3,y3,a,b,c)) |
|
return false; |
|
float derivative=2.0*a+b; |
|
if ( (y1<y2) && (y2<y3)) // monotonically increasing |
|
{ |
|
if (derivative>=0.0) |
|
return true; |
|
} |
|
else |
|
{ |
|
if ( (y1>y2) && (y2>y3)) // monotonically decreasing |
|
{ |
|
if (derivative<=0.0) |
|
return true; |
|
} |
|
else |
|
return true; |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
|
|
// solves for "a, b, c" where "1/(a x^2 + b x + c ) = y", return true if solution exists |
|
bool SolveInverseReciprocalQuadratic( float x1, float y1, float x2, float y2, float x3, float y3, float &a, float &b, float &c ) |
|
{ |
|
float det = (x1 - x2)*(x1 - x3)*(x2 - x3)*y1*y2*y3; |
|
|
|
// FIXME: check with some sort of epsilon |
|
if (det == 0.0) |
|
return false; |
|
|
|
a = (x1*y1*(y2 - y3) + x3*(y1 - y2)*y3 + x2*y2*(-y1 + y3)) / det; |
|
|
|
b = (x2*x2*y2*(y1 - y3) + x3*x3*(-y1 + y2)*y3 + x1*x1*y1*(-y2 + y3)) / det; |
|
|
|
c = (x2*(x2 - x3)*x3*y2*y3 + x1*x1*y1*(x2*y2 - x3*y3) + x1*(-(x2*x2*y1*y2) + x3*x3*y1*y3)) / det; |
|
|
|
return true; |
|
} |
|
|
|
|
|
// Rotate a vector around the Z axis (YAW) |
|
void VectorYawRotate( const Vector &in, float flYaw, Vector &out) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
if (&in == &out ) |
|
{ |
|
Vector tmp; |
|
tmp = in; |
|
VectorYawRotate( tmp, flYaw, out ); |
|
return; |
|
} |
|
|
|
float sy, cy; |
|
|
|
SinCos( DEG2RAD(flYaw), &sy, &cy ); |
|
|
|
out.x = in.x * cy - in.y * sy; |
|
out.y = in.x * sy + in.y * cy; |
|
out.z = in.z; |
|
} |
|
|
|
|
|
|
|
float Bias( float x, float biasAmt ) |
|
{ |
|
// WARNING: not thread safe |
|
static float lastAmt = -1; |
|
static float lastExponent = 0; |
|
if( lastAmt != biasAmt ) |
|
{ |
|
lastExponent = log( biasAmt ) * -1.4427f; // (-1.4427 = 1 / log(0.5)) |
|
} |
|
float fRet = pow( x, lastExponent ); |
|
Assert ( !IS_NAN( fRet ) ); |
|
return fRet; |
|
} |
|
|
|
|
|
float Gain( float x, float biasAmt ) |
|
{ |
|
// WARNING: not thread safe |
|
if( x < 0.5 ) |
|
return 0.5f * Bias( 2*x, 1-biasAmt ); |
|
else |
|
return 1 - 0.5f * Bias( 2 - 2*x, 1-biasAmt ); |
|
} |
|
|
|
|
|
float SmoothCurve( float x ) |
|
{ |
|
// Actual smooth curve. Visualization: |
|
// http://www.wolframalpha.com/input/?i=plot%5B+0.5+*+%281+-+cos%5B2+*+pi+*+x%5D%29+for+x+%3D+%280%2C+1%29+%5D |
|
return 0.5f * (1 - cos( 2.0f * M_PI * x ) ); |
|
} |
|
|
|
|
|
inline float MovePeak( float x, float flPeakPos ) |
|
{ |
|
// Todo: make this higher-order? |
|
if( x < flPeakPos ) |
|
return x * 0.5f / flPeakPos; |
|
else |
|
return 0.5 + 0.5 * (x - flPeakPos) / (1 - flPeakPos); |
|
} |
|
|
|
|
|
float SmoothCurve_Tweak( float x, float flPeakPos, float flPeakSharpness ) |
|
{ |
|
float flMovedPeak = MovePeak( x, flPeakPos ); |
|
float flSharpened = Gain( flMovedPeak, flPeakSharpness ); |
|
return SmoothCurve( flSharpened ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// make sure quaternions are within 180 degrees of one another, if not, reverse q |
|
//----------------------------------------------------------------------------- |
|
|
|
void QuaternionAlign( const Quaternion &p, const Quaternion &q, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
// FIXME: can this be done with a quat dot product? |
|
|
|
int i; |
|
// decide if one of the quaternions is backwards |
|
float a = 0; |
|
float b = 0; |
|
for (i = 0; i < 4; i++) |
|
{ |
|
a += (p[i]-q[i])*(p[i]-q[i]); |
|
b += (p[i]+q[i])*(p[i]+q[i]); |
|
} |
|
if (a > b) |
|
{ |
|
for (i = 0; i < 4; i++) |
|
{ |
|
qt[i] = -q[i]; |
|
} |
|
} |
|
else if (&qt != &q) |
|
{ |
|
for (i = 0; i < 4; i++) |
|
{ |
|
qt[i] = q[i]; |
|
} |
|
} |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Do a piecewise addition of the quaternion elements. This actually makes little |
|
// mathematical sense, but it's a cheap way to simulate a slerp. |
|
//----------------------------------------------------------------------------- |
|
void QuaternionBlend( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
#if ALLOW_SIMD_QUATERNION_MATH |
|
fltx4 psimd, qsimd, qtsimd; |
|
psimd = LoadUnalignedSIMD( p.Base() ); |
|
qsimd = LoadUnalignedSIMD( q.Base() ); |
|
qtsimd = QuaternionBlendSIMD( psimd, qsimd, t ); |
|
StoreUnalignedSIMD( qt.Base(), qtsimd ); |
|
#else |
|
// decide if one of the quaternions is backwards |
|
Quaternion q2; |
|
QuaternionAlign( p, q, q2 ); |
|
QuaternionBlendNoAlign( p, q2, t, qt ); |
|
#endif |
|
} |
|
|
|
|
|
void QuaternionBlendNoAlign( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float sclp, sclq; |
|
int i; |
|
|
|
// 0.0 returns p, 1.0 return q. |
|
sclp = 1.0f - t; |
|
sclq = t; |
|
for (i = 0; i < 4; i++) { |
|
qt[i] = sclp * p[i] + sclq * q[i]; |
|
} |
|
QuaternionNormalize( qt ); |
|
} |
|
|
|
|
|
|
|
void QuaternionIdentityBlend( const Quaternion &p, float t, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float sclp; |
|
|
|
sclp = 1.0f - t; |
|
|
|
qt.x = p.x * sclp; |
|
qt.y = p.y * sclp; |
|
qt.z = p.z * sclp; |
|
if (qt.w < 0.0) |
|
{ |
|
qt.w = p.w * sclp - t; |
|
} |
|
else |
|
{ |
|
qt.w = p.w * sclp + t; |
|
} |
|
QuaternionNormalize( qt ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Quaternion sphereical linear interpolation |
|
//----------------------------------------------------------------------------- |
|
|
|
void QuaternionSlerp( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ) |
|
{ |
|
Quaternion q2; |
|
// 0.0 returns p, 1.0 return q. |
|
|
|
// decide if one of the quaternions is backwards |
|
QuaternionAlign( p, q, q2 ); |
|
|
|
QuaternionSlerpNoAlign( p, q2, t, qt ); |
|
} |
|
|
|
|
|
void QuaternionSlerpNoAlign( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float omega, cosom, sinom, sclp, sclq; |
|
int i; |
|
|
|
// 0.0 returns p, 1.0 return q. |
|
|
|
cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3]; |
|
|
|
if ((1.0f + cosom) > 0.000001f) { |
|
if ((1.0f - cosom) > 0.000001f) { |
|
omega = acos( cosom ); |
|
sinom = sin( omega ); |
|
sclp = sin( (1.0f - t)*omega) / sinom; |
|
sclq = sin( t*omega ) / sinom; |
|
} |
|
else { |
|
// TODO: add short circuit for cosom == 1.0f? |
|
sclp = 1.0f - t; |
|
sclq = t; |
|
} |
|
for (i = 0; i < 4; i++) { |
|
qt[i] = sclp * p[i] + sclq * q[i]; |
|
} |
|
} |
|
else { |
|
Assert( &qt != &q ); |
|
|
|
qt[0] = -q[1]; |
|
qt[1] = q[0]; |
|
qt[2] = -q[3]; |
|
qt[3] = q[2]; |
|
sclp = sin( (1.0f - t) * (0.5f * M_PI)); |
|
sclq = sin( t * (0.5f * M_PI)); |
|
for (i = 0; i < 3; i++) { |
|
qt[i] = sclp * p[i] + sclq * qt[i]; |
|
} |
|
} |
|
|
|
Assert( qt.IsValid() ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Returns the angular delta between the two normalized quaternions in degrees. |
|
//----------------------------------------------------------------------------- |
|
float QuaternionAngleDiff( const Quaternion &p, const Quaternion &q ) |
|
{ |
|
#if 1 |
|
// this code path is here for 2 reasons: |
|
// 1 - acos maps 1-epsilon to values much larger than epsilon (vs asin, which maps epsilon to itself) |
|
// this means that in floats, anything below ~0.05 degrees truncates to 0 |
|
// 2 - normalized quaternions are frequently slightly non-normalized due to float precision issues, |
|
// and the epsilon off of normalized can be several percents of a degree |
|
Quaternion qInv, diff; |
|
QuaternionConjugate( q, qInv ); |
|
QuaternionMult( p, qInv, diff ); |
|
|
|
// Note if the quaternion is slightly non-normalized the square root below may be more than 1, |
|
// the value is clamped to one otherwise it may result in asin() returning an undefined result. |
|
float sinang = MIN( 1.0f, sqrt( diff.x * diff.x + diff.y * diff.y + diff.z * diff.z ) ); |
|
float angle = RAD2DEG( 2 * asin( sinang ) ); |
|
return angle; |
|
#else |
|
Quaternion q2; |
|
QuaternionAlign( p, q, q2 ); |
|
|
|
Assert( s_bMathlibInitialized ); |
|
float cosom = p.x * q2.x + p.y * q2.y + p.z * q2.z + p.w * q2.w; |
|
|
|
if ( cosom > -1.0f ) |
|
{ |
|
if ( cosom < 1.0f ) |
|
{ |
|
float omega = 2 * fabs( acos( cosom ) ); |
|
return RAD2DEG( omega ); |
|
} |
|
return 0.0f; |
|
} |
|
|
|
return 180.0f; |
|
#endif |
|
} |
|
|
|
void QuaternionConjugate( const Quaternion &p, Quaternion &q ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( q.IsValid() ); |
|
|
|
q.x = -p.x; |
|
q.y = -p.y; |
|
q.z = -p.z; |
|
q.w = p.w; |
|
} |
|
|
|
void QuaternionInvert( const Quaternion &p, Quaternion &q ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( q.IsValid() ); |
|
|
|
QuaternionConjugate( p, q ); |
|
|
|
float magnitudeSqr = QuaternionDotProduct( p, p ); |
|
Assert( magnitudeSqr ); |
|
if ( magnitudeSqr ) |
|
{ |
|
float inv = 1.0f / magnitudeSqr; |
|
q.x *= inv; |
|
q.y *= inv; |
|
q.z *= inv; |
|
q.w *= inv; |
|
} |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Make sure the quaternion is of unit length |
|
//----------------------------------------------------------------------------- |
|
float QuaternionNormalize( Quaternion &q ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float radius, iradius; |
|
|
|
Assert( q.IsValid() ); |
|
|
|
radius = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; |
|
|
|
if ( radius ) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON)) |
|
{ |
|
radius = sqrt(radius); |
|
iradius = 1.0f/radius; |
|
q[3] *= iradius; |
|
q[2] *= iradius; |
|
q[1] *= iradius; |
|
q[0] *= iradius; |
|
} |
|
return radius; |
|
} |
|
|
|
|
|
void QuaternionScale( const Quaternion &p, float t, Quaternion &q ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
#if 0 |
|
Quaternion p0; |
|
Quaternion q; |
|
p0.Init( 0.0, 0.0, 0.0, 1.0 ); |
|
|
|
// slerp in "reverse order" so that p doesn't get realigned |
|
QuaternionSlerp( p, p0, 1.0 - fabs( t ), q ); |
|
if (t < 0.0) |
|
{ |
|
q.w = -q.w; |
|
} |
|
#else |
|
float r; |
|
|
|
// FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to |
|
// use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale. |
|
float sinom = sqrt( DotProduct( &p.x, &p.x ) ); |
|
sinom = min( sinom, 1.f ); |
|
|
|
float sinsom = sin( asin( sinom ) * t ); |
|
|
|
t = sinsom / (sinom + FLT_EPSILON); |
|
VectorScale( &p.x, t, &q.x ); |
|
|
|
// rescale rotation |
|
r = 1.0f - sinsom * sinsom; |
|
|
|
// Assert( r >= 0 ); |
|
if (r < 0.0f) |
|
r = 0.0f; |
|
r = sqrt( r ); |
|
|
|
// keep sign of rotation |
|
if (p.w < 0) |
|
q.w = -r; |
|
else |
|
q.w = r; |
|
#endif |
|
|
|
Assert( q.IsValid() ); |
|
|
|
return; |
|
} |
|
|
|
|
|
void QuaternionAdd( const Quaternion &p, const Quaternion &q, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( p.IsValid() ); |
|
Assert( q.IsValid() ); |
|
|
|
// decide if one of the quaternions is backwards |
|
Quaternion q2; |
|
QuaternionAlign( p, q, q2 ); |
|
|
|
// is this right??? |
|
qt[0] = p[0] + q2[0]; |
|
qt[1] = p[1] + q2[1]; |
|
qt[2] = p[2] + q2[2]; |
|
qt[3] = p[3] + q2[3]; |
|
|
|
return; |
|
} |
|
|
|
|
|
float QuaternionDotProduct( const Quaternion &p, const Quaternion &q ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( p.IsValid() ); |
|
Assert( q.IsValid() ); |
|
|
|
return p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w; |
|
} |
|
|
|
|
|
// qt = p * q |
|
void QuaternionMult( const Quaternion &p, const Quaternion &q, Quaternion &qt ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( p.IsValid() ); |
|
Assert( q.IsValid() ); |
|
|
|
if (&p == &qt) |
|
{ |
|
Quaternion p2 = p; |
|
QuaternionMult( p2, q, qt ); |
|
return; |
|
} |
|
|
|
// decide if one of the quaternions is backwards |
|
Quaternion q2; |
|
QuaternionAlign( p, q, q2 ); |
|
|
|
qt.x = p.x * q2.w + p.y * q2.z - p.z * q2.y + p.w * q2.x; |
|
qt.y = -p.x * q2.z + p.y * q2.w + p.z * q2.x + p.w * q2.y; |
|
qt.z = p.x * q2.y - p.y * q2.x + p.z * q2.w + p.w * q2.z; |
|
qt.w = -p.x * q2.x - p.y * q2.y - p.z * q2.z + p.w * q2.w; |
|
} |
|
|
|
|
|
void QuaternionMatrix( const Quaternion &q, const Vector &pos, matrix3x4_t& matrix ) |
|
{ |
|
if ( !HushAsserts() ) |
|
{ |
|
Assert( pos.IsValid() ); |
|
} |
|
|
|
QuaternionMatrix( q, matrix ); |
|
|
|
matrix[0][3] = pos.x; |
|
matrix[1][3] = pos.y; |
|
matrix[2][3] = pos.z; |
|
} |
|
|
|
void QuaternionMatrix( const Quaternion &q, matrix3x4_t& matrix ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
if ( !HushAsserts() ) |
|
{ |
|
Assert( q.IsValid() ); |
|
} |
|
|
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "QuaternionMatrix", "Mathlib" ); |
|
#endif |
|
|
|
// Original code |
|
// This should produce the same code as below with optimization, but looking at the assmebly, |
|
// it doesn't. There are 7 extra multiplies in the release build of this, go figure. |
|
#if 1 |
|
matrix[0][0] = 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z; |
|
matrix[1][0] = 2.0 * q.x * q.y + 2.0 * q.w * q.z; |
|
matrix[2][0] = 2.0 * q.x * q.z - 2.0 * q.w * q.y; |
|
|
|
matrix[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z; |
|
matrix[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z; |
|
matrix[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x; |
|
|
|
matrix[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y; |
|
matrix[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x; |
|
matrix[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y; |
|
|
|
matrix[0][3] = 0.0f; |
|
matrix[1][3] = 0.0f; |
|
matrix[2][3] = 0.0f; |
|
#else |
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; |
|
|
|
// precalculate common multiplitcations |
|
x2 = q.x + q.x; |
|
y2 = q.y + q.y; |
|
z2 = q.z + q.z; |
|
xx = q.x * x2; |
|
xy = q.x * y2; |
|
xz = q.x * z2; |
|
yy = q.y * y2; |
|
yz = q.y * z2; |
|
zz = q.z * z2; |
|
wx = q.w * x2; |
|
wy = q.w * y2; |
|
wz = q.w * z2; |
|
|
|
matrix[0][0] = 1.0 - (yy + zz); |
|
matrix[0][1] = xy - wz; |
|
matrix[0][2] = xz + wy; |
|
matrix[0][3] = 0.0f; |
|
|
|
matrix[1][0] = xy + wz; |
|
matrix[1][1] = 1.0 - (xx + zz); |
|
matrix[1][2] = yz - wx; |
|
matrix[1][3] = 0.0f; |
|
|
|
matrix[2][0] = xz - wy; |
|
matrix[2][1] = yz + wx; |
|
matrix[2][2] = 1.0 - (xx + yy); |
|
matrix[2][3] = 0.0f; |
|
#endif |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts a quaternion into engine angles |
|
// Input : *quaternion - q3 + q0.i + q1.j + q2.k |
|
// *outAngles - PITCH, YAW, ROLL |
|
//----------------------------------------------------------------------------- |
|
void QuaternionAngles( const Quaternion &q, QAngle &angles ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( q.IsValid() ); |
|
|
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "QuaternionAngles", "Mathlib" ); |
|
#endif |
|
|
|
#if 1 |
|
// FIXME: doing it this way calculates too much data, needs to do an optimized version... |
|
matrix3x4_t matrix; |
|
QuaternionMatrix( q, matrix ); |
|
MatrixAngles( matrix, angles ); |
|
#else |
|
float m11, m12, m13, m23, m33; |
|
|
|
m11 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.x * q.x ) - 1.0f; |
|
m12 = ( 2.0f * q.x * q.y ) + ( 2.0f * q.w * q.z ); |
|
m13 = ( 2.0f * q.x * q.z ) - ( 2.0f * q.w * q.y ); |
|
m23 = ( 2.0f * q.y * q.z ) + ( 2.0f * q.w * q.x ); |
|
m33 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.z * q.z ) - 1.0f; |
|
|
|
// FIXME: this code has a singularity near PITCH +-90 |
|
angles[YAW] = RAD2DEG( atan2(m12, m11) ); |
|
angles[PITCH] = RAD2DEG( asin(-m13) ); |
|
angles[ROLL] = RAD2DEG( atan2(m23, m33) ); |
|
#endif |
|
|
|
Assert( angles.IsValid() ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts a quaternion to an axis / angle in degrees |
|
// (exponential map) |
|
//----------------------------------------------------------------------------- |
|
void QuaternionAxisAngle( const Quaternion &q, Vector &axis, float &angle ) |
|
{ |
|
angle = RAD2DEG(2 * acos(q.w)); |
|
if ( angle > 180 ) |
|
{ |
|
angle -= 360; |
|
} |
|
axis.x = q.x; |
|
axis.y = q.y; |
|
axis.z = q.z; |
|
VectorNormalize( axis ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts an exponential map (ang/axis) to a quaternion |
|
//----------------------------------------------------------------------------- |
|
void AxisAngleQuaternion( const Vector &axis, float angle, Quaternion &q ) |
|
{ |
|
float sa, ca; |
|
|
|
SinCos( DEG2RAD(angle) * 0.5f, &sa, &ca ); |
|
|
|
q.x = axis.x * sa; |
|
q.y = axis.y * sa; |
|
q.z = axis.z * sa; |
|
q.w = ca; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts radian-euler axis aligned angles to a quaternion |
|
// Input : *pfAngles - Right-handed Euler angles in radians |
|
// *outQuat - quaternion of form (i,j,k,real) |
|
//----------------------------------------------------------------------------- |
|
void AngleQuaternion( const RadianEuler &angles, Quaternion &outQuat ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
// Assert( angles.IsValid() ); |
|
|
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "AngleQuaternion", "Mathlib" ); |
|
#endif |
|
|
|
float sr, sp, sy, cr, cp, cy; |
|
|
|
#ifdef _X360 |
|
fltx4 radians, scale, sine, cosine; |
|
radians = LoadUnaligned3SIMD( &angles.x ); |
|
scale = ReplicateX4( 0.5f ); |
|
radians = MulSIMD( radians, scale ); |
|
SinCos3SIMD( sine, cosine, radians ); |
|
|
|
// NOTE: The ordering here is *different* from the AngleQuaternion below |
|
// because p, y, r are not in the same locations in QAngle + RadianEuler. Yay! |
|
sr = SubFloat( sine, 0 ); sp = SubFloat( sine, 1 ); sy = SubFloat( sine, 2 ); |
|
cr = SubFloat( cosine, 0 ); cp = SubFloat( cosine, 1 ); cy = SubFloat( cosine, 2 ); |
|
#else |
|
SinCos( angles.z * 0.5f, &sy, &cy ); |
|
SinCos( angles.y * 0.5f, &sp, &cp ); |
|
SinCos( angles.x * 0.5f, &sr, &cr ); |
|
#endif |
|
|
|
// NJS: for some reason VC6 wasn't recognizing the common subexpressions: |
|
float srXcp = sr * cp, crXsp = cr * sp; |
|
outQuat.x = srXcp*cy-crXsp*sy; // X |
|
outQuat.y = crXsp*cy+srXcp*sy; // Y |
|
|
|
float crXcp = cr * cp, srXsp = sr * sp; |
|
outQuat.z = crXcp*sy-srXsp*cy; // Z |
|
outQuat.w = crXcp*cy+srXsp*sy; // W (real component) |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts engine-format euler angles to a quaternion |
|
// Input : angles - Right-handed Euler angles in degrees as follows: |
|
// [0]: PITCH: Clockwise rotation around the Y axis. |
|
// [1]: YAW: Counterclockwise rotation around the Z axis. |
|
// [2]: ROLL: Counterclockwise rotation around the X axis. |
|
// *outQuat - quaternion of form (i,j,k,real) |
|
//----------------------------------------------------------------------------- |
|
void AngleQuaternion( const QAngle &angles, Quaternion &outQuat ) |
|
{ |
|
#ifdef _VPROF_MATHLIB |
|
VPROF_BUDGET( "AngleQuaternion", "Mathlib" ); |
|
#endif |
|
|
|
float sr, sp, sy, cr, cp, cy; |
|
|
|
#ifdef _X360 |
|
fltx4 radians, scale, sine, cosine; |
|
radians = LoadUnaligned3SIMD( angles.Base() ); |
|
scale = ReplicateX4( 0.5f * M_PI_F / 180.f ); |
|
radians = MulSIMD( radians, scale ); |
|
SinCos3SIMD( sine, cosine, radians ); |
|
|
|
// NOTE: The ordering here is *different* from the AngleQuaternion above |
|
// because p, y, r are not in the same locations in QAngle + RadianEuler. Yay! |
|
sp = SubFloat( sine, 0 ); sy = SubFloat( sine, 1 ); sr = SubFloat( sine, 2 ); |
|
cp = SubFloat( cosine, 0 ); cy = SubFloat( cosine, 1 ); cr = SubFloat( cosine, 2 ); |
|
#else |
|
SinCos( DEG2RAD( angles.y ) * 0.5f, &sy, &cy ); |
|
SinCos( DEG2RAD( angles.x ) * 0.5f, &sp, &cp ); |
|
SinCos( DEG2RAD( angles.z ) * 0.5f, &sr, &cr ); |
|
#endif |
|
|
|
// NJS: for some reason VC6 wasn't recognizing the common subexpressions: |
|
float srXcp = sr * cp, crXsp = cr * sp; |
|
outQuat.x = srXcp*cy-crXsp*sy; // X |
|
outQuat.y = crXsp*cy+srXcp*sy; // Y |
|
|
|
float crXcp = cr * cp, srXsp = sr * sp; |
|
outQuat.z = crXcp*sy-srXsp*cy; // Z |
|
outQuat.w = crXcp*cy+srXsp*sy; // W (real component) |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts a basis to a quaternion |
|
//----------------------------------------------------------------------------- |
|
void BasisToQuaternion( const Vector &vecForward, const Vector &vecRight, const Vector &vecUp, Quaternion &q ) |
|
{ |
|
Assert( fabs( vecForward.LengthSqr() - 1.0f ) < 1e-3 ); |
|
Assert( fabs( vecRight.LengthSqr() - 1.0f ) < 1e-3 ); |
|
Assert( fabs( vecUp.LengthSqr() - 1.0f ) < 1e-3 ); |
|
|
|
Vector vecLeft; |
|
VectorMultiply( vecRight, -1.0f, vecLeft ); |
|
|
|
// FIXME: Don't know why, but this doesn't match at all with other result |
|
// so we can't use this super-fast way. |
|
/* |
|
// Find the trace of the matrix: |
|
float flTrace = vecForward.x + vecLeft.y + vecUp.z + 1.0f; |
|
if ( flTrace > 1e-6 ) |
|
{ |
|
float flSqrtTrace = FastSqrt( flTrace ); |
|
float s = 0.5f / flSqrtTrace; |
|
q.x = ( vecUp.y - vecLeft.z ) * s; |
|
q.y = ( vecForward.z - vecUp.x ) * s; |
|
q.z = ( vecLeft.x - vecForward.y ) * s; |
|
q.w = 0.5f * flSqrtTrace; |
|
} |
|
else |
|
{ |
|
if (( vecForward.x > vecLeft.y ) && ( vecForward.x > vecUp.z ) ) |
|
{ |
|
float flSqrtTrace = FastSqrt( 1.0f + vecForward.x - vecLeft.y - vecUp.z ); |
|
float s = 0.5f / flSqrtTrace; |
|
q.x = 0.5f * flSqrtTrace; |
|
q.y = ( vecForward.y + vecLeft.x ) * s; |
|
q.z = ( vecUp.x + vecForward.z ) * s; |
|
q.w = ( vecUp.y - vecLeft.z ) * s; |
|
} |
|
else if ( vecLeft.y > vecUp.z ) |
|
{ |
|
float flSqrtTrace = FastSqrt( 1.0f + vecLeft.y - vecForward.x - vecUp.z ); |
|
float s = 0.5f / flSqrtTrace; |
|
q.x = ( vecForward.y + vecLeft.x ) * s; |
|
q.y = 0.5f * flSqrtTrace; |
|
q.z = ( vecUp.y + vecLeft.z ) * s; |
|
q.w = ( vecForward.z - vecUp.x ) * s; |
|
} |
|
else |
|
{ |
|
float flSqrtTrace = FastSqrt( 1.0 + vecUp.z - vecForward.x - vecLeft.y ); |
|
float s = 0.5f / flSqrtTrace; |
|
q.x = ( vecUp.x + vecForward.z ) * s; |
|
q.y = ( vecUp.y + vecLeft.z ) * s; |
|
q.z = 0.5f * flSqrtTrace; |
|
q.w = ( vecLeft.x - vecForward.y ) * s; |
|
} |
|
} |
|
QuaternionNormalize( q ); |
|
*/ |
|
|
|
// Version 2: Go through angles |
|
|
|
matrix3x4_t mat; |
|
MatrixSetColumn( vecForward, 0, mat ); |
|
MatrixSetColumn( vecLeft, 1, mat ); |
|
MatrixSetColumn( vecUp, 2, mat ); |
|
|
|
QAngle angles; |
|
MatrixAngles( mat, angles ); |
|
|
|
// Quaternion q2; |
|
AngleQuaternion( angles, q ); |
|
|
|
// Assert( fabs(q.x - q2.x) < 1e-3 ); |
|
// Assert( fabs(q.y - q2.y) < 1e-3 ); |
|
// Assert( fabs(q.z - q2.z) < 1e-3 ); |
|
// Assert( fabs(q.w - q2.w) < 1e-3 ); |
|
} |
|
|
|
// FIXME: Optimize! |
|
void MatrixQuaternion( const matrix3x4_t &mat, Quaternion &q ) |
|
{ |
|
QAngle angles; |
|
MatrixAngles( mat, angles ); |
|
AngleQuaternion( angles, q ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Converts a quaternion into engine angles |
|
// Input : *quaternion - q3 + q0.i + q1.j + q2.k |
|
// *outAngles - PITCH, YAW, ROLL |
|
//----------------------------------------------------------------------------- |
|
void QuaternionAngles( const Quaternion &q, RadianEuler &angles ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Assert( q.IsValid() ); |
|
|
|
// FIXME: doing it this way calculates too much data, needs to do an optimized version... |
|
matrix3x4_t matrix; |
|
QuaternionMatrix( q, matrix ); |
|
MatrixAngles( matrix, angles ); |
|
|
|
Assert( angles.IsValid() ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: A helper function to normalize p2.x->p1.x and p3.x->p4.x to |
|
// be the same length as p2.x->p3.x |
|
// Input : &p2 - |
|
// &p4 - |
|
// p4n - |
|
//----------------------------------------------------------------------------- |
|
void Spline_Normalize( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
Vector& p1n, |
|
Vector& p4n ) |
|
{ |
|
float dt = p3.x - p2.x; |
|
|
|
p1n = p1; |
|
p4n = p4; |
|
|
|
if ( dt != 0.0 ) |
|
{ |
|
if (p1.x != p2.x) |
|
{ |
|
// Equivalent to p1n = p2 - (p2 - p1) * (dt / (p2.x - p1.x)); |
|
VectorLerp( p2, p1, dt / (p2.x - p1.x), p1n ); |
|
} |
|
if (p4.x != p3.x) |
|
{ |
|
// Equivalent to p4n = p3 + (p4 - p3) * (dt / (p4.x - p3.x)); |
|
VectorLerp( p3, p4, dt / (p4.x - p3.x), p4n ); |
|
} |
|
} |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: |
|
// Input : |
|
//----------------------------------------------------------------------------- |
|
|
|
void Catmull_Rom_Spline( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float tSqr = t*t*0.5f; |
|
float tSqrSqr = t*tSqr; |
|
t *= 0.5f; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
VectorScale( p1, -tSqrSqr, a ); // 0.5 t^3 * [ (-1*p1) + ( 3*p2) + (-3*p3) + p4 ] |
|
VectorScale( p2, tSqrSqr*3, b ); |
|
VectorScale( p3, tSqrSqr*-3, c ); |
|
VectorScale( p4, tSqrSqr, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 2 |
|
VectorScale( p1, tSqr*2, a ); // 0.5 t^2 * [ ( 2*p1) + (-5*p2) + ( 4*p3) - p4 ] |
|
VectorScale( p2, tSqr*-5, b ); |
|
VectorScale( p3, tSqr*4, c ); |
|
VectorScale( p4, -tSqr, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 3 |
|
VectorScale( p1, -t, a ); // 0.5 t * [ (-1*p1) + p3 ] |
|
VectorScale( p3, t, b ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
|
|
// matrix row 4 |
|
VectorAdd( p2, output, output ); // p2 |
|
} |
|
|
|
void Catmull_Rom_Spline_Tangent( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float tOne = 3*t*t*0.5f; |
|
float tTwo = 2*t*0.5f; |
|
float tThree = 0.5; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
VectorScale( p1, -tOne, a ); // 0.5 t^3 * [ (-1*p1) + ( 3*p2) + (-3*p3) + p4 ] |
|
VectorScale( p2, tOne*3, b ); |
|
VectorScale( p3, tOne*-3, c ); |
|
VectorScale( p4, tOne, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 2 |
|
VectorScale( p1, tTwo*2, a ); // 0.5 t^2 * [ ( 2*p1) + (-5*p2) + ( 4*p3) - p4 ] |
|
VectorScale( p2, tTwo*-5, b ); |
|
VectorScale( p3, tTwo*4, c ); |
|
VectorScale( p4, -tTwo, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 3 |
|
VectorScale( p1, -tThree, a ); // 0.5 t * [ (-1*p1) + p3 ] |
|
VectorScale( p3, tThree, b ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
} |
|
|
|
// area under the curve [0..t] |
|
void Catmull_Rom_Spline_Integral( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
output = p2*t |
|
-0.25f*(p1 - p3)*t*t |
|
+ (1.0f/6.0f)*(2.0f*p1 - 5.0f*p2 + 4.0f*p3 - p4)*t*t*t |
|
- 0.125f*(p1 - 3.0f*p2 + 3.0f*p3 - p4)*t*t*t*t; |
|
} |
|
|
|
|
|
// area under the curve [0..1] |
|
void Catmull_Rom_Spline_Integral( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
Vector& output ) |
|
{ |
|
output = (-0.25f * p1 + 3.25f * p2 + 3.25f * p3 - 0.25f * p4) * (1.0f / 6.0f); |
|
} |
|
|
|
|
|
void Catmull_Rom_Spline_Normalize( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3 |
|
float dt = p3.DistTo(p2); |
|
|
|
Vector p1n, p4n; |
|
VectorSubtract( p1, p2, p1n ); |
|
VectorSubtract( p4, p3, p4n ); |
|
|
|
VectorNormalize( p1n ); |
|
VectorNormalize( p4n ); |
|
|
|
VectorMA( p2, dt, p1n, p1n ); |
|
VectorMA( p3, dt, p4n, p4n ); |
|
|
|
Catmull_Rom_Spline( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
|
|
void Catmull_Rom_Spline_Integral_Normalize( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3 |
|
float dt = p3.DistTo(p2); |
|
|
|
Vector p1n, p4n; |
|
VectorSubtract( p1, p2, p1n ); |
|
VectorSubtract( p4, p3, p4n ); |
|
|
|
VectorNormalize( p1n ); |
|
VectorNormalize( p4n ); |
|
|
|
VectorMA( p2, dt, p1n, p1n ); |
|
VectorMA( p3, dt, p4n, p4n ); |
|
|
|
Catmull_Rom_Spline_Integral( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
|
|
void Catmull_Rom_Spline_NormalizeX( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Vector p1n, p4n; |
|
Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); |
|
Catmull_Rom_Spline( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: basic hermite spline. t = 0 returns p1, t = 1 returns p2, |
|
// d1 and d2 are used to entry and exit slope of curve |
|
// Input : |
|
//----------------------------------------------------------------------------- |
|
|
|
void Hermite_Spline( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &d1, |
|
const Vector &d2, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float tSqr = t*t; |
|
float tCube = t*tSqr; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &d1 ); |
|
Assert( &output != &d2 ); |
|
|
|
float b1 = 2.0f*tCube-3.0f*tSqr+1.0f; |
|
float b2 = 1.0f - b1; // -2*tCube+3*tSqr; |
|
float b3 = tCube-2*tSqr+t; |
|
float b4 = tCube-tSqr; |
|
|
|
VectorScale( p1, b1, output ); |
|
VectorMA( output, b2, p2, output ); |
|
VectorMA( output, b3, d1, output ); |
|
VectorMA( output, b4, d2, output ); |
|
} |
|
|
|
float Hermite_Spline( |
|
float p1, |
|
float p2, |
|
float d1, |
|
float d2, |
|
float t ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
float output; |
|
float tSqr = t*t; |
|
float tCube = t*tSqr; |
|
|
|
float b1 = 2.0f*tCube-3.0f*tSqr+1.0f; |
|
float b2 = 1.0f - b1; // -2*tCube+3*tSqr; |
|
float b3 = tCube-2*tSqr+t; |
|
float b4 = tCube-tSqr; |
|
|
|
output = p1 * b1; |
|
output += p2 * b2; |
|
output += d1 * b3; |
|
output += d2 * b4; |
|
|
|
return output; |
|
} |
|
|
|
|
|
void Hermite_SplineBasis( float t, float basis[4] ) |
|
{ |
|
float tSqr = t*t; |
|
float tCube = t*tSqr; |
|
|
|
basis[0] = 2.0f*tCube-3.0f*tSqr+1.0f; |
|
basis[1] = 1.0f - basis[0]; // -2*tCube+3*tSqr; |
|
basis[2] = tCube-2*tSqr+t; |
|
basis[3] = tCube-tSqr; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: simple three data point hermite spline. |
|
// t = 0 returns p1, t = 1 returns p2, |
|
// slopes are generated from the p0->p1 and p1->p2 segments |
|
// this is reasonable C1 method when there's no "p3" data yet. |
|
// Input : |
|
//----------------------------------------------------------------------------- |
|
|
|
// BUG: the VectorSubtract()'s calls go away if the global optimizer is enabled |
|
#pragma optimize( "g", off ) |
|
|
|
void Hermite_Spline( const Vector &p0, const Vector &p1, const Vector &p2, float t, Vector& output ) |
|
{ |
|
Vector e10, e21; |
|
VectorSubtract( p1, p0, e10 ); |
|
VectorSubtract( p2, p1, e21 ); |
|
Hermite_Spline( p1, p2, e10, e21, t, output ); |
|
} |
|
|
|
#pragma optimize( "", on ) |
|
|
|
float Hermite_Spline( float p0, float p1, float p2, float t ) |
|
{ |
|
return Hermite_Spline( p1, p2, p1 - p0, p2 - p1, t ); |
|
} |
|
|
|
|
|
void Hermite_Spline( const Quaternion &q0, const Quaternion &q1, const Quaternion &q2, float t, Quaternion &output ) |
|
{ |
|
// cheap, hacked version of quaternions |
|
Quaternion q0a; |
|
Quaternion q1a; |
|
|
|
QuaternionAlign( q2, q0, q0a ); |
|
QuaternionAlign( q2, q1, q1a ); |
|
|
|
output.x = Hermite_Spline( q0a.x, q1a.x, q2.x, t ); |
|
output.y = Hermite_Spline( q0a.y, q1a.y, q2.y, t ); |
|
output.z = Hermite_Spline( q0a.z, q1a.z, q2.z, t ); |
|
output.w = Hermite_Spline( q0a.w, q1a.w, q2.w, t ); |
|
|
|
QuaternionNormalize( output ); |
|
} |
|
|
|
// See http://en.wikipedia.org/wiki/Kochanek-Bartels_curves |
|
// |
|
// Tension: -1 = Round -> 1 = Tight |
|
// Bias: -1 = Pre-shoot (bias left) -> 1 = Post-shoot (bias right) |
|
// Continuity: -1 = Box corners -> 1 = Inverted corners |
|
// |
|
// If T=B=C=0 it's the same matrix as Catmull-Rom. |
|
// If T=1 & B=C=0 it's the same as Cubic. |
|
// If T=B=0 & C=-1 it's just linear interpolation |
|
// |
|
// See http://news.povray.org/povray.binaries.tutorials/attachment/%3CXns91B880592482seed7@povray.org%3E/Splines.bas.txt |
|
// for example code and descriptions of various spline types... |
|
// |
|
void Kochanek_Bartels_Spline( |
|
float tension, |
|
float bias, |
|
float continuity, |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float ffa, ffb, ffc, ffd; |
|
|
|
ffa = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f + bias ); |
|
ffb = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f - bias ); |
|
ffc = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f + bias ); |
|
ffd = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f - bias ); |
|
|
|
float tSqr = t*t*0.5f; |
|
float tSqrSqr = t*tSqr; |
|
t *= 0.5f; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
VectorScale( p1, tSqrSqr * -ffa, a ); |
|
VectorScale( p2, tSqrSqr * ( 4.0f + ffa - ffb - ffc ), b ); |
|
VectorScale( p3, tSqrSqr * ( -4.0f + ffb + ffc - ffd ), c ); |
|
VectorScale( p4, tSqrSqr * ffd, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 2 |
|
VectorScale( p1, tSqr* 2 * ffa, a ); |
|
VectorScale( p2, tSqr * ( -6 - 2 * ffa + 2 * ffb + ffc ), b ); |
|
VectorScale( p3, tSqr * ( 6 - 2 * ffb - ffc + ffd ), c ); |
|
VectorScale( p4, tSqr * -ffd, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 3 |
|
VectorScale( p1, t * -ffa, a ); |
|
VectorScale( p2, t * ( ffa - ffb ), b ); |
|
VectorScale( p3, t * ffb, c ); |
|
// p4 unchanged |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 4 |
|
// p1, p3, p4 unchanged |
|
// p2 is multiplied by 1 and added, so just added it directly |
|
|
|
VectorAdd( p2, output, output ); |
|
} |
|
|
|
void Kochanek_Bartels_Spline_NormalizeX( |
|
float tension, |
|
float bias, |
|
float continuity, |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Vector p1n, p4n; |
|
Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); |
|
Kochanek_Bartels_Spline( tension, bias, continuity, p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
void Cubic_Spline( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float tSqr = t*t; |
|
float tSqrSqr = t*tSqr; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
VectorScale( p2, tSqrSqr * 2, b ); |
|
VectorScale( p3, tSqrSqr * -2, c ); |
|
|
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 2 |
|
VectorScale( p2, tSqr * -3, b ); |
|
VectorScale( p3, tSqr * 3, c ); |
|
|
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 3 |
|
// no influence |
|
// p4 unchanged |
|
|
|
// matrix row 4 |
|
// p1, p3, p4 unchanged |
|
VectorAdd( p2, output, output ); |
|
} |
|
|
|
void Cubic_Spline_NormalizeX( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Vector p1n, p4n; |
|
Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); |
|
Cubic_Spline( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
void BSpline( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float oneOver6 = 1.0f / 6.0f; |
|
|
|
float tSqr = t * t * oneOver6; |
|
float tSqrSqr = t*tSqr; |
|
t *= oneOver6; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
VectorScale( p1, -tSqrSqr, a ); |
|
VectorScale( p2, tSqrSqr * 3.0f, b ); |
|
VectorScale( p3, tSqrSqr * -3.0f, c ); |
|
VectorScale( p4, tSqrSqr, d ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
VectorAdd( d, output, output ); |
|
|
|
// matrix row 2 |
|
VectorScale( p1, tSqr * 3.0f, a ); |
|
VectorScale( p2, tSqr * -6.0f, b ); |
|
VectorScale( p3, tSqr * 3.0f, c ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 3 |
|
VectorScale( p1, t * -3.0f, a ); |
|
VectorScale( p3, t * 3.0f, c ); |
|
// p4 unchanged |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 4 |
|
// p1 and p3 scaled by 1.0f, so done below |
|
VectorScale( p1, oneOver6, a ); |
|
VectorScale( p2, 4.0f * oneOver6, b ); |
|
VectorScale( p3, oneOver6, c ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
} |
|
|
|
void BSpline_NormalizeX( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Vector p1n, p4n; |
|
Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); |
|
BSpline( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
void Parabolic_Spline( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
|
|
float tSqr = t*t*0.5f; |
|
t *= 0.5f; |
|
|
|
Assert( &output != &p1 ); |
|
Assert( &output != &p2 ); |
|
Assert( &output != &p3 ); |
|
Assert( &output != &p4 ); |
|
|
|
output.Init(); |
|
|
|
Vector a, b, c, d; |
|
|
|
// matrix row 1 |
|
// no influence from t cubed |
|
|
|
// matrix row 2 |
|
VectorScale( p1, tSqr, a ); |
|
VectorScale( p2, tSqr * -2.0f, b ); |
|
VectorScale( p3, tSqr, c ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
VectorAdd( c, output, output ); |
|
|
|
// matrix row 3 |
|
VectorScale( p1, t * -2.0f, a ); |
|
VectorScale( p2, t * 2.0f, b ); |
|
// p4 unchanged |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
|
|
// matrix row 4 |
|
VectorScale( p1, 0.5f, a ); |
|
VectorScale( p2, 0.5f, b ); |
|
|
|
VectorAdd( a, output, output ); |
|
VectorAdd( b, output, output ); |
|
} |
|
|
|
void Parabolic_Spline_NormalizeX( |
|
const Vector &p1, |
|
const Vector &p2, |
|
const Vector &p3, |
|
const Vector &p4, |
|
float t, |
|
Vector& output ) |
|
{ |
|
Vector p1n, p4n; |
|
Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); |
|
Parabolic_Spline( p1n, p2, p3, p4n, t, output ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Compress the input values for a ranged result such that from 75% to 200% smoothly of the range maps |
|
//----------------------------------------------------------------------------- |
|
|
|
float RangeCompressor( float flValue, float flMin, float flMax, float flBase ) |
|
{ |
|
// clamp base |
|
if (flBase < flMin) |
|
flBase = flMin; |
|
if (flBase > flMax) |
|
flBase = flMax; |
|
|
|
flValue += flBase; |
|
|
|
// convert to 0 to 1 value |
|
float flMid = (flValue - flMin) / (flMax - flMin); |
|
// convert to -1 to 1 value |
|
float flTarget = flMid * 2 - 1; |
|
|
|
if (fabs(flTarget) > 0.75) |
|
{ |
|
float t = (fabs(flTarget) - 0.75) / (1.25); |
|
if (t < 1.0) |
|
{ |
|
if (flTarget > 0) |
|
{ |
|
flTarget = Hermite_Spline( 0.75, 1, 0.75, 0, t ); |
|
} |
|
else |
|
{ |
|
flTarget = -Hermite_Spline( 0.75, 1, 0.75, 0, t ); |
|
} |
|
} |
|
else |
|
{ |
|
flTarget = (flTarget > 0) ? 1.0f : -1.0f; |
|
} |
|
} |
|
|
|
flMid = (flTarget + 1 ) / 2.0; |
|
flValue = flMin * (1 - flMid) + flMax * flMid; |
|
|
|
flValue -= flBase; |
|
|
|
return flValue; |
|
} |
|
|
|
|
|
//#pragma optimize( "", on ) |
|
|
|
//----------------------------------------------------------------------------- |
|
// Transforms a AABB into another space; which will inherently grow the box. |
|
//----------------------------------------------------------------------------- |
|
void TransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) |
|
{ |
|
Vector localCenter; |
|
VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); |
|
localCenter *= 0.5f; |
|
|
|
Vector localExtents; |
|
VectorSubtract( vecMaxsIn, localCenter, localExtents ); |
|
|
|
Vector worldCenter; |
|
VectorTransform( localCenter, transform, worldCenter ); |
|
|
|
Vector worldExtents; |
|
worldExtents.x = DotProductAbs( localExtents, transform[0] ); |
|
worldExtents.y = DotProductAbs( localExtents, transform[1] ); |
|
worldExtents.z = DotProductAbs( localExtents, transform[2] ); |
|
|
|
VectorSubtract( worldCenter, worldExtents, vecMinsOut ); |
|
VectorAdd( worldCenter, worldExtents, vecMaxsOut ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Uses the inverse transform of in1 |
|
//----------------------------------------------------------------------------- |
|
void ITransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) |
|
{ |
|
Vector worldCenter; |
|
VectorAdd( vecMinsIn, vecMaxsIn, worldCenter ); |
|
worldCenter *= 0.5f; |
|
|
|
Vector worldExtents; |
|
VectorSubtract( vecMaxsIn, worldCenter, worldExtents ); |
|
|
|
Vector localCenter; |
|
VectorITransform( worldCenter, transform, localCenter ); |
|
|
|
Vector localExtents; |
|
localExtents.x = FloatMakePositive( worldExtents.x * transform[0][0] ) + |
|
FloatMakePositive( worldExtents.y * transform[1][0] ) + |
|
FloatMakePositive( worldExtents.z * transform[2][0] ); |
|
localExtents.y = FloatMakePositive( worldExtents.x * transform[0][1] ) + |
|
FloatMakePositive( worldExtents.y * transform[1][1] ) + |
|
FloatMakePositive( worldExtents.z * transform[2][1] ); |
|
localExtents.z = FloatMakePositive( worldExtents.x * transform[0][2] ) + |
|
FloatMakePositive( worldExtents.y * transform[1][2] ) + |
|
FloatMakePositive( worldExtents.z * transform[2][2] ); |
|
|
|
VectorSubtract( localCenter, localExtents, vecMinsOut ); |
|
VectorAdd( localCenter, localExtents, vecMaxsOut ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Rotates a AABB into another space; which will inherently grow the box. |
|
// (same as TransformAABB, but doesn't take the translation into account) |
|
//----------------------------------------------------------------------------- |
|
void RotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) |
|
{ |
|
Vector localCenter; |
|
VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); |
|
localCenter *= 0.5f; |
|
|
|
Vector localExtents; |
|
VectorSubtract( vecMaxsIn, localCenter, localExtents ); |
|
|
|
Vector newCenter; |
|
VectorRotate( localCenter, transform, newCenter ); |
|
|
|
Vector newExtents; |
|
newExtents.x = DotProductAbs( localExtents, transform[0] ); |
|
newExtents.y = DotProductAbs( localExtents, transform[1] ); |
|
newExtents.z = DotProductAbs( localExtents, transform[2] ); |
|
|
|
VectorSubtract( newCenter, newExtents, vecMinsOut ); |
|
VectorAdd( newCenter, newExtents, vecMaxsOut ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Uses the inverse transform of in1 |
|
//----------------------------------------------------------------------------- |
|
void IRotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) |
|
{ |
|
Vector oldCenter; |
|
VectorAdd( vecMinsIn, vecMaxsIn, oldCenter ); |
|
oldCenter *= 0.5f; |
|
|
|
Vector oldExtents; |
|
VectorSubtract( vecMaxsIn, oldCenter, oldExtents ); |
|
|
|
Vector newCenter; |
|
VectorIRotate( oldCenter, transform, newCenter ); |
|
|
|
Vector newExtents; |
|
newExtents.x = FloatMakePositive( oldExtents.x * transform[0][0] ) + |
|
FloatMakePositive( oldExtents.y * transform[1][0] ) + |
|
FloatMakePositive( oldExtents.z * transform[2][0] ); |
|
newExtents.y = FloatMakePositive( oldExtents.x * transform[0][1] ) + |
|
FloatMakePositive( oldExtents.y * transform[1][1] ) + |
|
FloatMakePositive( oldExtents.z * transform[2][1] ); |
|
newExtents.z = FloatMakePositive( oldExtents.x * transform[0][2] ) + |
|
FloatMakePositive( oldExtents.y * transform[1][2] ) + |
|
FloatMakePositive( oldExtents.z * transform[2][2] ); |
|
|
|
VectorSubtract( newCenter, newExtents, vecMinsOut ); |
|
VectorAdd( newCenter, newExtents, vecMaxsOut ); |
|
} |
|
|
|
|
|
float CalcSqrDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point ) |
|
{ |
|
float flDelta; |
|
float flDistSqr = 0.0f; |
|
|
|
if ( point.x < mins.x ) |
|
{ |
|
flDelta = (mins.x - point.x); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
else if ( point.x > maxs.x ) |
|
{ |
|
flDelta = (point.x - maxs.x); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
|
|
if ( point.y < mins.y ) |
|
{ |
|
flDelta = (mins.y - point.y); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
else if ( point.y > maxs.y ) |
|
{ |
|
flDelta = (point.y - maxs.y); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
|
|
if ( point.z < mins.z ) |
|
{ |
|
flDelta = (mins.z - point.z); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
else if ( point.z > maxs.z ) |
|
{ |
|
flDelta = (point.z - maxs.z); |
|
flDistSqr += flDelta * flDelta; |
|
} |
|
|
|
return flDistSqr; |
|
} |
|
|
|
|
|
void CalcClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut ) |
|
{ |
|
closestOut.x = clamp( point.x, mins.x, maxs.x ); |
|
closestOut.y = clamp( point.y, mins.y, maxs.y ); |
|
closestOut.z = clamp( point.z, mins.z, maxs.z ); |
|
} |
|
|
|
void CalcSqrDistAndClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut, float &distSqrOut ) |
|
{ |
|
distSqrOut = 0.0f; |
|
for ( int i = 0; i < 3; i++ ) |
|
{ |
|
if ( point[i] < mins[i] ) |
|
{ |
|
closestOut[i] = mins[i]; |
|
float flDelta = closestOut[i] - mins[i]; |
|
distSqrOut += flDelta * flDelta; |
|
} |
|
else if ( point[i] > maxs[i] ) |
|
{ |
|
closestOut[i] = maxs[i]; |
|
float flDelta = closestOut[i] - maxs[i]; |
|
distSqrOut += flDelta * flDelta; |
|
} |
|
else |
|
{ |
|
closestOut[i] = point[i]; |
|
} |
|
} |
|
|
|
} |
|
|
|
float CalcClosestPointToLineT( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vDir ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
VectorSubtract( vLineB, vLineA, vDir ); |
|
|
|
// D dot [P - (A + D*t)] = 0 |
|
// t = ( DP - DA) / DD |
|
float div = vDir.Dot( vDir ); |
|
if( div < 0.00001f ) |
|
{ |
|
return 0; |
|
} |
|
else |
|
{ |
|
return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; |
|
} |
|
} |
|
|
|
void CalcClosestPointOnLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector vDir; |
|
float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); |
|
if ( outT ) *outT = t; |
|
vClosest.MulAdd( vLineA, vDir, t ); |
|
} |
|
|
|
|
|
float CalcDistanceToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector vClosest; |
|
CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistTo(vClosest); |
|
} |
|
|
|
float CalcDistanceSqrToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector vClosest; |
|
CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistToSqr(vClosest); |
|
} |
|
|
|
void CalcClosestPointOnLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) |
|
{ |
|
Vector vDir; |
|
float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); |
|
t = clamp( t, 0.f, 1.f ); |
|
if ( outT ) |
|
{ |
|
*outT = t; |
|
} |
|
vClosest.MulAdd( vLineA, vDir, t ); |
|
} |
|
|
|
|
|
float CalcDistanceToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector vClosest; |
|
CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistTo( vClosest ); |
|
} |
|
|
|
float CalcDistanceSqrToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector vClosest; |
|
CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistToSqr(vClosest); |
|
} |
|
|
|
float CalcClosestPointToLineT2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vDir ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2DSubtract( vLineB, vLineA, vDir ); |
|
|
|
// D dot [P - (A + D*t)] = 0 |
|
// t = (DP - DA) / DD |
|
float div = vDir.Dot( vDir ); |
|
if( div < 0.00001f ) |
|
{ |
|
return 0; |
|
} |
|
else |
|
{ |
|
return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; |
|
} |
|
} |
|
|
|
void CalcClosestPointOnLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2D vDir; |
|
float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); |
|
if ( outT ) *outT = t; |
|
vClosest.MulAdd( vLineA, vDir, t ); |
|
} |
|
|
|
float CalcDistanceToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2D vClosest; |
|
CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistTo( vClosest ); |
|
} |
|
|
|
float CalcDistanceSqrToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2D vClosest; |
|
CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistToSqr(vClosest); |
|
} |
|
|
|
void CalcClosestPointOnLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) |
|
{ |
|
Vector2D vDir; |
|
float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); |
|
t = clamp( t, 0.f, 1.f ); |
|
if ( outT ) |
|
{ |
|
*outT = t; |
|
} |
|
vClosest.MulAdd( vLineA, vDir, t ); |
|
} |
|
|
|
float CalcDistanceToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2D vClosest; |
|
CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistTo( vClosest ); |
|
} |
|
|
|
float CalcDistanceSqrToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
Vector2D vClosest; |
|
CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); |
|
return P.DistToSqr( vClosest ); |
|
} |
|
|
|
// Do we have another epsilon we could use |
|
#define LINE_EPS ( 0.000001f ) |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Given lines p1->p2 and p3->p4, computes a line segment (pa->pb) and returns the parameters 0->1 multipliers |
|
// along each segment for the returned points |
|
// Input : p1 - |
|
// p2 - |
|
// p3 - |
|
// p4 - |
|
// *s1 - |
|
// *s2 - |
|
// Output : Returns true on success, false on failure. |
|
//----------------------------------------------------------------------------- |
|
bool CalcLineToLineIntersectionSegment( |
|
const Vector& p1,const Vector& p2,const Vector& p3,const Vector& p4,Vector *s1,Vector *s2, |
|
float *t1, float *t2) |
|
{ |
|
Vector p13,p43,p21; |
|
float d1343,d4321,d1321,d4343,d2121; |
|
float numer,denom; |
|
|
|
p13.x = p1.x - p3.x; |
|
p13.y = p1.y - p3.y; |
|
p13.z = p1.z - p3.z; |
|
p43.x = p4.x - p3.x; |
|
p43.y = p4.y - p3.y; |
|
p43.z = p4.z - p3.z; |
|
|
|
if (fabs(p43.x) < LINE_EPS && fabs(p43.y) < LINE_EPS && fabs(p43.z) < LINE_EPS) |
|
return false; |
|
p21.x = p2.x - p1.x; |
|
p21.y = p2.y - p1.y; |
|
p21.z = p2.z - p1.z; |
|
if (fabs(p21.x) < LINE_EPS && fabs(p21.y) < LINE_EPS && fabs(p21.z) < LINE_EPS) |
|
return false; |
|
|
|
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z; |
|
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z; |
|
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z; |
|
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z; |
|
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z; |
|
|
|
denom = d2121 * d4343 - d4321 * d4321; |
|
if (fabs(denom) < LINE_EPS) |
|
return false; |
|
numer = d1343 * d4321 - d1321 * d4343; |
|
|
|
*t1 = numer / denom; |
|
*t2 = (d1343 + d4321 * (*t1)) / d4343; |
|
|
|
s1->x = p1.x + *t1 * p21.x; |
|
s1->y = p1.y + *t1 * p21.y; |
|
s1->z = p1.z + *t1 * p21.z; |
|
s2->x = p3.x + *t2 * p43.x; |
|
s2->y = p3.y + *t2 * p43.y; |
|
s2->z = p3.z + *t2 * p43.z; |
|
|
|
return true; |
|
} |
|
|
|
#pragma optimize( "", off ) |
|
|
|
#ifndef EXCEPTION_EXECUTE_HANDLER |
|
#define EXCEPTION_EXECUTE_HANDLER 1 |
|
#endif |
|
|
|
#pragma optimize( "", on ) |
|
|
|
static bool s_b3DNowEnabled = false; |
|
static bool s_bMMXEnabled = false; |
|
static bool s_bSSEEnabled = false; |
|
static bool s_bSSE2Enabled = false; |
|
|
|
void MathLib_Init( float gamma, float texGamma, float brightness, int overbright, bool bAllow3DNow, bool bAllowSSE, bool bAllowSSE2, bool bAllowMMX ) |
|
{ |
|
if ( s_bMathlibInitialized ) |
|
return; |
|
|
|
// FIXME: Hook SSE into VectorAligned + Vector4DAligned |
|
|
|
#if !defined( _X360 ) |
|
// Grab the processor information: |
|
const CPUInformation& pi = *GetCPUInformation(); |
|
|
|
// Select the default generic routines. |
|
pfSqrt = _sqrtf; |
|
pfRSqrt = _rsqrtf; |
|
pfRSqrtFast = _rsqrtf; |
|
pfVectorNormalize = _VectorNormalize; |
|
pfVectorNormalizeFast = _VectorNormalizeFast; |
|
pfInvRSquared = _InvRSquared; |
|
pfFastSinCos = SinCos; |
|
pfFastCos = cosf; |
|
|
|
if ( bAllowMMX && pi.m_bMMX ) |
|
{ |
|
// Select the MMX specific routines if available |
|
// (MMX routines were used by SW span fillers - not currently used for HW) |
|
s_bMMXEnabled = true; |
|
} |
|
else |
|
{ |
|
s_bMMXEnabled = false; |
|
} |
|
|
|
// SSE Generally performs better than 3DNow when present, so this is placed |
|
// first to allow SSE to override these settings. |
|
#if !defined( OSX ) && !defined( PLATFORM_WINDOWS_PC64 ) && !defined(LINUX) && !defined(PLATFORM_BSD) |
|
if ( bAllow3DNow && pi.m_b3DNow ) |
|
{ |
|
s_b3DNowEnabled = true; |
|
|
|
// Select the 3DNow specific routines if available; |
|
pfVectorNormalize = _3DNow_VectorNormalize; |
|
pfVectorNormalizeFast = _3DNow_VectorNormalizeFast; |
|
pfInvRSquared = _3DNow_InvRSquared; |
|
pfSqrt = _3DNow_Sqrt; |
|
pfRSqrt = _3DNow_RSqrt; |
|
pfRSqrtFast = _3DNow_RSqrt; |
|
} |
|
else |
|
#endif |
|
{ |
|
s_b3DNowEnabled = false; |
|
} |
|
|
|
if ( bAllowSSE && pi.m_bSSE ) |
|
{ |
|
s_bSSEEnabled = true; |
|
|
|
#ifndef PLATFORM_WINDOWS_PC64 |
|
// These are not yet available. |
|
// Select the SSE specific routines if available |
|
pfVectorNormalize = _VectorNormalize; |
|
pfVectorNormalizeFast = _SSE_VectorNormalizeFast; |
|
pfInvRSquared = _SSE_InvRSquared; |
|
pfSqrt = _SSE_Sqrt; |
|
pfRSqrt = _SSE_RSqrtAccurate; |
|
pfRSqrtFast = _SSE_RSqrtFast; |
|
#endif |
|
#ifdef PLATFORM_WINDOWS_PC32 |
|
pfFastSinCos = _SSE_SinCos; |
|
pfFastCos = _SSE_cos; |
|
#endif |
|
} |
|
else |
|
{ |
|
s_bSSEEnabled = false; |
|
} |
|
|
|
if ( bAllowSSE2 && pi.m_bSSE2 ) |
|
{ |
|
s_bSSE2Enabled = true; |
|
#ifdef PLATFORM_WINDOWS_PC32 |
|
pfFastSinCos = _SSE2_SinCos; |
|
pfFastCos = _SSE2_cos; |
|
#endif |
|
} |
|
else |
|
{ |
|
s_bSSE2Enabled = false; |
|
} |
|
#endif // !_X360 |
|
|
|
s_bMathlibInitialized = true; |
|
|
|
InitSinCosTable(); |
|
BuildGammaTable( gamma, texGamma, brightness, overbright ); |
|
} |
|
|
|
bool MathLib_3DNowEnabled( void ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return s_b3DNowEnabled; |
|
} |
|
|
|
bool MathLib_MMXEnabled( void ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return s_bMMXEnabled; |
|
} |
|
|
|
bool MathLib_SSEEnabled( void ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return s_bSSEEnabled; |
|
} |
|
|
|
bool MathLib_SSE2Enabled( void ) |
|
{ |
|
Assert( s_bMathlibInitialized ); |
|
return s_bSSE2Enabled; |
|
} |
|
|
|
float Approach( float target, float value, float speed ) |
|
{ |
|
float delta = target - value; |
|
|
|
if ( delta > speed ) |
|
value += speed; |
|
else if ( delta < -speed ) |
|
value -= speed; |
|
else |
|
value = target; |
|
|
|
return value; |
|
} |
|
|
|
// BUGBUG: Why doesn't this call angle diff?!?!? |
|
float ApproachAngle( float target, float value, float speed ) |
|
{ |
|
target = anglemod( target ); |
|
value = anglemod( value ); |
|
|
|
float delta = target - value; |
|
|
|
// Speed is assumed to be positive |
|
if ( speed < 0 ) |
|
speed = -speed; |
|
|
|
if ( delta < -180 ) |
|
delta += 360; |
|
else if ( delta > 180 ) |
|
delta -= 360; |
|
|
|
if ( delta > speed ) |
|
value += speed; |
|
else if ( delta < -speed ) |
|
value -= speed; |
|
else |
|
value = target; |
|
|
|
return value; |
|
} |
|
|
|
|
|
// BUGBUG: Why do we need both of these? |
|
float AngleDiff( float destAngle, float srcAngle ) |
|
{ |
|
float delta; |
|
|
|
delta = fmodf(destAngle - srcAngle, 360.0f); |
|
if ( destAngle > srcAngle ) |
|
{ |
|
if ( delta >= 180 ) |
|
delta -= 360; |
|
} |
|
else |
|
{ |
|
if ( delta <= -180 ) |
|
delta += 360; |
|
} |
|
return delta; |
|
} |
|
|
|
|
|
float AngleDistance( float next, float cur ) |
|
{ |
|
float delta = next - cur; |
|
|
|
if ( delta < -180 ) |
|
delta += 360; |
|
else if ( delta > 180 ) |
|
delta -= 360; |
|
|
|
return delta; |
|
} |
|
|
|
|
|
float AngleNormalize( float angle ) |
|
{ |
|
angle = fmodf(angle, 360.0f); |
|
if (angle > 180) |
|
{ |
|
angle -= 360; |
|
} |
|
if (angle < -180) |
|
{ |
|
angle += 360; |
|
} |
|
return angle; |
|
} |
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
// ensure that 0 <= angle <= 360 |
|
float AngleNormalizePositive( float angle ) |
|
{ |
|
angle = fmodf( angle, 360.0f ); |
|
|
|
if (angle < 0.0f) |
|
{ |
|
angle += 360.0f; |
|
} |
|
|
|
return angle; |
|
} |
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
bool AnglesAreEqual( float a, float b, float tolerance ) |
|
{ |
|
return (fabs( AngleDiff( a, b ) ) < tolerance); |
|
} |
|
|
|
void RotationDeltaAxisAngle( const QAngle &srcAngles, const QAngle &destAngles, Vector &deltaAxis, float &deltaAngle ) |
|
{ |
|
Quaternion srcQuat, destQuat, srcQuatInv, out; |
|
AngleQuaternion( srcAngles, srcQuat ); |
|
AngleQuaternion( destAngles, destQuat ); |
|
QuaternionScale( srcQuat, -1, srcQuatInv ); |
|
QuaternionMult( destQuat, srcQuatInv, out ); |
|
|
|
QuaternionNormalize( out ); |
|
QuaternionAxisAngle( out, deltaAxis, deltaAngle ); |
|
} |
|
|
|
void RotationDelta( const QAngle &srcAngles, const QAngle &destAngles, QAngle *out ) |
|
{ |
|
matrix3x4_t src, srcInv; |
|
matrix3x4_t dest; |
|
AngleMatrix( srcAngles, src ); |
|
AngleMatrix( destAngles, dest ); |
|
// xform = src(-1) * dest |
|
MatrixInvert( src, srcInv ); |
|
matrix3x4_t xform; |
|
ConcatTransforms( dest, srcInv, xform ); |
|
QAngle xformAngles; |
|
MatrixAngles( xform, xformAngles ); |
|
if ( out ) |
|
{ |
|
*out = xformAngles; |
|
} |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: Computes a triangle normal |
|
//----------------------------------------------------------------------------- |
|
void ComputeTrianglePlane( const Vector& v1, const Vector& v2, const Vector& v3, Vector& normal, float& intercept ) |
|
{ |
|
Vector e1, e2; |
|
VectorSubtract( v2, v1, e1 ); |
|
VectorSubtract( v3, v1, e2 ); |
|
CrossProduct( e1, e2, normal ); |
|
VectorNormalize( normal ); |
|
intercept = DotProduct( normal, v1 ); |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: This is a clone of BaseWindingForPlane() |
|
// Input : *outVerts - an array of preallocated verts to build the polygon in |
|
// normal - the plane normal |
|
// dist - the plane constant |
|
// Output : int - vert count (always 4) |
|
//----------------------------------------------------------------------------- |
|
int PolyFromPlane( Vector *outVerts, const Vector& normal, float dist, float fHalfScale ) |
|
{ |
|
int i, x; |
|
vec_t max, v; |
|
Vector org, vright, vup; |
|
|
|
// find the major axis |
|
|
|
max = -16384; //MAX_COORD_INTEGER |
|
x = -1; |
|
for (i=0 ; i<3; i++) |
|
{ |
|
v = fabs(normal[i]); |
|
if (v > max) |
|
{ |
|
x = i; |
|
max = v; |
|
} |
|
} |
|
|
|
if (x==-1) |
|
return 0; |
|
|
|
// Build a unit vector along something other than the major axis |
|
VectorCopy (vec3_origin, vup); |
|
switch (x) |
|
{ |
|
case 0: |
|
case 1: |
|
vup[2] = 1; |
|
break; |
|
case 2: |
|
vup[0] = 1; |
|
break; |
|
} |
|
|
|
// Remove the component of this vector along the normal |
|
v = DotProduct (vup, normal); |
|
VectorMA (vup, -v, normal, vup); |
|
// Make it a unit (perpendicular) |
|
VectorNormalize (vup); |
|
|
|
// Center of the poly is at normal * dist |
|
VectorScale (normal, dist, org); |
|
// Calculate the third orthonormal basis vector for our plane space (this one and vup are in the plane) |
|
CrossProduct (vup, normal, vright); |
|
|
|
// Make the plane's basis vectors big (these are the half-sides of the polygon we're making) |
|
VectorScale (vup, fHalfScale, vup); |
|
VectorScale (vright, fHalfScale, vright); |
|
|
|
// Move diagonally away from org to create the corner verts |
|
VectorSubtract (org, vright, outVerts[0]); // left |
|
VectorAdd (outVerts[0], vup, outVerts[0]); // up |
|
|
|
VectorAdd (org, vright, outVerts[1]); // right |
|
VectorAdd (outVerts[1], vup, outVerts[1]); // up |
|
|
|
VectorAdd (org, vright, outVerts[2]); // right |
|
VectorSubtract (outVerts[2], vup, outVerts[2]); // down |
|
|
|
VectorSubtract (org, vright, outVerts[3]); // left |
|
VectorSubtract (outVerts[3], vup, outVerts[3]); // down |
|
|
|
// The four corners form a planar quadrilateral normal to "normal" |
|
return 4; |
|
} |
|
|
|
//----------------------------------------------------------------------------- |
|
// Purpose: clip a poly to the plane and return the poly on the front side of the plane |
|
// Input : *inVerts - input polygon |
|
// vertCount - # verts in input poly |
|
// *outVerts - destination poly |
|
// normal - plane normal |
|
// dist - plane constant |
|
// Output : int - # verts in output poly |
|
//----------------------------------------------------------------------------- |
|
|
|
int ClipPolyToPlane( Vector *inVerts, int vertCount, Vector *outVerts, const Vector& normal, float dist, float fOnPlaneEpsilon ) |
|
{ |
|
vec_t *dists = (vec_t *)stackalloc( sizeof(vec_t) * vertCount * 4 ); //4x vertcount should cover all cases |
|
int *sides = (int *)stackalloc( sizeof(vec_t) * vertCount * 4 ); |
|
int counts[3]; |
|
vec_t dot; |
|
int i, j; |
|
Vector mid = vec3_origin; |
|
int outCount; |
|
|
|
counts[0] = counts[1] = counts[2] = 0; |
|
|
|
// determine sides for each point |
|
for ( i = 0; i < vertCount; i++ ) |
|
{ |
|
dot = DotProduct( inVerts[i], normal) - dist; |
|
dists[i] = dot; |
|
if ( dot > fOnPlaneEpsilon ) |
|
{ |
|
sides[i] = SIDE_FRONT; |
|
} |
|
else if ( dot < -fOnPlaneEpsilon ) |
|
{ |
|
sides[i] = SIDE_BACK; |
|
} |
|
else |
|
{ |
|
sides[i] = SIDE_ON; |
|
} |
|
counts[sides[i]]++; |
|
} |
|
sides[i] = sides[0]; |
|
dists[i] = dists[0]; |
|
|
|
if (!counts[0]) |
|
return 0; |
|
|
|
if (!counts[1]) |
|
{ |
|
// Copy to output verts |
|
for ( i = 0; i < vertCount; i++ ) |
|
{ |
|
VectorCopy( inVerts[i], outVerts[i] ); |
|
} |
|
return vertCount; |
|
} |
|
|
|
outCount = 0; |
|
for ( i = 0; i < vertCount; i++ ) |
|
{ |
|
Vector& p1 = inVerts[i]; |
|
|
|
if (sides[i] == SIDE_ON) |
|
{ |
|
VectorCopy( p1, outVerts[outCount]); |
|
outCount++; |
|
continue; |
|
} |
|
|
|
if (sides[i] == SIDE_FRONT) |
|
{ |
|
VectorCopy( p1, outVerts[outCount]); |
|
outCount++; |
|
} |
|
|
|
if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) |
|
continue; |
|
|
|
// generate a split point |
|
Vector& p2 = inVerts[(i+1)%vertCount]; |
|
|
|
dot = dists[i] / (dists[i]-dists[i+1]); |
|
for (j=0 ; j<3 ; j++) |
|
{ // avoid round off error when possible |
|
if (normal[j] == 1) |
|
mid[j] = dist; |
|
else if (normal[j] == -1) |
|
mid[j] = -dist; |
|
else |
|
mid[j] = p1[j] + dot*(p2[j]-p1[j]); |
|
} |
|
|
|
VectorCopy (mid, outVerts[outCount]); |
|
outCount++; |
|
} |
|
|
|
return outCount; |
|
} |
|
|
|
|
|
int ClipPolyToPlane_Precise( double *inVerts, int vertCount, double *outVerts, const double *normal, double dist, double fOnPlaneEpsilon ) |
|
{ |
|
double *dists = (double *)stackalloc( sizeof(double) * vertCount * 4 ); //4x vertcount should cover all cases |
|
int *sides = (int *)stackalloc( sizeof(double) * vertCount * 4 ); |
|
int counts[3]; |
|
double dot; |
|
int i, j; |
|
//Vector mid = vec3_origin; |
|
double mid[3]; |
|
mid[0] = 0.0; |
|
mid[1] = 0.0; |
|
mid[2] = 0.0; |
|
int outCount; |
|
|
|
counts[0] = counts[1] = counts[2] = 0; |
|
|
|
// determine sides for each point |
|
for ( i = 0; i < vertCount; i++ ) |
|
{ |
|
//dot = DotProduct( inVerts[i], normal) - dist; |
|
dot = ((inVerts[i*3 + 0] * normal[0]) + (inVerts[i*3 + 1] * normal[1]) + (inVerts[i*3 + 2] * normal[2])) - dist; |
|
dists[i] = dot; |
|
if ( dot > fOnPlaneEpsilon ) |
|
{ |
|
sides[i] = SIDE_FRONT; |
|
} |
|
else if ( dot < -fOnPlaneEpsilon ) |
|
{ |
|
sides[i] = SIDE_BACK; |
|
} |
|
else |
|
{ |
|
sides[i] = SIDE_ON; |
|
} |
|
counts[sides[i]]++; |
|
} |
|
sides[i] = sides[0]; |
|
dists[i] = dists[0]; |
|
|
|
if (!counts[0]) |
|
return 0; |
|
|
|
if (!counts[1]) |
|
{ |
|
// Copy to output verts |
|
//for ( i = 0; i < vertCount; i++ ) |
|
for ( i = 0; i < vertCount * 3; i++ ) |
|
{ |
|
//VectorCopy( inVerts[i], outVerts[i] ); |
|
outVerts[i] = inVerts[i]; |
|
} |
|
return vertCount; |
|
} |
|
|
|
outCount = 0; |
|
for ( i = 0; i < vertCount; i++ ) |
|
{ |
|
//Vector& p1 = inVerts[i]; |
|
double *p1 = &inVerts[i*3]; |
|
//p1[0] = inVerts[i*3 + 0]; |
|
//p1[1] = inVerts[i*3 + 1]; |
|
//p1[2] = inVerts[i*3 + 2]; |
|
|
|
if (sides[i] == SIDE_ON) |
|
{ |
|
//VectorCopy( p1, outVerts[outCount]); |
|
outVerts[outCount*3 + 0] = p1[0]; |
|
outVerts[outCount*3 + 1] = p1[1]; |
|
outVerts[outCount*3 + 2] = p1[2]; |
|
outCount++; |
|
continue; |
|
} |
|
|
|
if (sides[i] == SIDE_FRONT) |
|
{ |
|
//VectorCopy( p1, outVerts[outCount]); |
|
outVerts[outCount*3 + 0] = p1[0]; |
|
outVerts[outCount*3 + 1] = p1[1]; |
|
outVerts[outCount*3 + 2] = p1[2]; |
|
outCount++; |
|
} |
|
|
|
if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) |
|
continue; |
|
|
|
// generate a split point |
|
//Vector& p2 = inVerts[(i+1)%vertCount]; |
|
int wrappedindex = (i+1)%vertCount; |
|
double *p2 = &inVerts[wrappedindex*3]; |
|
//p2[0] = inVerts[wrappedindex*3 + 0]; |
|
//p2[1] = inVerts[wrappedindex*3 + 1]; |
|
//p2[2] = inVerts[wrappedindex*3 + 2]; |
|
|
|
dot = dists[i] / (dists[i]-dists[i+1]); |
|
for (j=0 ; j<3 ; j++) |
|
{ |
|
mid[j] = (double)p1[j] + dot*((double)p2[j]-(double)p1[j]); |
|
} |
|
|
|
//VectorCopy (mid, outVerts[outCount]); |
|
outVerts[outCount*3 + 0] = mid[0]; |
|
outVerts[outCount*3 + 1] = mid[1]; |
|
outVerts[outCount*3 + 2] = mid[2]; |
|
outCount++; |
|
} |
|
|
|
return outCount; |
|
} |
|
|
|
int CeilPow2( int in ) |
|
{ |
|
int retval; |
|
|
|
retval = 1; |
|
while( retval < in ) |
|
retval <<= 1; |
|
return retval; |
|
} |
|
|
|
int FloorPow2( int in ) |
|
{ |
|
int retval; |
|
|
|
retval = 1; |
|
while( retval < in ) |
|
retval <<= 1; |
|
return retval >> 1; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Computes Y fov from an X fov and a screen aspect ratio |
|
//----------------------------------------------------------------------------- |
|
float CalcFovY( float flFovX, float flAspect ) |
|
{ |
|
if ( flFovX < 1 || flFovX > 179) |
|
{ |
|
flFovX = 90; // error, set to 90 |
|
} |
|
|
|
// The long, but illustrative version (more closely matches CShaderAPIDX8::PerspectiveX, which |
|
// is what it's based on). |
|
// |
|
//float width = 2 * zNear * tan( DEG2RAD( fov_x / 2.0 ) ); |
|
//float height = width / screenaspect; |
|
//float yRadians = atan( (height/2.0) / zNear ); |
|
//return RAD2DEG( yRadians ) * 2; |
|
|
|
// The short and sweet version. |
|
float val = atan( tan( DEG2RAD( flFovX ) * 0.5f ) / flAspect ); |
|
val = RAD2DEG( val ) * 2.0f; |
|
return val; |
|
} |
|
|
|
float CalcFovX( float flFovY, float flAspect ) |
|
{ |
|
return RAD2DEG( atan( tan( DEG2RAD( flFovY ) * 0.5f ) * flAspect ) ) * 2.0f; |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Generate a frustum based on perspective view parameters |
|
//----------------------------------------------------------------------------- |
|
void GeneratePerspectiveFrustum( const Vector& origin, const Vector &forward, |
|
const Vector &right, const Vector &up, float flZNear, float flZFar, |
|
float flFovX, float flFovY, Frustum_t &frustum ) |
|
{ |
|
float flIntercept = DotProduct( origin, forward ); |
|
|
|
// Setup the near and far planes. |
|
frustum.SetPlane( FRUSTUM_FARZ, PLANE_ANYZ, -forward, -flZFar - flIntercept ); |
|
frustum.SetPlane( FRUSTUM_NEARZ, PLANE_ANYZ, forward, flZNear + flIntercept ); |
|
|
|
flFovX *= 0.5f; |
|
flFovY *= 0.5f; |
|
|
|
float flTanX = tan( DEG2RAD( flFovX ) ); |
|
float flTanY = tan( DEG2RAD( flFovY ) ); |
|
|
|
// OPTIMIZE: Normalizing these planes is not necessary for culling |
|
Vector normalPos, normalNeg; |
|
|
|
VectorMA( right, flTanX, forward, normalPos ); |
|
VectorMA( normalPos, -2.0f, right, normalNeg ); |
|
|
|
VectorNormalize( normalPos ); |
|
VectorNormalize( normalNeg ); |
|
|
|
frustum.SetPlane( FRUSTUM_LEFT, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); |
|
frustum.SetPlane( FRUSTUM_RIGHT, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); |
|
|
|
VectorMA( up, flTanY, forward, normalPos ); |
|
VectorMA( normalPos, -2.0f, up, normalNeg ); |
|
|
|
VectorNormalize( normalPos ); |
|
VectorNormalize( normalNeg ); |
|
|
|
frustum.SetPlane( FRUSTUM_BOTTOM, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); |
|
frustum.SetPlane( FRUSTUM_TOP, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Version that accepts angles instead of vectors |
|
//----------------------------------------------------------------------------- |
|
void GeneratePerspectiveFrustum( const Vector& origin, const QAngle &angles, float flZNear, float flZFar, float flFovX, float flAspectRatio, Frustum_t &frustum ) |
|
{ |
|
Vector vecForward, vecRight, vecUp; |
|
AngleVectors( angles, &vecForward, &vecRight, &vecUp ); |
|
float flFovY = CalcFovY( flFovX, flAspectRatio ); |
|
GeneratePerspectiveFrustum( origin, vecForward, vecRight, vecUp, flZNear, flZFar, flFovX, flFovY, frustum ); |
|
} |
|
|
|
bool R_CullBox( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) |
|
{ |
|
return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_NEARZ) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); |
|
} |
|
|
|
bool R_CullBoxSkipNear( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) |
|
{ |
|
return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || |
|
( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); |
|
} |
|
|
|
|
|
// NOTE: This routine was taken (and modified) from NVidia's BlinnReflection demo |
|
// Creates basis vectors, based on a vertex and index list. |
|
// See the NVidia white paper 'GDC2K PerPixel Lighting' for a description |
|
// of how this computation works |
|
#define SMALL_FLOAT 1e-12 |
|
|
|
void CalcTriangleTangentSpace( const Vector &p0, const Vector &p1, const Vector &p2, |
|
const Vector2D &t0, const Vector2D &t1, const Vector2D& t2, |
|
Vector &sVect, Vector &tVect ) |
|
{ |
|
/* Compute the partial derivatives of X, Y, and Z with respect to S and T. */ |
|
sVect.Init( 0.0f, 0.0f, 0.0f ); |
|
tVect.Init( 0.0f, 0.0f, 0.0f ); |
|
|
|
// x, s, t |
|
Vector edge01( p1.x - p0.x, t1.x - t0.x, t1.y - t0.y ); |
|
Vector edge02( p2.x - p0.x, t2.x - t0.x, t2.y - t0.y ); |
|
|
|
Vector cross; |
|
CrossProduct( edge01, edge02, cross ); |
|
if ( fabs( cross.x ) > SMALL_FLOAT ) |
|
{ |
|
sVect.x += -cross.y / cross.x; |
|
tVect.x += -cross.z / cross.x; |
|
} |
|
|
|
// y, s, t |
|
edge01.Init( p1.y - p0.y, t1.x - t0.x, t1.y - t0.y ); |
|
edge02.Init( p2.y - p0.y, t2.x - t0.x, t2.y - t0.y ); |
|
|
|
CrossProduct( edge01, edge02, cross ); |
|
if ( fabs( cross.x ) > SMALL_FLOAT ) |
|
{ |
|
sVect.y += -cross.y / cross.x; |
|
tVect.y += -cross.z / cross.x; |
|
} |
|
|
|
// z, s, t |
|
edge01.Init( p1.z - p0.z, t1.x - t0.x, t1.y - t0.y ); |
|
edge02.Init( p2.z - p0.z, t2.x - t0.x, t2.y - t0.y ); |
|
|
|
CrossProduct( edge01, edge02, cross ); |
|
if( fabs( cross.x ) > SMALL_FLOAT ) |
|
{ |
|
sVect.z += -cross.y / cross.x; |
|
tVect.z += -cross.z / cross.x; |
|
} |
|
|
|
// Normalize sVect and tVect |
|
VectorNormalize( sVect ); |
|
VectorNormalize( tVect ); |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Convert RGB to HSV |
|
//----------------------------------------------------------------------------- |
|
void RGBtoHSV( const Vector &rgb, Vector &hsv ) |
|
{ |
|
float flMax = max( rgb.x, rgb.y ); |
|
flMax = max( flMax, rgb.z ); |
|
float flMin = min( rgb.x, rgb.y ); |
|
flMin = min( flMin, rgb.z ); |
|
|
|
// hsv.z is the value |
|
hsv.z = flMax; |
|
|
|
// hsv.y is the saturation |
|
if (flMax != 0.0F) |
|
{ |
|
hsv.y = (flMax - flMin) / flMax; |
|
} |
|
else |
|
{ |
|
hsv.y = 0.0F; |
|
} |
|
|
|
// hsv.x is the hue |
|
if (hsv.y == 0.0F) |
|
{ |
|
hsv.x = -1.0f; |
|
} |
|
else |
|
{ |
|
float32 d = flMax - flMin; |
|
if (rgb.x == flMax) |
|
{ |
|
hsv.x = (rgb.y - rgb.z) / d; |
|
} |
|
else if (rgb.y == flMax) |
|
{ |
|
hsv.x = 2.0F + (rgb.z - rgb.x) / d; |
|
} |
|
else |
|
{ |
|
hsv.x = 4.0F + (rgb.x - rgb.y) / d; |
|
} |
|
hsv.x *= 60.0F; |
|
if ( hsv.x < 0.0F ) |
|
{ |
|
hsv.x += 360.0F; |
|
} |
|
} |
|
} |
|
|
|
|
|
//----------------------------------------------------------------------------- |
|
// Convert HSV to RGB |
|
//----------------------------------------------------------------------------- |
|
void HSVtoRGB( const Vector &hsv, Vector &rgb ) |
|
{ |
|
if ( hsv.y == 0.0F ) |
|
{ |
|
rgb.Init( hsv.z, hsv.z, hsv.z ); |
|
return; |
|
} |
|
|
|
float32 hue = hsv.x; |
|
if (hue == 360.0F) |
|
{ |
|
hue = 0.0F; |
|
} |
|
hue /= 60.0F; |
|
int i = hue; // integer part |
|
float32 f = hue - i; // fractional part |
|
float32 p = hsv.z * (1.0F - hsv.y); |
|
float32 q = hsv.z * (1.0F - hsv.y * f); |
|
float32 t = hsv.z * (1.0F - hsv.y * (1.0F - f)); |
|
switch(i) |
|
{ |
|
case 0: rgb.Init( hsv.z, t, p ); break; |
|
case 1: rgb.Init( q, hsv.z, p ); break; |
|
case 2: rgb.Init( p, hsv.z, t ); break; |
|
case 3: rgb.Init( p, q, hsv.z ); break; |
|
case 4: rgb.Init( t, p, hsv.z ); break; |
|
case 5: rgb.Init( hsv.z, p, q ); break; |
|
} |
|
} |
|
|
|
|
|
void GetInterpolationData( float const *pKnotPositions, |
|
float const *pKnotValues, |
|
int nNumValuesinList, |
|
int nInterpolationRange, |
|
float flPositionToInterpolateAt, |
|
bool bWrap, |
|
float *pValueA, |
|
float *pValueB, |
|
float *pInterpolationValue) |
|
{ |
|
// first, find the bracketting knots by looking for the first knot >= our index |
|
|
|
int idx; |
|
for(idx = 0; idx < nNumValuesinList; idx++ ) |
|
{ |
|
if ( pKnotPositions[idx] >= flPositionToInterpolateAt ) |
|
break; |
|
} |
|
int nKnot1, nKnot2; |
|
float flOffsetFromStartOfGap, flSizeOfGap; |
|
if ( idx == 0) |
|
{ |
|
if ( bWrap ) |
|
{ |
|
nKnot1 = nNumValuesinList-1; |
|
nKnot2 = 0; |
|
flSizeOfGap = |
|
( pKnotPositions[nKnot2] + ( nInterpolationRange-pKnotPositions[nKnot1] ) ); |
|
flOffsetFromStartOfGap = |
|
flPositionToInterpolateAt + ( nInterpolationRange-pKnotPositions[nKnot1] ); |
|
} |
|
else |
|
{ |
|
*pValueA = *pValueB = pKnotValues[0]; |
|
*pInterpolationValue = 1.0; |
|
return; |
|
} |
|
} |
|
else if ( idx == nNumValuesinList ) // ran out of values |
|
{ |
|
if ( bWrap ) |
|
{ |
|
nKnot1 = nNumValuesinList -1; |
|
nKnot2 = 0; |
|
flSizeOfGap = ( pKnotPositions[nKnot2] + |
|
( nInterpolationRange-pKnotPositions[nKnot1] ) ); |
|
flOffsetFromStartOfGap = flPositionToInterpolateAt - pKnotPositions[nKnot1]; |
|
} |
|
else |
|
{ |
|
*pValueA = *pValueB = pKnotValues[nNumValuesinList-1]; |
|
*pInterpolationValue = 1.0; |
|
return; |
|
} |
|
|
|
} |
|
else |
|
{ |
|
nKnot1 = idx-1; |
|
nKnot2 = idx; |
|
flSizeOfGap = pKnotPositions[nKnot2]-pKnotPositions[nKnot1]; |
|
flOffsetFromStartOfGap = flPositionToInterpolateAt-pKnotPositions[nKnot1]; |
|
} |
|
|
|
*pValueA = pKnotValues[nKnot1]; |
|
*pValueB = pKnotValues[nKnot2]; |
|
*pInterpolationValue = FLerp( 0, 1, 0, flSizeOfGap, flOffsetFromStartOfGap ); |
|
return; |
|
} |
|
|
|
float RandomVectorInUnitSphere( Vector *pVector ) |
|
{ |
|
// Guarantee uniform random distribution within a sphere |
|
// Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") |
|
float u = ((float)rand() / VALVE_RAND_MAX); |
|
float v = ((float)rand() / VALVE_RAND_MAX); |
|
float w = ((float)rand() / VALVE_RAND_MAX); |
|
|
|
float flPhi = acos( 1 - 2 * u ); |
|
float flTheta = 2 * M_PI * v; |
|
float flRadius = powf( w, 1.0f / 3.0f ); |
|
|
|
float flSinPhi, flCosPhi; |
|
float flSinTheta, flCosTheta; |
|
SinCos( flPhi, &flSinPhi, &flCosPhi ); |
|
SinCos( flTheta, &flSinTheta, &flCosTheta ); |
|
|
|
pVector->x = flRadius * flSinPhi * flCosTheta; |
|
pVector->y = flRadius * flSinPhi * flSinTheta; |
|
pVector->z = flRadius * flCosPhi; |
|
return flRadius; |
|
} |
|
|
|
float RandomVectorInUnitCircle( Vector2D *pVector ) |
|
{ |
|
// Guarantee uniform random distribution within a sphere |
|
// Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") |
|
float u = ((float)rand() / VALVE_RAND_MAX); |
|
float v = ((float)rand() / VALVE_RAND_MAX); |
|
|
|
float flTheta = 2 * M_PI * v; |
|
float flRadius = powf( u, 1.0f / 2.0f ); |
|
|
|
float flSinTheta, flCosTheta; |
|
SinCos( flTheta, &flSinTheta, &flCosTheta ); |
|
|
|
pVector->x = flRadius * flCosTheta; |
|
pVector->y = flRadius * flSinTheta; |
|
return flRadius; |
|
} |
|
#ifdef FP_EXCEPTIONS_ENABLED |
|
#include <float.h> // For _clearfp and _controlfp_s |
|
#endif |
|
|
|
// FPExceptionDisable and FPExceptionEnabler taken from my blog post |
|
// at http://www.altdevblogaday.com/2012/04/20/exceptional-floating-point/ |
|
|
|
#ifdef FP_EXCEPTIONS_ENABLED |
|
// These functions are all inlined NOPs if FP_EXCEPTIONS_ENABLED is not defined. |
|
FPExceptionDisabler::FPExceptionDisabler() |
|
{ |
|
// Retrieve the current state of the exception flags. This |
|
// must be done before changing them. _MCW_EM is a bit |
|
// mask representing all available exception masks. |
|
_controlfp_s(&mOldValues, 0, 0); |
|
// Set all of the exception flags, which suppresses FP |
|
// exceptions on the x87 and SSE units. |
|
_controlfp_s(0, _MCW_EM, _MCW_EM); |
|
} |
|
|
|
FPExceptionDisabler::~FPExceptionDisabler() |
|
{ |
|
// Clear any pending FP exceptions. This must be done |
|
// prior to enabling FP exceptions since otherwise there |
|
// may be a 'deferred crash' as soon the exceptions are |
|
// enabled. |
|
_clearfp(); |
|
|
|
// Reset (possibly enabling) the exception status. |
|
_controlfp_s(0, mOldValues, _MCW_EM); |
|
} |
|
|
|
// Overflow, divide-by-zero, and invalid-operation are the FP |
|
// exceptions most frequently associated with bugs. |
|
FPExceptionEnabler::FPExceptionEnabler(unsigned int enableBits /*= _EM_OVERFLOW | _EM_ZERODIVIDE | _EM_INVALID*/) |
|
{ |
|
// Retrieve the current state of the exception flags. This |
|
// must be done before changing them. _MCW_EM is a bit |
|
// mask representing all available exception masks. |
|
_controlfp_s(&mOldValues, 0, 0); |
|
|
|
// Make sure no non-exception flags have been specified, |
|
// to avoid accidental changing of rounding modes, etc. |
|
enableBits &= _MCW_EM; |
|
|
|
// Clear any pending FP exceptions. This must be done |
|
// prior to enabling FP exceptions since otherwise there |
|
// may be a 'deferred crash' as soon the exceptions are |
|
// enabled. |
|
_clearfp(); |
|
|
|
// Zero out the specified bits, leaving other bits alone. |
|
_controlfp_s(0, ~enableBits, enableBits); |
|
} |
|
|
|
FPExceptionEnabler::~FPExceptionEnabler() |
|
{ |
|
// Reset the exception state. |
|
_controlfp_s(0, mOldValues, _MCW_EM); |
|
} |
|
#endif
|
|
|