FWGS/Paranoia2
2
0
Fork 2
forked from a1batross/Paranoia2_original
Code Issues Pull Requests 1 Releases Wiki Activity
b5b7697b33
Paranoia2/game_shared/mathlib.cpp

1737 lines
36 KiB
C++
Raw Normal View History

CRLF to LF
2020-08-31 18:50:41 +02:00
//=======================================================================
// 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 )
{
for( int 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 &center )
{
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));
1.51 Aug30 2020 https://hlfx.ru/forum/showthread.php?s=&postid=196728#post196728
2020-08-31 00:15:53 +02:00
}
Reference in New Issue Copy Permalink