352 lines
7.9 KiB
C
352 lines
7.9 KiB
C
/***
|
|
*
|
|
* 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.
|
|
*
|
|
****/
|
|
|
|
// mathlib.c -- math primitives
|
|
|
|
#pragma warning( disable : 4244 )
|
|
#pragma warning( disable : 4237 )
|
|
#pragma warning( disable : 4305 )
|
|
|
|
#include "cmdlib.h"
|
|
#include "mathlib.h"
|
|
|
|
vec3_t vec3_origin = {0,0,0};
|
|
|
|
|
|
double VectorLength(vec3_t v)
|
|
{
|
|
int i;
|
|
double length;
|
|
|
|
length = 0;
|
|
for (i=0 ; i< 3 ; i++)
|
|
length += v[i]*v[i];
|
|
length = sqrt (length); // FIXME
|
|
|
|
return length;
|
|
}
|
|
|
|
|
|
int VectorCompare (vec3_t v1, vec3_t v2)
|
|
{
|
|
int i;
|
|
|
|
for (i=0 ; i<3 ; i++)
|
|
if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
vec_t Q_rint (vec_t in)
|
|
{
|
|
return floor (in + 0.5);
|
|
}
|
|
|
|
void VectorMA (vec3_t va, double scale, vec3_t vb, vec3_t vc)
|
|
{
|
|
vc[0] = va[0] + scale*vb[0];
|
|
vc[1] = va[1] + scale*vb[1];
|
|
vc[2] = va[2] + scale*vb[2];
|
|
}
|
|
|
|
void CrossProduct (vec3_t v1, 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];
|
|
}
|
|
|
|
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 va, vec3_t vb, vec3_t out)
|
|
{
|
|
out[0] = va[0]-vb[0];
|
|
out[1] = va[1]-vb[1];
|
|
out[2] = va[2]-vb[2];
|
|
}
|
|
|
|
void _VectorAdd (vec3_t va, vec3_t vb, vec3_t out)
|
|
{
|
|
out[0] = va[0]+vb[0];
|
|
out[1] = va[1]+vb[1];
|
|
out[2] = va[2]+vb[2];
|
|
}
|
|
|
|
void _VectorCopy (vec3_t in, vec3_t out)
|
|
{
|
|
out[0] = in[0];
|
|
out[1] = in[1];
|
|
out[2] = in[2];
|
|
}
|
|
|
|
void _VectorScale (vec3_t v, vec_t scale, vec3_t out)
|
|
{
|
|
out[0] = v[0] * scale;
|
|
out[1] = v[1] * scale;
|
|
out[2] = v[2] * scale;
|
|
}
|
|
|
|
vec_t VectorNormalize (vec3_t v)
|
|
{
|
|
int i;
|
|
double length;
|
|
|
|
if ( fabs(v[1] - 0.000215956) < 0.0001)
|
|
i=1;
|
|
|
|
length = 0;
|
|
for (i=0 ; i< 3 ; i++)
|
|
length += v[i]*v[i];
|
|
length = sqrt (length);
|
|
if (length == 0)
|
|
return 0;
|
|
|
|
for (i=0 ; i< 3 ; i++)
|
|
v[i] /= length;
|
|
|
|
return length;
|
|
}
|
|
|
|
void VectorInverse (vec3_t v)
|
|
{
|
|
v[0] = -v[0];
|
|
v[1] = -v[1];
|
|
v[2] = -v[2];
|
|
}
|
|
|
|
void ClearBounds (vec3_t mins, vec3_t maxs)
|
|
{
|
|
mins[0] = mins[1] = mins[2] = 99999;
|
|
maxs[0] = maxs[1] = maxs[2] = -99999;
|
|
}
|
|
|
|
void AddPointToBounds (vec3_t v, vec3_t mins, vec3_t maxs)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
void AngleMatrix (const vec3_t angles, float (*matrix)[4] )
|
|
{
|
|
float angle;
|
|
float sr, sp, sy, cr, cp, cy;
|
|
|
|
angle = angles[2] * (Q_PI*2 / 360);
|
|
sy = sin(angle);
|
|
cy = cos(angle);
|
|
angle = angles[1] * (Q_PI*2 / 360);
|
|
sp = sin(angle);
|
|
cp = cos(angle);
|
|
angle = angles[0] * (Q_PI*2 / 360);
|
|
sr = sin(angle);
|
|
cr = cos(angle);
|
|
|
|
// matrix = (Z * Y) * X
|
|
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[2] * (Q_PI*2 / 360);
|
|
sy = sin(angle);
|
|
cy = cos(angle);
|
|
angle = angles[1] * (Q_PI*2 / 360);
|
|
sp = sin(angle);
|
|
cp = cos(angle);
|
|
angle = angles[0] * (Q_PI*2 / 360);
|
|
sr = sin(angle);
|
|
cr = cos(angle);
|
|
|
|
// matrix = (Z * Y) * X
|
|
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 R_ConcatTransforms (const float in1[3][4], const 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];
|
|
}
|
|
|
|
|
|
|
|
void VectorRotate (const vec3_t in1, const float in2[3][4], vec3_t out)
|
|
{
|
|
out[0] = DotProduct(in1, in2[0]);
|
|
out[1] = DotProduct(in1, in2[1]);
|
|
out[2] = DotProduct(in1, in2[2]);
|
|
}
|
|
|
|
|
|
// rotate by the inverse of the matrix
|
|
void VectorIRotate (const vec3_t in1, const float in2[3][4], vec3_t 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];
|
|
}
|
|
|
|
|
|
void VectorTransform (const vec3_t in1, const 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];
|
|
}
|
|
|
|
|
|
|
|
void AngleQuaternion( const vec3_t 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
|
|
}
|
|
|
|
void QuaternionMatrix( const 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];
|
|
}
|
|
|
|
void QuaternionSlerp( const 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.00000001) {
|
|
if ((1.0 - cosom) > 0.00000001) {
|
|
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] = -p[1];
|
|
qt[1] = p[0];
|
|
qt[2] = -p[3];
|
|
qt[3] = p[2];
|
|
sclp = sin( (1.0 - t) * 0.5 * Q_PI);
|
|
sclq = sin( t * 0.5 * Q_PI);
|
|
for (i = 0; i < 3; i++) {
|
|
qt[i] = sclp * p[i] + sclq * qt[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
|