xash3d-fwgs/engine/common/mathlib.c

745 lines
14 KiB
C

/*
mathlib.c - internal mathlib
Copyright (C) 2010 Uncle Mike
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*/
#include <math.h>
#include "common.h"
#include "mathlib.h"
#include "eiface.h"
#define NUM_HULL_ROUNDS ARRAYSIZE( hull_table )
#define HULL_PRECISION 4
vec3_t vec3_origin = { 0, 0, 0 };
static word hull_table[] = { 2, 4, 6, 8, 12, 16, 18, 24, 28, 32, 36, 40, 48, 54, 56, 60, 64, 72, 80, 112, 120, 128, 140, 176 };
int boxpnt[6][4] =
{
{ 0, 4, 6, 2 }, // +X
{ 0, 1, 5, 4 }, // +Y
{ 0, 2, 3, 1 }, // +Z
{ 7, 5, 1, 3 }, // -X
{ 7, 3, 2, 6 }, // -Y
{ 7, 6, 4, 5 }, // -Z
};
// pre-quantized table normals from Quake1
const float m_bytenormals[NUMVERTEXNORMALS][3] =
{
#include "anorms.h"
};
/*
=================
anglemod
=================
*/
float anglemod( float a )
{
a = (360.0 / 65536) * ((int)(a*(65536/360.0)) & 65535);
return a;
}
/*
=================
SimpleSpline
NOTE: ripped from hl2 source
hermite basis function for smooth interpolation
Similar to Gain() above, but very cheap to call
value should be between 0 & 1 inclusive
=================
*/
float SimpleSpline( float value )
{
float valueSquared = value * value;
// nice little ease-in, ease-out spline-like curve
return (3.0f * valueSquared - 2.0f * valueSquared * value);
}
word FloatToHalf( float v )
{
unsigned int i = *((unsigned int *)&v);
unsigned int e = (i >> 23) & 0x00ff;
unsigned int m = i & 0x007fffff;
unsigned short h;
if( e <= 127 - 15 )
h = ((m | 0x00800000) >> (127 - 14 - e)) >> 13;
else h = (i >> 13) & 0x3fff;
h |= (i >> 16) & 0xc000;
return h;
}
float HalfToFloat( word h )
{
unsigned int f = (h << 16) & 0x80000000;
unsigned int em = h & 0x7fff;
if( em > 0x03ff )
{
f |= (em << 13) + ((127 - 15) << 23);
}
else
{
unsigned int m = em & 0x03ff;
if( m != 0 )
{
unsigned int e = (em >> 10) & 0x1f;
while(( m & 0x0400 ) == 0 )
{
m <<= 1;
e--;
}
m &= 0x3ff;
f |= ((e + (127 - 14)) << 23) | (m << 13);
}
}
return *((float *)&f);
}
/*
=================
RoundUpHullSize
round the hullsize to nearest 'right' value
=================
*/
void RoundUpHullSize( vec3_t size )
{
int i, j;
for( i = 0; i < 3; i++)
{
qboolean negative = false;
float result, value;
value = size[i];
if( value < 0.0f ) negative = true;
value = Q_ceil( fabs( value ));
// lookup hull table to find nearest supposed value
for( j = 0; j < NUM_HULL_ROUNDS; j++ )
{
if( value > hull_table[j] )
continue; // ceil only
if( negative )
{
result = ( value - hull_table[j] );
if( result <= HULL_PRECISION )
{
result = -hull_table[j];
break;
}
}
else
{
result = ( value - hull_table[j] );
if( result <= HULL_PRECISION )
{
result = hull_table[j];
break;
}
}
}
size[i] = result;
}
}
/*
=================
SignbitsForPlane
fast box on planeside test
=================
*/
int SignbitsForPlane( const vec3_t normal )
{
int bits, i;
for( bits = i = 0; i < 3; i++ )
if( normal[i] < 0.0f ) bits |= 1<<i;
return bits;
}
/*
=================
PlaneTypeForNormal
=================
*/
int PlaneTypeForNormal( const vec3_t normal )
{
if( normal[0] == 1.0f )
return PLANE_X;
if( normal[1] == 1.0f )
return PLANE_Y;
if( normal[2] == 1.0f )
return PLANE_Z;
return PLANE_NONAXIAL;
}
/*
=================
PlanesGetIntersectionPoint
=================
*/
qboolean PlanesGetIntersectionPoint( const mplane_t *plane1, const mplane_t *plane2, const mplane_t *plane3, vec3_t out )
{
vec3_t n1, n2, n3;
vec3_t n1n2, n2n3, n3n1;
float denom;
VectorNormalize2( plane1->normal, n1 );
VectorNormalize2( plane2->normal, n2 );
VectorNormalize2( plane3->normal, n3 );
CrossProduct( n1, n2, n1n2 );
CrossProduct( n2, n3, n2n3 );
CrossProduct( n3, n1, n3n1 );
denom = DotProduct( n1, n2n3 );
VectorClear( out );
// check if the denominator is zero (which would mean that no intersection is to be found
if( denom == 0.0f )
{
// no intersection could be found, return <0,0,0>
return false;
}
// compute intersection point
#if 0
VectorMAMAM( plane1->dist, n2n3, plane2->dist, n3n1, plane3->dist, n1n2, out );
#else
VectorMA( out, plane1->dist, n2n3, out );
VectorMA( out, plane2->dist, n3n1, out );
VectorMA( out, plane3->dist, n1n2, out );
#endif
VectorScale( out, ( 1.0f / denom ), out );
return true;
}
/*
=================
NearestPOW
=================
*/
int NearestPOW( int value, qboolean roundDown )
{
int n = 1;
if( value <= 0 ) return 1;
while( n < value ) n <<= 1;
if( roundDown )
{
if( n > value ) n >>= 1;
}
return n;
}
// remap a value in the range [A,B] to [C,D].
float RemapVal( float val, float A, float B, float C, float D )
{
return C + (D - C) * (val - A) / (B - A);
}
float ApproachVal( 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;
}
/*
=================
rsqrt
=================
*/
float rsqrt( float number )
{
int i;
float x, y;
if( number == 0.0f )
return 0.0f;
x = number * 0.5f;
i = *(int *)&number; // evil floating point bit level hacking
i = 0x5f3759df - (i >> 1); // what the fuck?
y = *(float *)&i;
y = y * (1.5f - (x * y * y)); // first iteration
return y;
}
/*
=================
SinCos
=================
*/
void SinCos( float radians, float *sine, float *cosine )
{
#if _MSC_VER == 1200
_asm
{
fld dword ptr [radians]
fsincos
mov edx, dword ptr [cosine]
mov eax, dword ptr [sine]
fstp dword ptr [edx]
fstp dword ptr [eax]
}
#else
*sine = sinf(radians);
*cosine = cosf(radians);
#endif
}
float VectorNormalizeLength2( const vec3_t v, vec3_t out )
{
float length, ilength;
length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
length = sqrt( length );
if( length )
{
ilength = 1.0f / length;
out[0] = v[0] * ilength;
out[1] = v[1] * ilength;
out[2] = v[2] * ilength;
}
return length;
}
void VectorVectors( const vec3_t forward, vec3_t right, vec3_t up )
{
float d;
right[0] = forward[2];
right[1] = -forward[0];
right[2] = forward[1];
d = DotProduct( forward, right );
VectorMA( right, -d, forward, right );
VectorNormalize( right );
CrossProduct( right, forward, up );
VectorNormalize( up );
}
/*
=================
AngleVectors
=================
*/
void AngleVectors( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up )
{
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[0] = cp * cy;
forward[1] = cp * sy;
forward[2] = -sp;
}
if( right )
{
right[0] = (-1.0f * sr * sp * cy + -1.0f * cr * -sy );
right[1] = (-1.0f * sr * sp * sy + -1.0f * cr * cy );
right[2] = (-1.0f * sr * cp);
}
if( up )
{
up[0] = (cr * sp * cy + -sr * -sy );
up[1] = (cr * sp * sy + -sr * cy );
up[2] = (cr * cp);
}
}
/*
=================
VectorAngles
=================
*/
void VectorAngles( const float *forward, float *angles )
{
float tmp, yaw, pitch;
if( !forward || !angles )
{
if( angles ) VectorClear( angles );
return;
}
if( forward[1] == 0 && forward[0] == 0 )
{
// fast case
yaw = 0;
if( forward[2] > 0 )
pitch = 90.0f;
else pitch = 270.0f;
}
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;
}
VectorSet( angles, pitch, yaw, 0 );
}
/*
=================
VectorsAngles
=================
*/
void VectorsAngles( const vec3_t forward, const vec3_t right, const vec3_t up, vec3_t angles )
{
float pitch, cpitch, yaw, roll;
pitch = -asin( forward[2] );
cpitch = cos( pitch );
if( fabs( cpitch ) > EQUAL_EPSILON ) // gimball lock?
{
cpitch = 1.0f / cpitch;
pitch = RAD2DEG( pitch );
yaw = RAD2DEG( atan2( forward[1] * cpitch, forward[0] * cpitch ));
roll = RAD2DEG( atan2( -right[2] * cpitch, up[2] * cpitch ));
}
else
{
pitch = forward[2] > 0 ? -90.0f : 90.0f;
yaw = RAD2DEG( atan2( right[0], -right[1] ));
roll = 180.0f;
}
angles[PITCH] = pitch;
angles[YAW] = yaw;
angles[ROLL] = roll;
}
//
// bounds operations
//
/*
=================
ClearBounds
=================
*/
void ClearBounds( vec3_t mins, vec3_t maxs )
{
// make bogus range
mins[0] = mins[1] = mins[2] = 999999.0f;
maxs[0] = maxs[1] = maxs[2] = -999999.0f;
}
/*
=================
AddPointToBounds
=================
*/
void AddPointToBounds( const vec3_t v, vec3_t mins, vec3_t maxs )
{
float val;
int i;
for( i = 0; i < 3; i++ )
{
val = v[i];
if( val < mins[i] ) mins[i] = val;
if( val > maxs[i] ) maxs[i] = val;
}
}
/*
=================
ExpandBounds
=================
*/
void ExpandBounds( vec3_t mins, vec3_t maxs, float offset )
{
mins[0] -= offset;
mins[1] -= offset;
mins[2] -= offset;
maxs[0] += offset;
maxs[1] += offset;
maxs[2] += offset;
}
/*
=================
BoundsIntersect
=================
*/
qboolean BoundsIntersect( const vec3_t mins1, const vec3_t maxs1, const vec3_t mins2, const vec3_t maxs2 )
{
if( mins1[0] > maxs2[0] || mins1[1] > maxs2[1] || mins1[2] > maxs2[2] )
return false;
if( maxs1[0] < mins2[0] || maxs1[1] < mins2[1] || maxs1[2] < mins2[2] )
return false;
return true;
}
/*
=================
BoundsAndSphereIntersect
=================
*/
qboolean BoundsAndSphereIntersect( const vec3_t mins, const vec3_t maxs, const vec3_t origin, float radius )
{
if( mins[0] > origin[0] + radius || mins[1] > origin[1] + radius || mins[2] > origin[2] + radius )
return false;
if( maxs[0] < origin[0] - radius || maxs[1] < origin[1] - radius || maxs[2] < origin[2] - radius )
return false;
return true;
}
/*
=================
SphereIntersect
=================
*/
qboolean SphereIntersect( const vec3_t vSphereCenter, float fSphereRadiusSquared, const vec3_t vLinePt, const vec3_t vLineDir )
{
float a, b, c, insideSqr;
vec3_t p;
// translate sphere to origin.
VectorSubtract( vLinePt, vSphereCenter, p );
a = DotProduct( vLineDir, vLineDir );
b = 2.0f * DotProduct( p, vLineDir );
c = DotProduct( p, p ) - fSphereRadiusSquared;
insideSqr = b * b - 4.0f * a * c;
if( insideSqr <= 0.000001f )
return false;
return true;
}
/*
=================
PlaneIntersect
find point where ray
was intersect with plane
=================
*/
void PlaneIntersect( const mplane_t *plane, const vec3_t p0, const vec3_t p1, vec3_t out )
{
float distToPlane = PlaneDiff( p0, plane );
float planeDotRay = DotProduct( plane->normal, p1 );
float sect = -(distToPlane) / planeDotRay;
VectorMA( p0, sect, p1, out );
}
/*
=================
RadiusFromBounds
=================
*/
float RadiusFromBounds( const vec3_t mins, const vec3_t maxs )
{
vec3_t corner;
int i;
for( i = 0; i < 3; i++ )
{
corner[i] = fabs( mins[i] ) > fabs( maxs[i] ) ? fabs( mins[i] ) : fabs( maxs[i] );
}
return VectorLength( corner );
}
//
// studio utils
//
/*
====================
AngleQuaternion
====================
*/
void AngleQuaternion( const vec3_t angles, vec4_t q, qboolean studio )
{
float sr, sp, sy, cr, cp, cy;
if( studio )
{
SinCos( angles[ROLL] * 0.5f, &sy, &cy );
SinCos( angles[YAW] * 0.5f, &sp, &cp );
SinCos( angles[PITCH] * 0.5f, &sr, &cr );
}
else
{
SinCos( DEG2RAD( angles[YAW] ) * 0.5f, &sy, &cy );
SinCos( DEG2RAD( angles[PITCH] ) * 0.5f, &sp, &cp );
SinCos( DEG2RAD( angles[ROLL] ) * 0.5f, &sr, &cr );
}
q[0] = sr * cp * cy - cr * sp * sy; // X
q[1] = cr * sp * cy + sr * cp * sy; // Y
q[2] = cr * cp * sy - sr * sp * cy; // Z
q[3] = cr * cp * cy + sr * sp * sy; // W
}
/*
====================
QuaternionAngle
====================
*/
void QuaternionAngle( const vec4_t q, vec3_t angles )
{
matrix3x4 mat;
Matrix3x4_FromOriginQuat( mat, q, vec3_origin );
Matrix3x4_AnglesFromMatrix( mat, angles );
}
/*
====================
QuaternionAlign
make sure quaternions are within 180 degrees of one another,
if not, reverse q
====================
*/
void QuaternionAlign( const vec4_t p, const vec4_t q, vec4_t qt )
{
// decide if one of the quaternions is backwards
float a = 0.0f;
float b = 0.0f;
int i;
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
{
for( i = 0; i < 4; i++ )
qt[i] = q[i];
}
}
/*
====================
QuaternionSlerpNoAlign
====================
*/
void QuaternionSlerpNoAlign( const vec4_t p, const vec4_t q, float t, vec4_t qt )
{
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
{
sclp = 1.0f - 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.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];
}
}
}
/*
====================
QuaternionSlerp
Quaternion sphereical linear interpolation
====================
*/
void QuaternionSlerp( const vec4_t p, const vec4_t q, float t, vec4_t qt )
{
vec4_t 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 );
}