358 lines
8.1 KiB
C++
358 lines
8.1 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];
|
|
}
|
|
}
|
|
}
|