forked from a1batross/Paranoia2_original
1739 lines
36 KiB
C++
1739 lines
36 KiB
C++
//=======================================================================
|
|
// Copyright (C) Shambler Team 2005
|
|
// mathlib.cpp - shared math library
|
|
//=======================================================================
|
|
|
|
#include "mathlib.h"
|
|
#include "const.h"
|
|
#include "com_model.h"
|
|
#include <math.h>
|
|
|
|
const Vector g_vecZero( 0, 0, 0 );
|
|
const Radian g_radZero( 0, 0, 0 );
|
|
|
|
float g_hullcolor[8][3] =
|
|
{
|
|
{ 1.0f, 1.0f, 1.0f },
|
|
{ 1.0f, 0.5f, 0.5f },
|
|
{ 0.5f, 1.0f, 0.5f },
|
|
{ 1.0f, 1.0f, 0.5f },
|
|
{ 0.5f, 0.5f, 1.0f },
|
|
{ 1.0f, 0.5f, 1.0f },
|
|
{ 0.5f, 1.0f, 1.0f },
|
|
{ 1.0f, 1.0f, 1.0f },
|
|
};
|
|
|
|
int g_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
|
|
};
|
|
|
|
/*
|
|
=================
|
|
SignbitsForPlane
|
|
|
|
fast box on planeside test
|
|
=================
|
|
*/
|
|
int SignbitsForPlane( const Vector &normal )
|
|
{
|
|
int bits, i;
|
|
|
|
for( bits = 0, i = 0; i < 3; i++ )
|
|
if( normal[i] < 0.0f )
|
|
bits |= 1<<i;
|
|
return bits;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
PlaneTypeForNormal
|
|
=================
|
|
*/
|
|
int PlaneTypeForNormal( const Vector &normal )
|
|
{
|
|
if( normal.x == 1.0f )
|
|
return PLANE_X;
|
|
if( normal.y == 1.0 )
|
|
return PLANE_Y;
|
|
if( normal.z == 1.0 )
|
|
return PLANE_Z;
|
|
return PLANE_NONAXIAL;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
SetPlane
|
|
=================
|
|
*/
|
|
void SetPlane( mplane_t *plane, const Vector &vecNormal, float flDist, int type )
|
|
{
|
|
if( type == -1 )
|
|
plane->type = PlaneTypeForNormal( vecNormal );
|
|
else plane->type = type;
|
|
plane->signbits = SignbitsForPlane( vecNormal );
|
|
plane->normal = vecNormal;
|
|
plane->dist = flDist;
|
|
}
|
|
|
|
/*
|
|
================
|
|
PlaneFromPoints
|
|
================
|
|
*/
|
|
void PlaneFromPoints( const Vector &p0, const Vector &p1, const Vector &p2, mplane_t *plane )
|
|
{
|
|
Vector t1 = p0 - p1;
|
|
Vector t2 = p2 - p1;
|
|
Vector normal = CrossProduct( t1, t2 ).Normalize();
|
|
float dist = DotProduct( p0, normal );
|
|
|
|
SetPlane( plane, normal, dist );
|
|
}
|
|
|
|
/*
|
|
=================
|
|
PlanesGetIntersectionPoint
|
|
|
|
=================
|
|
*/
|
|
bool PlanesGetIntersectionPoint( const mplane_t *plane1, const mplane_t *plane2, const mplane_t *plane3, Vector &out )
|
|
{
|
|
Vector n1 = plane1->normal.Normalize();
|
|
Vector n2 = plane2->normal.Normalize();
|
|
Vector n3 = plane3->normal.Normalize();
|
|
|
|
Vector n1n2 = CrossProduct( n1, n2 );
|
|
Vector n2n3 = CrossProduct( n2, n3 );
|
|
Vector n3n1 = CrossProduct( n3, n1 );
|
|
|
|
float denom = DotProduct( n1, n2n3 );
|
|
out = g_vecZero;
|
|
|
|
// 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
|
|
out += n2n3 * plane1->dist;
|
|
out += n3n1 * plane2->dist;
|
|
out += n1n2 * plane3->dist;
|
|
out *= (1.0f / denom );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
PlaneIntersect
|
|
|
|
find point where ray
|
|
was intersect with plane
|
|
=================
|
|
*/
|
|
Vector PlaneIntersect( mplane_t *plane, const Vector& p0, const Vector& p1 )
|
|
{
|
|
float distToPlane = PlaneDiff( p0, plane );
|
|
float planeDotRay = DotProduct( plane->normal, p1 );
|
|
float sect = -(distToPlane) / planeDotRay;
|
|
|
|
return p0 + p1 * sect;
|
|
}
|
|
|
|
void PerpendicularVector( Vector &dst, const Vector &src )
|
|
{
|
|
// LordHavoc: optimized to death and beyond
|
|
int pos;
|
|
float minelem;
|
|
|
|
if( src.x )
|
|
{
|
|
dst.x = 0;
|
|
if( src.y )
|
|
{
|
|
dst.y = 0;
|
|
if( src.z )
|
|
{
|
|
dst.z = 0;
|
|
pos = 0;
|
|
minelem = fabs( src.x );
|
|
if( fabs( src.y ) < minelem )
|
|
{
|
|
pos = 1;
|
|
minelem = fabs( src.y );
|
|
}
|
|
if( fabs( src.y ) < minelem )
|
|
pos = 2;
|
|
|
|
dst[pos] = 1;
|
|
dst.x -= src[pos] * src.x;
|
|
dst.y -= src[pos] * src.y;
|
|
dst.z -= src[pos] * src.z;
|
|
|
|
// normalize the result
|
|
dst = dst.Normalize();
|
|
}
|
|
else dst.z = 1;
|
|
}
|
|
else
|
|
{
|
|
dst.y = 1;
|
|
dst.z = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dst.x = 1;
|
|
dst.y = 0;
|
|
dst.z = 0;
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
VectorAngles
|
|
|
|
=================
|
|
*/
|
|
void VectorAngles( const Vector &forward, Vector &angles )
|
|
{
|
|
angles[ROLL] = 0.0f;
|
|
|
|
if( forward.x || forward.y )
|
|
{
|
|
float tmp;
|
|
|
|
angles[YAW] = RAD2DEG( atan2( forward.y, forward.x ));
|
|
if( angles[YAW] < 0 ) angles[YAW] += 360;
|
|
|
|
tmp = sqrt( forward.x * forward.x + forward.y * forward.y );
|
|
angles[PITCH] = RAD2DEG( atan2( forward.z, tmp ));
|
|
if( angles[PITCH] < 0 ) angles[PITCH] += 360;
|
|
}
|
|
else
|
|
{
|
|
// fast case
|
|
angles[YAW] = 0.0f;
|
|
if( forward.z > 0 )
|
|
angles[PITCH] = 90.0f;
|
|
else angles[PITCH] = 270.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
VectorAngles2
|
|
|
|
this version without stupid quake bug
|
|
=================
|
|
*/
|
|
void VectorAngles2( const Vector &forward, Vector &angles )
|
|
{
|
|
angles[ROLL] = 0.0f;
|
|
|
|
if( forward.x || forward.y )
|
|
{
|
|
float tmp;
|
|
|
|
angles[YAW] = RAD2DEG( atan2( forward.y, forward.x ));
|
|
if( angles[YAW] < 0.0f ) angles[YAW] += 360.0f;
|
|
|
|
tmp = sqrt( forward.x * forward.x + forward.y * forward.y );
|
|
angles[PITCH] = RAD2DEG( atan2( -forward.z, tmp ));
|
|
if( angles[PITCH] < 0.0f ) angles[PITCH] += 360.0f;
|
|
}
|
|
else
|
|
{
|
|
// fast case
|
|
angles[YAW] = 0.0f;
|
|
if( forward.z > 0.0f )
|
|
angles[PITCH] = 270.0f;
|
|
else angles[PITCH] = 90.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
NearestPOW
|
|
=================
|
|
*/
|
|
int NearestPOW( int value, bool roundDown )
|
|
{
|
|
int n = 1;
|
|
|
|
if( value <= 0 ) return 1;
|
|
while( n < value ) n <<= 1;
|
|
|
|
if( roundDown )
|
|
{
|
|
if( n > value ) n >>= 1;
|
|
}
|
|
return n;
|
|
}
|
|
|
|
void MakeAxial( float normal[3] )
|
|
{
|
|
int i, type;
|
|
|
|
for( type = 0; type < 3; type++ )
|
|
{
|
|
if( fabs( normal[type] ) > 0.9999f )
|
|
break;
|
|
}
|
|
|
|
// make positive and pure axial
|
|
for( i = 0; i < 3 && type != 3; i++ )
|
|
{
|
|
if( i == type )
|
|
normal[i] = 1.0f;
|
|
else normal[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Transforms a AABB into another space; which will inherently grow the box.
|
|
//-----------------------------------------------------------------------------
|
|
void TransformAABB( const matrix4x4& world, const Vector &mins, const Vector &maxs, Vector &absmin, Vector &absmax )
|
|
{
|
|
Vector localCenter = (mins + maxs) * 0.5f;
|
|
Vector localExtents = maxs - localCenter;
|
|
Vector worldCenter = world.VectorTransform( localCenter );
|
|
matrix4x4 iworld = world.Transpose();
|
|
Vector worldExtents;
|
|
|
|
worldExtents.x = DotProductAbs( localExtents, iworld.GetForward( ));
|
|
worldExtents.y = DotProductAbs( localExtents, iworld.GetRight( ));
|
|
worldExtents.z = DotProductAbs( localExtents, iworld.GetUp( ));
|
|
|
|
absmin = worldCenter - worldExtents;
|
|
absmax = worldCenter + worldExtents;
|
|
}
|
|
|
|
/*
|
|
==================
|
|
TransformAABBLocal
|
|
==================
|
|
*/
|
|
void TransformAABBLocal( const matrix4x4& world, const Vector &mins, const Vector &maxs, Vector &outmins, Vector &outmaxs )
|
|
{
|
|
matrix4x4 im = world.Invert();
|
|
ClearBounds( outmins, outmaxs );
|
|
Vector p1, p2;
|
|
int i;
|
|
|
|
// compute a full bounding box
|
|
for( i = 0; i < 8; i++ )
|
|
{
|
|
p1.x = ( i & 1 ) ? mins.x : maxs.x;
|
|
p1.y = ( i & 2 ) ? mins.y : maxs.y;
|
|
p1.z = ( i & 4 ) ? mins.z : maxs.z;
|
|
|
|
p2.x = DotProduct( p1, im[0] );
|
|
p2.y = DotProduct( p1, im[1] );
|
|
p2.z = DotProduct( p1, im[2] );
|
|
|
|
if( p2.x < outmins.x ) outmins.x = p2.x;
|
|
if( p2.x > outmaxs.x ) outmaxs.x = p2.x;
|
|
if( p2.y < outmins.y ) outmins.y = p2.y;
|
|
if( p2.y > outmaxs.y ) outmaxs.y = p2.y;
|
|
if( p2.z < outmins.z ) outmins.z = p2.z;
|
|
if( p2.z > outmaxs.z ) outmaxs.z = p2.z;
|
|
}
|
|
|
|
// sanity check
|
|
for( i = 0; i < 3; i++ )
|
|
{
|
|
if( outmins[i] > outmaxs[i] )
|
|
{
|
|
outmins = g_vecZero;
|
|
outmaxs = g_vecZero;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
float CalcSqrDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point )
|
|
{
|
|
float flDelta;
|
|
float flDistSqr = 0.0f;
|
|
|
|
if( point.x < mins.x )
|
|
{
|
|
flDelta = (mins.x - point.x);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
else if( point.x > maxs.x )
|
|
{
|
|
flDelta = (point.x - maxs.x);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
|
|
if( point.y < mins.y )
|
|
{
|
|
flDelta = (mins.y - point.y);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
else if( point.y > maxs.y )
|
|
{
|
|
flDelta = (point.y - maxs.y);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
|
|
if( point.z < mins.z )
|
|
{
|
|
flDelta = (mins.z - point.z);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
else if( point.z > maxs.z )
|
|
{
|
|
flDelta = (point.z - maxs.z);
|
|
flDistSqr += flDelta * flDelta;
|
|
}
|
|
|
|
return flDistSqr;
|
|
}
|
|
|
|
// returns true if the sphere and cone intersect
|
|
// NOTE: cone sine/cosine are the half angle of the cone
|
|
bool IsSphereIntersectingCone( const Vector &sphereCenter, float sphereRadius, const Vector &coneOrigin, const Vector &coneNormal, float coneSine, float coneCosine )
|
|
{
|
|
Vector backCenter = coneOrigin - (sphereRadius / coneSine) * coneNormal;
|
|
Vector delta = sphereCenter - backCenter;
|
|
float deltaLen = delta.Length();
|
|
|
|
if( DotProduct( coneNormal, delta ) >= deltaLen * coneCosine )
|
|
{
|
|
delta = sphereCenter - coneOrigin;
|
|
deltaLen = delta.Length();
|
|
if( -DotProduct(coneNormal, delta) >= deltaLen * coneSine )
|
|
return ( deltaLen <= sphereRadius ) ? true : false;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void CalcClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut )
|
|
{
|
|
closestOut.x = bound( mins.x, point.x, maxs.x );
|
|
closestOut.y = bound( mins.y, point.y, maxs.y );
|
|
closestOut.z = bound( mins.z, point.z, maxs.z );
|
|
}
|
|
|
|
//
|
|
// bounds operations
|
|
//
|
|
/*
|
|
=================
|
|
ClearBounds
|
|
=================
|
|
*/
|
|
void ClearBounds( Vector &mins, Vector &maxs )
|
|
{
|
|
// make bogus range
|
|
mins.x = mins.y = mins.z = 999999;
|
|
maxs.x = maxs.y = maxs.z = -999999;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
ClearBounds
|
|
=================
|
|
*/
|
|
void ClearBounds( Vector2D &mins, Vector2D &maxs )
|
|
{
|
|
// make bogus range
|
|
mins.x = mins.y = 999999;
|
|
maxs.x = maxs.y = -999999;
|
|
}
|
|
|
|
bool BoundsIsCleared( const Vector &mins, const Vector &maxs )
|
|
{
|
|
if( mins.x <= maxs.x || mins.y <= maxs.y || mins.z <= maxs.z )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
bool BoundsIsNull( const Vector &mins, const Vector &maxs )
|
|
{
|
|
if( mins.x != maxs.x || mins.y != maxs.y || mins.z != maxs.z )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
ExpandBounds
|
|
=================
|
|
*/
|
|
void ExpandBounds( Vector &mins, Vector &maxs, float offset )
|
|
{
|
|
mins[0] -= offset;
|
|
mins[1] -= offset;
|
|
mins[2] -= offset;
|
|
maxs[0] += offset;
|
|
maxs[1] += offset;
|
|
maxs[2] += offset;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
AddPointToBounds
|
|
=================
|
|
*/
|
|
void AddPointToBounds( const Vector &v, Vector &mins, Vector &maxs, float limit )
|
|
{
|
|
if( limit )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( v[i] < mins[i] ) mins[i] = Q_max( v[i], mins[i] - limit );
|
|
if( v[i] > maxs[i] ) maxs[i] = Q_min( v[i], maxs[i] + limit );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( v[i] < mins[i] ) mins[i] = v[i];
|
|
if( v[i] > maxs[i] ) maxs[i] = v[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
AddPointToBounds
|
|
=================
|
|
*/
|
|
void AddPointToBounds( const Vector2D &v, Vector2D &mins, Vector2D &maxs )
|
|
{
|
|
for( int i = 0; i < 2; i++ )
|
|
{
|
|
if( v[i] < mins[i] ) mins[i] = v[i];
|
|
if( v[i] > maxs[i] ) maxs[i] = v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsIntersect
|
|
=================
|
|
*/
|
|
bool BoundsIntersect( const Vector &mins1, const Vector &maxs1, const Vector &mins2, const Vector &maxs2 )
|
|
{
|
|
if( mins1.x > maxs2.x || mins1.y > maxs2.y || mins1.z > maxs2.z )
|
|
return false;
|
|
if( maxs1.x < mins2.x || maxs1.y < mins2.y || maxs1.z < mins2.z )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsIntersect
|
|
=================
|
|
*/
|
|
bool BoundsIntersect( const Vector2D &mins1, const Vector2D &maxs1, const Vector2D &mins2, const Vector2D &maxs2 )
|
|
{
|
|
if( mins1.x > maxs2.x || mins1.y > maxs2.y )
|
|
return false;
|
|
if( maxs1.x < mins2.x || maxs1.y < mins2.y )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsAndSphereIntersect
|
|
=================
|
|
*/
|
|
bool BoundsAndSphereIntersect( const Vector &mins, const Vector &maxs, const Vector &origin, float radius )
|
|
{
|
|
if( mins.x > origin.x + radius || mins.y > origin.y + radius || mins.z > origin.z + radius )
|
|
return false;
|
|
if( maxs.x < origin.x - radius || maxs.y < origin.y - radius || maxs.z < origin.z - radius )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
BoundsAndSphereIntersect
|
|
=================
|
|
*/
|
|
bool BoundsAndSphereIntersect( const Vector2D &mins, const Vector2D &maxs, const Vector2D &origin, float radius )
|
|
{
|
|
if( mins.x > origin.x + radius || mins.y > origin.y + radius )
|
|
return false;
|
|
if( maxs.x < origin.x - radius || maxs.y < origin.y - radius )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
RadiusFromBounds
|
|
=================
|
|
*/
|
|
float RadiusFromBounds( const Vector &mins, const Vector &maxs )
|
|
{
|
|
Vector corner;
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
corner[i] = fabs( mins[i] ) > fabs( maxs[i] ) ? fabs( mins[i] ) : fabs( maxs[i] );
|
|
}
|
|
return corner.Length();
|
|
}
|
|
|
|
//
|
|
// quaternion operations
|
|
//
|
|
|
|
/*
|
|
====================
|
|
AngleQuaternion
|
|
|
|
degrees euler XYZ version
|
|
====================
|
|
*/
|
|
void AngleQuaternion( const Vector &angles, Vector4D &quat )
|
|
{
|
|
float sr, sp, sy, cr, cp, cy;
|
|
|
|
SinCos( DEG2RAD( angles.y ) * 0.5f, &sy, &cy );
|
|
SinCos( DEG2RAD( angles.x ) * 0.5f, &sp, &cp );
|
|
SinCos( DEG2RAD( angles.z ) * 0.5f, &sr, &cr );
|
|
|
|
float srXcp = sr * cp, crXsp = cr * sp;
|
|
quat.x = srXcp * cy - crXsp * sy; // X
|
|
quat.y = crXsp * cy + srXcp * sy; // Y
|
|
|
|
float crXcp = cr * cp, srXsp = sr * sp;
|
|
quat.z = crXcp * sy - srXsp * cy; // Z
|
|
quat.w = crXcp * cy + srXsp * sy; // W (real component)
|
|
}
|
|
|
|
/*
|
|
====================
|
|
AngleQuaternion
|
|
|
|
radian euler YZX version
|
|
====================
|
|
*/
|
|
void AngleQuaternion( const Radian &angles, Vector4D &quat )
|
|
{
|
|
float sr, sp, sy, cr, cp, cy;
|
|
|
|
SinCos( angles.z * 0.5f, &sy, &cy );
|
|
SinCos( angles.y * 0.5f, &sp, &cp );
|
|
SinCos( angles.x * 0.5f, &sr, &cr );
|
|
|
|
float srXcp = sr * cp, crXsp = cr * sp;
|
|
quat.x = srXcp * cy - crXsp * sy; // X
|
|
quat.y = crXsp * cy + srXcp * sy; // Y
|
|
|
|
float crXcp = cr * cp, srXsp = sr * sp;
|
|
quat.z = crXcp * sy - srXsp * cy; // Z
|
|
quat.w = crXcp * cy + srXsp * sy; // W (real component)
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionAngle
|
|
|
|
====================
|
|
*/
|
|
void QuaternionAngle( const Vector4D &quat, Vector &angles )
|
|
{
|
|
// g-cont. it's incredible stupid way but...
|
|
matrix3x3 temp( quat );
|
|
temp.GetAngles( angles );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionAngle
|
|
|
|
====================
|
|
*/
|
|
void QuaternionAngle( const Vector4D &quat, Radian &angles )
|
|
{
|
|
// g-cont. it's incredible stupid way but...
|
|
matrix3x3 temp( quat );
|
|
temp.GetAngles( angles );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionAlign
|
|
|
|
make sure quaternions are within 180 degrees of one another,
|
|
if not, reverse q
|
|
====================
|
|
*/
|
|
void QuaternionAlign( const Vector4D &p, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
// decide if one of the quaternions is backwards
|
|
float a = 0;
|
|
float b = 0;
|
|
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 if( &qt != &q )
|
|
{
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
qt[i] = q[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionSlerpNoAlign
|
|
====================
|
|
*/
|
|
void QuaternionSlerpNoAlign( const Vector4D &p, const Vector4D &q, float t, Vector4D &qt )
|
|
{
|
|
float omega, cosom, sinom, sclp, sclq;
|
|
|
|
// 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
|
|
{
|
|
// TODO: add short circuit for cosom == 1.0f?
|
|
sclp = 1.0f - t;
|
|
sclq = t;
|
|
}
|
|
|
|
for( int 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( int i = 0; i < 3; i++ )
|
|
{
|
|
qt[i] = sclp * p[i] + sclq * qt[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionSlerp
|
|
|
|
Quaternion sphereical linear interpolation
|
|
====================
|
|
*/
|
|
void QuaternionSlerp( const Vector4D &p, const Vector4D &q, float t, Vector4D &qt )
|
|
{
|
|
Vector4D 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 );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionSlerp
|
|
|
|
Quaternion sphereical linear interpolation
|
|
====================
|
|
*/
|
|
void QuaternionSlerp( const Radian &r0, const Radian &r1, float t, Radian &r2 )
|
|
{
|
|
Vector4D q0, q1, q2;
|
|
|
|
AngleQuaternion( r0, q0 );
|
|
AngleQuaternion( r1, q1 );
|
|
QuaternionSlerp( q0, q1, t, q2 );
|
|
QuaternionAngle( q2, r2 );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionBlend
|
|
====================
|
|
*/
|
|
void QuaternionBlend( const Vector4D &p, const Vector4D &q, float t, Vector4D &qt )
|
|
{
|
|
// decide if one of the quaternions is backwards
|
|
Vector4D q2;
|
|
|
|
QuaternionAlign( p, q, q2 );
|
|
QuaternionBlendNoAlign( p, q2, t, qt );
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionBlendNoAlign
|
|
====================
|
|
*/
|
|
void QuaternionBlendNoAlign( const Vector4D &p, const Vector4D &q, float t, Vector4D &qt )
|
|
{
|
|
float sclp, sclq;
|
|
|
|
// 0.0 returns p, 1.0 return q.
|
|
sclp = 1.0f - t;
|
|
sclq = t;
|
|
|
|
for( int i = 0; i < 4; i++ )
|
|
{
|
|
qt[i] = sclp * p[i] + sclq * q[i];
|
|
}
|
|
|
|
qt = qt.Normalize();
|
|
}
|
|
|
|
void QuaternionAdd( const Vector4D &p, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
Vector4D q2;
|
|
QuaternionAlign( p, q, q2 );
|
|
qt = p + q2;
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionMultiply
|
|
|
|
multiply two quaternions
|
|
====================
|
|
*/
|
|
void QuaternionMultiply( const Vector4D &q1, const Vector4D &q2, Vector4D &out )
|
|
{
|
|
out[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
|
|
out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2];
|
|
out[2] = q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0];
|
|
out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionVectorTransform
|
|
|
|
transform vector by quaternion
|
|
====================
|
|
*/
|
|
void QuaternionVectorTransform( const Vector4D &q, const Vector &v, Vector &out )
|
|
{
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
|
|
|
// 9 muls, 3 adds
|
|
x2 = q[0] + q[0]; y2 = q[1] + q[1]; z2 = q[2] + q[2];
|
|
xx = q[0] * x2; xy = q[0] * y2; xz = q[0] * z2;
|
|
yy = q[1] * y2; yz = q[1] * z2; zz = q[2] * z2;
|
|
wx = q[3] * x2; wy = q[3] * y2; wz = q[3] * z2;
|
|
|
|
// 9 muls, 9 subs, 9 adds
|
|
out[0] = ( 1.0f - yy - zz ) * v[0] + ( xy - wz ) * v[1] + ( xz + wy ) * v[2];
|
|
out[1] = ( xy + wz ) * v[0] + ( 1.0f - xx - zz ) * v[1] + ( yz - wx ) * v[2];
|
|
out[2] = ( xz - wy ) * v[0] + ( yz + wx ) * v[1] + ( 1.0f - xx - yy ) * v[2];
|
|
}
|
|
|
|
/*
|
|
====================
|
|
QuaternionConcatTransforms
|
|
|
|
transform quat\vector by another quat\vector
|
|
====================
|
|
*/
|
|
void QuaternionConcatTransforms( const Vector4D &q1, const Vector &v1, const Vector4D &q2, const Vector &v2, Vector4D &q, Vector &v )
|
|
{
|
|
QuaternionMultiply( q1, q2, q );
|
|
QuaternionVectorTransform( q1, v2, v );
|
|
v += v1;
|
|
}
|
|
|
|
void QuaternionMult( const Vector4D &p, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
if( &p == &qt )
|
|
{
|
|
Vector4D p2 = p;
|
|
QuaternionMult( p2, q, qt );
|
|
return;
|
|
}
|
|
|
|
// decide if one of the quaternions is backwards
|
|
Vector4D q2;
|
|
|
|
QuaternionAlign( p, q, q2 );
|
|
qt.x = p.x * q2.w + p.y * q2.z - p.z * q2.y + p.w * q2.x;
|
|
qt.y = -p.x * q2.z + p.y * q2.w + p.z * q2.x + p.w * q2.y;
|
|
qt.z = p.x * q2.y - p.y * q2.x + p.z * q2.w + p.w * q2.z;
|
|
qt.w = -p.x * q2.x - p.y * q2.y - p.z * q2.z + p.w * q2.w;
|
|
}
|
|
|
|
void QuaternionScale( const Vector4D &p, float t, Vector4D &q )
|
|
{
|
|
float r;
|
|
|
|
// FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to
|
|
// use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale.
|
|
#if 1
|
|
Vector ps = Vector( p.x, p.y, p.z );
|
|
float sinom = ps.Length(); // !!!
|
|
#else
|
|
float sinom = p.Length(); // !!!
|
|
#endif
|
|
sinom = Q_min( sinom, 1.0f );
|
|
float sinsom = sin( asin( sinom ) * t );
|
|
|
|
t = sinsom / (sinom + FLT_EPSILON);
|
|
q.x = p.x * t;
|
|
q.y = p.y * t;
|
|
q.z = p.z * t;
|
|
|
|
// rescale rotation
|
|
r = 1.0f - sinsom * sinsom;
|
|
|
|
if( r < 0.0f )
|
|
r = 0.0f;
|
|
r = sqrt( r );
|
|
|
|
// keep sign of rotation
|
|
if( p.w < 0 ) q.w = -r;
|
|
else q.w = r;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: Converts an exponential map (ang/axis) to a quaternion
|
|
//-----------------------------------------------------------------------------
|
|
void AxisAngleQuaternion( const Vector &axis, float angle, Vector4D &q )
|
|
{
|
|
float sa, ca;
|
|
|
|
SinCos( DEG2RAD( angle ) * 0.5f, &sa, &ca );
|
|
|
|
q.x = axis.x * sa;
|
|
q.y = axis.y * sa;
|
|
q.z = axis.z * sa;
|
|
q.w = ca;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: qt = ( s * p ) * q
|
|
//-----------------------------------------------------------------------------
|
|
void QuaternionSM( float s, const Vector4D &p, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
Vector4D p1, q1;
|
|
|
|
QuaternionScale( p, s, p1 );
|
|
QuaternionMult( p1, q, q1 );
|
|
qt = q1.Normalize();
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: qt = p * ( s * q )
|
|
//-----------------------------------------------------------------------------
|
|
void QuaternionMA( const Vector4D &p, float s, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
Vector4D p1, q1;
|
|
|
|
QuaternionScale( q, s, q1 );
|
|
QuaternionMult( p, q1, p1 );
|
|
qt = p1.Normalize();
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: qt = p + s * q
|
|
//-----------------------------------------------------------------------------
|
|
void QuaternionAccumulate( const Vector4D &p, float s, const Vector4D &q, Vector4D &qt )
|
|
{
|
|
Vector4D q2;
|
|
|
|
QuaternionAlign( p, q, q2 );
|
|
qt[0] = p[0] + s * q2[0];
|
|
qt[1] = p[1] + s * q2[1];
|
|
qt[2] = p[2] + s * q2[2];
|
|
qt[3] = p[3] + s * q2[3];
|
|
}
|
|
|
|
//
|
|
// lerping stuff
|
|
//
|
|
|
|
/*
|
|
===================
|
|
InterpolateOrigin
|
|
|
|
Interpolate position.
|
|
Frac is 0.0 to 1.0
|
|
===================
|
|
*/
|
|
void InterpolateOrigin( const Vector& start, const Vector& end, Vector& output, float frac, bool back )
|
|
{
|
|
if( back ) output += frac * ( end - start );
|
|
else output = start + frac * ( end - start );
|
|
}
|
|
|
|
/*
|
|
===================
|
|
InterpolateAngles
|
|
|
|
Interpolate Euler angles.
|
|
Frac is 0.0 to 1.0 ( i.e., should probably be clamped, but doesn't have to be )
|
|
===================
|
|
*/
|
|
void InterpolateAngles( const Vector& start, const Vector& end, Vector& output, float frac, bool back )
|
|
{
|
|
#if 0
|
|
Vector4D src, dest;
|
|
|
|
// convert to quaternions
|
|
AngleQuaternion( start, src );
|
|
AngleQuaternion( end, dest );
|
|
|
|
Vector4D result;
|
|
Vector out;
|
|
|
|
// slerp
|
|
QuaternionSlerp( src, dest, frac, result );
|
|
|
|
// convert to euler
|
|
QuaternionAngle( result, out );
|
|
|
|
if( back ) output += out;
|
|
else output = out;
|
|
#else
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
float ang1, ang2;
|
|
|
|
ang1 = end[i];
|
|
ang2 = start[i];
|
|
|
|
float d = ang1 - ang2;
|
|
|
|
if( d > 180 ) d -= 360;
|
|
else if( d < -180 ) d += 360;
|
|
|
|
output[i] += d * frac;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
====================
|
|
RotatePointAroundVector
|
|
====================
|
|
*/
|
|
void RotatePointAroundVector( Vector &dst, const Vector &dir, const Vector &point, float degrees )
|
|
{
|
|
float t0, t1;
|
|
float angle, c, s;
|
|
Vector vr, vu, vf = dir;
|
|
|
|
angle = DEG2RAD( degrees );
|
|
SinCos( angle, &s, &c );
|
|
VectorMatrix( vf, vr, vu );
|
|
|
|
t0 = vr.x * c + vu.x * -s;
|
|
t1 = vr.x * s + vu.x * c;
|
|
dst.x = (t0 * vr.x + t1 * vu.x + vf.x * vf.x) * point.x
|
|
+ (t0 * vr.y + t1 * vu.y + vf.x * vf.y) * point.y
|
|
+ (t0 * vr.z + t1 * vu.z + vf.x * vf.z) * point.z;
|
|
|
|
t0 = vr.y * c + vu.y * -s;
|
|
t1 = vr.y * s + vu.y * c;
|
|
dst.y = (t0 * vr.x + t1 * vu.x + vf.y * vf.x) * point.x
|
|
+ (t0 * vr.y + t1 * vu.y + vf.y * vf.y) * point.y
|
|
+ (t0 * vr.z + t1 * vu.z + vf.y * vf.z) * point.z;
|
|
|
|
t0 = vr.z * c + vu.z * -s;
|
|
t1 = vr.z * s + vu.z * c;
|
|
dst.z = (t0 * vr.x + t1 * vu.x + vf.z * vf.x) * point.x
|
|
+ (t0 * vr.y + t1 * vu.y + vf.z * vf.y) * point.y
|
|
+ (t0 * vr.z + t1 * vu.z + vf.z * vf.z) * point.z;
|
|
}
|
|
|
|
void NormalizeAngles( Vector &angles )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( angles[i] > 180.0f )
|
|
{
|
|
angles[i] -= 360.0f;
|
|
}
|
|
else if( angles[i] < -180.0f )
|
|
{
|
|
angles[i] += 360.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
===================
|
|
AngleBetweenVectors
|
|
|
|
===================
|
|
*/
|
|
float AngleBetweenVectors( const Vector v1, const Vector v2 )
|
|
{
|
|
float l1 = v1.Length();
|
|
float l2 = v2.Length();
|
|
|
|
if( !l1 || !l2 )
|
|
return 0.0f;
|
|
|
|
float angle = acos( DotProduct( v1, v2 )) / (l1 * l2);
|
|
|
|
return RAD2DEG( angle );
|
|
}
|
|
|
|
void VectorMatrix( Vector &forward, Vector &right, Vector &up )
|
|
{
|
|
if( forward.x || forward.y )
|
|
{
|
|
right = Vector( forward.y, -forward.x, 0 );
|
|
right = right.Normalize();
|
|
up = CrossProduct( forward, right );
|
|
}
|
|
else
|
|
{
|
|
right = Vector( 1.0f, 0.0f, 0.0f );
|
|
up = Vector( 0.0f, 1.0f, 0.0f );
|
|
}
|
|
}
|
|
|
|
/*
|
|
==================
|
|
BoxOnPlaneSide
|
|
|
|
Returns 1, 2, or 1 + 2
|
|
==================
|
|
*/
|
|
int BoxOnPlaneSide( const Vector &emins, const Vector &emaxs, const mplane_t *p )
|
|
{
|
|
float dist1, dist2;
|
|
int sides = 0;
|
|
|
|
// general case
|
|
switch( p->signbits )
|
|
{
|
|
case 0:
|
|
dist1 = p->normal.x * emaxs.x + p->normal.y * emaxs.y + p->normal.z * emaxs.z;
|
|
dist2 = p->normal.x * emins.x + p->normal.y * emins.y + p->normal.z * emins.z;
|
|
break;
|
|
case 1:
|
|
dist1 = p->normal.x * emins.x + p->normal.y * emaxs.y + p->normal.z * emaxs.z;
|
|
dist2 = p->normal.x * emaxs.x + p->normal.y * emins.y + p->normal.z * emins.z;
|
|
break;
|
|
case 2:
|
|
dist1 = p->normal.x * emaxs.x + p->normal.y * emins.y + p->normal.z * emaxs.z;
|
|
dist2 = p->normal.x * emins.x + p->normal.y * emaxs.y + p->normal.z * emins.z;
|
|
break;
|
|
case 3:
|
|
dist1 = p->normal.x * emins.x + p->normal.y * emins.y + p->normal.z * emaxs.z;
|
|
dist2 = p->normal.x * emaxs.x + p->normal.y * emaxs.y + p->normal.z * emins.z;
|
|
break;
|
|
case 4:
|
|
dist1 = p->normal.x * emaxs.x + p->normal.y * emaxs.y + p->normal.z * emins.z;
|
|
dist2 = p->normal.x * emins.x + p->normal.y * emins.y + p->normal.z * emaxs.z;
|
|
break;
|
|
case 5:
|
|
dist1 = p->normal.x * emins.x + p->normal.y * emaxs.y + p->normal.z * emins.z;
|
|
dist2 = p->normal.x * emaxs.x + p->normal.y * emins.y + p->normal.z * emaxs.z;
|
|
break;
|
|
case 6:
|
|
dist1 = p->normal.x * emaxs.x + p->normal.y * emins.y + p->normal.z * emins.z;
|
|
dist2 = p->normal.x * emins.x + p->normal.y * emaxs.y + p->normal.z * emaxs.z;
|
|
break;
|
|
case 7:
|
|
dist1 = p->normal.x * emins.x + p->normal.y * emins.y + p->normal.z * emins.z;
|
|
dist2 = p->normal.x * emaxs.x + p->normal.y * emaxs.y + p->normal.z * emaxs.z;
|
|
break;
|
|
default:
|
|
// shut up compiler
|
|
dist1 = dist2 = 0;
|
|
break;
|
|
}
|
|
|
|
if( dist1 >= p->dist )
|
|
sides = 1;
|
|
if( dist2 < p->dist )
|
|
sides |= 2;
|
|
|
|
return sides;
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
PlaneFromPoints
|
|
|
|
Returns false if the triangle is degenrate.
|
|
The normal will point out of the clock for clockwise ordered points
|
|
=====================
|
|
*/
|
|
bool PlaneFromPoints( const Vector triangle[3], mplane_t *plane )
|
|
{
|
|
Vector v1 = triangle[1] - triangle[0];
|
|
Vector v2 = triangle[2] - triangle[0];
|
|
plane->normal = CrossProduct( v2, v1 );
|
|
|
|
if( plane->normal.Length() == 0.0f )
|
|
{
|
|
plane->normal = g_vecZero;
|
|
return false;
|
|
}
|
|
|
|
plane->normal = plane->normal.Normalize();
|
|
|
|
plane->dist = DotProduct( triangle[0], plane->normal );
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
CategorizePlane
|
|
|
|
A slightly more complex version of SignbitsForPlane and PlaneTypeForNormal,
|
|
which also tries to fix possible floating point glitches (like -0.00000 cases)
|
|
=================
|
|
*/
|
|
void CategorizePlane( mplane_t *plane )
|
|
{
|
|
plane->signbits = 0;
|
|
plane->type = PLANE_NONAXIAL;
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( plane->normal[i] < 0 )
|
|
{
|
|
plane->signbits |= (1<<i);
|
|
|
|
if( plane->normal[i] == -1.0f )
|
|
{
|
|
plane->signbits = (1<<i);
|
|
plane->normal = g_vecZero;
|
|
plane->normal[i] = -1.0f;
|
|
break;
|
|
}
|
|
}
|
|
else if( plane->normal[i] == 1.0f )
|
|
{
|
|
plane->type = i;
|
|
plane->signbits = 0;
|
|
plane->normal = g_vecZero;
|
|
plane->normal[i] = 1.0f;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
=================
|
|
ComparePlanes
|
|
=================
|
|
*/
|
|
bool ComparePlanes( mplane_t *plane, const Vector &normal, float dist )
|
|
{
|
|
if( fabs( plane->normal.x - normal.x ) < PLANE_NORMAL_EPSILON
|
|
&& fabs( plane->normal.y - normal.y ) < PLANE_NORMAL_EPSILON
|
|
&& fabs( plane->normal.z - normal.z ) < PLANE_NORMAL_EPSILON
|
|
&& fabs( plane->dist - dist ) < PLANE_DIST_EPSILON )
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
==================
|
|
SnapVectorToGrid
|
|
|
|
==================
|
|
*/
|
|
void SnapVectorToGrid( Vector &normal )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( fabs( normal[i] - 1.0f ) < PLANE_NORMAL_EPSILON )
|
|
{
|
|
normal = g_vecZero;
|
|
normal[i] = 1.0f;
|
|
break;
|
|
}
|
|
|
|
if( fabs( normal[i] - -1.0f ) < PLANE_NORMAL_EPSILON )
|
|
{
|
|
normal = g_vecZero;
|
|
normal[i] = -1.0f;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
==============
|
|
SnapPlaneToGrid
|
|
|
|
==============
|
|
*/
|
|
void SnapPlaneToGrid( mplane_t *plane )
|
|
{
|
|
SnapVectorToGrid( plane->normal );
|
|
|
|
if( fabs( plane->dist - Q_rint( plane->dist )) < PLANE_DIST_EPSILON )
|
|
plane->dist = Q_rint( plane->dist );
|
|
}
|
|
|
|
/*
|
|
================
|
|
VectorIsOnAxis
|
|
================
|
|
*/
|
|
bool VectorIsOnAxis( const Vector &v )
|
|
{
|
|
int count = 0;
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( v[i] == 0.0 )
|
|
count++;
|
|
}
|
|
|
|
// the zero vector will be on axis.
|
|
return (count > 1) ? true : false;
|
|
}
|
|
|
|
/*
|
|
==============
|
|
VectorCompareEpsilon
|
|
|
|
==============
|
|
*/
|
|
bool VectorCompareEpsilon( const Vector &vec1, const Vector &vec2, float epsilon )
|
|
{
|
|
float ax, ay, az;
|
|
|
|
ax = fabs( vec1.x - vec2.x );
|
|
ay = fabs( vec1.y - vec2.y );
|
|
az = fabs( vec1.z - vec2.z );
|
|
|
|
if(( ax < epsilon ) && ( ay < epsilon ) && ( az < epsilon ))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
bool RadianCompareEpsilon( const Radian &vec1, const Radian &vec2, float epsilon )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
// clamp to 2pi
|
|
float a1 = fmod( vec1[i], (float)(M_PI * 2));
|
|
float a2 = fmod( vec2[i], (float)(M_PI * 2));
|
|
float delta = fabs( a1 - a2 );
|
|
|
|
// use the smaller angle (359 == 1 degree off)
|
|
if( delta > M_PI )
|
|
{
|
|
delta = 2 * M_PI - delta;
|
|
}
|
|
|
|
if( delta > epsilon )
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
==================
|
|
VertexHashKey
|
|
==================
|
|
*/
|
|
unsigned int VertexHashKey( const vec3_t point, unsigned int hashSize )
|
|
{
|
|
unsigned int hashKey = 0;
|
|
|
|
hashKey ^= int( fabs( point[0] ));
|
|
hashKey ^= int( fabs( point[1] ));
|
|
hashKey ^= int( fabs( point[2] ));
|
|
|
|
hashKey &= (hashSize - 1);
|
|
|
|
return hashKey;
|
|
}
|
|
|
|
// rotate a vector around the Z axis (YAW)
|
|
Vector VectorYawRotate( const Vector &in, float flYaw )
|
|
{
|
|
Vector out;
|
|
float sy, cy;
|
|
|
|
SinCos( DEG2RAD(flYaw), &sy, &cy );
|
|
|
|
out.x = in.x * cy - in.y * sy;
|
|
out.y = in.x * sy + in.y * cy;
|
|
out.z = in.z;
|
|
|
|
return out;
|
|
}
|
|
|
|
// solve a x^2 + b x + c = 0
|
|
bool SolveQuadratic( float a, float b, float c, float &root1, float &root2 )
|
|
{
|
|
if( a == 0 )
|
|
{
|
|
if( b != 0 )
|
|
{
|
|
// no x^2 component, it's a linear system
|
|
root1 = root2 = -c / b;
|
|
return true;
|
|
}
|
|
|
|
if( c == 0 )
|
|
{
|
|
// all zero's
|
|
root1 = root2 = 0;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
float tmp = b * b - 4.0f * a * c;
|
|
|
|
if( tmp < 0 )
|
|
{
|
|
// imaginary number, bah, no solution.
|
|
return false;
|
|
}
|
|
|
|
tmp = sqrt( tmp );
|
|
root1 = (-b + tmp) / (2.0f * a);
|
|
root2 = (-b - tmp) / (2.0f * a);
|
|
|
|
return true;
|
|
}
|
|
|
|
// solves for "a, b, c" where "a x^2 + b x + c = y", return true if solution exists
|
|
bool SolveInverseQuadratic( float x1, float y1, float x2, float y2, float x3, float y3, float &a, float &b, float &c )
|
|
{
|
|
float det = (x1 - x2) * (x1 - x3) * (x2 - x3);
|
|
|
|
// FIXME: check with some sort of epsilon
|
|
if( det == 0.0 ) return false;
|
|
|
|
a = (x3*(-y1 + y2) + x2*(y1 - y3) + x1*(-y2 + y3)) / det;
|
|
|
|
b = (x3*x3*(y1 - y2) + x1*x1*(y2 - y3) + x2*x2*(-y1 + y3)) / det;
|
|
|
|
c = (x1*x3*(-x1 + x3)*y2 + x2*x2*(x3*y1 - x1*y3) + x2*(-(x3*x3*y1) + x1*x1*y3)) / det;
|
|
|
|
return true;
|
|
}
|
|
|
|
float ColorNormalize( const Vector &in, Vector &out )
|
|
{
|
|
float max, scale;
|
|
|
|
max = in.x;
|
|
if( in.y > max )
|
|
max = in.y;
|
|
if( in.z > max )
|
|
max = in.z;
|
|
|
|
if( max == 0.0f )
|
|
return 0.0f;
|
|
|
|
scale = 1.0f / max;
|
|
out = in * scale;
|
|
|
|
return max;
|
|
}
|
|
|
|
void CalcTBN( const Vector &p0, const Vector &p1, const Vector &p2, const Vector2D &t0, const Vector2D &t1, const Vector2D& t2, Vector &s, Vector &t, bool areaweight )
|
|
{
|
|
// Compute the partial derivatives of X, Y, and Z with respect to S and T.
|
|
s.Init( 0.0f, 0.0f, 0.0f );
|
|
t.Init( 0.0f, 0.0f, 0.0f );
|
|
|
|
// x, s, t
|
|
Vector edge01( p1.x - p0.x, t1.x - t0.x, t1.y - t0.y );
|
|
Vector edge02( p2.x - p0.x, t2.x - t0.x, t2.y - t0.y );
|
|
|
|
Vector cross = CrossProduct( edge01, edge02 );
|
|
|
|
if( fabs( cross.x ) > SMALL_FLOAT )
|
|
{
|
|
s.x += -cross.y / cross.x;
|
|
t.x += -cross.z / cross.x;
|
|
}
|
|
|
|
// y, s, t
|
|
edge01.Init( p1.y - p0.y, t1.x - t0.x, t1.y - t0.y );
|
|
edge02.Init( p2.y - p0.y, t2.x - t0.x, t2.y - t0.y );
|
|
|
|
cross = CrossProduct( edge01, edge02 );
|
|
|
|
if( fabs( cross.x ) > SMALL_FLOAT )
|
|
{
|
|
s.y += -cross.y / cross.x;
|
|
t.y += -cross.z / cross.x;
|
|
}
|
|
|
|
// z, s, t
|
|
edge01.Init( p1.z - p0.z, t1.x - t0.x, t1.y - t0.y );
|
|
edge02.Init( p2.z - p0.z, t2.x - t0.x, t2.y - t0.y );
|
|
|
|
cross = CrossProduct( edge01, edge02 );
|
|
|
|
if( fabs( cross.x ) > SMALL_FLOAT )
|
|
{
|
|
s.z += -cross.y / cross.x;
|
|
t.z += -cross.z / cross.x;
|
|
}
|
|
|
|
if( !areaweight )
|
|
{
|
|
// Normalize s and t
|
|
s = s.Normalize();
|
|
t = t.Normalize();
|
|
}
|
|
}
|
|
|
|
/*
|
|
==================
|
|
UTIL_MoveBounds
|
|
==================
|
|
*/
|
|
void UTIL_MoveBounds( const Vector &start, const Vector &mins, const Vector &maxs, const Vector &end, Vector &outmins, Vector &outmaxs )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
if( end[i] > start[i] )
|
|
{
|
|
outmins[i] = start[i] + mins[i] - 1;
|
|
outmaxs[i] = end[i] + maxs[i] + 1;
|
|
}
|
|
else
|
|
{
|
|
outmins[i] = end[i] + mins[i] - 1;
|
|
outmaxs[i] = start[i] + maxs[i] + 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
unsigned short 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( unsigned short 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);
|
|
}
|
|
|
|
signed char FloatToChar( float v )
|
|
{
|
|
float in = v * 127.0f;
|
|
float out = bound( -128.0f, in, 127.0f );
|
|
return (char)out;
|
|
}
|
|
|
|
/*
|
|
====================
|
|
V_CalcFov
|
|
====================
|
|
*/
|
|
float V_CalcFov( float &fov_x, float width, float height )
|
|
{
|
|
float x, half_fov_y;
|
|
|
|
if( fov_x < 1 || fov_x > 170 )
|
|
fov_x = 90;
|
|
|
|
x = width / tan( DEG2RAD( fov_x ) * 0.5f );
|
|
half_fov_y = atan( height / x );
|
|
|
|
return RAD2DEG( half_fov_y ) * 2;
|
|
}
|
|
|
|
/*
|
|
====================
|
|
V_AdjustFov
|
|
====================
|
|
*/
|
|
void V_AdjustFov( float &fov_x, float &fov_y, float width, float height, bool lock_x )
|
|
{
|
|
float x, y;
|
|
|
|
if(( width * 3 ) == ( 4 * height ) || ( width * 4 ) == ( height * 5 ))
|
|
{
|
|
// 4:3 or 5:4 ratio
|
|
return;
|
|
}
|
|
|
|
if( lock_x )
|
|
{
|
|
fov_y = 2 * atan(( width * 3 ) / ( height * 4 ) * tan( fov_y * M_PI / 360.0 * 0.5 )) * 360 / M_PI;
|
|
return;
|
|
}
|
|
|
|
y = V_CalcFov( fov_x, 640, 480 );
|
|
x = fov_x;
|
|
|
|
fov_x = V_CalcFov( y, height, width );
|
|
|
|
if( fov_x < x )
|
|
fov_x = x;
|
|
else fov_y = y;
|
|
}
|
|
|
|
/*
|
|
====================
|
|
ColorToFloat
|
|
|
|
pack RGB into single float
|
|
====================
|
|
*/
|
|
float ColorToFloat( const Vector &color )
|
|
{
|
|
int icolor[3];
|
|
|
|
icolor[0] = color.x * 255;
|
|
icolor[1] = color.y * 255;
|
|
icolor[2] = color.z * 255;
|
|
|
|
int pack = (icolor[0] << 16) | (icolor[1] << 8) | icolor[2];
|
|
return (float)((double)pack / (double)(1 << 24));
|
|
}
|
|
|
|
/*
|
|
====================
|
|
ColorToNormal
|
|
|
|
pack XYZ into single float
|
|
====================
|
|
*/
|
|
float NormalToFloat( const Vector &normal )
|
|
{
|
|
int inormal[3];
|
|
Vector n = normal.Normalize();
|
|
|
|
inormal[0] = n.x * 127 + 128;
|
|
inormal[1] = n.y * 127 + 128;
|
|
inormal[2] = n.z * 127 + 128;
|
|
|
|
int pack = (inormal[0] << 16) | (inormal[1] << 8) | inormal[2];
|
|
return (float)((double)pack / (double)(1 << 24));
|
|
}
|
|
|
|
/*
|
|
====================
|
|
ColorToFloat
|
|
|
|
pack local coords into single float
|
|
====================
|
|
*/
|
|
float OriginToFloat( const Vector &pos, const Vector ¢er )
|
|
{
|
|
Vector local = (pos - center) * 0.5f; // expand the range to -256\+256 units
|
|
int ilocal[3];
|
|
|
|
// clamp to -128\127
|
|
ilocal[0] = bound( -128, local.x, 127 ) + 128;
|
|
ilocal[1] = bound( -128, local.y, 127 ) + 128;
|
|
ilocal[2] = bound( -128, local.z, 127 ) + 128;
|
|
|
|
int pack = (ilocal[0] << 16) | (ilocal[1] << 8) | ilocal[2];
|
|
return (float)((double)pack / (double)(1 << 24));
|
|
}
|
|
|
|
float PackColor( const Vector &color )
|
|
{
|
|
byte r = color.x, g = color.y, b = color.z;
|
|
return (float)((double)((r << 16) | (g << 8) | b) / (double)(1 << 24));
|
|
}
|
|
|
|
float PackNormal( const Vector &normal )
|
|
{
|
|
Vector lb;
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
lb[i] = normal[i] * 127.0f + 128.0f;
|
|
lb[i] = bound( 0, lb[i], 255.0 );
|
|
}
|
|
|
|
byte x = lb.x, y = lb.y, z = lb.z;
|
|
return (float)((double)((x << 16) | (y << 8) | z) / (double)(1 << 24));
|
|
} |