diff --git a/game/client/cdll_util.cpp b/game/client/cdll_util.cpp index b2e7255e..8f2f26e9 100644 --- a/game/client/cdll_util.cpp +++ b/game/client/cdll_util.cpp @@ -943,7 +943,7 @@ static unsigned char ComputeDistanceFade( C_BaseEntity *pEntity, float flMinDist if( flMinDist > flMaxDist ) { - swap( flMinDist, flMaxDist ); + V_swap( flMinDist, flMaxDist ); } // If a negative value is provided for the min fade distance, then base it off the max. diff --git a/game/client/clientleafsystem.cpp b/game/client/clientleafsystem.cpp index 9ff7a28f..bbb46383 100644 --- a/game/client/clientleafsystem.cpp +++ b/game/client/clientleafsystem.cpp @@ -1722,8 +1722,8 @@ void CClientLeafSystem::SortEntities( const Vector &vecRenderOrigin, const Vecto { if( dists[i] > dists[i+stepSize] ) { - swap( pEntities[i], pEntities[i+stepSize] ); - swap( dists[i], dists[i+stepSize] ); + V_swap( pEntities[i], pEntities[i+stepSize] ); + V_swap( dists[i], dists[i+stepSize] ); if( i == 0 ) { diff --git a/game/client/clientshadowmgr.cpp b/game/client/clientshadowmgr.cpp index e63ae3e6..48927a25 100644 --- a/game/client/clientshadowmgr.cpp +++ b/game/client/clientshadowmgr.cpp @@ -1138,7 +1138,7 @@ void CVisibleShadowList::PrioritySort() flLargestArea = m_ShadowsInView[nIndex].m_flArea; } } - swap( m_PriorityIndex[i], m_PriorityIndex[nLargestInd] ); + V_swap( m_PriorityIndex[i], m_PriorityIndex[nLargestInd] ); } } diff --git a/game/client/detailobjectsystem.cpp b/game/client/detailobjectsystem.cpp index 16cf4631..31a012f9 100644 --- a/game/client/detailobjectsystem.cpp +++ b/game/client/detailobjectsystem.cpp @@ -1613,7 +1613,7 @@ void CDetailObjectSystem::UnserializeDetailSprites( CUtlBuffer& buf ) buf.Get( &m_DetailSpriteDict[i], sizeof(DetailSpriteDictLump_t) ); int flipi = m_DetailSpriteDictFlipped.AddToTail(); m_DetailSpriteDictFlipped[flipi] = m_DetailSpriteDict[i]; - swap( m_DetailSpriteDictFlipped[flipi].m_TexUL.x, m_DetailSpriteDictFlipped[flipi].m_TexLR.x ); + V_swap( m_DetailSpriteDictFlipped[flipi].m_TexUL.x, m_DetailSpriteDictFlipped[flipi].m_TexLR.x ); } } diff --git a/game/server/episodic/npc_advisor.cpp b/game/server/episodic/npc_advisor.cpp index 02e806a3..cbe89919 100644 --- a/game/server/episodic/npc_advisor.cpp +++ b/game/server/episodic/npc_advisor.cpp @@ -821,7 +821,7 @@ void CNPC_Advisor::RunTask( const Task_t *pTask ) // swap them if necessary (1 must be the bottom) if (m_levitateCallback.m_vecGoalPos1.z > m_levitateCallback.m_vecGoalPos2.z) { - swap(m_levitateCallback.m_vecGoalPos1,m_levitateCallback.m_vecGoalPos2); + V_swap(m_levitateCallback.m_vecGoalPos1,m_levitateCallback.m_vecGoalPos2); } m_levitateCallback.m_flFloat = 0.06f; // this is an absolute accumulation upon gravity diff --git a/game/server/episodic/vehicle_jeep_episodic.cpp b/game/server/episodic/vehicle_jeep_episodic.cpp index 98be4352..48ad8c47 100644 --- a/game/server/episodic/vehicle_jeep_episodic.cpp +++ b/game/server/episodic/vehicle_jeep_episodic.cpp @@ -1626,13 +1626,13 @@ void CPropJeepEpisodic::InputOutsideTransition( inputdata_t &inputdata ) VectorRotate( vecSurroundMaxs, vecTeleportAngles, vecMaxs ); if ( vecMaxs.x < vecMins.x ) - swap( vecMins.x, vecMaxs.x ); + V_swap( vecMins.x, vecMaxs.x ); if ( vecMaxs.y < vecMins.y ) - swap( vecMins.y, vecMaxs.y ); + V_swap( vecMins.y, vecMaxs.y ); if ( vecMaxs.z < vecMins.z ) - swap( vecMins.z, vecMaxs.z ); + V_swap( vecMins.z, vecMaxs.z ); // Move up vecTeleportPos.z += ( vecMaxs.z - vecMins.z ); diff --git a/game/server/filters.cpp b/game/server/filters.cpp index d9ee2b34..09ee23bc 100644 --- a/game/server/filters.cpp +++ b/game/server/filters.cpp @@ -536,7 +536,7 @@ bool CFilterEnemy::PassesProximityFilter( CBaseEntity *pCaller, CBaseEntity *pEn float flSmallerRadius = m_flRadius; if ( flSmallerRadius > flLargerRadius ) { - swap( flLargerRadius, flSmallerRadius ); + V_swap( flLargerRadius, flSmallerRadius ); } float flDist; diff --git a/game/server/hl2/npc_attackchopper.cpp b/game/server/hl2/npc_attackchopper.cpp index 310fbb4a..134d725f 100644 --- a/game/server/hl2/npc_attackchopper.cpp +++ b/game/server/hl2/npc_attackchopper.cpp @@ -1843,7 +1843,7 @@ void CNPC_AttackHelicopter::ShootAtPlayer( const Vector &vBasePos, const Vector for ( i = 0; i < nActualTargets; ++i ) { int nSwap = random->RandomInt( 0, nActualTargets - 1 ); - swap( ppNearbyTargets[i], ppNearbyTargets[nSwap] ); + V_swap( ppNearbyTargets[i], ppNearbyTargets[nSwap] ); } // Just shoot where we're facing @@ -2120,7 +2120,7 @@ void CNPC_AttackHelicopter::ShootAtVehicle( const Vector &vBasePos, const Vector for ( i = 0; i < nActualTargets; ++i ) { int nSwap = random->RandomInt( 0, nActualTargets - 1 ); - swap( ppNearbyTargets[i], ppNearbyTargets[nSwap] ); + V_swap( ppNearbyTargets[i], ppNearbyTargets[nSwap] ); } // Just shoot where we're facing diff --git a/game/server/hl2/npc_combinecamera.cpp b/game/server/hl2/npc_combinecamera.cpp index 4bb4f44a..0c962533 100644 --- a/game/server/hl2/npc_combinecamera.cpp +++ b/game/server/hl2/npc_combinecamera.cpp @@ -355,7 +355,7 @@ void CNPC_CombineCamera::Spawn() if (m_nOuterRadius < m_nInnerRadius) { - swap(m_nOuterRadius, m_nInnerRadius); + V_swap(m_nOuterRadius, m_nInnerRadius); } // Do we start active? @@ -1117,17 +1117,17 @@ void CNPC_CombineCamera::SetHeight(float height) if (mins.x > maxs.x) { - swap(mins.x, maxs.x); + V_swap(mins.x, maxs.x); } if (mins.y > maxs.y) { - swap(mins.y, maxs.y); + V_swap(mins.y, maxs.y); } if (mins.z > maxs.z) { - swap(mins.z, maxs.z); + V_swap(mins.z, maxs.z); } SetCollisionBounds(mins, maxs); diff --git a/game/server/hl2/npc_turret_ceiling.cpp b/game/server/hl2/npc_turret_ceiling.cpp index 8c0c0f78..ee14a17a 100644 --- a/game/server/hl2/npc_turret_ceiling.cpp +++ b/game/server/hl2/npc_turret_ceiling.cpp @@ -1093,17 +1093,17 @@ void CNPC_CeilingTurret::SetHeight( float height ) if ( mins.x > maxs.x ) { - swap( mins.x, maxs.x ); + V_swap( mins.x, maxs.x ); } if ( mins.y > maxs.y ) { - swap( mins.y, maxs.y ); + V_swap( mins.y, maxs.y ); } if ( mins.z > maxs.z ) { - swap( mins.z, maxs.z ); + V_swap( mins.z, maxs.z ); } SetCollisionBounds( mins, maxs ); diff --git a/game/server/hltvdirector.cpp b/game/server/hltvdirector.cpp index 1971ac1a..42b02f91 100644 --- a/game/server/hltvdirector.cpp +++ b/game/server/hltvdirector.cpp @@ -562,7 +562,7 @@ void CHLTVDirector::CreateShotFromEvent( CGameEvent *event ) // if we show ineye view, show it more likely from killer if ( RandomFloat(0,1) > (bInEye?0.3f:0.7f) ) { - swap( attacker, victim ); + V_swap( attacker, victim ); } // hurting a victim is shown as chase more often diff --git a/game/server/physconstraint.cpp b/game/server/physconstraint.cpp index d7acbea9..7b24bc58 100644 --- a/game/server/physconstraint.cpp +++ b/game/server/physconstraint.cpp @@ -1280,7 +1280,7 @@ IPhysicsConstraint *CPhysSlideConstraint::CreateConstraint( IPhysicsConstraintGr sliding.limitMax = DotProduct( axisDirection, m_axisEnd ); if ( sliding.limitMax < sliding.limitMin ) { - swap( sliding.limitMin, sliding.limitMax ); + V_swap( sliding.limitMin, sliding.limitMax ); } // expand limits to make initial position of the attached object valid diff --git a/game/server/props.cpp b/game/server/props.cpp index 68d245e3..9726f433 100644 --- a/game/server/props.cpp +++ b/game/server/props.cpp @@ -4789,7 +4789,7 @@ void CPropDoorRotating::Spawn() // that the model already be set. if ( IsHingeOnLeft() ) { - swap( m_angRotationOpenForward, m_angRotationOpenBack ); + V_swap( m_angRotationOpenForward, m_angRotationOpenBack ); } // Figure out our volumes of movement as this door opens diff --git a/game/shared/gamerules.cpp b/game/shared/gamerules.cpp index 614a24f0..29b47d2f 100644 --- a/game/shared/gamerules.cpp +++ b/game/shared/gamerules.cpp @@ -637,7 +637,7 @@ bool CGameRules::ShouldCollide( int collisionGroup0, int collisionGroup1 ) if ( collisionGroup0 > collisionGroup1 ) { // swap so that lowest is always first - swap(collisionGroup0,collisionGroup1); + V_swap(collisionGroup0,collisionGroup1); } #ifndef HL2MP diff --git a/game/shared/hl2mp/hl2mp_gamerules.cpp b/game/shared/hl2mp/hl2mp_gamerules.cpp index 146e08b1..5a18e5fc 100644 --- a/game/shared/hl2mp/hl2mp_gamerules.cpp +++ b/game/shared/hl2mp/hl2mp_gamerules.cpp @@ -866,7 +866,7 @@ bool CHL2MPRules::ShouldCollide( int collisionGroup0, int collisionGroup1 ) if ( collisionGroup0 > collisionGroup1 ) { // swap so that lowest is always first - swap(collisionGroup0,collisionGroup1); + V_swap(collisionGroup0,collisionGroup1); } if ( (collisionGroup0 == COLLISION_GROUP_PLAYER || collisionGroup0 == COLLISION_GROUP_PLAYER_MOVEMENT) && diff --git a/game/shared/sdk/sdk_gamerules.cpp b/game/shared/sdk/sdk_gamerules.cpp index 82d287c7..2802c332 100644 --- a/game/shared/sdk/sdk_gamerules.cpp +++ b/game/shared/sdk/sdk_gamerules.cpp @@ -295,7 +295,7 @@ bool CSDKGameRules::ShouldCollide( int collisionGroup0, int collisionGroup1 ) if ( collisionGroup0 > collisionGroup1 ) { // swap so that lowest is always first - swap(collisionGroup0,collisionGroup1); + V_swap(collisionGroup0,collisionGroup1); } //Don't stand on COLLISION_GROUP_WEAPON diff --git a/mathlib/mathlib_base.cpp b/mathlib/mathlib_base.cpp index 919bc4bf..63ba0b60 100644 --- a/mathlib/mathlib_base.cpp +++ b/mathlib/mathlib_base.cpp @@ -380,9 +380,9 @@ void MatrixInvert( const matrix3x4_t& in, matrix3x4_t& out ) Assert( s_bMathlibInitialized ); if ( &in == &out ) { - swap(out[0][1],out[1][0]); - swap(out[0][2],out[2][0]); - swap(out[1][2],out[2][1]); + 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 { @@ -1264,18 +1264,18 @@ bool SolveInverseQuadraticMonotonic( float x1, float y1, float x2, float y2, flo // first, sort parameters if (x1>x2) { - swap(x1,x2); - swap(y1,y2); + V_swap(x1,x2); + V_swap(y1,y2); } if (x2>x3) { - swap(x2,x3); - swap(y2,y3); + V_swap(x2,x3); + V_swap(y2,y3); } if (x1>x2) { - swap(x1,x2); - swap(y1,y2); + 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 diff --git a/mathlib/mathlib_base.cpp.orig b/mathlib/mathlib_base.cpp.orig new file mode 100644 index 00000000..63ba0b60 --- /dev/null +++ b/mathlib/mathlib_base.cpp.orig @@ -0,0 +1,4071 @@ +//===== Copyright © 1996-2005, Valve Corporation, All rights reserved. ======// +// +// Purpose: Math primitives. +// +//===========================================================================// + +/// FIXME: As soon as all references to mathlib.c are gone, include it in here + +#include +#include // Needed for FLT_EPSILON + +#include "tier0/basetypes.h" +#include +#include "tier0/dbg.h" + +#include "tier0/vprof.h" +//#define _VPROF_MATHLIB + +#ifdef _MSC_VER +#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" +#endif + +#include "mathlib/mathlib.h" +#include "mathlib/vector.h" +#if !defined( _X360 ) +#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; +} + + +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]; +} + + +/* +================ +R_ConcatTransforms +================ +*/ +void ConcatTransforms (const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out) +{ + Assert( s_bMathlibInitialized ); + if ( &in1 == &out ) + { + matrix3x4_t in1b; + MatrixCopy( in1, in1b ); + ConcatTransforms( in1b, in2, out ); + return; + } + if ( &in2 == &out ) + { + matrix3x4_t in2b; + MatrixCopy( in2, in2b ); + ConcatTransforms( in1, in2b, out ); + return; + } + 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[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + + in1[0][2] * in2[2][3] + in1[0][3]; + 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[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + + in1[1][2] * in2[2][3] + in1[1][3]; + 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]; + out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + + in1[2][2] * in2[2][3] + in1[2][3]; +} + + +/* +=================== +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 (&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<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=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)) + } + return pow( x, lastExponent ); +} + + +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 ) +{ + return (1 - cos( x * M_PI )) * 0.5f; +} + + +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 ); + + float sinang = 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 ) +{ + 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 ); + 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 +#ifdef _MSC_VER +#pragma optimize( "g", off ) +#endif + +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 ); +} + +#ifdef _MSC_VER +#pragma optimize( "", on ) +#endif + +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, 1 ); + 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, 1 ); + 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; +} + +#ifdef _MSC_VER +#pragma optimize( "", off ) +#endif + +#ifndef EXCEPTION_EXECUTE_HANDLER +#define EXCEPTION_EXECUTE_HANDLER 1 +#endif + +#ifdef _MSC_VER +#pragma optimize( "", on ) +#endif + +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; + } + + s_b3DNowEnabled = false; + + if ( bAllowSSE && pi.m_bSSE ) + { + s_bSSEEnabled = true; + + // Select the SSE specific routines if available + pfVectorNormalize = _VectorNormalize; + pfVectorNormalizeFast = _SSE_VectorNormalizeFast; + pfInvRSquared = _SSE_InvRSquared; + pfSqrt = _SSE_Sqrt; + pfRSqrt = _SSE_RSqrtAccurate; + pfRSqrtFast = _SSE_RSqrtFast; +#ifdef _WIN32 + pfFastSinCos = _SSE_SinCos; + pfFastCos = _SSE_cos; +#endif + } + else + { + s_bSSEEnabled = false; + } + + if ( bAllowSSE2 && pi.m_bSSE2 ) + { + s_bSSE2Enabled = true; +#ifdef _WIN32 + pfFastSinCos = _SSE2_SinCos; + pfFastCos = _SSE2_cos; +#endif + } + else + { + s_bSSE2Enabled = false; + } +#endif + + 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 = static_cast(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; +} diff --git a/mathlib/mathlib_base.cpp.rej b/mathlib/mathlib_base.cpp.rej new file mode 100644 index 00000000..dcb256f4 --- /dev/null +++ b/mathlib/mathlib_base.cpp.rej @@ -0,0 +1,25 @@ +--- mathlib/mathlib_base.cpp ++++ mathlib/mathlib_base.cpp +@@ -1653,19 +1653,19 @@ void MatrixCopy( const matrix3x4_t& in, matrix3x4_t& out ) + memcpy( out.Base(), in.Base(), sizeof( float ) * 3 * 4 ); + } + + 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]); ++ 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 + { + // I'm guessing this only works on a 3x4 orthonormal matrix + out[0][0] = in[0][0]; + out[0][1] = in[1][0]; + out[0][2] = in[2][0]; + diff --git a/public/mathlib/mathlib.h b/public/mathlib/mathlib.h index 80acf3a6..f6beed03 100644 --- a/public/mathlib/mathlib.h +++ b/public/mathlib/mathlib.h @@ -608,7 +608,7 @@ template<> FORCEINLINE QAngleByValue Lerp( float flPercent, const // Swap two of anything. template -FORCEINLINE void swap( T& x, T& y ) +FORCEINLINE void V_swap( T& x, T& y ) { T temp = x; x = y; diff --git a/public/particles/particles.h b/public/particles/particles.h index 45cb145f..6078a636 100644 --- a/public/particles/particles.h +++ b/public/particles/particles.h @@ -1452,7 +1452,7 @@ inline void CParticleCollection::SwapPosAndPrevPos( void ) { // strides better be the same! Assert( m_nParticleFloatStrides[PARTICLE_ATTRIBUTE_XYZ] == m_nParticleFloatStrides[ PARTICLE_ATTRIBUTE_PREV_XYZ ] ); - swap( m_pParticleAttributes[ PARTICLE_ATTRIBUTE_XYZ ], m_pParticleAttributes[ PARTICLE_ATTRIBUTE_PREV_XYZ ] ); + V_swap( m_pParticleAttributes[ PARTICLE_ATTRIBUTE_XYZ ], m_pParticleAttributes[ PARTICLE_ATTRIBUTE_PREV_XYZ ] ); } inline void CParticleCollection::LoanKillListTo( CParticleCollection *pBorrower ) const diff --git a/public/tier1/utlblockmemory.h b/public/tier1/utlblockmemory.h index 7f08c40b..19b34c72 100644 --- a/public/tier1/utlblockmemory.h +++ b/public/tier1/utlblockmemory.h @@ -137,10 +137,10 @@ CUtlBlockMemory::~CUtlBlockMemory() template< class T, class I > void CUtlBlockMemory::Swap( CUtlBlockMemory< T, I > &mem ) { - swap( m_pMemory, mem.m_pMemory ); - swap( m_nBlocks, mem.m_nBlocks ); - swap( m_nIndexMask, mem.m_nIndexMask ); - swap( m_nIndexShift, mem.m_nIndexShift ); + V_swap( m_pMemory, mem.m_pMemory ); + V_swap( m_nBlocks, mem.m_nBlocks ); + V_swap( m_nIndexMask, mem.m_nIndexMask ); + V_swap( m_nIndexShift, mem.m_nIndexShift ); } diff --git a/public/tier1/utlfixedmemory.h b/public/tier1/utlfixedmemory.h index c5587f07..bb84844d 100644 --- a/public/tier1/utlfixedmemory.h +++ b/public/tier1/utlfixedmemory.h @@ -181,9 +181,9 @@ CUtlFixedMemory::~CUtlFixedMemory() template< class T > void CUtlFixedMemory::Swap( CUtlFixedMemory< T > &mem ) { - swap( m_pBlocks, mem.m_pBlocks ); - swap( m_nAllocationCount, mem.m_nAllocationCount ); - swap( m_nGrowSize, mem.m_nGrowSize ); + V_swap( m_pBlocks, mem.m_pBlocks ); + V_swap( m_nAllocationCount, mem.m_nAllocationCount ); + V_swap( m_nGrowSize, mem.m_nGrowSize ); } diff --git a/public/tier1/utlmemory.h b/public/tier1/utlmemory.h index 2d494412..232efcfc 100644 --- a/public/tier1/utlmemory.h +++ b/public/tier1/utlmemory.h @@ -328,9 +328,9 @@ void CUtlMemory::Init( int nGrowSize /*= 0*/, int nInitSize /*= 0*/ ) template< class T, class I > void CUtlMemory::Swap( CUtlMemory &mem ) { - swap( m_nGrowSize, mem.m_nGrowSize ); - swap( m_pMemory, mem.m_pMemory ); - swap( m_nAllocationCount, mem.m_nAllocationCount ); + V_swap( m_nGrowSize, mem.m_nGrowSize ); + V_swap( m_pMemory, mem.m_pMemory ); + V_swap( m_nAllocationCount, mem.m_nAllocationCount ); } diff --git a/public/tier1/utlrbtree.h b/public/tier1/utlrbtree.h index 1e401aaf..b9b49eb8 100644 --- a/public/tier1/utlrbtree.h +++ b/public/tier1/utlrbtree.h @@ -1547,12 +1547,12 @@ template < class T, class I, typename L, class M > void CUtlRBTree::Swap( CUtlRBTree< T, I, L > &that ) { m_Elements.Swap( that.m_Elements ); - swap( m_LessFunc, that.m_LessFunc ); - swap( m_Root, that.m_Root ); - swap( m_NumElements, that.m_NumElements ); - swap( m_FirstFree, that.m_FirstFree ); - swap( m_pElements, that.m_pElements ); - swap( m_LastAlloc, that.m_LastAlloc ); + V_swap( m_LessFunc, that.m_LessFunc ); + V_swap( m_Root, that.m_Root ); + V_swap( m_NumElements, that.m_NumElements ); + V_swap( m_FirstFree, that.m_FirstFree ); + V_swap( m_pElements, that.m_pElements ); + V_swap( m_LastAlloc, that.m_LastAlloc ); Assert( IsValid() ); Assert( m_Elements.IsValidIterator( m_LastAlloc ) || ( m_NumElements == 0 && m_FirstFree == InvalidIndex() ) ); } diff --git a/public/tier1/utlvector.h b/public/tier1/utlvector.h index 3d8b621a..9b04d00d 100644 --- a/public/tier1/utlvector.h +++ b/public/tier1/utlvector.h @@ -415,7 +415,7 @@ void CUtlVector::Sort( int (__cdecl *pfnCompare)(const T *, const T *) ) { if ( pfnCompare( &Element( j - 1 ), &Element( j ) ) < 0 ) { - swap( Element( j - 1 ), Element( j ) ); + V_swap( Element( j - 1 ), Element( j ) ); } } } @@ -605,8 +605,8 @@ template< typename T, class A > void CUtlVector::Swap( CUtlVector< T, A > &vec ) { m_Memory.Swap( vec.m_Memory ); - swap( m_Size, vec.m_Size ); - swap( m_pElements, vec.m_pElements ); + V_swap( m_Size, vec.m_Size ); + V_swap( m_pElements, vec.m_pElements ); } template< typename T, class A > diff --git a/utils/common/mstristrip.cpp b/utils/common/mstristrip.cpp index a4abc485..84601017 100644 --- a/utils/common/mstristrip.cpp +++ b/utils/common/mstristrip.cpp @@ -365,7 +365,7 @@ STRIPLIST::iterator FindBestCachedStrip(STRIPLIST *pstriplist, // make sure we keep the list in order and always pull off // the first dude. if(istriplistbest != pstriplist->begin()) - swap(*istriplistbest, *pstriplist->begin()); + V_swap(*istriplistbest, *pstriplist->begin()); return pstriplist->begin(); } diff --git a/utils/vbsp/disp_ivp.cpp b/utils/vbsp/disp_ivp.cpp index 1b17a056..31ae5839 100644 --- a/utils/vbsp/disp_ivp.cpp +++ b/utils/vbsp/disp_ivp.cpp @@ -207,7 +207,7 @@ CDispMeshEvent::CDispMeshEvent( unsigned short *pIndices, int indexCount, CCoreD } for ( int i = 0; i < indexCount/2; i++ ) { - swap( pIndices[i], pIndices[(indexCount-i)-1] ); + V_swap( pIndices[i], pIndices[(indexCount-i)-1] ); } int count = maxIndex + 1; m_verts.SetCount( count );