/*** * * Copyright (c) 1996-2002, Valve LLC. All rights reserved. * * This product contains software technology licensed from Id * Software, Inc. ("Id Technology"). Id Technology (c) 1996 Id Software, Inc. * All Rights Reserved. * * Use, distribution, and modification of this source code and/or resulting * object code is restricted to non-commercial enhancements to products from * Valve LLC. All other use, distribution, or modification is prohibited * without written permission from Valve LLC. * ****/ // pm_math.c -- math primitives #include "mathlib.h" #include "const.h" #include // up / down #define PITCH 0 // left / right #define YAW 1 // fall over #define ROLL 2 #ifdef _MSC_VER #pragma warning(disable : 4244) #endif vec3_t vec3_origin = { 0,0,0 }; int nanmask = 255 << 23; float anglemod( float a ) { a = ( 360.0 / 65536 ) * ( (int)( a * ( 65536 / 360.0 ) ) & 65535 ); return a; } void AngleVectors( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up ) { float angle; float sr, sp, sy, cr, cp, cy; angle = angles[YAW] * ( M_PI * 2 / 360 ); sy = sin( angle ); cy = cos( angle ); angle = angles[PITCH] * ( M_PI*2 / 360 ); sp = sin( angle ); cp = cos( angle ); angle = angles[ROLL] * ( M_PI*2 / 360 ); sr = sin( angle ); cr = cos( angle ); if( forward ) { forward[0] = cp * cy; forward[1] = cp * sy; forward[2] = -sp; } if( right ) { right[0] = ( -1 * sr * sp * cy + -1 * cr * -sy ); right[1] = ( -1 * sr * sp * sy + -1 * cr * cy ); right[2] = -1 * sr * cp; } if( up ) { up[0] = ( cr * sp * cy + -sr * -sy ); up[1] = ( cr * sp * sy + -sr * cy ); up[2] = cr * cp; } } void AngleVectorsTranspose( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up ) { float angle; float sr, sp, sy, cr, cp, cy; angle = angles[YAW] * ( M_PI * 2 / 360 ); sy = sin( angle ); cy = cos( angle ); angle = angles[PITCH] * ( M_PI * 2 / 360 ); sp = sin( angle ); cp = cos( angle ); angle = angles[ROLL] * ( M_PI * 2 / 360 ); sr = sin( angle ); cr = cos( angle ); if( forward ) { forward[0] = cp * cy; forward[1] = ( sr * sp * cy + cr * -sy ); forward[2] = ( cr * sp * cy + -sr * -sy ); } if( right ) { right[0] = cp * sy; right[1] = ( sr * sp * sy + cr * cy ); right[2] = ( cr * sp * sy + -sr * cy ); } if( up ) { up[0] = -sp; up[1] = sr * cp; up[2] = cr * cp; } } void AngleMatrix( const vec3_t angles, float (*matrix)[4] ) { float angle; float sr, sp, sy, cr, cp, cy; angle = angles[YAW] * ( M_PI * 2 / 360 ); sy = sin( angle ); cy = cos( angle ); angle = angles[PITCH] * ( M_PI * 2 / 360 ); sp = sin( angle ); cp = cos( angle ); angle = angles[ROLL] * ( M_PI * 2 / 360 ); sr = sin( angle ); cr = cos( angle ); // matrix = ( YAW * PITCH ) * ROLL matrix[0][0] = cp * cy; matrix[1][0] = cp * sy; matrix[2][0] = -sp; matrix[0][1] = sr * sp * cy + cr * -sy; matrix[1][1] = sr * sp * sy + cr * cy; matrix[2][1] = sr * cp; matrix[0][2] = ( cr * sp * cy + -sr * -sy ); matrix[1][2] = ( cr * sp * sy + -sr * cy ); matrix[2][2] = cr * cp; matrix[0][3] = 0.0; matrix[1][3] = 0.0; matrix[2][3] = 0.0; } void AngleIMatrix( const vec3_t angles, float matrix[3][4] ) { float angle; float sr, sp, sy, cr, cp, cy; angle = angles[YAW] * ( M_PI * 2 / 360 ); sy = sin( angle ); cy = cos( angle ); angle = angles[PITCH] * ( M_PI * 2 / 360 ); sp = sin( angle ); cp = cos( angle ); angle = angles[ROLL] * ( M_PI * 2 / 360 ); sr = sin( angle ); cr = cos( angle ); // 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.0; matrix[1][3] = 0.0; matrix[2][3] = 0.0; } void NormalizeAngles( float *angles ) { int i; // Normalize angles for( i = 0; i < 3; i++ ) { if( angles[i] > 180.0 ) { angles[i] -= 360.0; } else if( angles[i] < -180.0 ) { angles[i] += 360.0; } } } /* =================== InterpolateAngles Interpolate Euler angles. FIXME: Use Quaternions to avoid discontinuities Frac is 0.0 to 1.0 ( i.e., should probably be clamped, but doesn't have to be ) =================== */ void InterpolateAngles( float *start, float *end, float *output, float frac ) { int i; float ang1, ang2; float d; NormalizeAngles( start ); NormalizeAngles( end ); for( i = 0; i < 3; i++ ) { ang1 = start[i]; ang2 = end[i]; d = ang2 - ang1; if( d > 180 ) { d -= 360; } else if( d < -180 ) { d += 360; } output[i] = ang1 + d * frac; } NormalizeAngles( output ); } /* =================== AngleBetweenVectors =================== */ float AngleBetweenVectors( const vec3_t v1, const vec3_t v2 ) { float angle; float l1 = Length( v1 ); float l2 = Length( v2 ); if( !l1 || !l2 ) return 0.0f; angle = acos( DotProduct( v1, v2 ) ) / ( l1 * l2 ); angle = ( angle * 180.0f ) / M_PI; return angle; } void VectorTransform( const vec3_t in1, float in2[3][4], vec3_t 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]; } int VectorCompare( const vec3_t v1, const vec3_t v2 ) { int i; for( i = 0; i < 3; i++ ) if( v1[i] != v2[i] ) return 0; return 1; } void VectorMA( const vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc ) { vecc[0] = veca[0] + scale * vecb[0]; vecc[1] = veca[1] + scale * vecb[1]; vecc[2] = veca[2] + scale * vecb[2]; } vec_t _DotProduct( vec3_t v1, vec3_t v2 ) { return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; } void _VectorSubtract( vec3_t veca, vec3_t vecb, vec3_t out ) { out[0] = veca[0] - vecb[0]; out[1] = veca[1] - vecb[1]; out[2] = veca[2] - vecb[2]; } void _VectorAdd( vec3_t veca, vec3_t vecb, vec3_t out ) { out[0] = veca[0] + vecb[0]; out[1] = veca[1] + vecb[1]; out[2] = veca[2] + vecb[2]; } void _VectorCopy( vec3_t in, vec3_t out ) { out[0] = in[0]; out[1] = in[1]; out[2] = in[2]; } void CrossProduct( const vec3_t v1, const vec3_t v2, vec3_t 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]; } double sqrt( double x ); float Length( const vec3_t v ) { int i; float length = 0.0f; for( i = 0; i < 3; i++ ) length += v[i] * v[i]; length = sqrt( length ); // FIXME return length; } float Distance( const vec3_t v1, const vec3_t v2 ) { vec3_t d; VectorSubtract( v2, v1, d ); return Length( d ); } float VectorNormalize( vec3_t v ) { float length, ilength; length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; length = sqrt( length ); // FIXME if( length ) { ilength = 1 / length; v[0] *= ilength; v[1] *= ilength; v[2] *= ilength; } return length; } void VectorInverse( vec3_t v ) { v[0] = -v[0]; v[1] = -v[1]; v[2] = -v[2]; } void VectorScale( const vec3_t in, vec_t scale, vec3_t out ) { out[0] = in[0] * scale; out[1] = in[1] * scale; out[2] = in[2] * scale; } int Q_log2( int val ) { int answer = 0; while( val >>= 1 ) answer++; return answer; } void VectorMatrix( vec3_t forward, vec3_t right, vec3_t up ) { vec3_t tmp; if( forward[0] == 0 && forward[1] == 0 ) { right[0] = 1; right[1] = 0; right[2] = 0; up[0] = -forward[2]; up[1] = 0; up[2] = 0; return; } tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0; CrossProduct( forward, tmp, right ); VectorNormalize( right ); CrossProduct( right, forward, up ); VectorNormalize( up ); } void VectorAngles( const vec3_t forward, vec3_t angles ) { float tmp, yaw, pitch; if( forward[1] == 0 && forward[0] == 0 ) { yaw = 0; if( forward[2] > 0 ) pitch = 90; else pitch = 270; } 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; }