From f56bb35301836e56582a575a75864392a0177875 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B8rgen=20P=2E=20Tjern=C3=B8?= Date: Mon, 2 Dec 2013 19:31:46 -0800 Subject: Fix line endings. WHAMMY. --- mp/src/mathlib/mathlib_base.cpp | 8586 +++++++++++++++++++-------------------- 1 file changed, 4293 insertions(+), 4293 deletions(-) (limited to 'mp/src/mathlib/mathlib_base.cpp') diff --git a/mp/src/mathlib/mathlib_base.cpp b/mp/src/mathlib/mathlib_base.cpp index fce3547b..a403ccfa 100644 --- a/mp/src/mathlib/mathlib_base.cpp +++ b/mp/src/mathlib/mathlib_base.cpp @@ -1,4293 +1,4293 @@ -//========= Copyright Valve Corporation, All rights reserved. ============// -// -// Purpose: Math primitives. -// -//===========================================================================// - -/// FIXME: As soon as all references to mathlib.c are gone, include it in here - -#include -#include // Needed for FLT_EPSILON - -#include "tier0/basetypes.h" -#include -#include "tier0/dbg.h" - -#include "tier0/vprof.h" -//#define _VPROF_MATHLIB - -#pragma warning(disable:4244) // "conversion from 'const int' to 'float', possible loss of data" -#pragma warning(disable:4730) // "mixing _m64 and floating point expressions may result in incorrect code" - -#include "mathlib/mathlib.h" -#include "mathlib/vector.h" -#if !defined( _X360 ) -#include "mathlib/amd3dx.h" -#ifndef OSX -#include "3dnow.h" -#endif -#include "sse.h" -#endif - -#include "mathlib/ssemath.h" -#include "mathlib/ssequaternion.h" - -// memdbgon must be the last include file in a .cpp file!!! -#include "tier0/memdbgon.h" - -bool s_bMathlibInitialized = false; - -#ifdef PARANOID -// User must provide an implementation of Sys_Error() -void Sys_Error (char *error, ...); -#endif - -const Vector vec3_origin(0,0,0); -const QAngle vec3_angle(0,0,0); -const Vector vec3_invalid( FLT_MAX, FLT_MAX, FLT_MAX ); -const int nanmask = 255<<23; - -//----------------------------------------------------------------------------- -// Standard C implementations of optimized routines: -//----------------------------------------------------------------------------- -float _sqrtf(float _X) -{ - Assert( s_bMathlibInitialized ); - return sqrtf(_X); -} - -float _rsqrtf(float x) -{ - Assert( s_bMathlibInitialized ); - - return 1.f / _sqrtf( x ); -} - -float FASTCALL _VectorNormalize (Vector& vec) -{ -#ifdef _VPROF_MATHLIB - VPROF_BUDGET( "_VectorNormalize", "Mathlib" ); -#endif - Assert( s_bMathlibInitialized ); - float radius = sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z); - - // FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. - float iradius = 1.f / ( radius + FLT_EPSILON ); - - vec.x *= iradius; - vec.y *= iradius; - vec.z *= iradius; - - return radius; -} - -// TODO: Add fast C VectorNormalizeFast. -// Perhaps use approximate rsqrt trick, if the accuracy isn't too bad. -void FASTCALL _VectorNormalizeFast (Vector& vec) -{ - Assert( s_bMathlibInitialized ); - - // FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. - float iradius = 1.f / ( sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z) + FLT_EPSILON ); - - vec.x *= iradius; - vec.y *= iradius; - vec.z *= iradius; - -} - -float _InvRSquared(const float* v) -{ - Assert( s_bMathlibInitialized ); - float r2 = DotProduct(v, v); - return r2 < 1.f ? 1.f : 1/r2; -} - -//----------------------------------------------------------------------------- -// Function pointers selecting the appropriate implementation -//----------------------------------------------------------------------------- -float (*pfSqrt)(float x) = _sqrtf; -float (*pfRSqrt)(float x) = _rsqrtf; -float (*pfRSqrtFast)(float x) = _rsqrtf; -float (FASTCALL *pfVectorNormalize)(Vector& v) = _VectorNormalize; -void (FASTCALL *pfVectorNormalizeFast)(Vector& v) = _VectorNormalizeFast; -float (*pfInvRSquared)(const float* v) = _InvRSquared; -void (*pfFastSinCos)(float x, float* s, float* c) = SinCos; -float (*pfFastCos)(float x) = cosf; - -float SinCosTable[SIN_TABLE_SIZE]; -void InitSinCosTable() -{ - for( int i = 0; i < SIN_TABLE_SIZE; i++ ) - { - SinCosTable[i] = sin(i * 2.0 * M_PI / SIN_TABLE_SIZE); - } -} - -qboolean VectorsEqual( const float *v1, const float *v2 ) -{ - Assert( s_bMathlibInitialized ); - return ( ( v1[0] == v2[0] ) && - ( v1[1] == v2[1] ) && - ( v1[2] == v2[2] ) ); -} - - -//----------------------------------------------------------------------------- -// Purpose: Generates Euler angles given a left-handed orientation matrix. The -// columns of the matrix contain the forward, left, and up vectors. -// Input : matrix - Left-handed orientation matrix. -// angles[PITCH, YAW, ROLL]. Receives right-handed counterclockwise -// rotations in degrees around Y, Z, and X respectively. -//----------------------------------------------------------------------------- - -void MatrixAngles( const matrix3x4_t& matrix, RadianEuler &angles, Vector &position ) -{ - MatrixGetColumn( matrix, 3, position ); - MatrixAngles( matrix, angles ); -} - -void MatrixAngles( const matrix3x4_t &matrix, Quaternion &q, Vector &pos ) -{ -#ifdef _VPROF_MATHLIB - VPROF_BUDGET( "MatrixQuaternion", "Mathlib" ); -#endif - float trace; - trace = matrix[0][0] + matrix[1][1] + matrix[2][2] + 1.0f; - if( trace > 1.0f + FLT_EPSILON ) - { - // VPROF_INCREMENT_COUNTER("MatrixQuaternion A",1); - q.x = ( matrix[2][1] - matrix[1][2] ); - q.y = ( matrix[0][2] - matrix[2][0] ); - q.z = ( matrix[1][0] - matrix[0][1] ); - q.w = trace; - } - else if ( matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2] ) - { - // VPROF_INCREMENT_COUNTER("MatrixQuaternion B",1); - trace = 1.0f + matrix[0][0] - matrix[1][1] - matrix[2][2]; - q.x = trace; - q.y = (matrix[1][0] + matrix[0][1] ); - q.z = (matrix[0][2] + matrix[2][0] ); - q.w = (matrix[2][1] - matrix[1][2] ); - } - else if (matrix[1][1] > matrix[2][2]) - { - // VPROF_INCREMENT_COUNTER("MatrixQuaternion C",1); - trace = 1.0f + matrix[1][1] - matrix[0][0] - matrix[2][2]; - q.x = (matrix[0][1] + matrix[1][0] ); - q.y = trace; - q.z = (matrix[2][1] + matrix[1][2] ); - q.w = (matrix[0][2] - matrix[2][0] ); - } - else - { - // VPROF_INCREMENT_COUNTER("MatrixQuaternion D",1); - trace = 1.0f + matrix[2][2] - matrix[0][0] - matrix[1][1]; - q.x = (matrix[0][2] + matrix[2][0] ); - q.y = (matrix[2][1] + matrix[1][2] ); - q.z = trace; - q.w = (matrix[1][0] - matrix[0][1] ); - } - - QuaternionNormalize( q ); - -#if 0 - // check against the angle version - RadianEuler ang; - MatrixAngles( matrix, ang ); - Quaternion test; - AngleQuaternion( ang, test ); - float d = QuaternionDotProduct( q, test ); - Assert( fabs(d) > 0.99 && fabs(d) < 1.01 ); -#endif - - MatrixGetColumn( matrix, 3, pos ); -} - -void MatrixAngles( const matrix3x4_t& matrix, float *angles ) -{ -#ifdef _VPROF_MATHLIB - VPROF_BUDGET( "MatrixAngles", "Mathlib" ); -#endif - Assert( s_bMathlibInitialized ); - float forward[3]; - float left[3]; - float up[3]; - - // - // Extract the basis vectors from the matrix. Since we only need the Z - // component of the up vector, we don't get X and Y. - // - forward[0] = matrix[0][0]; - forward[1] = matrix[1][0]; - forward[2] = matrix[2][0]; - left[0] = matrix[0][1]; - left[1] = matrix[1][1]; - left[2] = matrix[2][1]; - up[2] = matrix[2][2]; - - float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); - - // enough here to get angles? - if ( xyDist > 0.001f ) - { - // (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis - angles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); - - // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); - angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); - - // (roll) z = ATAN( left.z, up.z ); - angles[2] = RAD2DEG( atan2f( left[2], up[2] ) ); - } - else // forward is mostly Z, gimbal lock- - { - // (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw - angles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); - - // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); - angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); - - // Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) - angles[2] = 0; - } -} - - -// transform in1 by the matrix in2 -void VectorTransform (const float *in1, const matrix3x4_t& in2, float *out) -{ - Assert( s_bMathlibInitialized ); - Assert( in1 != out ); - out[0] = DotProduct(in1, in2[0]) + in2[0][3]; - out[1] = DotProduct(in1, in2[1]) + in2[1][3]; - out[2] = DotProduct(in1, in2[2]) + in2[2][3]; -} - - -// assuming the matrix is orthonormal, transform in1 by the transpose (also the inverse in this case) of in2. -void VectorITransform (const float *in1, const matrix3x4_t& in2, float *out) -{ - Assert( s_bMathlibInitialized ); - float in1t[3]; - - in1t[0] = in1[0] - in2[0][3]; - in1t[1] = in1[1] - in2[1][3]; - in1t[2] = in1[2] - in2[2][3]; - - out[0] = in1t[0] * in2[0][0] + in1t[1] * in2[1][0] + in1t[2] * in2[2][0]; - out[1] = in1t[0] * in2[0][1] + in1t[1] * in2[1][1] + in1t[2] * in2[2][1]; - out[2] = in1t[0] * in2[0][2] + in1t[1] * in2[1][2] + in1t[2] * in2[2][2]; -} - - -// assume in2 is a rotation and rotate the input vector -void VectorRotate( const float *in1, const matrix3x4_t& in2, float *out ) -{ - Assert( s_bMathlibInitialized ); - Assert( in1 != out ); - out[0] = DotProduct( in1, in2[0] ); - out[1] = DotProduct( in1, in2[1] ); - out[2] = DotProduct( in1, in2[2] ); -} - -// assume in2 is a rotation and rotate the input vector -void VectorRotate( const Vector &in1, const QAngle &in2, Vector &out ) -{ - matrix3x4_t matRotate; - AngleMatrix( in2, matRotate ); - VectorRotate( in1, matRotate, out ); -} - -// assume in2 is a rotation and rotate the input vector -void VectorRotate( const Vector &in1, const Quaternion &in2, Vector &out ) -{ - matrix3x4_t matRotate; - QuaternionMatrix( in2, matRotate ); - VectorRotate( in1, matRotate, out ); -} - - -// rotate by the inverse of the matrix -void VectorIRotate( const float *in1, const matrix3x4_t& in2, float *out ) -{ - Assert( s_bMathlibInitialized ); - Assert( in1 != out ); - out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0]; - out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1]; - out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2]; -} - -#ifndef VECTOR_NO_SLOW_OPERATIONS -// transform a set of angles in the output space of parentMatrix to the input space -QAngle TransformAnglesToLocalSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) -{ - matrix3x4_t angToWorld, worldToParent, localMatrix; - MatrixInvert( parentMatrix, worldToParent ); - AngleMatrix( angles, angToWorld ); - ConcatTransforms( worldToParent, angToWorld, localMatrix ); - - QAngle out; - MatrixAngles( localMatrix, out ); - return out; -} - -// transform a set of angles in the input space of parentMatrix to the output space -QAngle TransformAnglesToWorldSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) -{ - matrix3x4_t angToParent, angToWorld; - AngleMatrix( angles, angToParent ); - ConcatTransforms( parentMatrix, angToParent, angToWorld ); - QAngle out; - MatrixAngles( angToWorld, out ); - return out; -} - -#endif // VECTOR_NO_SLOW_OPERATIONS - -void MatrixInitialize( matrix3x4_t &mat, const Vector &vecOrigin, const Vector &vecXAxis, const Vector &vecYAxis, const Vector &vecZAxis ) -{ - MatrixSetColumn( vecXAxis, 0, mat ); - MatrixSetColumn( vecYAxis, 1, mat ); - MatrixSetColumn( vecZAxis, 2, mat ); - MatrixSetColumn( vecOrigin, 3, mat ); -} - -void MatrixCopy( const matrix3x4_t& in, matrix3x4_t& out ) -{ - Assert( s_bMathlibInitialized ); - memcpy( out.Base(), in.Base(), sizeof( float ) * 3 * 4 ); -} - -//----------------------------------------------------------------------------- -// Matrix equality test -//----------------------------------------------------------------------------- -bool MatricesAreEqual( const matrix3x4_t &src1, const matrix3x4_t &src2, float flTolerance ) -{ - for ( int i = 0; i < 3; ++i ) - { - for ( int j = 0; j < 4; ++j ) - { - if ( fabs( src1[i][j] - src2[i][j] ) > flTolerance ) - return false; - } - } - return true; -} - -// NOTE: This is just the transpose not a general inverse -void MatrixInvert( const matrix3x4_t& in, matrix3x4_t& out ) -{ - Assert( s_bMathlibInitialized ); - if ( &in == &out ) - { - V_swap(out[0][1],out[1][0]); - V_swap(out[0][2],out[2][0]); - V_swap(out[1][2],out[2][1]); - } - else - { - // transpose the matrix - out[0][0] = in[0][0]; - out[0][1] = in[1][0]; - out[0][2] = in[2][0]; - - out[1][0] = in[0][1]; - out[1][1] = in[1][1]; - out[1][2] = in[2][1]; - - out[2][0] = in[0][2]; - out[2][1] = in[1][2]; - out[2][2] = in[2][2]; - } - - // now fix up the translation to be in the other space - float tmp[3]; - tmp[0] = in[0][3]; - tmp[1] = in[1][3]; - tmp[2] = in[2][3]; - - out[0][3] = -DotProduct( tmp, out[0] ); - out[1][3] = -DotProduct( tmp, out[1] ); - out[2][3] = -DotProduct( tmp, out[2] ); -} - -void MatrixGetColumn( const matrix3x4_t& in, int column, Vector &out ) -{ - out.x = in[0][column]; - out.y = in[1][column]; - out.z = in[2][column]; -} - -void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out ) -{ - out[0][column] = in.x; - out[1][column] = in.y; - out[2][column] = in.z; -} - -void MatrixScaleBy ( const float flScale, matrix3x4_t &out ) -{ - out[0][0] *= flScale; - out[1][0] *= flScale; - out[2][0] *= flScale; - out[0][1] *= flScale; - out[1][1] *= flScale; - out[2][1] *= flScale; - out[0][2] *= flScale; - out[1][2] *= flScale; - out[2][2] *= flScale; -} - -void MatrixScaleByZero ( matrix3x4_t &out ) -{ - out[0][0] = 0.0f; - out[1][0] = 0.0f; - out[2][0] = 0.0f; - out[0][1] = 0.0f; - out[1][1] = 0.0f; - out[2][1] = 0.0f; - out[0][2] = 0.0f; - out[1][2] = 0.0f; - out[2][2] = 0.0f; -} - - - -int VectorCompare (const float *v1, const float *v2) -{ - Assert( s_bMathlibInitialized ); - int i; - - for (i=0 ; i<3 ; i++) - if (v1[i] != v2[i]) - return 0; - - return 1; -} - -void CrossProduct (const float* v1, const float* v2, float* cross) -{ - Assert( s_bMathlibInitialized ); - Assert( v1 != cross ); - Assert( v2 != cross ); - cross[0] = v1[1]*v2[2] - v1[2]*v2[1]; - cross[1] = v1[2]*v2[0] - v1[0]*v2[2]; - cross[2] = v1[0]*v2[1] - v1[1]*v2[0]; -} - -int Q_log2(int val) -{ - int answer=0; - while (val>>=1) - answer++; - return answer; -} - -// Matrix is right-handed x=forward, y=left, z=up. We a left-handed convention for vectors in the game code (forward, right, up) -void MatrixVectors( const matrix3x4_t &matrix, Vector* pForward, Vector *pRight, Vector *pUp ) -{ - MatrixGetColumn( matrix, 0, *pForward ); - MatrixGetColumn( matrix, 1, *pRight ); - MatrixGetColumn( matrix, 2, *pUp ); - *pRight *= -1.0f; -} - - -void VectorVectors( const Vector &forward, Vector &right, Vector &up ) -{ - Assert( s_bMathlibInitialized ); - Vector tmp; - - if (forward[0] == 0 && forward[1] == 0) - { - // pitch 90 degrees up/down from identity - right[0] = 0; - right[1] = -1; - right[2] = 0; - up[0] = -forward[2]; - up[1] = 0; - up[2] = 0; - } - else - { - tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0; - CrossProduct( forward, tmp, right ); - VectorNormalize( right ); - CrossProduct( right, forward, up ); - VectorNormalize( up ); - } -} - -void VectorMatrix( const Vector &forward, matrix3x4_t& matrix) -{ - Assert( s_bMathlibInitialized ); - Vector right, up; - VectorVectors(forward, right, up); - - MatrixSetColumn( forward, 0, matrix ); - MatrixSetColumn( -right, 1, matrix ); - MatrixSetColumn( up, 2, matrix ); -} - - -void VectorAngles( const float *forward, float *angles ) -{ - Assert( s_bMathlibInitialized ); - float tmp, yaw, pitch; - - if (forward[1] == 0 && forward[0] == 0) - { - yaw = 0; - if (forward[2] > 0) - pitch = 270; - else - pitch = 90; - } - else - { - yaw = (atan2(forward[1], forward[0]) * 180 / M_PI); - if (yaw < 0) - yaw += 360; - - tmp = sqrt (forward[0]*forward[0] + forward[1]*forward[1]); - pitch = (atan2(-forward[2], tmp) * 180 / M_PI); - if (pitch < 0) - pitch += 360; - } - - angles[0] = pitch; - angles[1] = yaw; - angles[2] = 0; -} - - -/* -================ -R_ConcatRotations -================ -*/ -void ConcatRotations (const float in1[3][3], const float in2[3][3], float out[3][3]) -{ - Assert( s_bMathlibInitialized ); - Assert( in1 != out ); - Assert( in2 != out ); - out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + - in1[0][2] * in2[2][0]; - out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + - in1[0][2] * in2[2][1]; - out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + - in1[0][2] * in2[2][2]; - out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + - in1[1][2] * in2[2][0]; - out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + - in1[1][2] * in2[2][1]; - out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + - in1[1][2] * in2[2][2]; - out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + - in1[2][2] * in2[2][0]; - out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + - in1[2][2] * in2[2][1]; - out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + - in1[2][2] * in2[2][2]; -} - -void ConcatTransforms_Aligned( const matrix3x4_t &m0, const matrix3x4_t &m1, matrix3x4_t &out ) -{ - Assert( (((size_t)&m0) % 16) == 0 ); - Assert( (((size_t)&m1) % 16) == 0 ); - Assert( (((size_t)&out) % 16) == 0 ); - - fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); - fltx4 rowA0 = LoadAlignedSIMD( m0.m_flMatVal[0] ); - fltx4 rowA1 = LoadAlignedSIMD( m0.m_flMatVal[1] ); - fltx4 rowA2 = LoadAlignedSIMD( m0.m_flMatVal[2] ); - - fltx4 rowB0 = LoadAlignedSIMD( m1.m_flMatVal[0] ); - fltx4 rowB1 = LoadAlignedSIMD( m1.m_flMatVal[1] ); - fltx4 rowB2 = LoadAlignedSIMD( m1.m_flMatVal[2] ); - - // now we have the rows of m0 and the columns of m1 - // first output row - fltx4 A0 = SplatXSIMD(rowA0); - fltx4 A1 = SplatYSIMD(rowA0); - fltx4 A2 = SplatZSIMD(rowA0); - fltx4 mul00 = MulSIMD( A0, rowB0 ); - fltx4 mul01 = MulSIMD( A1, rowB1 ); - fltx4 mul02 = MulSIMD( A2, rowB2 ); - fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); - - // second output row - A0 = SplatXSIMD(rowA1); - A1 = SplatYSIMD(rowA1); - A2 = SplatZSIMD(rowA1); - fltx4 mul10 = MulSIMD( A0, rowB0 ); - fltx4 mul11 = MulSIMD( A1, rowB1 ); - fltx4 mul12 = MulSIMD( A2, rowB2 ); - fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); - - // third output row - A0 = SplatXSIMD(rowA2); - A1 = SplatYSIMD(rowA2); - A2 = SplatZSIMD(rowA2); - fltx4 mul20 = MulSIMD( A0, rowB0 ); - fltx4 mul21 = MulSIMD( A1, rowB1 ); - fltx4 mul22 = MulSIMD( A2, rowB2 ); - fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); - - // add in translation vector - A0 = AndSIMD(rowA0,lastMask); - A1 = AndSIMD(rowA1,lastMask); - A2 = AndSIMD(rowA2,lastMask); - out0 = AddSIMD(out0, A0); - out1 = AddSIMD(out1, A1); - out2 = AddSIMD(out2, A2); - - StoreAlignedSIMD( out.m_flMatVal[0], out0 ); - StoreAlignedSIMD( out.m_flMatVal[1], out1 ); - StoreAlignedSIMD( out.m_flMatVal[2], out2 ); -} - -/* -================ -R_ConcatTransforms -================ -*/ - -void ConcatTransforms (const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out) -{ -#if 0 - // test for ones that'll be 2x faster - if ( (((size_t)&in1) % 16) == 0 && (((size_t)&in2) % 16) == 0 && (((size_t)&out) % 16) == 0 ) - { - ConcatTransforms_Aligned( in1, in2, out ); - return; - } -#endif - - fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); - fltx4 rowA0 = LoadUnalignedSIMD( in1.m_flMatVal[0] ); - fltx4 rowA1 = LoadUnalignedSIMD( in1.m_flMatVal[1] ); - fltx4 rowA2 = LoadUnalignedSIMD( in1.m_flMatVal[2] ); - - fltx4 rowB0 = LoadUnalignedSIMD( in2.m_flMatVal[0] ); - fltx4 rowB1 = LoadUnalignedSIMD( in2.m_flMatVal[1] ); - fltx4 rowB2 = LoadUnalignedSIMD( in2.m_flMatVal[2] ); - - // now we have the rows of m0 and the columns of m1 - // first output row - fltx4 A0 = SplatXSIMD(rowA0); - fltx4 A1 = SplatYSIMD(rowA0); - fltx4 A2 = SplatZSIMD(rowA0); - fltx4 mul00 = MulSIMD( A0, rowB0 ); - fltx4 mul01 = MulSIMD( A1, rowB1 ); - fltx4 mul02 = MulSIMD( A2, rowB2 ); - fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); - - // second output row - A0 = SplatXSIMD(rowA1); - A1 = SplatYSIMD(rowA1); - A2 = SplatZSIMD(rowA1); - fltx4 mul10 = MulSIMD( A0, rowB0 ); - fltx4 mul11 = MulSIMD( A1, rowB1 ); - fltx4 mul12 = MulSIMD( A2, rowB2 ); - fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); - - // third output row - A0 = SplatXSIMD(rowA2); - A1 = SplatYSIMD(rowA2); - A2 = SplatZSIMD(rowA2); - fltx4 mul20 = MulSIMD( A0, rowB0 ); - fltx4 mul21 = MulSIMD( A1, rowB1 ); - fltx4 mul22 = MulSIMD( A2, rowB2 ); - fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); - - // add in translation vector - A0 = AndSIMD(rowA0,lastMask); - A1 = AndSIMD(rowA1,lastMask); - A2 = AndSIMD(rowA2,lastMask); - out0 = AddSIMD(out0, A0); - out1 = AddSIMD(out1, A1); - out2 = AddSIMD(out2, A2); - - // write to output - StoreUnalignedSIMD( out.m_flMatVal[0], out0 ); - StoreUnalignedSIMD( out.m_flMatVal[1], out1 ); - StoreUnalignedSIMD( out.m_flMatVal[2], out2 ); -} - - -/* -=================== -FloorDivMod - -Returns mathematically correct (floor-based) quotient and remainder for -numer and denom, both of which should contain no fractional part. The -quotient must fit in 32 bits. -==================== -*/ - -void FloorDivMod (double numer, double denom, int *quotient, - int *rem) -{ - Assert( s_bMathlibInitialized ); - int q, r; - double x; - -#ifdef PARANOID - if (denom <= 0.0) - Sys_Error ("FloorDivMod: bad denominator %d\n", denom); - -// if ((floor(numer) != numer) || (floor(denom) != denom)) -// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n", -// numer, denom); -#endif - - if (numer >= 0.0) - { - - x = floor(numer / denom); - q = (int)x; - r = Floor2Int(numer - (x * denom)); - } - else - { - // - // perform operations with positive values, and fix mod to make floor-based - // - x = floor(-numer / denom); - q = -(int)x; - r = Floor2Int(-numer - (x * denom)); - if (r != 0) - { - q--; - r = (int)denom - r; - } - } - - *quotient = q; - *rem = r; -} - - -/* -=================== -GreatestCommonDivisor -==================== -*/ -int GreatestCommonDivisor (int i1, int i2) -{ - Assert( s_bMathlibInitialized ); - if (i1 > i2) - { - if (i2 == 0) - return (i1); - return GreatestCommonDivisor (i2, i1 % i2); - } - else - { - if (i1 == 0) - return (i2); - return GreatestCommonDivisor (i1, i2 % i1); - } -} - - -bool IsDenormal( const float &val ) -{ - const int x = *reinterpret_cast (&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 ); - - // Note if the quaternion is slightly non-normalized the square root below may be more than 1, - // the value is clamped to one otherwise it may result in asin() returning an undefined result. - float sinang = MIN( 1.0f, sqrt( diff.x * diff.x + diff.y * diff.y + diff.z * diff.z ) ); - float angle = RAD2DEG( 2 * asin( sinang ) ); - return angle; -#else - Quaternion q2; - QuaternionAlign( p, q, q2 ); - - Assert( s_bMathlibInitialized ); - float cosom = p.x * q2.x + p.y * q2.y + p.z * q2.z + p.w * q2.w; - - if ( cosom > -1.0f ) - { - if ( cosom < 1.0f ) - { - float omega = 2 * fabs( acos( cosom ) ); - return RAD2DEG( omega ); - } - return 0.0f; - } - - return 180.0f; -#endif -} - -void QuaternionConjugate( const Quaternion &p, Quaternion &q ) -{ - Assert( s_bMathlibInitialized ); - Assert( q.IsValid() ); - - q.x = -p.x; - q.y = -p.y; - q.z = -p.z; - q.w = p.w; -} - -void QuaternionInvert( const Quaternion &p, Quaternion &q ) -{ - Assert( s_bMathlibInitialized ); - Assert( q.IsValid() ); - - QuaternionConjugate( p, q ); - - float magnitudeSqr = QuaternionDotProduct( p, p ); - Assert( magnitudeSqr ); - if ( magnitudeSqr ) - { - float inv = 1.0f / magnitudeSqr; - q.x *= inv; - q.y *= inv; - q.z *= inv; - q.w *= inv; - } -} - -//----------------------------------------------------------------------------- -// Make sure the quaternion is of unit length -//----------------------------------------------------------------------------- -float QuaternionNormalize( Quaternion &q ) -{ - Assert( s_bMathlibInitialized ); - float radius, iradius; - - Assert( q.IsValid() ); - - radius = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; - - if ( radius ) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON)) - { - radius = sqrt(radius); - iradius = 1.0f/radius; - q[3] *= iradius; - q[2] *= iradius; - q[1] *= iradius; - q[0] *= iradius; - } - return radius; -} - - -void QuaternionScale( const Quaternion &p, float t, Quaternion &q ) -{ - Assert( s_bMathlibInitialized ); - -#if 0 - Quaternion p0; - Quaternion q; - p0.Init( 0.0, 0.0, 0.0, 1.0 ); - - // slerp in "reverse order" so that p doesn't get realigned - QuaternionSlerp( p, p0, 1.0 - fabs( t ), q ); - if (t < 0.0) - { - q.w = -q.w; - } -#else - float r; - - // FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to - // use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale. - float sinom = sqrt( DotProduct( &p.x, &p.x ) ); - sinom = min( sinom, 1.f ); - - float sinsom = sin( asin( sinom ) * t ); - - t = sinsom / (sinom + FLT_EPSILON); - VectorScale( &p.x, t, &q.x ); - - // rescale rotation - r = 1.0f - sinsom * sinsom; - - // Assert( r >= 0 ); - if (r < 0.0f) - r = 0.0f; - r = sqrt( r ); - - // keep sign of rotation - if (p.w < 0) - q.w = -r; - else - q.w = r; -#endif - - Assert( q.IsValid() ); - - return; -} - - -void QuaternionAdd( const Quaternion &p, const Quaternion &q, Quaternion &qt ) -{ - Assert( s_bMathlibInitialized ); - Assert( p.IsValid() ); - Assert( q.IsValid() ); - - // decide if one of the quaternions is backwards - Quaternion q2; - QuaternionAlign( p, q, q2 ); - - // is this right??? - qt[0] = p[0] + q2[0]; - qt[1] = p[1] + q2[1]; - qt[2] = p[2] + q2[2]; - qt[3] = p[3] + q2[3]; - - return; -} - - -float QuaternionDotProduct( const Quaternion &p, const Quaternion &q ) -{ - Assert( s_bMathlibInitialized ); - Assert( p.IsValid() ); - Assert( q.IsValid() ); - - return p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w; -} - - -// qt = p * q -void QuaternionMult( const Quaternion &p, const Quaternion &q, Quaternion &qt ) -{ - Assert( s_bMathlibInitialized ); - Assert( p.IsValid() ); - Assert( q.IsValid() ); - - if (&p == &qt) - { - Quaternion p2 = p; - QuaternionMult( p2, q, qt ); - return; - } - - // decide if one of the quaternions is backwards - Quaternion q2; - QuaternionAlign( p, q, q2 ); - - qt.x = p.x * q2.w + p.y * q2.z - p.z * q2.y + p.w * q2.x; - qt.y = -p.x * q2.z + p.y * q2.w + p.z * q2.x + p.w * q2.y; - qt.z = p.x * q2.y - p.y * q2.x + p.z * q2.w + p.w * q2.z; - qt.w = -p.x * q2.x - p.y * q2.y - p.z * q2.z + p.w * q2.w; -} - - -void QuaternionMatrix( const Quaternion &q, const Vector &pos, matrix3x4_t& matrix ) -{ - 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 -#pragma optimize( "g", off ) - -void Hermite_Spline( const Vector &p0, const Vector &p1, const Vector &p2, float t, Vector& output ) -{ - Vector e10, e21; - VectorSubtract( p1, p0, e10 ); - VectorSubtract( p2, p1, e21 ); - Hermite_Spline( p1, p2, e10, e21, t, output ); -} - -#pragma optimize( "", on ) - -float Hermite_Spline( float p0, float p1, float p2, float t ) -{ - return Hermite_Spline( p1, p2, p1 - p0, p2 - p1, t ); -} - - -void Hermite_Spline( const Quaternion &q0, const Quaternion &q1, const Quaternion &q2, float t, Quaternion &output ) -{ - // cheap, hacked version of quaternions - Quaternion q0a; - Quaternion q1a; - - QuaternionAlign( q2, q0, q0a ); - QuaternionAlign( q2, q1, q1a ); - - output.x = Hermite_Spline( q0a.x, q1a.x, q2.x, t ); - output.y = Hermite_Spline( q0a.y, q1a.y, q2.y, t ); - output.z = Hermite_Spline( q0a.z, q1a.z, q2.z, t ); - output.w = Hermite_Spline( q0a.w, q1a.w, q2.w, t ); - - QuaternionNormalize( output ); -} - -// See http://en.wikipedia.org/wiki/Kochanek-Bartels_curves -// -// Tension: -1 = Round -> 1 = Tight -// Bias: -1 = Pre-shoot (bias left) -> 1 = Post-shoot (bias right) -// Continuity: -1 = Box corners -> 1 = Inverted corners -// -// If T=B=C=0 it's the same matrix as Catmull-Rom. -// If T=1 & B=C=0 it's the same as Cubic. -// If T=B=0 & C=-1 it's just linear interpolation -// -// See http://news.povray.org/povray.binaries.tutorials/attachment/%3CXns91B880592482seed7@povray.org%3E/Splines.bas.txt -// for example code and descriptions of various spline types... -// -void Kochanek_Bartels_Spline( - float tension, - float bias, - float continuity, - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Assert( s_bMathlibInitialized ); - - float ffa, ffb, ffc, ffd; - - ffa = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f + bias ); - ffb = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f - bias ); - ffc = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f + bias ); - ffd = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f - bias ); - - float tSqr = t*t*0.5f; - float tSqrSqr = t*tSqr; - t *= 0.5f; - - Assert( &output != &p1 ); - Assert( &output != &p2 ); - Assert( &output != &p3 ); - Assert( &output != &p4 ); - - output.Init(); - - Vector a, b, c, d; - - // matrix row 1 - VectorScale( p1, tSqrSqr * -ffa, a ); - VectorScale( p2, tSqrSqr * ( 4.0f + ffa - ffb - ffc ), b ); - VectorScale( p3, tSqrSqr * ( -4.0f + ffb + ffc - ffd ), c ); - VectorScale( p4, tSqrSqr * ffd, d ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - VectorAdd( d, output, output ); - - // matrix row 2 - VectorScale( p1, tSqr* 2 * ffa, a ); - VectorScale( p2, tSqr * ( -6 - 2 * ffa + 2 * ffb + ffc ), b ); - VectorScale( p3, tSqr * ( 6 - 2 * ffb - ffc + ffd ), c ); - VectorScale( p4, tSqr * -ffd, d ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - VectorAdd( d, output, output ); - - // matrix row 3 - VectorScale( p1, t * -ffa, a ); - VectorScale( p2, t * ( ffa - ffb ), b ); - VectorScale( p3, t * ffb, c ); - // p4 unchanged - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - - // matrix row 4 - // p1, p3, p4 unchanged - // p2 is multiplied by 1 and added, so just added it directly - - VectorAdd( p2, output, output ); -} - -void Kochanek_Bartels_Spline_NormalizeX( - float tension, - float bias, - float continuity, - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Vector p1n, p4n; - Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); - Kochanek_Bartels_Spline( tension, bias, continuity, p1n, p2, p3, p4n, t, output ); -} - -void Cubic_Spline( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Assert( s_bMathlibInitialized ); - - float tSqr = t*t; - float tSqrSqr = t*tSqr; - - Assert( &output != &p1 ); - Assert( &output != &p2 ); - Assert( &output != &p3 ); - Assert( &output != &p4 ); - - output.Init(); - - Vector a, b, c, d; - - // matrix row 1 - VectorScale( p2, tSqrSqr * 2, b ); - VectorScale( p3, tSqrSqr * -2, c ); - - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - - // matrix row 2 - VectorScale( p2, tSqr * -3, b ); - VectorScale( p3, tSqr * 3, c ); - - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - - // matrix row 3 - // no influence - // p4 unchanged - - // matrix row 4 - // p1, p3, p4 unchanged - VectorAdd( p2, output, output ); -} - -void Cubic_Spline_NormalizeX( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Vector p1n, p4n; - Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); - Cubic_Spline( p1n, p2, p3, p4n, t, output ); -} - -void BSpline( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Assert( s_bMathlibInitialized ); - - float oneOver6 = 1.0f / 6.0f; - - float tSqr = t * t * oneOver6; - float tSqrSqr = t*tSqr; - t *= oneOver6; - - Assert( &output != &p1 ); - Assert( &output != &p2 ); - Assert( &output != &p3 ); - Assert( &output != &p4 ); - - output.Init(); - - Vector a, b, c, d; - - // matrix row 1 - VectorScale( p1, -tSqrSqr, a ); - VectorScale( p2, tSqrSqr * 3.0f, b ); - VectorScale( p3, tSqrSqr * -3.0f, c ); - VectorScale( p4, tSqrSqr, d ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - VectorAdd( d, output, output ); - - // matrix row 2 - VectorScale( p1, tSqr * 3.0f, a ); - VectorScale( p2, tSqr * -6.0f, b ); - VectorScale( p3, tSqr * 3.0f, c ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - - // matrix row 3 - VectorScale( p1, t * -3.0f, a ); - VectorScale( p3, t * 3.0f, c ); - // p4 unchanged - - VectorAdd( a, output, output ); - VectorAdd( c, output, output ); - - // matrix row 4 - // p1 and p3 scaled by 1.0f, so done below - VectorScale( p1, oneOver6, a ); - VectorScale( p2, 4.0f * oneOver6, b ); - VectorScale( p3, oneOver6, c ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); -} - -void BSpline_NormalizeX( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Vector p1n, p4n; - Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); - BSpline( p1n, p2, p3, p4n, t, output ); -} - -void Parabolic_Spline( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Assert( s_bMathlibInitialized ); - - float tSqr = t*t*0.5f; - t *= 0.5f; - - Assert( &output != &p1 ); - Assert( &output != &p2 ); - Assert( &output != &p3 ); - Assert( &output != &p4 ); - - output.Init(); - - Vector a, b, c, d; - - // matrix row 1 - // no influence from t cubed - - // matrix row 2 - VectorScale( p1, tSqr, a ); - VectorScale( p2, tSqr * -2.0f, b ); - VectorScale( p3, tSqr, c ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - VectorAdd( c, output, output ); - - // matrix row 3 - VectorScale( p1, t * -2.0f, a ); - VectorScale( p2, t * 2.0f, b ); - // p4 unchanged - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); - - // matrix row 4 - VectorScale( p1, 0.5f, a ); - VectorScale( p2, 0.5f, b ); - - VectorAdd( a, output, output ); - VectorAdd( b, output, output ); -} - -void Parabolic_Spline_NormalizeX( - const Vector &p1, - const Vector &p2, - const Vector &p3, - const Vector &p4, - float t, - Vector& output ) -{ - Vector p1n, p4n; - Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); - Parabolic_Spline( p1n, p2, p3, p4n, t, output ); -} - -//----------------------------------------------------------------------------- -// Purpose: Compress the input values for a ranged result such that from 75% to 200% smoothly of the range maps -//----------------------------------------------------------------------------- - -float RangeCompressor( float flValue, float flMin, float flMax, float flBase ) -{ - // clamp base - if (flBase < flMin) - flBase = flMin; - if (flBase > flMax) - flBase = flMax; - - flValue += flBase; - - // convert to 0 to 1 value - float flMid = (flValue - flMin) / (flMax - flMin); - // convert to -1 to 1 value - float flTarget = flMid * 2 - 1; - - if (fabs(flTarget) > 0.75) - { - float t = (fabs(flTarget) - 0.75) / (1.25); - if (t < 1.0) - { - if (flTarget > 0) - { - flTarget = Hermite_Spline( 0.75, 1, 0.75, 0, t ); - } - else - { - flTarget = -Hermite_Spline( 0.75, 1, 0.75, 0, t ); - } - } - else - { - flTarget = (flTarget > 0) ? 1.0f : -1.0f; - } - } - - flMid = (flTarget + 1 ) / 2.0; - flValue = flMin * (1 - flMid) + flMax * flMid; - - flValue -= flBase; - - return flValue; -} - - -//#pragma optimize( "", on ) - -//----------------------------------------------------------------------------- -// Transforms a AABB into another space; which will inherently grow the box. -//----------------------------------------------------------------------------- -void TransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) -{ - Vector localCenter; - VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); - localCenter *= 0.5f; - - Vector localExtents; - VectorSubtract( vecMaxsIn, localCenter, localExtents ); - - Vector worldCenter; - VectorTransform( localCenter, transform, worldCenter ); - - Vector worldExtents; - worldExtents.x = DotProductAbs( localExtents, transform[0] ); - worldExtents.y = DotProductAbs( localExtents, transform[1] ); - worldExtents.z = DotProductAbs( localExtents, transform[2] ); - - VectorSubtract( worldCenter, worldExtents, vecMinsOut ); - VectorAdd( worldCenter, worldExtents, vecMaxsOut ); -} - - -//----------------------------------------------------------------------------- -// Uses the inverse transform of in1 -//----------------------------------------------------------------------------- -void ITransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) -{ - Vector worldCenter; - VectorAdd( vecMinsIn, vecMaxsIn, worldCenter ); - worldCenter *= 0.5f; - - Vector worldExtents; - VectorSubtract( vecMaxsIn, worldCenter, worldExtents ); - - Vector localCenter; - VectorITransform( worldCenter, transform, localCenter ); - - Vector localExtents; - localExtents.x = FloatMakePositive( worldExtents.x * transform[0][0] ) + - FloatMakePositive( worldExtents.y * transform[1][0] ) + - FloatMakePositive( worldExtents.z * transform[2][0] ); - localExtents.y = FloatMakePositive( worldExtents.x * transform[0][1] ) + - FloatMakePositive( worldExtents.y * transform[1][1] ) + - FloatMakePositive( worldExtents.z * transform[2][1] ); - localExtents.z = FloatMakePositive( worldExtents.x * transform[0][2] ) + - FloatMakePositive( worldExtents.y * transform[1][2] ) + - FloatMakePositive( worldExtents.z * transform[2][2] ); - - VectorSubtract( localCenter, localExtents, vecMinsOut ); - VectorAdd( localCenter, localExtents, vecMaxsOut ); -} - - -//----------------------------------------------------------------------------- -// Rotates a AABB into another space; which will inherently grow the box. -// (same as TransformAABB, but doesn't take the translation into account) -//----------------------------------------------------------------------------- -void RotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) -{ - Vector localCenter; - VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); - localCenter *= 0.5f; - - Vector localExtents; - VectorSubtract( vecMaxsIn, localCenter, localExtents ); - - Vector newCenter; - VectorRotate( localCenter, transform, newCenter ); - - Vector newExtents; - newExtents.x = DotProductAbs( localExtents, transform[0] ); - newExtents.y = DotProductAbs( localExtents, transform[1] ); - newExtents.z = DotProductAbs( localExtents, transform[2] ); - - VectorSubtract( newCenter, newExtents, vecMinsOut ); - VectorAdd( newCenter, newExtents, vecMaxsOut ); -} - - -//----------------------------------------------------------------------------- -// Uses the inverse transform of in1 -//----------------------------------------------------------------------------- -void IRotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) -{ - Vector oldCenter; - VectorAdd( vecMinsIn, vecMaxsIn, oldCenter ); - oldCenter *= 0.5f; - - Vector oldExtents; - VectorSubtract( vecMaxsIn, oldCenter, oldExtents ); - - Vector newCenter; - VectorIRotate( oldCenter, transform, newCenter ); - - Vector newExtents; - newExtents.x = FloatMakePositive( oldExtents.x * transform[0][0] ) + - FloatMakePositive( oldExtents.y * transform[1][0] ) + - FloatMakePositive( oldExtents.z * transform[2][0] ); - newExtents.y = FloatMakePositive( oldExtents.x * transform[0][1] ) + - FloatMakePositive( oldExtents.y * transform[1][1] ) + - FloatMakePositive( oldExtents.z * transform[2][1] ); - newExtents.z = FloatMakePositive( oldExtents.x * transform[0][2] ) + - FloatMakePositive( oldExtents.y * transform[1][2] ) + - FloatMakePositive( oldExtents.z * transform[2][2] ); - - VectorSubtract( newCenter, newExtents, vecMinsOut ); - VectorAdd( newCenter, newExtents, vecMaxsOut ); -} - - -float CalcSqrDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point ) -{ - float flDelta; - float flDistSqr = 0.0f; - - if ( point.x < mins.x ) - { - flDelta = (mins.x - point.x); - flDistSqr += flDelta * flDelta; - } - else if ( point.x > maxs.x ) - { - flDelta = (point.x - maxs.x); - flDistSqr += flDelta * flDelta; - } - - if ( point.y < mins.y ) - { - flDelta = (mins.y - point.y); - flDistSqr += flDelta * flDelta; - } - else if ( point.y > maxs.y ) - { - flDelta = (point.y - maxs.y); - flDistSqr += flDelta * flDelta; - } - - if ( point.z < mins.z ) - { - flDelta = (mins.z - point.z); - flDistSqr += flDelta * flDelta; - } - else if ( point.z > maxs.z ) - { - flDelta = (point.z - maxs.z); - flDistSqr += flDelta * flDelta; - } - - return flDistSqr; -} - - -void CalcClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut ) -{ - closestOut.x = clamp( point.x, mins.x, maxs.x ); - closestOut.y = clamp( point.y, mins.y, maxs.y ); - closestOut.z = clamp( point.z, mins.z, maxs.z ); -} - -void CalcSqrDistAndClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut, float &distSqrOut ) -{ - distSqrOut = 0.0f; - for ( int i = 0; i < 3; i++ ) - { - if ( point[i] < mins[i] ) - { - closestOut[i] = mins[i]; - float flDelta = closestOut[i] - mins[i]; - distSqrOut += flDelta * flDelta; - } - else if ( point[i] > maxs[i] ) - { - closestOut[i] = maxs[i]; - float flDelta = closestOut[i] - maxs[i]; - distSqrOut += flDelta * flDelta; - } - else - { - closestOut[i] = point[i]; - } - } - -} - -float CalcClosestPointToLineT( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vDir ) -{ - Assert( s_bMathlibInitialized ); - VectorSubtract( vLineB, vLineA, vDir ); - - // D dot [P - (A + D*t)] = 0 - // t = ( DP - DA) / DD - float div = vDir.Dot( vDir ); - if( div < 0.00001f ) - { - return 0; - } - else - { - return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; - } -} - -void CalcClosestPointOnLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector vDir; - float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); - if ( outT ) *outT = t; - vClosest.MulAdd( vLineA, vDir, t ); -} - - -float CalcDistanceToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector vClosest; - CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); - return P.DistTo(vClosest); -} - -float CalcDistanceSqrToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector vClosest; - CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); - return P.DistToSqr(vClosest); -} - -void CalcClosestPointOnLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) -{ - Vector vDir; - float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); - t = clamp( t, 0.f, 1.f ); - if ( outT ) - { - *outT = t; - } - vClosest.MulAdd( vLineA, vDir, t ); -} - - -float CalcDistanceToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector vClosest; - CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); - return P.DistTo( vClosest ); -} - -float CalcDistanceSqrToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector vClosest; - CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); - return P.DistToSqr(vClosest); -} - -float CalcClosestPointToLineT2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vDir ) -{ - Assert( s_bMathlibInitialized ); - Vector2DSubtract( vLineB, vLineA, vDir ); - - // D dot [P - (A + D*t)] = 0 - // t = (DP - DA) / DD - float div = vDir.Dot( vDir ); - if( div < 0.00001f ) - { - return 0; - } - else - { - return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; - } -} - -void CalcClosestPointOnLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector2D vDir; - float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); - if ( outT ) *outT = t; - vClosest.MulAdd( vLineA, vDir, t ); -} - -float CalcDistanceToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector2D vClosest; - CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); - return P.DistTo( vClosest ); -} - -float CalcDistanceSqrToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector2D vClosest; - CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); - return P.DistToSqr(vClosest); -} - -void CalcClosestPointOnLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) -{ - Vector2D vDir; - float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); - t = clamp( t, 0.f, 1.f ); - if ( outT ) - { - *outT = t; - } - vClosest.MulAdd( vLineA, vDir, t ); -} - -float CalcDistanceToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector2D vClosest; - CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); - return P.DistTo( vClosest ); -} - -float CalcDistanceSqrToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) -{ - Assert( s_bMathlibInitialized ); - Vector2D vClosest; - CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); - return P.DistToSqr( vClosest ); -} - -// Do we have another epsilon we could use -#define LINE_EPS ( 0.000001f ) - -//----------------------------------------------------------------------------- -// Purpose: Given lines p1->p2 and p3->p4, computes a line segment (pa->pb) and returns the parameters 0->1 multipliers -// along each segment for the returned points -// Input : p1 - -// p2 - -// p3 - -// p4 - -// *s1 - -// *s2 - -// Output : Returns true on success, false on failure. -//----------------------------------------------------------------------------- -bool CalcLineToLineIntersectionSegment( - const Vector& p1,const Vector& p2,const Vector& p3,const Vector& p4,Vector *s1,Vector *s2, - float *t1, float *t2) -{ - Vector p13,p43,p21; - float d1343,d4321,d1321,d4343,d2121; - float numer,denom; - - p13.x = p1.x - p3.x; - p13.y = p1.y - p3.y; - p13.z = p1.z - p3.z; - p43.x = p4.x - p3.x; - p43.y = p4.y - p3.y; - p43.z = p4.z - p3.z; - - if (fabs(p43.x) < LINE_EPS && fabs(p43.y) < LINE_EPS && fabs(p43.z) < LINE_EPS) - return false; - p21.x = p2.x - p1.x; - p21.y = p2.y - p1.y; - p21.z = p2.z - p1.z; - if (fabs(p21.x) < LINE_EPS && fabs(p21.y) < LINE_EPS && fabs(p21.z) < LINE_EPS) - return false; - - d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z; - d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z; - d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z; - d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z; - d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z; - - denom = d2121 * d4343 - d4321 * d4321; - if (fabs(denom) < LINE_EPS) - return false; - numer = d1343 * d4321 - d1321 * d4343; - - *t1 = numer / denom; - *t2 = (d1343 + d4321 * (*t1)) / d4343; - - s1->x = p1.x + *t1 * p21.x; - s1->y = p1.y + *t1 * p21.y; - s1->z = p1.z + *t1 * p21.z; - s2->x = p3.x + *t2 * p43.x; - s2->y = p3.y + *t2 * p43.y; - s2->z = p3.z + *t2 * p43.z; - - return true; -} - -#pragma optimize( "", off ) - -#ifndef EXCEPTION_EXECUTE_HANDLER -#define EXCEPTION_EXECUTE_HANDLER 1 -#endif - -#pragma optimize( "", on ) - -static bool s_b3DNowEnabled = false; -static bool s_bMMXEnabled = false; -static bool s_bSSEEnabled = false; -static bool s_bSSE2Enabled = false; - -void MathLib_Init( float gamma, float texGamma, float brightness, int overbright, bool bAllow3DNow, bool bAllowSSE, bool bAllowSSE2, bool bAllowMMX ) -{ - if ( s_bMathlibInitialized ) - return; - - // FIXME: Hook SSE into VectorAligned + Vector4DAligned - -#if !defined( _X360 ) - // Grab the processor information: - const CPUInformation& pi = *GetCPUInformation(); - - // Select the default generic routines. - pfSqrt = _sqrtf; - pfRSqrt = _rsqrtf; - pfRSqrtFast = _rsqrtf; - pfVectorNormalize = _VectorNormalize; - pfVectorNormalizeFast = _VectorNormalizeFast; - pfInvRSquared = _InvRSquared; - pfFastSinCos = SinCos; - pfFastCos = cosf; - - if ( bAllowMMX && pi.m_bMMX ) - { - // Select the MMX specific routines if available - // (MMX routines were used by SW span fillers - not currently used for HW) - s_bMMXEnabled = true; - } - else - { - s_bMMXEnabled = false; - } - - // SSE Generally performs better than 3DNow when present, so this is placed - // first to allow SSE to override these settings. -#if !defined( OSX ) && !defined( PLATFORM_WINDOWS_PC64 ) && !defined(LINUX) - if ( bAllow3DNow && pi.m_b3DNow ) - { - s_b3DNowEnabled = true; - - // Select the 3DNow specific routines if available; - pfVectorNormalize = _3DNow_VectorNormalize; - pfVectorNormalizeFast = _3DNow_VectorNormalizeFast; - pfInvRSquared = _3DNow_InvRSquared; - pfSqrt = _3DNow_Sqrt; - pfRSqrt = _3DNow_RSqrt; - pfRSqrtFast = _3DNow_RSqrt; - } - else -#endif - { - s_b3DNowEnabled = false; - } - - if ( bAllowSSE && pi.m_bSSE ) - { - s_bSSEEnabled = true; - -#ifndef PLATFORM_WINDOWS_PC64 - // These are not yet available. - // Select the SSE specific routines if available - pfVectorNormalize = _VectorNormalize; - pfVectorNormalizeFast = _SSE_VectorNormalizeFast; - pfInvRSquared = _SSE_InvRSquared; - pfSqrt = _SSE_Sqrt; - pfRSqrt = _SSE_RSqrtAccurate; - pfRSqrtFast = _SSE_RSqrtFast; -#endif -#ifdef PLATFORM_WINDOWS_PC32 - pfFastSinCos = _SSE_SinCos; - pfFastCos = _SSE_cos; -#endif - } - else - { - s_bSSEEnabled = false; - } - - if ( bAllowSSE2 && pi.m_bSSE2 ) - { - s_bSSE2Enabled = true; -#ifdef PLATFORM_WINDOWS_PC32 - pfFastSinCos = _SSE2_SinCos; - pfFastCos = _SSE2_cos; -#endif - } - else - { - s_bSSE2Enabled = false; - } -#endif - - s_bMathlibInitialized = true; - - InitSinCosTable(); - BuildGammaTable( gamma, texGamma, brightness, overbright ); -} - -bool MathLib_3DNowEnabled( void ) -{ - Assert( s_bMathlibInitialized ); - return s_b3DNowEnabled; -} - -bool MathLib_MMXEnabled( void ) -{ - Assert( s_bMathlibInitialized ); - return s_bMMXEnabled; -} - -bool MathLib_SSEEnabled( void ) -{ - Assert( s_bMathlibInitialized ); - return s_bSSEEnabled; -} - -bool MathLib_SSE2Enabled( void ) -{ - Assert( s_bMathlibInitialized ); - return s_bSSE2Enabled; -} - -float Approach( float target, float value, float speed ) -{ - float delta = target - value; - - if ( delta > speed ) - value += speed; - else if ( delta < -speed ) - value -= speed; - else - value = target; - - return value; -} - -// BUGBUG: Why doesn't this call angle diff?!?!? -float ApproachAngle( float target, float value, float speed ) -{ - target = anglemod( target ); - value = anglemod( value ); - - float delta = target - value; - - // Speed is assumed to be positive - if ( speed < 0 ) - speed = -speed; - - if ( delta < -180 ) - delta += 360; - else if ( delta > 180 ) - delta -= 360; - - if ( delta > speed ) - value += speed; - else if ( delta < -speed ) - value -= speed; - else - value = target; - - return value; -} - - -// BUGBUG: Why do we need both of these? -float AngleDiff( float destAngle, float srcAngle ) -{ - float delta; - - delta = fmodf(destAngle - srcAngle, 360.0f); - if ( destAngle > srcAngle ) - { - if ( delta >= 180 ) - delta -= 360; - } - else - { - if ( delta <= -180 ) - delta += 360; - } - return delta; -} - - -float AngleDistance( float next, float cur ) -{ - float delta = next - cur; - - if ( delta < -180 ) - delta += 360; - else if ( delta > 180 ) - delta -= 360; - - return delta; -} - - -float AngleNormalize( float angle ) -{ - angle = fmodf(angle, 360.0f); - if (angle > 180) - { - angle -= 360; - } - if (angle < -180) - { - angle += 360; - } - return angle; -} - -//-------------------------------------------------------------------------------------------------------------- -// ensure that 0 <= angle <= 360 -float AngleNormalizePositive( float angle ) -{ - angle = fmodf( angle, 360.0f ); - - if (angle < 0.0f) - { - angle += 360.0f; - } - - return angle; -} - -//-------------------------------------------------------------------------------------------------------------- -bool AnglesAreEqual( float a, float b, float tolerance ) -{ - return (fabs( AngleDiff( a, b ) ) < tolerance); -} - -void RotationDeltaAxisAngle( const QAngle &srcAngles, const QAngle &destAngles, Vector &deltaAxis, float &deltaAngle ) -{ - Quaternion srcQuat, destQuat, srcQuatInv, out; - AngleQuaternion( srcAngles, srcQuat ); - AngleQuaternion( destAngles, destQuat ); - QuaternionScale( srcQuat, -1, srcQuatInv ); - QuaternionMult( destQuat, srcQuatInv, out ); - - QuaternionNormalize( out ); - QuaternionAxisAngle( out, deltaAxis, deltaAngle ); -} - -void RotationDelta( const QAngle &srcAngles, const QAngle &destAngles, QAngle *out ) -{ - matrix3x4_t src, srcInv; - matrix3x4_t dest; - AngleMatrix( srcAngles, src ); - AngleMatrix( destAngles, dest ); - // xform = src(-1) * dest - MatrixInvert( src, srcInv ); - matrix3x4_t xform; - ConcatTransforms( dest, srcInv, xform ); - QAngle xformAngles; - MatrixAngles( xform, xformAngles ); - if ( out ) - { - *out = xformAngles; - } -} - -//----------------------------------------------------------------------------- -// Purpose: Computes a triangle normal -//----------------------------------------------------------------------------- -void ComputeTrianglePlane( const Vector& v1, const Vector& v2, const Vector& v3, Vector& normal, float& intercept ) -{ - Vector e1, e2; - VectorSubtract( v2, v1, e1 ); - VectorSubtract( v3, v1, e2 ); - CrossProduct( e1, e2, normal ); - VectorNormalize( normal ); - intercept = DotProduct( normal, v1 ); -} - -//----------------------------------------------------------------------------- -// Purpose: This is a clone of BaseWindingForPlane() -// Input : *outVerts - an array of preallocated verts to build the polygon in -// normal - the plane normal -// dist - the plane constant -// Output : int - vert count (always 4) -//----------------------------------------------------------------------------- -int PolyFromPlane( Vector *outVerts, const Vector& normal, float dist, float fHalfScale ) -{ - int i, x; - vec_t max, v; - Vector org, vright, vup; - - // find the major axis - - max = -16384; //MAX_COORD_INTEGER - x = -1; - for (i=0 ; i<3; i++) - { - v = fabs(normal[i]); - if (v > max) - { - x = i; - max = v; - } - } - - if (x==-1) - return 0; - - // Build a unit vector along something other than the major axis - VectorCopy (vec3_origin, vup); - switch (x) - { - case 0: - case 1: - vup[2] = 1; - break; - case 2: - vup[0] = 1; - break; - } - - // Remove the component of this vector along the normal - v = DotProduct (vup, normal); - VectorMA (vup, -v, normal, vup); - // Make it a unit (perpendicular) - VectorNormalize (vup); - - // Center of the poly is at normal * dist - VectorScale (normal, dist, org); - // Calculate the third orthonormal basis vector for our plane space (this one and vup are in the plane) - CrossProduct (vup, normal, vright); - - // Make the plane's basis vectors big (these are the half-sides of the polygon we're making) - VectorScale (vup, fHalfScale, vup); - VectorScale (vright, fHalfScale, vright); - - // Move diagonally away from org to create the corner verts - VectorSubtract (org, vright, outVerts[0]); // left - VectorAdd (outVerts[0], vup, outVerts[0]); // up - - VectorAdd (org, vright, outVerts[1]); // right - VectorAdd (outVerts[1], vup, outVerts[1]); // up - - VectorAdd (org, vright, outVerts[2]); // right - VectorSubtract (outVerts[2], vup, outVerts[2]); // down - - VectorSubtract (org, vright, outVerts[3]); // left - VectorSubtract (outVerts[3], vup, outVerts[3]); // down - - // The four corners form a planar quadrilateral normal to "normal" - return 4; -} - -//----------------------------------------------------------------------------- -// Purpose: clip a poly to the plane and return the poly on the front side of the plane -// Input : *inVerts - input polygon -// vertCount - # verts in input poly -// *outVerts - destination poly -// normal - plane normal -// dist - plane constant -// Output : int - # verts in output poly -//----------------------------------------------------------------------------- - -int ClipPolyToPlane( Vector *inVerts, int vertCount, Vector *outVerts, const Vector& normal, float dist, float fOnPlaneEpsilon ) -{ - vec_t *dists = (vec_t *)stackalloc( sizeof(vec_t) * vertCount * 4 ); //4x vertcount should cover all cases - int *sides = (int *)stackalloc( sizeof(vec_t) * vertCount * 4 ); - int counts[3]; - vec_t dot; - int i, j; - Vector mid = vec3_origin; - int outCount; - - counts[0] = counts[1] = counts[2] = 0; - - // determine sides for each point - for ( i = 0; i < vertCount; i++ ) - { - dot = DotProduct( inVerts[i], normal) - dist; - dists[i] = dot; - if ( dot > fOnPlaneEpsilon ) - { - sides[i] = SIDE_FRONT; - } - else if ( dot < -fOnPlaneEpsilon ) - { - sides[i] = SIDE_BACK; - } - else - { - sides[i] = SIDE_ON; - } - counts[sides[i]]++; - } - sides[i] = sides[0]; - dists[i] = dists[0]; - - if (!counts[0]) - return 0; - - if (!counts[1]) - { - // Copy to output verts - for ( i = 0; i < vertCount; i++ ) - { - VectorCopy( inVerts[i], outVerts[i] ); - } - return vertCount; - } - - outCount = 0; - for ( i = 0; i < vertCount; i++ ) - { - Vector& p1 = inVerts[i]; - - if (sides[i] == SIDE_ON) - { - VectorCopy( p1, outVerts[outCount]); - outCount++; - continue; - } - - if (sides[i] == SIDE_FRONT) - { - VectorCopy( p1, outVerts[outCount]); - outCount++; - } - - if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) - continue; - - // generate a split point - Vector& p2 = inVerts[(i+1)%vertCount]; - - dot = dists[i] / (dists[i]-dists[i+1]); - for (j=0 ; j<3 ; j++) - { // avoid round off error when possible - if (normal[j] == 1) - mid[j] = dist; - else if (normal[j] == -1) - mid[j] = -dist; - else - mid[j] = p1[j] + dot*(p2[j]-p1[j]); - } - - VectorCopy (mid, outVerts[outCount]); - outCount++; - } - - return outCount; -} - - -int ClipPolyToPlane_Precise( double *inVerts, int vertCount, double *outVerts, const double *normal, double dist, double fOnPlaneEpsilon ) -{ - double *dists = (double *)stackalloc( sizeof(double) * vertCount * 4 ); //4x vertcount should cover all cases - int *sides = (int *)stackalloc( sizeof(double) * vertCount * 4 ); - int counts[3]; - double dot; - int i, j; - //Vector mid = vec3_origin; - double mid[3]; - mid[0] = 0.0; - mid[1] = 0.0; - mid[2] = 0.0; - int outCount; - - counts[0] = counts[1] = counts[2] = 0; - - // determine sides for each point - for ( i = 0; i < vertCount; i++ ) - { - //dot = DotProduct( inVerts[i], normal) - dist; - dot = ((inVerts[i*3 + 0] * normal[0]) + (inVerts[i*3 + 1] * normal[1]) + (inVerts[i*3 + 2] * normal[2])) - dist; - dists[i] = dot; - if ( dot > fOnPlaneEpsilon ) - { - sides[i] = SIDE_FRONT; - } - else if ( dot < -fOnPlaneEpsilon ) - { - sides[i] = SIDE_BACK; - } - else - { - sides[i] = SIDE_ON; - } - counts[sides[i]]++; - } - sides[i] = sides[0]; - dists[i] = dists[0]; - - if (!counts[0]) - return 0; - - if (!counts[1]) - { - // Copy to output verts - //for ( i = 0; i < vertCount; i++ ) - for ( i = 0; i < vertCount * 3; i++ ) - { - //VectorCopy( inVerts[i], outVerts[i] ); - outVerts[i] = inVerts[i]; - } - return vertCount; - } - - outCount = 0; - for ( i = 0; i < vertCount; i++ ) - { - //Vector& p1 = inVerts[i]; - double *p1 = &inVerts[i*3]; - //p1[0] = inVerts[i*3 + 0]; - //p1[1] = inVerts[i*3 + 1]; - //p1[2] = inVerts[i*3 + 2]; - - if (sides[i] == SIDE_ON) - { - //VectorCopy( p1, outVerts[outCount]); - outVerts[outCount*3 + 0] = p1[0]; - outVerts[outCount*3 + 1] = p1[1]; - outVerts[outCount*3 + 2] = p1[2]; - outCount++; - continue; - } - - if (sides[i] == SIDE_FRONT) - { - //VectorCopy( p1, outVerts[outCount]); - outVerts[outCount*3 + 0] = p1[0]; - outVerts[outCount*3 + 1] = p1[1]; - outVerts[outCount*3 + 2] = p1[2]; - outCount++; - } - - if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) - continue; - - // generate a split point - //Vector& p2 = inVerts[(i+1)%vertCount]; - int wrappedindex = (i+1)%vertCount; - double *p2 = &inVerts[wrappedindex*3]; - //p2[0] = inVerts[wrappedindex*3 + 0]; - //p2[1] = inVerts[wrappedindex*3 + 1]; - //p2[2] = inVerts[wrappedindex*3 + 2]; - - dot = dists[i] / (dists[i]-dists[i+1]); - for (j=0 ; j<3 ; j++) - { - mid[j] = (double)p1[j] + dot*((double)p2[j]-(double)p1[j]); - } - - //VectorCopy (mid, outVerts[outCount]); - outVerts[outCount*3 + 0] = mid[0]; - outVerts[outCount*3 + 1] = mid[1]; - outVerts[outCount*3 + 2] = mid[2]; - outCount++; - } - - return outCount; -} - -int CeilPow2( int in ) -{ - int retval; - - retval = 1; - while( retval < in ) - retval <<= 1; - return retval; -} - -int FloorPow2( int in ) -{ - int retval; - - retval = 1; - while( retval < in ) - retval <<= 1; - return retval >> 1; -} - - -//----------------------------------------------------------------------------- -// Computes Y fov from an X fov and a screen aspect ratio -//----------------------------------------------------------------------------- -float CalcFovY( float flFovX, float flAspect ) -{ - if ( flFovX < 1 || flFovX > 179) - { - flFovX = 90; // error, set to 90 - } - - // The long, but illustrative version (more closely matches CShaderAPIDX8::PerspectiveX, which - // is what it's based on). - // - //float width = 2 * zNear * tan( DEG2RAD( fov_x / 2.0 ) ); - //float height = width / screenaspect; - //float yRadians = atan( (height/2.0) / zNear ); - //return RAD2DEG( yRadians ) * 2; - - // The short and sweet version. - float val = atan( tan( DEG2RAD( flFovX ) * 0.5f ) / flAspect ); - val = RAD2DEG( val ) * 2.0f; - return val; -} - -float CalcFovX( float flFovY, float flAspect ) -{ - return RAD2DEG( atan( tan( DEG2RAD( flFovY ) * 0.5f ) * flAspect ) ) * 2.0f; -} - - -//----------------------------------------------------------------------------- -// Generate a frustum based on perspective view parameters -//----------------------------------------------------------------------------- -void GeneratePerspectiveFrustum( const Vector& origin, const Vector &forward, - const Vector &right, const Vector &up, float flZNear, float flZFar, - float flFovX, float flFovY, Frustum_t &frustum ) -{ - float flIntercept = DotProduct( origin, forward ); - - // Setup the near and far planes. - frustum.SetPlane( FRUSTUM_FARZ, PLANE_ANYZ, -forward, -flZFar - flIntercept ); - frustum.SetPlane( FRUSTUM_NEARZ, PLANE_ANYZ, forward, flZNear + flIntercept ); - - flFovX *= 0.5f; - flFovY *= 0.5f; - - float flTanX = tan( DEG2RAD( flFovX ) ); - float flTanY = tan( DEG2RAD( flFovY ) ); - - // OPTIMIZE: Normalizing these planes is not necessary for culling - Vector normalPos, normalNeg; - - VectorMA( right, flTanX, forward, normalPos ); - VectorMA( normalPos, -2.0f, right, normalNeg ); - - VectorNormalize( normalPos ); - VectorNormalize( normalNeg ); - - frustum.SetPlane( FRUSTUM_LEFT, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); - frustum.SetPlane( FRUSTUM_RIGHT, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); - - VectorMA( up, flTanY, forward, normalPos ); - VectorMA( normalPos, -2.0f, up, normalNeg ); - - VectorNormalize( normalPos ); - VectorNormalize( normalNeg ); - - frustum.SetPlane( FRUSTUM_BOTTOM, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); - frustum.SetPlane( FRUSTUM_TOP, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); -} - - -//----------------------------------------------------------------------------- -// Version that accepts angles instead of vectors -//----------------------------------------------------------------------------- -void GeneratePerspectiveFrustum( const Vector& origin, const QAngle &angles, float flZNear, float flZFar, float flFovX, float flAspectRatio, Frustum_t &frustum ) -{ - Vector vecForward, vecRight, vecUp; - AngleVectors( angles, &vecForward, &vecRight, &vecUp ); - float flFovY = CalcFovY( flFovX, flAspectRatio ); - GeneratePerspectiveFrustum( origin, vecForward, vecRight, vecUp, flZNear, flZFar, flFovX, flFovY, frustum ); -} - -bool R_CullBox( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) -{ - return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_NEARZ) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); -} - -bool R_CullBoxSkipNear( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) -{ - return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || - ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); -} - - -// NOTE: This routine was taken (and modified) from NVidia's BlinnReflection demo -// Creates basis vectors, based on a vertex and index list. -// See the NVidia white paper 'GDC2K PerPixel Lighting' for a description -// of how this computation works -#define SMALL_FLOAT 1e-12 - -void CalcTriangleTangentSpace( const Vector &p0, const Vector &p1, const Vector &p2, - const Vector2D &t0, const Vector2D &t1, const Vector2D& t2, - Vector &sVect, Vector &tVect ) -{ - /* Compute the partial derivatives of X, Y, and Z with respect to S and T. */ - sVect.Init( 0.0f, 0.0f, 0.0f ); - tVect.Init( 0.0f, 0.0f, 0.0f ); - - // x, s, t - Vector edge01( p1.x - p0.x, t1.x - t0.x, t1.y - t0.y ); - Vector edge02( p2.x - p0.x, t2.x - t0.x, t2.y - t0.y ); - - Vector cross; - CrossProduct( edge01, edge02, cross ); - if ( fabs( cross.x ) > SMALL_FLOAT ) - { - sVect.x += -cross.y / cross.x; - tVect.x += -cross.z / cross.x; - } - - // y, s, t - edge01.Init( p1.y - p0.y, t1.x - t0.x, t1.y - t0.y ); - edge02.Init( p2.y - p0.y, t2.x - t0.x, t2.y - t0.y ); - - CrossProduct( edge01, edge02, cross ); - if ( fabs( cross.x ) > SMALL_FLOAT ) - { - sVect.y += -cross.y / cross.x; - tVect.y += -cross.z / cross.x; - } - - // z, s, t - edge01.Init( p1.z - p0.z, t1.x - t0.x, t1.y - t0.y ); - edge02.Init( p2.z - p0.z, t2.x - t0.x, t2.y - t0.y ); - - CrossProduct( edge01, edge02, cross ); - if( fabs( cross.x ) > SMALL_FLOAT ) - { - sVect.z += -cross.y / cross.x; - tVect.z += -cross.z / cross.x; - } - - // Normalize sVect and tVect - VectorNormalize( sVect ); - VectorNormalize( tVect ); -} - - -//----------------------------------------------------------------------------- -// Convert RGB to HSV -//----------------------------------------------------------------------------- -void RGBtoHSV( const Vector &rgb, Vector &hsv ) -{ - float flMax = max( rgb.x, rgb.y ); - flMax = max( flMax, rgb.z ); - float flMin = min( rgb.x, rgb.y ); - flMin = min( flMin, rgb.z ); - - // hsv.z is the value - hsv.z = flMax; - - // hsv.y is the saturation - if (flMax != 0.0F) - { - hsv.y = (flMax - flMin) / flMax; - } - else - { - hsv.y = 0.0F; - } - - // hsv.x is the hue - if (hsv.y == 0.0F) - { - hsv.x = -1.0f; - } - else - { - float32 d = flMax - flMin; - if (rgb.x == flMax) - { - hsv.x = (rgb.y - rgb.z) / d; - } - else if (rgb.y == flMax) - { - hsv.x = 2.0F + (rgb.z - rgb.x) / d; - } - else - { - hsv.x = 4.0F + (rgb.x - rgb.y) / d; - } - hsv.x *= 60.0F; - if ( hsv.x < 0.0F ) - { - hsv.x += 360.0F; - } - } -} - - -//----------------------------------------------------------------------------- -// Convert HSV to RGB -//----------------------------------------------------------------------------- -void HSVtoRGB( const Vector &hsv, Vector &rgb ) -{ - if ( hsv.y == 0.0F ) - { - rgb.Init( hsv.z, hsv.z, hsv.z ); - return; - } - - float32 hue = hsv.x; - if (hue == 360.0F) - { - hue = 0.0F; - } - hue /= 60.0F; - int i = hue; // integer part - float32 f = hue - i; // fractional part - float32 p = hsv.z * (1.0F - hsv.y); - float32 q = hsv.z * (1.0F - hsv.y * f); - float32 t = hsv.z * (1.0F - hsv.y * (1.0F - f)); - switch(i) - { - case 0: rgb.Init( hsv.z, t, p ); break; - case 1: rgb.Init( q, hsv.z, p ); break; - case 2: rgb.Init( p, hsv.z, t ); break; - case 3: rgb.Init( p, q, hsv.z ); break; - case 4: rgb.Init( t, p, hsv.z ); break; - case 5: rgb.Init( hsv.z, p, q ); break; - } -} - - -void GetInterpolationData( float const *pKnotPositions, - float const *pKnotValues, - int nNumValuesinList, - int nInterpolationRange, - float flPositionToInterpolateAt, - bool bWrap, - float *pValueA, - float *pValueB, - float *pInterpolationValue) -{ - // first, find the bracketting knots by looking for the first knot >= our index - - int idx; - for(idx = 0; idx < nNumValuesinList; idx++ ) - { - if ( pKnotPositions[idx] >= flPositionToInterpolateAt ) - break; - } - int nKnot1, nKnot2; - float flOffsetFromStartOfGap, flSizeOfGap; - if ( idx == 0) - { - if ( bWrap ) - { - nKnot1 = nNumValuesinList-1; - nKnot2 = 0; - flSizeOfGap = - ( pKnotPositions[nKnot2] + ( nInterpolationRange-pKnotPositions[nKnot1] ) ); - flOffsetFromStartOfGap = - flPositionToInterpolateAt + ( nInterpolationRange-pKnotPositions[nKnot1] ); - } - else - { - *pValueA = *pValueB = pKnotValues[0]; - *pInterpolationValue = 1.0; - return; - } - } - else if ( idx == nNumValuesinList ) // ran out of values - { - if ( bWrap ) - { - nKnot1 = nNumValuesinList -1; - nKnot2 = 0; - flSizeOfGap = ( pKnotPositions[nKnot2] + - ( nInterpolationRange-pKnotPositions[nKnot1] ) ); - flOffsetFromStartOfGap = flPositionToInterpolateAt - pKnotPositions[nKnot1]; - } - else - { - *pValueA = *pValueB = pKnotValues[nNumValuesinList-1]; - *pInterpolationValue = 1.0; - return; - } - - } - else - { - nKnot1 = idx-1; - nKnot2 = idx; - flSizeOfGap = pKnotPositions[nKnot2]-pKnotPositions[nKnot1]; - flOffsetFromStartOfGap = flPositionToInterpolateAt-pKnotPositions[nKnot1]; - } - - *pValueA = pKnotValues[nKnot1]; - *pValueB = pKnotValues[nKnot2]; - *pInterpolationValue = FLerp( 0, 1, 0, flSizeOfGap, flOffsetFromStartOfGap ); - return; -} - -float RandomVectorInUnitSphere( Vector *pVector ) -{ - // Guarantee uniform random distribution within a sphere - // Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") - float u = ((float)rand() / VALVE_RAND_MAX); - float v = ((float)rand() / VALVE_RAND_MAX); - float w = ((float)rand() / VALVE_RAND_MAX); - - float flPhi = acos( 1 - 2 * u ); - float flTheta = 2 * M_PI * v; - float flRadius = powf( w, 1.0f / 3.0f ); - - float flSinPhi, flCosPhi; - float flSinTheta, flCosTheta; - SinCos( flPhi, &flSinPhi, &flCosPhi ); - SinCos( flTheta, &flSinTheta, &flCosTheta ); - - pVector->x = flRadius * flSinPhi * flCosTheta; - pVector->y = flRadius * flSinPhi * flSinTheta; - pVector->z = flRadius * flCosPhi; - return flRadius; -} - -float RandomVectorInUnitCircle( Vector2D *pVector ) -{ - // Guarantee uniform random distribution within a sphere - // Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") - float u = ((float)rand() / VALVE_RAND_MAX); - float v = ((float)rand() / VALVE_RAND_MAX); - - float flTheta = 2 * M_PI * v; - float flRadius = powf( u, 1.0f / 2.0f ); - - float flSinTheta, flCosTheta; - SinCos( flTheta, &flSinTheta, &flCosTheta ); - - pVector->x = flRadius * flCosTheta; - pVector->y = flRadius * flSinTheta; - return flRadius; -} -#ifdef FP_EXCEPTIONS_ENABLED -#include // For _clearfp and _controlfp_s -#endif - -// FPExceptionDisable and FPExceptionEnabler taken from my blog post -// at http://www.altdevblogaday.com/2012/04/20/exceptional-floating-point/ - -#ifdef FP_EXCEPTIONS_ENABLED -// These functions are all inlined NOPs if FP_EXCEPTIONS_ENABLED is not defined. -FPExceptionDisabler::FPExceptionDisabler() -{ - // Retrieve the current state of the exception flags. This - // must be done before changing them. _MCW_EM is a bit - // mask representing all available exception masks. - _controlfp_s(&mOldValues, _MCW_EM, _MCW_EM); - // Set all of the exception flags, which suppresses FP - // exceptions on the x87 and SSE units. - _controlfp_s(0, _MCW_EM, _MCW_EM); -} - -FPExceptionDisabler::~FPExceptionDisabler() -{ - // Clear any pending FP exceptions. This must be done - // prior to enabling FP exceptions since otherwise there - // may be a 'deferred crash' as soon the exceptions are - // enabled. - _clearfp(); - - // Reset (possibly enabling) the exception status. - _controlfp_s(0, mOldValues, _MCW_EM); -} - -// Overflow, divide-by-zero, and invalid-operation are the FP -// exceptions most frequently associated with bugs. -FPExceptionEnabler::FPExceptionEnabler(unsigned int enableBits /*= _EM_OVERFLOW | _EM_ZERODIVIDE | _EM_INVALID*/) -{ - // Retrieve the current state of the exception flags. This - // must be done before changing them. _MCW_EM is a bit - // mask representing all available exception masks. - _controlfp_s(&mOldValues, _MCW_EM, _MCW_EM); - - // Make sure no non-exception flags have been specified, - // to avoid accidental changing of rounding modes, etc. - enableBits &= _MCW_EM; - - // Clear any pending FP exceptions. This must be done - // prior to enabling FP exceptions since otherwise there - // may be a 'deferred crash' as soon the exceptions are - // enabled. - _clearfp(); - - // Zero out the specified bits, leaving other bits alone. - _controlfp_s(0, ~enableBits, enableBits); -} - -FPExceptionEnabler::~FPExceptionEnabler() -{ - // Reset the exception state. - _controlfp_s(0, mOldValues, _MCW_EM); -} -#endif +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: Math primitives. +// +//===========================================================================// + +/// FIXME: As soon as all references to mathlib.c are gone, include it in here + +#include +#include // Needed for FLT_EPSILON + +#include "tier0/basetypes.h" +#include +#include "tier0/dbg.h" + +#include "tier0/vprof.h" +//#define _VPROF_MATHLIB + +#pragma warning(disable:4244) // "conversion from 'const int' to 'float', possible loss of data" +#pragma warning(disable:4730) // "mixing _m64 and floating point expressions may result in incorrect code" + +#include "mathlib/mathlib.h" +#include "mathlib/vector.h" +#if !defined( _X360 ) +#include "mathlib/amd3dx.h" +#ifndef OSX +#include "3dnow.h" +#endif +#include "sse.h" +#endif + +#include "mathlib/ssemath.h" +#include "mathlib/ssequaternion.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +bool s_bMathlibInitialized = false; + +#ifdef PARANOID +// User must provide an implementation of Sys_Error() +void Sys_Error (char *error, ...); +#endif + +const Vector vec3_origin(0,0,0); +const QAngle vec3_angle(0,0,0); +const Vector vec3_invalid( FLT_MAX, FLT_MAX, FLT_MAX ); +const int nanmask = 255<<23; + +//----------------------------------------------------------------------------- +// Standard C implementations of optimized routines: +//----------------------------------------------------------------------------- +float _sqrtf(float _X) +{ + Assert( s_bMathlibInitialized ); + return sqrtf(_X); +} + +float _rsqrtf(float x) +{ + Assert( s_bMathlibInitialized ); + + return 1.f / _sqrtf( x ); +} + +float FASTCALL _VectorNormalize (Vector& vec) +{ +#ifdef _VPROF_MATHLIB + VPROF_BUDGET( "_VectorNormalize", "Mathlib" ); +#endif + Assert( s_bMathlibInitialized ); + float radius = sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z); + + // FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. + float iradius = 1.f / ( radius + FLT_EPSILON ); + + vec.x *= iradius; + vec.y *= iradius; + vec.z *= iradius; + + return radius; +} + +// TODO: Add fast C VectorNormalizeFast. +// Perhaps use approximate rsqrt trick, if the accuracy isn't too bad. +void FASTCALL _VectorNormalizeFast (Vector& vec) +{ + Assert( s_bMathlibInitialized ); + + // FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero. + float iradius = 1.f / ( sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z) + FLT_EPSILON ); + + vec.x *= iradius; + vec.y *= iradius; + vec.z *= iradius; + +} + +float _InvRSquared(const float* v) +{ + Assert( s_bMathlibInitialized ); + float r2 = DotProduct(v, v); + return r2 < 1.f ? 1.f : 1/r2; +} + +//----------------------------------------------------------------------------- +// Function pointers selecting the appropriate implementation +//----------------------------------------------------------------------------- +float (*pfSqrt)(float x) = _sqrtf; +float (*pfRSqrt)(float x) = _rsqrtf; +float (*pfRSqrtFast)(float x) = _rsqrtf; +float (FASTCALL *pfVectorNormalize)(Vector& v) = _VectorNormalize; +void (FASTCALL *pfVectorNormalizeFast)(Vector& v) = _VectorNormalizeFast; +float (*pfInvRSquared)(const float* v) = _InvRSquared; +void (*pfFastSinCos)(float x, float* s, float* c) = SinCos; +float (*pfFastCos)(float x) = cosf; + +float SinCosTable[SIN_TABLE_SIZE]; +void InitSinCosTable() +{ + for( int i = 0; i < SIN_TABLE_SIZE; i++ ) + { + SinCosTable[i] = sin(i * 2.0 * M_PI / SIN_TABLE_SIZE); + } +} + +qboolean VectorsEqual( const float *v1, const float *v2 ) +{ + Assert( s_bMathlibInitialized ); + return ( ( v1[0] == v2[0] ) && + ( v1[1] == v2[1] ) && + ( v1[2] == v2[2] ) ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Generates Euler angles given a left-handed orientation matrix. The +// columns of the matrix contain the forward, left, and up vectors. +// Input : matrix - Left-handed orientation matrix. +// angles[PITCH, YAW, ROLL]. Receives right-handed counterclockwise +// rotations in degrees around Y, Z, and X respectively. +//----------------------------------------------------------------------------- + +void MatrixAngles( const matrix3x4_t& matrix, RadianEuler &angles, Vector &position ) +{ + MatrixGetColumn( matrix, 3, position ); + MatrixAngles( matrix, angles ); +} + +void MatrixAngles( const matrix3x4_t &matrix, Quaternion &q, Vector &pos ) +{ +#ifdef _VPROF_MATHLIB + VPROF_BUDGET( "MatrixQuaternion", "Mathlib" ); +#endif + float trace; + trace = matrix[0][0] + matrix[1][1] + matrix[2][2] + 1.0f; + if( trace > 1.0f + FLT_EPSILON ) + { + // VPROF_INCREMENT_COUNTER("MatrixQuaternion A",1); + q.x = ( matrix[2][1] - matrix[1][2] ); + q.y = ( matrix[0][2] - matrix[2][0] ); + q.z = ( matrix[1][0] - matrix[0][1] ); + q.w = trace; + } + else if ( matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2] ) + { + // VPROF_INCREMENT_COUNTER("MatrixQuaternion B",1); + trace = 1.0f + matrix[0][0] - matrix[1][1] - matrix[2][2]; + q.x = trace; + q.y = (matrix[1][0] + matrix[0][1] ); + q.z = (matrix[0][2] + matrix[2][0] ); + q.w = (matrix[2][1] - matrix[1][2] ); + } + else if (matrix[1][1] > matrix[2][2]) + { + // VPROF_INCREMENT_COUNTER("MatrixQuaternion C",1); + trace = 1.0f + matrix[1][1] - matrix[0][0] - matrix[2][2]; + q.x = (matrix[0][1] + matrix[1][0] ); + q.y = trace; + q.z = (matrix[2][1] + matrix[1][2] ); + q.w = (matrix[0][2] - matrix[2][0] ); + } + else + { + // VPROF_INCREMENT_COUNTER("MatrixQuaternion D",1); + trace = 1.0f + matrix[2][2] - matrix[0][0] - matrix[1][1]; + q.x = (matrix[0][2] + matrix[2][0] ); + q.y = (matrix[2][1] + matrix[1][2] ); + q.z = trace; + q.w = (matrix[1][0] - matrix[0][1] ); + } + + QuaternionNormalize( q ); + +#if 0 + // check against the angle version + RadianEuler ang; + MatrixAngles( matrix, ang ); + Quaternion test; + AngleQuaternion( ang, test ); + float d = QuaternionDotProduct( q, test ); + Assert( fabs(d) > 0.99 && fabs(d) < 1.01 ); +#endif + + MatrixGetColumn( matrix, 3, pos ); +} + +void MatrixAngles( const matrix3x4_t& matrix, float *angles ) +{ +#ifdef _VPROF_MATHLIB + VPROF_BUDGET( "MatrixAngles", "Mathlib" ); +#endif + Assert( s_bMathlibInitialized ); + float forward[3]; + float left[3]; + float up[3]; + + // + // Extract the basis vectors from the matrix. Since we only need the Z + // component of the up vector, we don't get X and Y. + // + forward[0] = matrix[0][0]; + forward[1] = matrix[1][0]; + forward[2] = matrix[2][0]; + left[0] = matrix[0][1]; + left[1] = matrix[1][1]; + left[2] = matrix[2][1]; + up[2] = matrix[2][2]; + + float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); + + // enough here to get angles? + if ( xyDist > 0.001f ) + { + // (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis + angles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); + + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); + + // (roll) z = ATAN( left.z, up.z ); + angles[2] = RAD2DEG( atan2f( left[2], up[2] ) ); + } + else // forward is mostly Z, gimbal lock- + { + // (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw + angles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); + + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + angles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); + + // Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) + angles[2] = 0; + } +} + + +// transform in1 by the matrix in2 +void VectorTransform (const float *in1, const matrix3x4_t& in2, float *out) +{ + Assert( s_bMathlibInitialized ); + Assert( in1 != out ); + out[0] = DotProduct(in1, in2[0]) + in2[0][3]; + out[1] = DotProduct(in1, in2[1]) + in2[1][3]; + out[2] = DotProduct(in1, in2[2]) + in2[2][3]; +} + + +// assuming the matrix is orthonormal, transform in1 by the transpose (also the inverse in this case) of in2. +void VectorITransform (const float *in1, const matrix3x4_t& in2, float *out) +{ + Assert( s_bMathlibInitialized ); + float in1t[3]; + + in1t[0] = in1[0] - in2[0][3]; + in1t[1] = in1[1] - in2[1][3]; + in1t[2] = in1[2] - in2[2][3]; + + out[0] = in1t[0] * in2[0][0] + in1t[1] * in2[1][0] + in1t[2] * in2[2][0]; + out[1] = in1t[0] * in2[0][1] + in1t[1] * in2[1][1] + in1t[2] * in2[2][1]; + out[2] = in1t[0] * in2[0][2] + in1t[1] * in2[1][2] + in1t[2] * in2[2][2]; +} + + +// assume in2 is a rotation and rotate the input vector +void VectorRotate( const float *in1, const matrix3x4_t& in2, float *out ) +{ + Assert( s_bMathlibInitialized ); + Assert( in1 != out ); + out[0] = DotProduct( in1, in2[0] ); + out[1] = DotProduct( in1, in2[1] ); + out[2] = DotProduct( in1, in2[2] ); +} + +// assume in2 is a rotation and rotate the input vector +void VectorRotate( const Vector &in1, const QAngle &in2, Vector &out ) +{ + matrix3x4_t matRotate; + AngleMatrix( in2, matRotate ); + VectorRotate( in1, matRotate, out ); +} + +// assume in2 is a rotation and rotate the input vector +void VectorRotate( const Vector &in1, const Quaternion &in2, Vector &out ) +{ + matrix3x4_t matRotate; + QuaternionMatrix( in2, matRotate ); + VectorRotate( in1, matRotate, out ); +} + + +// rotate by the inverse of the matrix +void VectorIRotate( const float *in1, const matrix3x4_t& in2, float *out ) +{ + Assert( s_bMathlibInitialized ); + Assert( in1 != out ); + out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0]; + out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1]; + out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2]; +} + +#ifndef VECTOR_NO_SLOW_OPERATIONS +// transform a set of angles in the output space of parentMatrix to the input space +QAngle TransformAnglesToLocalSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) +{ + matrix3x4_t angToWorld, worldToParent, localMatrix; + MatrixInvert( parentMatrix, worldToParent ); + AngleMatrix( angles, angToWorld ); + ConcatTransforms( worldToParent, angToWorld, localMatrix ); + + QAngle out; + MatrixAngles( localMatrix, out ); + return out; +} + +// transform a set of angles in the input space of parentMatrix to the output space +QAngle TransformAnglesToWorldSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ) +{ + matrix3x4_t angToParent, angToWorld; + AngleMatrix( angles, angToParent ); + ConcatTransforms( parentMatrix, angToParent, angToWorld ); + QAngle out; + MatrixAngles( angToWorld, out ); + return out; +} + +#endif // VECTOR_NO_SLOW_OPERATIONS + +void MatrixInitialize( matrix3x4_t &mat, const Vector &vecOrigin, const Vector &vecXAxis, const Vector &vecYAxis, const Vector &vecZAxis ) +{ + MatrixSetColumn( vecXAxis, 0, mat ); + MatrixSetColumn( vecYAxis, 1, mat ); + MatrixSetColumn( vecZAxis, 2, mat ); + MatrixSetColumn( vecOrigin, 3, mat ); +} + +void MatrixCopy( const matrix3x4_t& in, matrix3x4_t& out ) +{ + Assert( s_bMathlibInitialized ); + memcpy( out.Base(), in.Base(), sizeof( float ) * 3 * 4 ); +} + +//----------------------------------------------------------------------------- +// Matrix equality test +//----------------------------------------------------------------------------- +bool MatricesAreEqual( const matrix3x4_t &src1, const matrix3x4_t &src2, float flTolerance ) +{ + for ( int i = 0; i < 3; ++i ) + { + for ( int j = 0; j < 4; ++j ) + { + if ( fabs( src1[i][j] - src2[i][j] ) > flTolerance ) + return false; + } + } + return true; +} + +// NOTE: This is just the transpose not a general inverse +void MatrixInvert( const matrix3x4_t& in, matrix3x4_t& out ) +{ + Assert( s_bMathlibInitialized ); + if ( &in == &out ) + { + V_swap(out[0][1],out[1][0]); + V_swap(out[0][2],out[2][0]); + V_swap(out[1][2],out[2][1]); + } + else + { + // transpose the matrix + out[0][0] = in[0][0]; + out[0][1] = in[1][0]; + out[0][2] = in[2][0]; + + out[1][0] = in[0][1]; + out[1][1] = in[1][1]; + out[1][2] = in[2][1]; + + out[2][0] = in[0][2]; + out[2][1] = in[1][2]; + out[2][2] = in[2][2]; + } + + // now fix up the translation to be in the other space + float tmp[3]; + tmp[0] = in[0][3]; + tmp[1] = in[1][3]; + tmp[2] = in[2][3]; + + out[0][3] = -DotProduct( tmp, out[0] ); + out[1][3] = -DotProduct( tmp, out[1] ); + out[2][3] = -DotProduct( tmp, out[2] ); +} + +void MatrixGetColumn( const matrix3x4_t& in, int column, Vector &out ) +{ + out.x = in[0][column]; + out.y = in[1][column]; + out.z = in[2][column]; +} + +void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out ) +{ + out[0][column] = in.x; + out[1][column] = in.y; + out[2][column] = in.z; +} + +void MatrixScaleBy ( const float flScale, matrix3x4_t &out ) +{ + out[0][0] *= flScale; + out[1][0] *= flScale; + out[2][0] *= flScale; + out[0][1] *= flScale; + out[1][1] *= flScale; + out[2][1] *= flScale; + out[0][2] *= flScale; + out[1][2] *= flScale; + out[2][2] *= flScale; +} + +void MatrixScaleByZero ( matrix3x4_t &out ) +{ + out[0][0] = 0.0f; + out[1][0] = 0.0f; + out[2][0] = 0.0f; + out[0][1] = 0.0f; + out[1][1] = 0.0f; + out[2][1] = 0.0f; + out[0][2] = 0.0f; + out[1][2] = 0.0f; + out[2][2] = 0.0f; +} + + + +int VectorCompare (const float *v1, const float *v2) +{ + Assert( s_bMathlibInitialized ); + int i; + + for (i=0 ; i<3 ; i++) + if (v1[i] != v2[i]) + return 0; + + return 1; +} + +void CrossProduct (const float* v1, const float* v2, float* cross) +{ + Assert( s_bMathlibInitialized ); + Assert( v1 != cross ); + Assert( v2 != cross ); + cross[0] = v1[1]*v2[2] - v1[2]*v2[1]; + cross[1] = v1[2]*v2[0] - v1[0]*v2[2]; + cross[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +int Q_log2(int val) +{ + int answer=0; + while (val>>=1) + answer++; + return answer; +} + +// Matrix is right-handed x=forward, y=left, z=up. We a left-handed convention for vectors in the game code (forward, right, up) +void MatrixVectors( const matrix3x4_t &matrix, Vector* pForward, Vector *pRight, Vector *pUp ) +{ + MatrixGetColumn( matrix, 0, *pForward ); + MatrixGetColumn( matrix, 1, *pRight ); + MatrixGetColumn( matrix, 2, *pUp ); + *pRight *= -1.0f; +} + + +void VectorVectors( const Vector &forward, Vector &right, Vector &up ) +{ + Assert( s_bMathlibInitialized ); + Vector tmp; + + if (forward[0] == 0 && forward[1] == 0) + { + // pitch 90 degrees up/down from identity + right[0] = 0; + right[1] = -1; + right[2] = 0; + up[0] = -forward[2]; + up[1] = 0; + up[2] = 0; + } + else + { + tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0; + CrossProduct( forward, tmp, right ); + VectorNormalize( right ); + CrossProduct( right, forward, up ); + VectorNormalize( up ); + } +} + +void VectorMatrix( const Vector &forward, matrix3x4_t& matrix) +{ + Assert( s_bMathlibInitialized ); + Vector right, up; + VectorVectors(forward, right, up); + + MatrixSetColumn( forward, 0, matrix ); + MatrixSetColumn( -right, 1, matrix ); + MatrixSetColumn( up, 2, matrix ); +} + + +void VectorAngles( const float *forward, float *angles ) +{ + Assert( s_bMathlibInitialized ); + float tmp, yaw, pitch; + + if (forward[1] == 0 && forward[0] == 0) + { + yaw = 0; + if (forward[2] > 0) + pitch = 270; + else + pitch = 90; + } + else + { + yaw = (atan2(forward[1], forward[0]) * 180 / M_PI); + if (yaw < 0) + yaw += 360; + + tmp = sqrt (forward[0]*forward[0] + forward[1]*forward[1]); + pitch = (atan2(-forward[2], tmp) * 180 / M_PI); + if (pitch < 0) + pitch += 360; + } + + angles[0] = pitch; + angles[1] = yaw; + angles[2] = 0; +} + + +/* +================ +R_ConcatRotations +================ +*/ +void ConcatRotations (const float in1[3][3], const float in2[3][3], float out[3][3]) +{ + Assert( s_bMathlibInitialized ); + Assert( in1 != out ); + Assert( in2 != out ); + out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + + in1[0][2] * in2[2][0]; + out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + + in1[0][2] * in2[2][1]; + out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + + in1[0][2] * in2[2][2]; + out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + + in1[1][2] * in2[2][0]; + out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + + in1[1][2] * in2[2][1]; + out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + + in1[1][2] * in2[2][2]; + out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + + in1[2][2] * in2[2][0]; + out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + + in1[2][2] * in2[2][1]; + out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + + in1[2][2] * in2[2][2]; +} + +void ConcatTransforms_Aligned( const matrix3x4_t &m0, const matrix3x4_t &m1, matrix3x4_t &out ) +{ + Assert( (((size_t)&m0) % 16) == 0 ); + Assert( (((size_t)&m1) % 16) == 0 ); + Assert( (((size_t)&out) % 16) == 0 ); + + fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); + fltx4 rowA0 = LoadAlignedSIMD( m0.m_flMatVal[0] ); + fltx4 rowA1 = LoadAlignedSIMD( m0.m_flMatVal[1] ); + fltx4 rowA2 = LoadAlignedSIMD( m0.m_flMatVal[2] ); + + fltx4 rowB0 = LoadAlignedSIMD( m1.m_flMatVal[0] ); + fltx4 rowB1 = LoadAlignedSIMD( m1.m_flMatVal[1] ); + fltx4 rowB2 = LoadAlignedSIMD( m1.m_flMatVal[2] ); + + // now we have the rows of m0 and the columns of m1 + // first output row + fltx4 A0 = SplatXSIMD(rowA0); + fltx4 A1 = SplatYSIMD(rowA0); + fltx4 A2 = SplatZSIMD(rowA0); + fltx4 mul00 = MulSIMD( A0, rowB0 ); + fltx4 mul01 = MulSIMD( A1, rowB1 ); + fltx4 mul02 = MulSIMD( A2, rowB2 ); + fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); + + // second output row + A0 = SplatXSIMD(rowA1); + A1 = SplatYSIMD(rowA1); + A2 = SplatZSIMD(rowA1); + fltx4 mul10 = MulSIMD( A0, rowB0 ); + fltx4 mul11 = MulSIMD( A1, rowB1 ); + fltx4 mul12 = MulSIMD( A2, rowB2 ); + fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); + + // third output row + A0 = SplatXSIMD(rowA2); + A1 = SplatYSIMD(rowA2); + A2 = SplatZSIMD(rowA2); + fltx4 mul20 = MulSIMD( A0, rowB0 ); + fltx4 mul21 = MulSIMD( A1, rowB1 ); + fltx4 mul22 = MulSIMD( A2, rowB2 ); + fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); + + // add in translation vector + A0 = AndSIMD(rowA0,lastMask); + A1 = AndSIMD(rowA1,lastMask); + A2 = AndSIMD(rowA2,lastMask); + out0 = AddSIMD(out0, A0); + out1 = AddSIMD(out1, A1); + out2 = AddSIMD(out2, A2); + + StoreAlignedSIMD( out.m_flMatVal[0], out0 ); + StoreAlignedSIMD( out.m_flMatVal[1], out1 ); + StoreAlignedSIMD( out.m_flMatVal[2], out2 ); +} + +/* +================ +R_ConcatTransforms +================ +*/ + +void ConcatTransforms (const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out) +{ +#if 0 + // test for ones that'll be 2x faster + if ( (((size_t)&in1) % 16) == 0 && (((size_t)&in2) % 16) == 0 && (((size_t)&out) % 16) == 0 ) + { + ConcatTransforms_Aligned( in1, in2, out ); + return; + } +#endif + + fltx4 lastMask = *(fltx4 *)(&g_SIMD_ComponentMask[3]); + fltx4 rowA0 = LoadUnalignedSIMD( in1.m_flMatVal[0] ); + fltx4 rowA1 = LoadUnalignedSIMD( in1.m_flMatVal[1] ); + fltx4 rowA2 = LoadUnalignedSIMD( in1.m_flMatVal[2] ); + + fltx4 rowB0 = LoadUnalignedSIMD( in2.m_flMatVal[0] ); + fltx4 rowB1 = LoadUnalignedSIMD( in2.m_flMatVal[1] ); + fltx4 rowB2 = LoadUnalignedSIMD( in2.m_flMatVal[2] ); + + // now we have the rows of m0 and the columns of m1 + // first output row + fltx4 A0 = SplatXSIMD(rowA0); + fltx4 A1 = SplatYSIMD(rowA0); + fltx4 A2 = SplatZSIMD(rowA0); + fltx4 mul00 = MulSIMD( A0, rowB0 ); + fltx4 mul01 = MulSIMD( A1, rowB1 ); + fltx4 mul02 = MulSIMD( A2, rowB2 ); + fltx4 out0 = AddSIMD( mul00, AddSIMD(mul01,mul02) ); + + // second output row + A0 = SplatXSIMD(rowA1); + A1 = SplatYSIMD(rowA1); + A2 = SplatZSIMD(rowA1); + fltx4 mul10 = MulSIMD( A0, rowB0 ); + fltx4 mul11 = MulSIMD( A1, rowB1 ); + fltx4 mul12 = MulSIMD( A2, rowB2 ); + fltx4 out1 = AddSIMD( mul10, AddSIMD(mul11,mul12) ); + + // third output row + A0 = SplatXSIMD(rowA2); + A1 = SplatYSIMD(rowA2); + A2 = SplatZSIMD(rowA2); + fltx4 mul20 = MulSIMD( A0, rowB0 ); + fltx4 mul21 = MulSIMD( A1, rowB1 ); + fltx4 mul22 = MulSIMD( A2, rowB2 ); + fltx4 out2 = AddSIMD( mul20, AddSIMD(mul21,mul22) ); + + // add in translation vector + A0 = AndSIMD(rowA0,lastMask); + A1 = AndSIMD(rowA1,lastMask); + A2 = AndSIMD(rowA2,lastMask); + out0 = AddSIMD(out0, A0); + out1 = AddSIMD(out1, A1); + out2 = AddSIMD(out2, A2); + + // write to output + StoreUnalignedSIMD( out.m_flMatVal[0], out0 ); + StoreUnalignedSIMD( out.m_flMatVal[1], out1 ); + StoreUnalignedSIMD( out.m_flMatVal[2], out2 ); +} + + +/* +=================== +FloorDivMod + +Returns mathematically correct (floor-based) quotient and remainder for +numer and denom, both of which should contain no fractional part. The +quotient must fit in 32 bits. +==================== +*/ + +void FloorDivMod (double numer, double denom, int *quotient, + int *rem) +{ + Assert( s_bMathlibInitialized ); + int q, r; + double x; + +#ifdef PARANOID + if (denom <= 0.0) + Sys_Error ("FloorDivMod: bad denominator %d\n", denom); + +// if ((floor(numer) != numer) || (floor(denom) != denom)) +// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n", +// numer, denom); +#endif + + if (numer >= 0.0) + { + + x = floor(numer / denom); + q = (int)x; + r = Floor2Int(numer - (x * denom)); + } + else + { + // + // perform operations with positive values, and fix mod to make floor-based + // + x = floor(-numer / denom); + q = -(int)x; + r = Floor2Int(-numer - (x * denom)); + if (r != 0) + { + q--; + r = (int)denom - r; + } + } + + *quotient = q; + *rem = r; +} + + +/* +=================== +GreatestCommonDivisor +==================== +*/ +int GreatestCommonDivisor (int i1, int i2) +{ + Assert( s_bMathlibInitialized ); + if (i1 > i2) + { + if (i2 == 0) + return (i1); + return GreatestCommonDivisor (i2, i1 % i2); + } + else + { + if (i1 == 0) + return (i2); + return GreatestCommonDivisor (i1, i2 % i1); + } +} + + +bool IsDenormal( const float &val ) +{ + const int x = *reinterpret_cast (&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 ); + + // Note if the quaternion is slightly non-normalized the square root below may be more than 1, + // the value is clamped to one otherwise it may result in asin() returning an undefined result. + float sinang = MIN( 1.0f, sqrt( diff.x * diff.x + diff.y * diff.y + diff.z * diff.z ) ); + float angle = RAD2DEG( 2 * asin( sinang ) ); + return angle; +#else + Quaternion q2; + QuaternionAlign( p, q, q2 ); + + Assert( s_bMathlibInitialized ); + float cosom = p.x * q2.x + p.y * q2.y + p.z * q2.z + p.w * q2.w; + + if ( cosom > -1.0f ) + { + if ( cosom < 1.0f ) + { + float omega = 2 * fabs( acos( cosom ) ); + return RAD2DEG( omega ); + } + return 0.0f; + } + + return 180.0f; +#endif +} + +void QuaternionConjugate( const Quaternion &p, Quaternion &q ) +{ + Assert( s_bMathlibInitialized ); + Assert( q.IsValid() ); + + q.x = -p.x; + q.y = -p.y; + q.z = -p.z; + q.w = p.w; +} + +void QuaternionInvert( const Quaternion &p, Quaternion &q ) +{ + Assert( s_bMathlibInitialized ); + Assert( q.IsValid() ); + + QuaternionConjugate( p, q ); + + float magnitudeSqr = QuaternionDotProduct( p, p ); + Assert( magnitudeSqr ); + if ( magnitudeSqr ) + { + float inv = 1.0f / magnitudeSqr; + q.x *= inv; + q.y *= inv; + q.z *= inv; + q.w *= inv; + } +} + +//----------------------------------------------------------------------------- +// Make sure the quaternion is of unit length +//----------------------------------------------------------------------------- +float QuaternionNormalize( Quaternion &q ) +{ + Assert( s_bMathlibInitialized ); + float radius, iradius; + + Assert( q.IsValid() ); + + radius = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + + if ( radius ) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON)) + { + radius = sqrt(radius); + iradius = 1.0f/radius; + q[3] *= iradius; + q[2] *= iradius; + q[1] *= iradius; + q[0] *= iradius; + } + return radius; +} + + +void QuaternionScale( const Quaternion &p, float t, Quaternion &q ) +{ + Assert( s_bMathlibInitialized ); + +#if 0 + Quaternion p0; + Quaternion q; + p0.Init( 0.0, 0.0, 0.0, 1.0 ); + + // slerp in "reverse order" so that p doesn't get realigned + QuaternionSlerp( p, p0, 1.0 - fabs( t ), q ); + if (t < 0.0) + { + q.w = -q.w; + } +#else + float r; + + // FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to + // use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale. + float sinom = sqrt( DotProduct( &p.x, &p.x ) ); + sinom = min( sinom, 1.f ); + + float sinsom = sin( asin( sinom ) * t ); + + t = sinsom / (sinom + FLT_EPSILON); + VectorScale( &p.x, t, &q.x ); + + // rescale rotation + r = 1.0f - sinsom * sinsom; + + // Assert( r >= 0 ); + if (r < 0.0f) + r = 0.0f; + r = sqrt( r ); + + // keep sign of rotation + if (p.w < 0) + q.w = -r; + else + q.w = r; +#endif + + Assert( q.IsValid() ); + + return; +} + + +void QuaternionAdd( const Quaternion &p, const Quaternion &q, Quaternion &qt ) +{ + Assert( s_bMathlibInitialized ); + Assert( p.IsValid() ); + Assert( q.IsValid() ); + + // decide if one of the quaternions is backwards + Quaternion q2; + QuaternionAlign( p, q, q2 ); + + // is this right??? + qt[0] = p[0] + q2[0]; + qt[1] = p[1] + q2[1]; + qt[2] = p[2] + q2[2]; + qt[3] = p[3] + q2[3]; + + return; +} + + +float QuaternionDotProduct( const Quaternion &p, const Quaternion &q ) +{ + Assert( s_bMathlibInitialized ); + Assert( p.IsValid() ); + Assert( q.IsValid() ); + + return p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w; +} + + +// qt = p * q +void QuaternionMult( const Quaternion &p, const Quaternion &q, Quaternion &qt ) +{ + Assert( s_bMathlibInitialized ); + Assert( p.IsValid() ); + Assert( q.IsValid() ); + + if (&p == &qt) + { + Quaternion p2 = p; + QuaternionMult( p2, q, qt ); + return; + } + + // decide if one of the quaternions is backwards + Quaternion q2; + QuaternionAlign( p, q, q2 ); + + qt.x = p.x * q2.w + p.y * q2.z - p.z * q2.y + p.w * q2.x; + qt.y = -p.x * q2.z + p.y * q2.w + p.z * q2.x + p.w * q2.y; + qt.z = p.x * q2.y - p.y * q2.x + p.z * q2.w + p.w * q2.z; + qt.w = -p.x * q2.x - p.y * q2.y - p.z * q2.z + p.w * q2.w; +} + + +void QuaternionMatrix( const Quaternion &q, const Vector &pos, matrix3x4_t& matrix ) +{ + 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 +#pragma optimize( "g", off ) + +void Hermite_Spline( const Vector &p0, const Vector &p1, const Vector &p2, float t, Vector& output ) +{ + Vector e10, e21; + VectorSubtract( p1, p0, e10 ); + VectorSubtract( p2, p1, e21 ); + Hermite_Spline( p1, p2, e10, e21, t, output ); +} + +#pragma optimize( "", on ) + +float Hermite_Spline( float p0, float p1, float p2, float t ) +{ + return Hermite_Spline( p1, p2, p1 - p0, p2 - p1, t ); +} + + +void Hermite_Spline( const Quaternion &q0, const Quaternion &q1, const Quaternion &q2, float t, Quaternion &output ) +{ + // cheap, hacked version of quaternions + Quaternion q0a; + Quaternion q1a; + + QuaternionAlign( q2, q0, q0a ); + QuaternionAlign( q2, q1, q1a ); + + output.x = Hermite_Spline( q0a.x, q1a.x, q2.x, t ); + output.y = Hermite_Spline( q0a.y, q1a.y, q2.y, t ); + output.z = Hermite_Spline( q0a.z, q1a.z, q2.z, t ); + output.w = Hermite_Spline( q0a.w, q1a.w, q2.w, t ); + + QuaternionNormalize( output ); +} + +// See http://en.wikipedia.org/wiki/Kochanek-Bartels_curves +// +// Tension: -1 = Round -> 1 = Tight +// Bias: -1 = Pre-shoot (bias left) -> 1 = Post-shoot (bias right) +// Continuity: -1 = Box corners -> 1 = Inverted corners +// +// If T=B=C=0 it's the same matrix as Catmull-Rom. +// If T=1 & B=C=0 it's the same as Cubic. +// If T=B=0 & C=-1 it's just linear interpolation +// +// See http://news.povray.org/povray.binaries.tutorials/attachment/%3CXns91B880592482seed7@povray.org%3E/Splines.bas.txt +// for example code and descriptions of various spline types... +// +void Kochanek_Bartels_Spline( + float tension, + float bias, + float continuity, + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Assert( s_bMathlibInitialized ); + + float ffa, ffb, ffc, ffd; + + ffa = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f + bias ); + ffb = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f - bias ); + ffc = ( 1.0f - tension ) * ( 1.0f - continuity ) * ( 1.0f + bias ); + ffd = ( 1.0f - tension ) * ( 1.0f + continuity ) * ( 1.0f - bias ); + + float tSqr = t*t*0.5f; + float tSqrSqr = t*tSqr; + t *= 0.5f; + + Assert( &output != &p1 ); + Assert( &output != &p2 ); + Assert( &output != &p3 ); + Assert( &output != &p4 ); + + output.Init(); + + Vector a, b, c, d; + + // matrix row 1 + VectorScale( p1, tSqrSqr * -ffa, a ); + VectorScale( p2, tSqrSqr * ( 4.0f + ffa - ffb - ffc ), b ); + VectorScale( p3, tSqrSqr * ( -4.0f + ffb + ffc - ffd ), c ); + VectorScale( p4, tSqrSqr * ffd, d ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + VectorAdd( d, output, output ); + + // matrix row 2 + VectorScale( p1, tSqr* 2 * ffa, a ); + VectorScale( p2, tSqr * ( -6 - 2 * ffa + 2 * ffb + ffc ), b ); + VectorScale( p3, tSqr * ( 6 - 2 * ffb - ffc + ffd ), c ); + VectorScale( p4, tSqr * -ffd, d ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + VectorAdd( d, output, output ); + + // matrix row 3 + VectorScale( p1, t * -ffa, a ); + VectorScale( p2, t * ( ffa - ffb ), b ); + VectorScale( p3, t * ffb, c ); + // p4 unchanged + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + + // matrix row 4 + // p1, p3, p4 unchanged + // p2 is multiplied by 1 and added, so just added it directly + + VectorAdd( p2, output, output ); +} + +void Kochanek_Bartels_Spline_NormalizeX( + float tension, + float bias, + float continuity, + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Vector p1n, p4n; + Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); + Kochanek_Bartels_Spline( tension, bias, continuity, p1n, p2, p3, p4n, t, output ); +} + +void Cubic_Spline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Assert( s_bMathlibInitialized ); + + float tSqr = t*t; + float tSqrSqr = t*tSqr; + + Assert( &output != &p1 ); + Assert( &output != &p2 ); + Assert( &output != &p3 ); + Assert( &output != &p4 ); + + output.Init(); + + Vector a, b, c, d; + + // matrix row 1 + VectorScale( p2, tSqrSqr * 2, b ); + VectorScale( p3, tSqrSqr * -2, c ); + + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + + // matrix row 2 + VectorScale( p2, tSqr * -3, b ); + VectorScale( p3, tSqr * 3, c ); + + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + + // matrix row 3 + // no influence + // p4 unchanged + + // matrix row 4 + // p1, p3, p4 unchanged + VectorAdd( p2, output, output ); +} + +void Cubic_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Vector p1n, p4n; + Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); + Cubic_Spline( p1n, p2, p3, p4n, t, output ); +} + +void BSpline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Assert( s_bMathlibInitialized ); + + float oneOver6 = 1.0f / 6.0f; + + float tSqr = t * t * oneOver6; + float tSqrSqr = t*tSqr; + t *= oneOver6; + + Assert( &output != &p1 ); + Assert( &output != &p2 ); + Assert( &output != &p3 ); + Assert( &output != &p4 ); + + output.Init(); + + Vector a, b, c, d; + + // matrix row 1 + VectorScale( p1, -tSqrSqr, a ); + VectorScale( p2, tSqrSqr * 3.0f, b ); + VectorScale( p3, tSqrSqr * -3.0f, c ); + VectorScale( p4, tSqrSqr, d ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + VectorAdd( d, output, output ); + + // matrix row 2 + VectorScale( p1, tSqr * 3.0f, a ); + VectorScale( p2, tSqr * -6.0f, b ); + VectorScale( p3, tSqr * 3.0f, c ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + + // matrix row 3 + VectorScale( p1, t * -3.0f, a ); + VectorScale( p3, t * 3.0f, c ); + // p4 unchanged + + VectorAdd( a, output, output ); + VectorAdd( c, output, output ); + + // matrix row 4 + // p1 and p3 scaled by 1.0f, so done below + VectorScale( p1, oneOver6, a ); + VectorScale( p2, 4.0f * oneOver6, b ); + VectorScale( p3, oneOver6, c ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); +} + +void BSpline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Vector p1n, p4n; + Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); + BSpline( p1n, p2, p3, p4n, t, output ); +} + +void Parabolic_Spline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Assert( s_bMathlibInitialized ); + + float tSqr = t*t*0.5f; + t *= 0.5f; + + Assert( &output != &p1 ); + Assert( &output != &p2 ); + Assert( &output != &p3 ); + Assert( &output != &p4 ); + + output.Init(); + + Vector a, b, c, d; + + // matrix row 1 + // no influence from t cubed + + // matrix row 2 + VectorScale( p1, tSqr, a ); + VectorScale( p2, tSqr * -2.0f, b ); + VectorScale( p3, tSqr, c ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + VectorAdd( c, output, output ); + + // matrix row 3 + VectorScale( p1, t * -2.0f, a ); + VectorScale( p2, t * 2.0f, b ); + // p4 unchanged + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); + + // matrix row 4 + VectorScale( p1, 0.5f, a ); + VectorScale( p2, 0.5f, b ); + + VectorAdd( a, output, output ); + VectorAdd( b, output, output ); +} + +void Parabolic_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ) +{ + Vector p1n, p4n; + Spline_Normalize( p1, p2, p3, p4, p1n, p4n ); + Parabolic_Spline( p1n, p2, p3, p4n, t, output ); +} + +//----------------------------------------------------------------------------- +// Purpose: Compress the input values for a ranged result such that from 75% to 200% smoothly of the range maps +//----------------------------------------------------------------------------- + +float RangeCompressor( float flValue, float flMin, float flMax, float flBase ) +{ + // clamp base + if (flBase < flMin) + flBase = flMin; + if (flBase > flMax) + flBase = flMax; + + flValue += flBase; + + // convert to 0 to 1 value + float flMid = (flValue - flMin) / (flMax - flMin); + // convert to -1 to 1 value + float flTarget = flMid * 2 - 1; + + if (fabs(flTarget) > 0.75) + { + float t = (fabs(flTarget) - 0.75) / (1.25); + if (t < 1.0) + { + if (flTarget > 0) + { + flTarget = Hermite_Spline( 0.75, 1, 0.75, 0, t ); + } + else + { + flTarget = -Hermite_Spline( 0.75, 1, 0.75, 0, t ); + } + } + else + { + flTarget = (flTarget > 0) ? 1.0f : -1.0f; + } + } + + flMid = (flTarget + 1 ) / 2.0; + flValue = flMin * (1 - flMid) + flMax * flMid; + + flValue -= flBase; + + return flValue; +} + + +//#pragma optimize( "", on ) + +//----------------------------------------------------------------------------- +// Transforms a AABB into another space; which will inherently grow the box. +//----------------------------------------------------------------------------- +void TransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) +{ + Vector localCenter; + VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); + localCenter *= 0.5f; + + Vector localExtents; + VectorSubtract( vecMaxsIn, localCenter, localExtents ); + + Vector worldCenter; + VectorTransform( localCenter, transform, worldCenter ); + + Vector worldExtents; + worldExtents.x = DotProductAbs( localExtents, transform[0] ); + worldExtents.y = DotProductAbs( localExtents, transform[1] ); + worldExtents.z = DotProductAbs( localExtents, transform[2] ); + + VectorSubtract( worldCenter, worldExtents, vecMinsOut ); + VectorAdd( worldCenter, worldExtents, vecMaxsOut ); +} + + +//----------------------------------------------------------------------------- +// Uses the inverse transform of in1 +//----------------------------------------------------------------------------- +void ITransformAABB( const matrix3x4_t& transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) +{ + Vector worldCenter; + VectorAdd( vecMinsIn, vecMaxsIn, worldCenter ); + worldCenter *= 0.5f; + + Vector worldExtents; + VectorSubtract( vecMaxsIn, worldCenter, worldExtents ); + + Vector localCenter; + VectorITransform( worldCenter, transform, localCenter ); + + Vector localExtents; + localExtents.x = FloatMakePositive( worldExtents.x * transform[0][0] ) + + FloatMakePositive( worldExtents.y * transform[1][0] ) + + FloatMakePositive( worldExtents.z * transform[2][0] ); + localExtents.y = FloatMakePositive( worldExtents.x * transform[0][1] ) + + FloatMakePositive( worldExtents.y * transform[1][1] ) + + FloatMakePositive( worldExtents.z * transform[2][1] ); + localExtents.z = FloatMakePositive( worldExtents.x * transform[0][2] ) + + FloatMakePositive( worldExtents.y * transform[1][2] ) + + FloatMakePositive( worldExtents.z * transform[2][2] ); + + VectorSubtract( localCenter, localExtents, vecMinsOut ); + VectorAdd( localCenter, localExtents, vecMaxsOut ); +} + + +//----------------------------------------------------------------------------- +// Rotates a AABB into another space; which will inherently grow the box. +// (same as TransformAABB, but doesn't take the translation into account) +//----------------------------------------------------------------------------- +void RotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) +{ + Vector localCenter; + VectorAdd( vecMinsIn, vecMaxsIn, localCenter ); + localCenter *= 0.5f; + + Vector localExtents; + VectorSubtract( vecMaxsIn, localCenter, localExtents ); + + Vector newCenter; + VectorRotate( localCenter, transform, newCenter ); + + Vector newExtents; + newExtents.x = DotProductAbs( localExtents, transform[0] ); + newExtents.y = DotProductAbs( localExtents, transform[1] ); + newExtents.z = DotProductAbs( localExtents, transform[2] ); + + VectorSubtract( newCenter, newExtents, vecMinsOut ); + VectorAdd( newCenter, newExtents, vecMaxsOut ); +} + + +//----------------------------------------------------------------------------- +// Uses the inverse transform of in1 +//----------------------------------------------------------------------------- +void IRotateAABB( const matrix3x4_t &transform, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ) +{ + Vector oldCenter; + VectorAdd( vecMinsIn, vecMaxsIn, oldCenter ); + oldCenter *= 0.5f; + + Vector oldExtents; + VectorSubtract( vecMaxsIn, oldCenter, oldExtents ); + + Vector newCenter; + VectorIRotate( oldCenter, transform, newCenter ); + + Vector newExtents; + newExtents.x = FloatMakePositive( oldExtents.x * transform[0][0] ) + + FloatMakePositive( oldExtents.y * transform[1][0] ) + + FloatMakePositive( oldExtents.z * transform[2][0] ); + newExtents.y = FloatMakePositive( oldExtents.x * transform[0][1] ) + + FloatMakePositive( oldExtents.y * transform[1][1] ) + + FloatMakePositive( oldExtents.z * transform[2][1] ); + newExtents.z = FloatMakePositive( oldExtents.x * transform[0][2] ) + + FloatMakePositive( oldExtents.y * transform[1][2] ) + + FloatMakePositive( oldExtents.z * transform[2][2] ); + + VectorSubtract( newCenter, newExtents, vecMinsOut ); + VectorAdd( newCenter, newExtents, vecMaxsOut ); +} + + +float CalcSqrDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point ) +{ + float flDelta; + float flDistSqr = 0.0f; + + if ( point.x < mins.x ) + { + flDelta = (mins.x - point.x); + flDistSqr += flDelta * flDelta; + } + else if ( point.x > maxs.x ) + { + flDelta = (point.x - maxs.x); + flDistSqr += flDelta * flDelta; + } + + if ( point.y < mins.y ) + { + flDelta = (mins.y - point.y); + flDistSqr += flDelta * flDelta; + } + else if ( point.y > maxs.y ) + { + flDelta = (point.y - maxs.y); + flDistSqr += flDelta * flDelta; + } + + if ( point.z < mins.z ) + { + flDelta = (mins.z - point.z); + flDistSqr += flDelta * flDelta; + } + else if ( point.z > maxs.z ) + { + flDelta = (point.z - maxs.z); + flDistSqr += flDelta * flDelta; + } + + return flDistSqr; +} + + +void CalcClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut ) +{ + closestOut.x = clamp( point.x, mins.x, maxs.x ); + closestOut.y = clamp( point.y, mins.y, maxs.y ); + closestOut.z = clamp( point.z, mins.z, maxs.z ); +} + +void CalcSqrDistAndClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut, float &distSqrOut ) +{ + distSqrOut = 0.0f; + for ( int i = 0; i < 3; i++ ) + { + if ( point[i] < mins[i] ) + { + closestOut[i] = mins[i]; + float flDelta = closestOut[i] - mins[i]; + distSqrOut += flDelta * flDelta; + } + else if ( point[i] > maxs[i] ) + { + closestOut[i] = maxs[i]; + float flDelta = closestOut[i] - maxs[i]; + distSqrOut += flDelta * flDelta; + } + else + { + closestOut[i] = point[i]; + } + } + +} + +float CalcClosestPointToLineT( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vDir ) +{ + Assert( s_bMathlibInitialized ); + VectorSubtract( vLineB, vLineA, vDir ); + + // D dot [P - (A + D*t)] = 0 + // t = ( DP - DA) / DD + float div = vDir.Dot( vDir ); + if( div < 0.00001f ) + { + return 0; + } + else + { + return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; + } +} + +void CalcClosestPointOnLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector vDir; + float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); + if ( outT ) *outT = t; + vClosest.MulAdd( vLineA, vDir, t ); +} + + +float CalcDistanceToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector vClosest; + CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); + return P.DistTo(vClosest); +} + +float CalcDistanceSqrToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector vClosest; + CalcClosestPointOnLine( P, vLineA, vLineB, vClosest, outT ); + return P.DistToSqr(vClosest); +} + +void CalcClosestPointOnLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *outT ) +{ + Vector vDir; + float t = CalcClosestPointToLineT( P, vLineA, vLineB, vDir ); + t = clamp( t, 0.f, 1.f ); + if ( outT ) + { + *outT = t; + } + vClosest.MulAdd( vLineA, vDir, t ); +} + + +float CalcDistanceToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector vClosest; + CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); + return P.DistTo( vClosest ); +} + +float CalcDistanceSqrToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector vClosest; + CalcClosestPointOnLineSegment( P, vLineA, vLineB, vClosest, outT ); + return P.DistToSqr(vClosest); +} + +float CalcClosestPointToLineT2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vDir ) +{ + Assert( s_bMathlibInitialized ); + Vector2DSubtract( vLineB, vLineA, vDir ); + + // D dot [P - (A + D*t)] = 0 + // t = (DP - DA) / DD + float div = vDir.Dot( vDir ); + if( div < 0.00001f ) + { + return 0; + } + else + { + return (vDir.Dot( P ) - vDir.Dot( vLineA )) / div; + } +} + +void CalcClosestPointOnLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector2D vDir; + float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); + if ( outT ) *outT = t; + vClosest.MulAdd( vLineA, vDir, t ); +} + +float CalcDistanceToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector2D vClosest; + CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); + return P.DistTo( vClosest ); +} + +float CalcDistanceSqrToLine2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector2D vClosest; + CalcClosestPointOnLine2D( P, vLineA, vLineB, vClosest, outT ); + return P.DistToSqr(vClosest); +} + +void CalcClosestPointOnLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, Vector2D &vClosest, float *outT ) +{ + Vector2D vDir; + float t = CalcClosestPointToLineT2D( P, vLineA, vLineB, vDir ); + t = clamp( t, 0.f, 1.f ); + if ( outT ) + { + *outT = t; + } + vClosest.MulAdd( vLineA, vDir, t ); +} + +float CalcDistanceToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector2D vClosest; + CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); + return P.DistTo( vClosest ); +} + +float CalcDistanceSqrToLineSegment2D( const Vector2D &P, const Vector2D &vLineA, const Vector2D &vLineB, float *outT ) +{ + Assert( s_bMathlibInitialized ); + Vector2D vClosest; + CalcClosestPointOnLineSegment2D( P, vLineA, vLineB, vClosest, outT ); + return P.DistToSqr( vClosest ); +} + +// Do we have another epsilon we could use +#define LINE_EPS ( 0.000001f ) + +//----------------------------------------------------------------------------- +// Purpose: Given lines p1->p2 and p3->p4, computes a line segment (pa->pb) and returns the parameters 0->1 multipliers +// along each segment for the returned points +// Input : p1 - +// p2 - +// p3 - +// p4 - +// *s1 - +// *s2 - +// Output : Returns true on success, false on failure. +//----------------------------------------------------------------------------- +bool CalcLineToLineIntersectionSegment( + const Vector& p1,const Vector& p2,const Vector& p3,const Vector& p4,Vector *s1,Vector *s2, + float *t1, float *t2) +{ + Vector p13,p43,p21; + float d1343,d4321,d1321,d4343,d2121; + float numer,denom; + + p13.x = p1.x - p3.x; + p13.y = p1.y - p3.y; + p13.z = p1.z - p3.z; + p43.x = p4.x - p3.x; + p43.y = p4.y - p3.y; + p43.z = p4.z - p3.z; + + if (fabs(p43.x) < LINE_EPS && fabs(p43.y) < LINE_EPS && fabs(p43.z) < LINE_EPS) + return false; + p21.x = p2.x - p1.x; + p21.y = p2.y - p1.y; + p21.z = p2.z - p1.z; + if (fabs(p21.x) < LINE_EPS && fabs(p21.y) < LINE_EPS && fabs(p21.z) < LINE_EPS) + return false; + + d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z; + d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z; + d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z; + d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z; + d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z; + + denom = d2121 * d4343 - d4321 * d4321; + if (fabs(denom) < LINE_EPS) + return false; + numer = d1343 * d4321 - d1321 * d4343; + + *t1 = numer / denom; + *t2 = (d1343 + d4321 * (*t1)) / d4343; + + s1->x = p1.x + *t1 * p21.x; + s1->y = p1.y + *t1 * p21.y; + s1->z = p1.z + *t1 * p21.z; + s2->x = p3.x + *t2 * p43.x; + s2->y = p3.y + *t2 * p43.y; + s2->z = p3.z + *t2 * p43.z; + + return true; +} + +#pragma optimize( "", off ) + +#ifndef EXCEPTION_EXECUTE_HANDLER +#define EXCEPTION_EXECUTE_HANDLER 1 +#endif + +#pragma optimize( "", on ) + +static bool s_b3DNowEnabled = false; +static bool s_bMMXEnabled = false; +static bool s_bSSEEnabled = false; +static bool s_bSSE2Enabled = false; + +void MathLib_Init( float gamma, float texGamma, float brightness, int overbright, bool bAllow3DNow, bool bAllowSSE, bool bAllowSSE2, bool bAllowMMX ) +{ + if ( s_bMathlibInitialized ) + return; + + // FIXME: Hook SSE into VectorAligned + Vector4DAligned + +#if !defined( _X360 ) + // Grab the processor information: + const CPUInformation& pi = *GetCPUInformation(); + + // Select the default generic routines. + pfSqrt = _sqrtf; + pfRSqrt = _rsqrtf; + pfRSqrtFast = _rsqrtf; + pfVectorNormalize = _VectorNormalize; + pfVectorNormalizeFast = _VectorNormalizeFast; + pfInvRSquared = _InvRSquared; + pfFastSinCos = SinCos; + pfFastCos = cosf; + + if ( bAllowMMX && pi.m_bMMX ) + { + // Select the MMX specific routines if available + // (MMX routines were used by SW span fillers - not currently used for HW) + s_bMMXEnabled = true; + } + else + { + s_bMMXEnabled = false; + } + + // SSE Generally performs better than 3DNow when present, so this is placed + // first to allow SSE to override these settings. +#if !defined( OSX ) && !defined( PLATFORM_WINDOWS_PC64 ) && !defined(LINUX) + if ( bAllow3DNow && pi.m_b3DNow ) + { + s_b3DNowEnabled = true; + + // Select the 3DNow specific routines if available; + pfVectorNormalize = _3DNow_VectorNormalize; + pfVectorNormalizeFast = _3DNow_VectorNormalizeFast; + pfInvRSquared = _3DNow_InvRSquared; + pfSqrt = _3DNow_Sqrt; + pfRSqrt = _3DNow_RSqrt; + pfRSqrtFast = _3DNow_RSqrt; + } + else +#endif + { + s_b3DNowEnabled = false; + } + + if ( bAllowSSE && pi.m_bSSE ) + { + s_bSSEEnabled = true; + +#ifndef PLATFORM_WINDOWS_PC64 + // These are not yet available. + // Select the SSE specific routines if available + pfVectorNormalize = _VectorNormalize; + pfVectorNormalizeFast = _SSE_VectorNormalizeFast; + pfInvRSquared = _SSE_InvRSquared; + pfSqrt = _SSE_Sqrt; + pfRSqrt = _SSE_RSqrtAccurate; + pfRSqrtFast = _SSE_RSqrtFast; +#endif +#ifdef PLATFORM_WINDOWS_PC32 + pfFastSinCos = _SSE_SinCos; + pfFastCos = _SSE_cos; +#endif + } + else + { + s_bSSEEnabled = false; + } + + if ( bAllowSSE2 && pi.m_bSSE2 ) + { + s_bSSE2Enabled = true; +#ifdef PLATFORM_WINDOWS_PC32 + pfFastSinCos = _SSE2_SinCos; + pfFastCos = _SSE2_cos; +#endif + } + else + { + s_bSSE2Enabled = false; + } +#endif + + s_bMathlibInitialized = true; + + InitSinCosTable(); + BuildGammaTable( gamma, texGamma, brightness, overbright ); +} + +bool MathLib_3DNowEnabled( void ) +{ + Assert( s_bMathlibInitialized ); + return s_b3DNowEnabled; +} + +bool MathLib_MMXEnabled( void ) +{ + Assert( s_bMathlibInitialized ); + return s_bMMXEnabled; +} + +bool MathLib_SSEEnabled( void ) +{ + Assert( s_bMathlibInitialized ); + return s_bSSEEnabled; +} + +bool MathLib_SSE2Enabled( void ) +{ + Assert( s_bMathlibInitialized ); + return s_bSSE2Enabled; +} + +float Approach( float target, float value, float speed ) +{ + float delta = target - value; + + if ( delta > speed ) + value += speed; + else if ( delta < -speed ) + value -= speed; + else + value = target; + + return value; +} + +// BUGBUG: Why doesn't this call angle diff?!?!? +float ApproachAngle( float target, float value, float speed ) +{ + target = anglemod( target ); + value = anglemod( value ); + + float delta = target - value; + + // Speed is assumed to be positive + if ( speed < 0 ) + speed = -speed; + + if ( delta < -180 ) + delta += 360; + else if ( delta > 180 ) + delta -= 360; + + if ( delta > speed ) + value += speed; + else if ( delta < -speed ) + value -= speed; + else + value = target; + + return value; +} + + +// BUGBUG: Why do we need both of these? +float AngleDiff( float destAngle, float srcAngle ) +{ + float delta; + + delta = fmodf(destAngle - srcAngle, 360.0f); + if ( destAngle > srcAngle ) + { + if ( delta >= 180 ) + delta -= 360; + } + else + { + if ( delta <= -180 ) + delta += 360; + } + return delta; +} + + +float AngleDistance( float next, float cur ) +{ + float delta = next - cur; + + if ( delta < -180 ) + delta += 360; + else if ( delta > 180 ) + delta -= 360; + + return delta; +} + + +float AngleNormalize( float angle ) +{ + angle = fmodf(angle, 360.0f); + if (angle > 180) + { + angle -= 360; + } + if (angle < -180) + { + angle += 360; + } + return angle; +} + +//-------------------------------------------------------------------------------------------------------------- +// ensure that 0 <= angle <= 360 +float AngleNormalizePositive( float angle ) +{ + angle = fmodf( angle, 360.0f ); + + if (angle < 0.0f) + { + angle += 360.0f; + } + + return angle; +} + +//-------------------------------------------------------------------------------------------------------------- +bool AnglesAreEqual( float a, float b, float tolerance ) +{ + return (fabs( AngleDiff( a, b ) ) < tolerance); +} + +void RotationDeltaAxisAngle( const QAngle &srcAngles, const QAngle &destAngles, Vector &deltaAxis, float &deltaAngle ) +{ + Quaternion srcQuat, destQuat, srcQuatInv, out; + AngleQuaternion( srcAngles, srcQuat ); + AngleQuaternion( destAngles, destQuat ); + QuaternionScale( srcQuat, -1, srcQuatInv ); + QuaternionMult( destQuat, srcQuatInv, out ); + + QuaternionNormalize( out ); + QuaternionAxisAngle( out, deltaAxis, deltaAngle ); +} + +void RotationDelta( const QAngle &srcAngles, const QAngle &destAngles, QAngle *out ) +{ + matrix3x4_t src, srcInv; + matrix3x4_t dest; + AngleMatrix( srcAngles, src ); + AngleMatrix( destAngles, dest ); + // xform = src(-1) * dest + MatrixInvert( src, srcInv ); + matrix3x4_t xform; + ConcatTransforms( dest, srcInv, xform ); + QAngle xformAngles; + MatrixAngles( xform, xformAngles ); + if ( out ) + { + *out = xformAngles; + } +} + +//----------------------------------------------------------------------------- +// Purpose: Computes a triangle normal +//----------------------------------------------------------------------------- +void ComputeTrianglePlane( const Vector& v1, const Vector& v2, const Vector& v3, Vector& normal, float& intercept ) +{ + Vector e1, e2; + VectorSubtract( v2, v1, e1 ); + VectorSubtract( v3, v1, e2 ); + CrossProduct( e1, e2, normal ); + VectorNormalize( normal ); + intercept = DotProduct( normal, v1 ); +} + +//----------------------------------------------------------------------------- +// Purpose: This is a clone of BaseWindingForPlane() +// Input : *outVerts - an array of preallocated verts to build the polygon in +// normal - the plane normal +// dist - the plane constant +// Output : int - vert count (always 4) +//----------------------------------------------------------------------------- +int PolyFromPlane( Vector *outVerts, const Vector& normal, float dist, float fHalfScale ) +{ + int i, x; + vec_t max, v; + Vector org, vright, vup; + + // find the major axis + + max = -16384; //MAX_COORD_INTEGER + x = -1; + for (i=0 ; i<3; i++) + { + v = fabs(normal[i]); + if (v > max) + { + x = i; + max = v; + } + } + + if (x==-1) + return 0; + + // Build a unit vector along something other than the major axis + VectorCopy (vec3_origin, vup); + switch (x) + { + case 0: + case 1: + vup[2] = 1; + break; + case 2: + vup[0] = 1; + break; + } + + // Remove the component of this vector along the normal + v = DotProduct (vup, normal); + VectorMA (vup, -v, normal, vup); + // Make it a unit (perpendicular) + VectorNormalize (vup); + + // Center of the poly is at normal * dist + VectorScale (normal, dist, org); + // Calculate the third orthonormal basis vector for our plane space (this one and vup are in the plane) + CrossProduct (vup, normal, vright); + + // Make the plane's basis vectors big (these are the half-sides of the polygon we're making) + VectorScale (vup, fHalfScale, vup); + VectorScale (vright, fHalfScale, vright); + + // Move diagonally away from org to create the corner verts + VectorSubtract (org, vright, outVerts[0]); // left + VectorAdd (outVerts[0], vup, outVerts[0]); // up + + VectorAdd (org, vright, outVerts[1]); // right + VectorAdd (outVerts[1], vup, outVerts[1]); // up + + VectorAdd (org, vright, outVerts[2]); // right + VectorSubtract (outVerts[2], vup, outVerts[2]); // down + + VectorSubtract (org, vright, outVerts[3]); // left + VectorSubtract (outVerts[3], vup, outVerts[3]); // down + + // The four corners form a planar quadrilateral normal to "normal" + return 4; +} + +//----------------------------------------------------------------------------- +// Purpose: clip a poly to the plane and return the poly on the front side of the plane +// Input : *inVerts - input polygon +// vertCount - # verts in input poly +// *outVerts - destination poly +// normal - plane normal +// dist - plane constant +// Output : int - # verts in output poly +//----------------------------------------------------------------------------- + +int ClipPolyToPlane( Vector *inVerts, int vertCount, Vector *outVerts, const Vector& normal, float dist, float fOnPlaneEpsilon ) +{ + vec_t *dists = (vec_t *)stackalloc( sizeof(vec_t) * vertCount * 4 ); //4x vertcount should cover all cases + int *sides = (int *)stackalloc( sizeof(vec_t) * vertCount * 4 ); + int counts[3]; + vec_t dot; + int i, j; + Vector mid = vec3_origin; + int outCount; + + counts[0] = counts[1] = counts[2] = 0; + + // determine sides for each point + for ( i = 0; i < vertCount; i++ ) + { + dot = DotProduct( inVerts[i], normal) - dist; + dists[i] = dot; + if ( dot > fOnPlaneEpsilon ) + { + sides[i] = SIDE_FRONT; + } + else if ( dot < -fOnPlaneEpsilon ) + { + sides[i] = SIDE_BACK; + } + else + { + sides[i] = SIDE_ON; + } + counts[sides[i]]++; + } + sides[i] = sides[0]; + dists[i] = dists[0]; + + if (!counts[0]) + return 0; + + if (!counts[1]) + { + // Copy to output verts + for ( i = 0; i < vertCount; i++ ) + { + VectorCopy( inVerts[i], outVerts[i] ); + } + return vertCount; + } + + outCount = 0; + for ( i = 0; i < vertCount; i++ ) + { + Vector& p1 = inVerts[i]; + + if (sides[i] == SIDE_ON) + { + VectorCopy( p1, outVerts[outCount]); + outCount++; + continue; + } + + if (sides[i] == SIDE_FRONT) + { + VectorCopy( p1, outVerts[outCount]); + outCount++; + } + + if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) + continue; + + // generate a split point + Vector& p2 = inVerts[(i+1)%vertCount]; + + dot = dists[i] / (dists[i]-dists[i+1]); + for (j=0 ; j<3 ; j++) + { // avoid round off error when possible + if (normal[j] == 1) + mid[j] = dist; + else if (normal[j] == -1) + mid[j] = -dist; + else + mid[j] = p1[j] + dot*(p2[j]-p1[j]); + } + + VectorCopy (mid, outVerts[outCount]); + outCount++; + } + + return outCount; +} + + +int ClipPolyToPlane_Precise( double *inVerts, int vertCount, double *outVerts, const double *normal, double dist, double fOnPlaneEpsilon ) +{ + double *dists = (double *)stackalloc( sizeof(double) * vertCount * 4 ); //4x vertcount should cover all cases + int *sides = (int *)stackalloc( sizeof(double) * vertCount * 4 ); + int counts[3]; + double dot; + int i, j; + //Vector mid = vec3_origin; + double mid[3]; + mid[0] = 0.0; + mid[1] = 0.0; + mid[2] = 0.0; + int outCount; + + counts[0] = counts[1] = counts[2] = 0; + + // determine sides for each point + for ( i = 0; i < vertCount; i++ ) + { + //dot = DotProduct( inVerts[i], normal) - dist; + dot = ((inVerts[i*3 + 0] * normal[0]) + (inVerts[i*3 + 1] * normal[1]) + (inVerts[i*3 + 2] * normal[2])) - dist; + dists[i] = dot; + if ( dot > fOnPlaneEpsilon ) + { + sides[i] = SIDE_FRONT; + } + else if ( dot < -fOnPlaneEpsilon ) + { + sides[i] = SIDE_BACK; + } + else + { + sides[i] = SIDE_ON; + } + counts[sides[i]]++; + } + sides[i] = sides[0]; + dists[i] = dists[0]; + + if (!counts[0]) + return 0; + + if (!counts[1]) + { + // Copy to output verts + //for ( i = 0; i < vertCount; i++ ) + for ( i = 0; i < vertCount * 3; i++ ) + { + //VectorCopy( inVerts[i], outVerts[i] ); + outVerts[i] = inVerts[i]; + } + return vertCount; + } + + outCount = 0; + for ( i = 0; i < vertCount; i++ ) + { + //Vector& p1 = inVerts[i]; + double *p1 = &inVerts[i*3]; + //p1[0] = inVerts[i*3 + 0]; + //p1[1] = inVerts[i*3 + 1]; + //p1[2] = inVerts[i*3 + 2]; + + if (sides[i] == SIDE_ON) + { + //VectorCopy( p1, outVerts[outCount]); + outVerts[outCount*3 + 0] = p1[0]; + outVerts[outCount*3 + 1] = p1[1]; + outVerts[outCount*3 + 2] = p1[2]; + outCount++; + continue; + } + + if (sides[i] == SIDE_FRONT) + { + //VectorCopy( p1, outVerts[outCount]); + outVerts[outCount*3 + 0] = p1[0]; + outVerts[outCount*3 + 1] = p1[1]; + outVerts[outCount*3 + 2] = p1[2]; + outCount++; + } + + if (sides[i+1] == SIDE_ON || sides[i+1] == sides[i]) + continue; + + // generate a split point + //Vector& p2 = inVerts[(i+1)%vertCount]; + int wrappedindex = (i+1)%vertCount; + double *p2 = &inVerts[wrappedindex*3]; + //p2[0] = inVerts[wrappedindex*3 + 0]; + //p2[1] = inVerts[wrappedindex*3 + 1]; + //p2[2] = inVerts[wrappedindex*3 + 2]; + + dot = dists[i] / (dists[i]-dists[i+1]); + for (j=0 ; j<3 ; j++) + { + mid[j] = (double)p1[j] + dot*((double)p2[j]-(double)p1[j]); + } + + //VectorCopy (mid, outVerts[outCount]); + outVerts[outCount*3 + 0] = mid[0]; + outVerts[outCount*3 + 1] = mid[1]; + outVerts[outCount*3 + 2] = mid[2]; + outCount++; + } + + return outCount; +} + +int CeilPow2( int in ) +{ + int retval; + + retval = 1; + while( retval < in ) + retval <<= 1; + return retval; +} + +int FloorPow2( int in ) +{ + int retval; + + retval = 1; + while( retval < in ) + retval <<= 1; + return retval >> 1; +} + + +//----------------------------------------------------------------------------- +// Computes Y fov from an X fov and a screen aspect ratio +//----------------------------------------------------------------------------- +float CalcFovY( float flFovX, float flAspect ) +{ + if ( flFovX < 1 || flFovX > 179) + { + flFovX = 90; // error, set to 90 + } + + // The long, but illustrative version (more closely matches CShaderAPIDX8::PerspectiveX, which + // is what it's based on). + // + //float width = 2 * zNear * tan( DEG2RAD( fov_x / 2.0 ) ); + //float height = width / screenaspect; + //float yRadians = atan( (height/2.0) / zNear ); + //return RAD2DEG( yRadians ) * 2; + + // The short and sweet version. + float val = atan( tan( DEG2RAD( flFovX ) * 0.5f ) / flAspect ); + val = RAD2DEG( val ) * 2.0f; + return val; +} + +float CalcFovX( float flFovY, float flAspect ) +{ + return RAD2DEG( atan( tan( DEG2RAD( flFovY ) * 0.5f ) * flAspect ) ) * 2.0f; +} + + +//----------------------------------------------------------------------------- +// Generate a frustum based on perspective view parameters +//----------------------------------------------------------------------------- +void GeneratePerspectiveFrustum( const Vector& origin, const Vector &forward, + const Vector &right, const Vector &up, float flZNear, float flZFar, + float flFovX, float flFovY, Frustum_t &frustum ) +{ + float flIntercept = DotProduct( origin, forward ); + + // Setup the near and far planes. + frustum.SetPlane( FRUSTUM_FARZ, PLANE_ANYZ, -forward, -flZFar - flIntercept ); + frustum.SetPlane( FRUSTUM_NEARZ, PLANE_ANYZ, forward, flZNear + flIntercept ); + + flFovX *= 0.5f; + flFovY *= 0.5f; + + float flTanX = tan( DEG2RAD( flFovX ) ); + float flTanY = tan( DEG2RAD( flFovY ) ); + + // OPTIMIZE: Normalizing these planes is not necessary for culling + Vector normalPos, normalNeg; + + VectorMA( right, flTanX, forward, normalPos ); + VectorMA( normalPos, -2.0f, right, normalNeg ); + + VectorNormalize( normalPos ); + VectorNormalize( normalNeg ); + + frustum.SetPlane( FRUSTUM_LEFT, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); + frustum.SetPlane( FRUSTUM_RIGHT, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); + + VectorMA( up, flTanY, forward, normalPos ); + VectorMA( normalPos, -2.0f, up, normalNeg ); + + VectorNormalize( normalPos ); + VectorNormalize( normalNeg ); + + frustum.SetPlane( FRUSTUM_BOTTOM, PLANE_ANYZ, normalPos, normalPos.Dot( origin ) ); + frustum.SetPlane( FRUSTUM_TOP, PLANE_ANYZ, normalNeg, normalNeg.Dot( origin ) ); +} + + +//----------------------------------------------------------------------------- +// Version that accepts angles instead of vectors +//----------------------------------------------------------------------------- +void GeneratePerspectiveFrustum( const Vector& origin, const QAngle &angles, float flZNear, float flZFar, float flFovX, float flAspectRatio, Frustum_t &frustum ) +{ + Vector vecForward, vecRight, vecUp; + AngleVectors( angles, &vecForward, &vecRight, &vecUp ); + float flFovY = CalcFovY( flFovX, flAspectRatio ); + GeneratePerspectiveFrustum( origin, vecForward, vecRight, vecUp, flZNear, flZFar, flFovX, flFovY, frustum ); +} + +bool R_CullBox( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) +{ + return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_NEARZ) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); +} + +bool R_CullBoxSkipNear( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ) +{ + return (( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_RIGHT) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_LEFT) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_TOP) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_BOTTOM) ) == 2 ) || + ( BoxOnPlaneSide( mins, maxs, frustum.GetPlane(FRUSTUM_FARZ) ) == 2 ) ); +} + + +// NOTE: This routine was taken (and modified) from NVidia's BlinnReflection demo +// Creates basis vectors, based on a vertex and index list. +// See the NVidia white paper 'GDC2K PerPixel Lighting' for a description +// of how this computation works +#define SMALL_FLOAT 1e-12 + +void CalcTriangleTangentSpace( const Vector &p0, const Vector &p1, const Vector &p2, + const Vector2D &t0, const Vector2D &t1, const Vector2D& t2, + Vector &sVect, Vector &tVect ) +{ + /* Compute the partial derivatives of X, Y, and Z with respect to S and T. */ + sVect.Init( 0.0f, 0.0f, 0.0f ); + tVect.Init( 0.0f, 0.0f, 0.0f ); + + // x, s, t + Vector edge01( p1.x - p0.x, t1.x - t0.x, t1.y - t0.y ); + Vector edge02( p2.x - p0.x, t2.x - t0.x, t2.y - t0.y ); + + Vector cross; + CrossProduct( edge01, edge02, cross ); + if ( fabs( cross.x ) > SMALL_FLOAT ) + { + sVect.x += -cross.y / cross.x; + tVect.x += -cross.z / cross.x; + } + + // y, s, t + edge01.Init( p1.y - p0.y, t1.x - t0.x, t1.y - t0.y ); + edge02.Init( p2.y - p0.y, t2.x - t0.x, t2.y - t0.y ); + + CrossProduct( edge01, edge02, cross ); + if ( fabs( cross.x ) > SMALL_FLOAT ) + { + sVect.y += -cross.y / cross.x; + tVect.y += -cross.z / cross.x; + } + + // z, s, t + edge01.Init( p1.z - p0.z, t1.x - t0.x, t1.y - t0.y ); + edge02.Init( p2.z - p0.z, t2.x - t0.x, t2.y - t0.y ); + + CrossProduct( edge01, edge02, cross ); + if( fabs( cross.x ) > SMALL_FLOAT ) + { + sVect.z += -cross.y / cross.x; + tVect.z += -cross.z / cross.x; + } + + // Normalize sVect and tVect + VectorNormalize( sVect ); + VectorNormalize( tVect ); +} + + +//----------------------------------------------------------------------------- +// Convert RGB to HSV +//----------------------------------------------------------------------------- +void RGBtoHSV( const Vector &rgb, Vector &hsv ) +{ + float flMax = max( rgb.x, rgb.y ); + flMax = max( flMax, rgb.z ); + float flMin = min( rgb.x, rgb.y ); + flMin = min( flMin, rgb.z ); + + // hsv.z is the value + hsv.z = flMax; + + // hsv.y is the saturation + if (flMax != 0.0F) + { + hsv.y = (flMax - flMin) / flMax; + } + else + { + hsv.y = 0.0F; + } + + // hsv.x is the hue + if (hsv.y == 0.0F) + { + hsv.x = -1.0f; + } + else + { + float32 d = flMax - flMin; + if (rgb.x == flMax) + { + hsv.x = (rgb.y - rgb.z) / d; + } + else if (rgb.y == flMax) + { + hsv.x = 2.0F + (rgb.z - rgb.x) / d; + } + else + { + hsv.x = 4.0F + (rgb.x - rgb.y) / d; + } + hsv.x *= 60.0F; + if ( hsv.x < 0.0F ) + { + hsv.x += 360.0F; + } + } +} + + +//----------------------------------------------------------------------------- +// Convert HSV to RGB +//----------------------------------------------------------------------------- +void HSVtoRGB( const Vector &hsv, Vector &rgb ) +{ + if ( hsv.y == 0.0F ) + { + rgb.Init( hsv.z, hsv.z, hsv.z ); + return; + } + + float32 hue = hsv.x; + if (hue == 360.0F) + { + hue = 0.0F; + } + hue /= 60.0F; + int i = hue; // integer part + float32 f = hue - i; // fractional part + float32 p = hsv.z * (1.0F - hsv.y); + float32 q = hsv.z * (1.0F - hsv.y * f); + float32 t = hsv.z * (1.0F - hsv.y * (1.0F - f)); + switch(i) + { + case 0: rgb.Init( hsv.z, t, p ); break; + case 1: rgb.Init( q, hsv.z, p ); break; + case 2: rgb.Init( p, hsv.z, t ); break; + case 3: rgb.Init( p, q, hsv.z ); break; + case 4: rgb.Init( t, p, hsv.z ); break; + case 5: rgb.Init( hsv.z, p, q ); break; + } +} + + +void GetInterpolationData( float const *pKnotPositions, + float const *pKnotValues, + int nNumValuesinList, + int nInterpolationRange, + float flPositionToInterpolateAt, + bool bWrap, + float *pValueA, + float *pValueB, + float *pInterpolationValue) +{ + // first, find the bracketting knots by looking for the first knot >= our index + + int idx; + for(idx = 0; idx < nNumValuesinList; idx++ ) + { + if ( pKnotPositions[idx] >= flPositionToInterpolateAt ) + break; + } + int nKnot1, nKnot2; + float flOffsetFromStartOfGap, flSizeOfGap; + if ( idx == 0) + { + if ( bWrap ) + { + nKnot1 = nNumValuesinList-1; + nKnot2 = 0; + flSizeOfGap = + ( pKnotPositions[nKnot2] + ( nInterpolationRange-pKnotPositions[nKnot1] ) ); + flOffsetFromStartOfGap = + flPositionToInterpolateAt + ( nInterpolationRange-pKnotPositions[nKnot1] ); + } + else + { + *pValueA = *pValueB = pKnotValues[0]; + *pInterpolationValue = 1.0; + return; + } + } + else if ( idx == nNumValuesinList ) // ran out of values + { + if ( bWrap ) + { + nKnot1 = nNumValuesinList -1; + nKnot2 = 0; + flSizeOfGap = ( pKnotPositions[nKnot2] + + ( nInterpolationRange-pKnotPositions[nKnot1] ) ); + flOffsetFromStartOfGap = flPositionToInterpolateAt - pKnotPositions[nKnot1]; + } + else + { + *pValueA = *pValueB = pKnotValues[nNumValuesinList-1]; + *pInterpolationValue = 1.0; + return; + } + + } + else + { + nKnot1 = idx-1; + nKnot2 = idx; + flSizeOfGap = pKnotPositions[nKnot2]-pKnotPositions[nKnot1]; + flOffsetFromStartOfGap = flPositionToInterpolateAt-pKnotPositions[nKnot1]; + } + + *pValueA = pKnotValues[nKnot1]; + *pValueB = pKnotValues[nKnot2]; + *pInterpolationValue = FLerp( 0, 1, 0, flSizeOfGap, flOffsetFromStartOfGap ); + return; +} + +float RandomVectorInUnitSphere( Vector *pVector ) +{ + // Guarantee uniform random distribution within a sphere + // Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") + float u = ((float)rand() / VALVE_RAND_MAX); + float v = ((float)rand() / VALVE_RAND_MAX); + float w = ((float)rand() / VALVE_RAND_MAX); + + float flPhi = acos( 1 - 2 * u ); + float flTheta = 2 * M_PI * v; + float flRadius = powf( w, 1.0f / 3.0f ); + + float flSinPhi, flCosPhi; + float flSinTheta, flCosTheta; + SinCos( flPhi, &flSinPhi, &flCosPhi ); + SinCos( flTheta, &flSinTheta, &flCosTheta ); + + pVector->x = flRadius * flSinPhi * flCosTheta; + pVector->y = flRadius * flSinPhi * flSinTheta; + pVector->z = flRadius * flCosPhi; + return flRadius; +} + +float RandomVectorInUnitCircle( Vector2D *pVector ) +{ + // Guarantee uniform random distribution within a sphere + // Graphics gems III contains this algorithm ("Nonuniform random point sets via warping") + float u = ((float)rand() / VALVE_RAND_MAX); + float v = ((float)rand() / VALVE_RAND_MAX); + + float flTheta = 2 * M_PI * v; + float flRadius = powf( u, 1.0f / 2.0f ); + + float flSinTheta, flCosTheta; + SinCos( flTheta, &flSinTheta, &flCosTheta ); + + pVector->x = flRadius * flCosTheta; + pVector->y = flRadius * flSinTheta; + return flRadius; +} +#ifdef FP_EXCEPTIONS_ENABLED +#include // For _clearfp and _controlfp_s +#endif + +// FPExceptionDisable and FPExceptionEnabler taken from my blog post +// at http://www.altdevblogaday.com/2012/04/20/exceptional-floating-point/ + +#ifdef FP_EXCEPTIONS_ENABLED +// These functions are all inlined NOPs if FP_EXCEPTIONS_ENABLED is not defined. +FPExceptionDisabler::FPExceptionDisabler() +{ + // Retrieve the current state of the exception flags. This + // must be done before changing them. _MCW_EM is a bit + // mask representing all available exception masks. + _controlfp_s(&mOldValues, _MCW_EM, _MCW_EM); + // Set all of the exception flags, which suppresses FP + // exceptions on the x87 and SSE units. + _controlfp_s(0, _MCW_EM, _MCW_EM); +} + +FPExceptionDisabler::~FPExceptionDisabler() +{ + // Clear any pending FP exceptions. This must be done + // prior to enabling FP exceptions since otherwise there + // may be a 'deferred crash' as soon the exceptions are + // enabled. + _clearfp(); + + // Reset (possibly enabling) the exception status. + _controlfp_s(0, mOldValues, _MCW_EM); +} + +// Overflow, divide-by-zero, and invalid-operation are the FP +// exceptions most frequently associated with bugs. +FPExceptionEnabler::FPExceptionEnabler(unsigned int enableBits /*= _EM_OVERFLOW | _EM_ZERODIVIDE | _EM_INVALID*/) +{ + // Retrieve the current state of the exception flags. This + // must be done before changing them. _MCW_EM is a bit + // mask representing all available exception masks. + _controlfp_s(&mOldValues, _MCW_EM, _MCW_EM); + + // Make sure no non-exception flags have been specified, + // to avoid accidental changing of rounding modes, etc. + enableBits &= _MCW_EM; + + // Clear any pending FP exceptions. This must be done + // prior to enabling FP exceptions since otherwise there + // may be a 'deferred crash' as soon the exceptions are + // enabled. + _clearfp(); + + // Zero out the specified bits, leaving other bits alone. + _controlfp_s(0, ~enableBits, enableBits); +} + +FPExceptionEnabler::~FPExceptionEnabler() +{ + // Reset the exception state. + _controlfp_s(0, mOldValues, _MCW_EM); +} +#endif -- cgit v1.2.3