You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
251 lines
5.9 KiB
251 lines
5.9 KiB
//========= Copyright © 1996-2002, Valve LLC, All rights reserved. ============ |
|
// |
|
// Purpose: |
|
// |
|
// $NoKeywords: $ |
|
//============================================================================= |
|
|
|
#include <memory.h> |
|
#include "hud.h" |
|
#include "cl_util.h" |
|
#include "const.h" |
|
#include "com_model.h" |
|
#include "studio_util.h" |
|
|
|
/* |
|
==================== |
|
AngleMatrix |
|
|
|
==================== |
|
*/ |
|
void AngleMatrix (const float *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; |
|
} |
|
|
|
/* |
|
==================== |
|
VectorCompare |
|
|
|
==================== |
|
*/ |
|
int VectorCompare (const float *v1, const float *v2) |
|
{ |
|
int i; |
|
|
|
for (i=0 ; i<3 ; i++) |
|
if (v1[i] != v2[i]) |
|
return 0; |
|
|
|
return 1; |
|
} |
|
|
|
/* |
|
==================== |
|
CrossProduct |
|
|
|
==================== |
|
*/ |
|
void CrossProduct (const float *v1, const float *v2, float *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]; |
|
} |
|
|
|
/* |
|
==================== |
|
VectorTransform |
|
|
|
==================== |
|
*/ |
|
void VectorTransform (const float *in1, float in2[3][4], float *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]; |
|
} |
|
|
|
/* |
|
================ |
|
ConcatTransforms |
|
|
|
================ |
|
*/ |
|
void ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4]) |
|
{ |
|
out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + |
|
in1[0][2] * in2[2][0]; |
|
out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + |
|
in1[0][2] * in2[2][1]; |
|
out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + |
|
in1[0][2] * in2[2][2]; |
|
out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + |
|
in1[0][2] * in2[2][3] + in1[0][3]; |
|
out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + |
|
in1[1][2] * in2[2][0]; |
|
out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + |
|
in1[1][2] * in2[2][1]; |
|
out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + |
|
in1[1][2] * in2[2][2]; |
|
out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + |
|
in1[1][2] * in2[2][3] + in1[1][3]; |
|
out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + |
|
in1[2][2] * in2[2][0]; |
|
out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + |
|
in1[2][2] * in2[2][1]; |
|
out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + |
|
in1[2][2] * in2[2][2]; |
|
out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + |
|
in1[2][2] * in2[2][3] + in1[2][3]; |
|
} |
|
|
|
// angles index are not the same as ROLL, PITCH, YAW |
|
|
|
/* |
|
==================== |
|
AngleQuaternion |
|
|
|
==================== |
|
*/ |
|
void AngleQuaternion( float *angles, vec4_t quaternion ) |
|
{ |
|
float angle; |
|
float sr, sp, sy, cr, cp, cy; |
|
|
|
// FIXME: rescale the inputs to 1/2 angle |
|
angle = angles[2] * 0.5; |
|
sy = sin(angle); |
|
cy = cos(angle); |
|
angle = angles[1] * 0.5; |
|
sp = sin(angle); |
|
cp = cos(angle); |
|
angle = angles[0] * 0.5; |
|
sr = sin(angle); |
|
cr = cos(angle); |
|
|
|
quaternion[0] = sr*cp*cy-cr*sp*sy; // X |
|
quaternion[1] = cr*sp*cy+sr*cp*sy; // Y |
|
quaternion[2] = cr*cp*sy-sr*sp*cy; // Z |
|
quaternion[3] = cr*cp*cy+sr*sp*sy; // W |
|
} |
|
|
|
/* |
|
==================== |
|
QuaternionSlerp |
|
|
|
==================== |
|
*/ |
|
void QuaternionSlerp( vec4_t p, vec4_t q, float t, vec4_t qt ) |
|
{ |
|
int i; |
|
float omega, cosom, sinom, sclp, sclq; |
|
|
|
// 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++) |
|
{ |
|
q[i] = -q[i]; |
|
} |
|
} |
|
|
|
cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3]; |
|
|
|
if ((1.0 + cosom) > 0.000001) |
|
{ |
|
if ((1.0 - cosom) > 0.000001) |
|
{ |
|
omega = acos( cosom ); |
|
sinom = sin( omega ); |
|
sclp = sin( (1.0 - t)*omega) / sinom; |
|
sclq = sin( t*omega ) / sinom; |
|
} |
|
else |
|
{ |
|
sclp = 1.0 - t; |
|
sclq = t; |
|
} |
|
for (i = 0; i < 4; i++) { |
|
qt[i] = sclp * p[i] + sclq * q[i]; |
|
} |
|
} |
|
else |
|
{ |
|
qt[0] = -q[1]; |
|
qt[1] = q[0]; |
|
qt[2] = -q[3]; |
|
qt[3] = q[2]; |
|
sclp = sin( (1.0 - t) * (0.5 * M_PI)); |
|
sclq = sin( t * (0.5 * M_PI)); |
|
for (i = 0; i < 3; i++) |
|
{ |
|
qt[i] = sclp * p[i] + sclq * qt[i]; |
|
} |
|
} |
|
} |
|
|
|
/* |
|
==================== |
|
QuaternionMatrix |
|
|
|
==================== |
|
*/ |
|
void QuaternionMatrix( vec4_t quaternion, float (*matrix)[4] ) |
|
{ |
|
matrix[0][0] = 1.0 - 2.0 * quaternion[1] * quaternion[1] - 2.0 * quaternion[2] * quaternion[2]; |
|
matrix[1][0] = 2.0 * quaternion[0] * quaternion[1] + 2.0 * quaternion[3] * quaternion[2]; |
|
matrix[2][0] = 2.0 * quaternion[0] * quaternion[2] - 2.0 * quaternion[3] * quaternion[1]; |
|
|
|
matrix[0][1] = 2.0 * quaternion[0] * quaternion[1] - 2.0 * quaternion[3] * quaternion[2]; |
|
matrix[1][1] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[2] * quaternion[2]; |
|
matrix[2][1] = 2.0 * quaternion[1] * quaternion[2] + 2.0 * quaternion[3] * quaternion[0]; |
|
|
|
matrix[0][2] = 2.0 * quaternion[0] * quaternion[2] + 2.0 * quaternion[3] * quaternion[1]; |
|
matrix[1][2] = 2.0 * quaternion[1] * quaternion[2] - 2.0 * quaternion[3] * quaternion[0]; |
|
matrix[2][2] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[1] * quaternion[1]; |
|
} |
|
|
|
/* |
|
==================== |
|
MatrixCopy |
|
|
|
==================== |
|
*/ |
|
void MatrixCopy( float in[3][4], float out[3][4] ) |
|
{ |
|
memcpy( out, in, sizeof( float ) * 3 * 4 ); |
|
} |