R5Reloaded/r5sdk
1
0
Fork 0
mirror of https://github.com/Mauler125/r5sdk.git synced 2025-02-09 19:15:03 +01:00
Code Issues Packages Projects Releases Wiki Activity
r5sdk/r5dev/mathlib/mathlib_base.cpp
Kawe Mazidjatari f120354e96 Initial port to CMake 
* All libraries have been isolated from each other, and build into separate artifacts.
* Project has been restructured to support isolating libraries.
* CCrashHandler now calls a callback on crash (setup from core/dllmain.cpp, this can be setup in any way for any project. This callback is getting called when the apllication crashes. Useful for flushing buffers before closing handles to logging files for example).
* Tier0 'CoreMsgV' function now calls a callback sink, which could be set by the user (currently setup to the SDK's internal logger in core/dllmain.cpp).

TODO:
* Add a batch file to autogenerate all projects.
* Add support for dedicated server.
* Add support for client dll.

Bugs:
* Game crashes on the title screen after the UI script compiler has finished (root cause unknown).
* Curl error messages are getting logged twice for the dedicated server due to the removal of all "DEDICATED" preprocessor directives to support isolating projects. This has to be fixed properly!
2023-05-10 00:05:38 +02:00

5938 lines
158 KiB
C++
Raw Blame History

//===== Copyright <20> 1996-2005, Valve Corporation, All rights reserved. ======//
//
// Purpose: Math primitives.
//
//===========================================================================//
/// FIXME: As soon as all references to mathlib.c are gone, include it in here
#include "tier0/basetypes.h"
//#include <memory.h>
#include "tier0/dbg.h"
#include "tier0/cpu.h"
//#include "tier0/vprof.h"
//#define _VPROF_MATHLIB
#if !defined(__SPU__)
#pragma warning(disable:4244) // "conversion from 'const int' to 'float', possible loss of data"
#pragma warning(disable:4730) // "mixing _m64 and floating point expressions may result in incorrect code"
#endif
#include "mathlib/mathlib.h"
#include "mathlib/vector.h"
#include "mathlib/vplane.h"
#if !defined(__SPU__)
#include "mathlib/vmatrix.h"
#endif
#if !defined( _X360 )
//#include "sse.h"
#endif
#include "mathlib/ssemath.h"
#include "mathlib/ssequaternion.h"
// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"
bool s_bMathlibInitialized = false;
#ifdef PARANOID
// User must provide an implementation of Sys_Error()
void Sys_Error(char* error, ...);
#endif
const Vector3D vec3_origin(0, 0, 0);
const QAngle vec3_angle(0, 0, 0);
const Quaternion quat_identity(0, 0, 0, 1);
const Vector3D vec3_invalid(FLT_MAX, FLT_MAX, FLT_MAX);
const int nanmask = 255 << 23;
const matrix3x4a_t g_MatrixIdentity(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0
);
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Standard C implementations of optimized routines:
//-----------------------------------------------------------------------------
float _sqrtf(float _X)
{
Assert(s_bMathlibInitialized);
return sqrtf(_X);
}
float _rsqrtf(float x)
{
Assert(s_bMathlibInitialized);
return 1.f / _sqrtf(x);
}
#ifndef PLATFORM_PPC
float VectorNormalize(Vector3D& vec)
{
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("_VectorNormalize", "Mathlib");
#endif
Assert(s_bMathlibInitialized);
float radius = sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
// FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero.
float iradius = 1.f / (radius + FLT_EPSILON);
vec.x *= iradius;
vec.y *= iradius;
vec.z *= iradius;
return radius;
}
#endif
// TODO: Add fast C VectorNormalizeFast.
// Perhaps use approximate rsqrt trick, if the accuracy isn't too bad.
void FASTCALL _VectorNormalizeFast(Vector3D& vec)
{
Assert(s_bMathlibInitialized);
// FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero.
float iradius = 1.f / (sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z) + FLT_EPSILON);
vec.x *= iradius;
vec.y *= iradius;
vec.z *= iradius;
}
float _InvRSquared(const float* v)
{
Assert(s_bMathlibInitialized);
float r2 = DotProduct(v, v);
return r2 < 1.f ? 1.f : 1 / r2;
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Function pointers selecting the appropriate implementation
//-----------------------------------------------------------------------------
void (FASTCALL* pfVectorNormalizeFast)(Vector3D& v) = _VectorNormalizeFast;
float SinCosTable[SIN_TABLE_SIZE];
void InitSinCosTable()
{
for (int i = 0; i < SIN_TABLE_SIZE; i++)
{
SinCosTable[i] = sin(i * 2.0 * M_PI / SIN_TABLE_SIZE);
}
}
#endif // !defined(__SPU__)
qboolean VectorsEqual(const float* v1, const float* v2)
{
Assert(s_bMathlibInitialized);
return ((v1[0] == v2[0]) &&
(v1[1] == v2[1]) &&
(v1[2] == v2[2]));
}
#endif // #if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: Generates Euler angles given a left-handed orientation matrix. The
// columns of the matrix contain the forward, left, and up vectors.
// Input : matrix - Left-handed orientation matrix.
// angles[PITCH, YAW, ROLL]. Receives right-handed counterclockwise
// rotations in degrees around Y, Z, and X respectively.
//-----------------------------------------------------------------------------
void MatrixAngles(const matrix3x4_t& matrix, RadianEuler& angles, Vector3D& position)
{
MatrixGetColumn(matrix, 3, position);
MatrixAngles(matrix, angles);
}
void MatrixAngles(const matrix3x4_t& matrix, Quaternion& q, Vector3D& pos)
{
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("MatrixQuaternion", "Mathlib");
#endif
float trace;
trace = matrix[0][0] + matrix[1][1] + matrix[2][2] + 1.0f;
if (trace > 1.0f + FLT_EPSILON)
{
// VPROF_INCREMENT_COUNTER("MatrixQuaternion A",1);
q.x = (matrix[2][1] - matrix[1][2]);
q.y = (matrix[0][2] - matrix[2][0]);
q.z = (matrix[1][0] - matrix[0][1]);
q.w = trace;
}
else if (matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2])
{
// VPROF_INCREMENT_COUNTER("MatrixQuaternion B",1);
trace = 1.0f + matrix[0][0] - matrix[1][1] - matrix[2][2];
q.x = trace;
q.y = (matrix[1][0] + matrix[0][1]);
q.z = (matrix[0][2] + matrix[2][0]);
q.w = (matrix[2][1] - matrix[1][2]);
}
else if (matrix[1][1] > matrix[2][2])
{
// VPROF_INCREMENT_COUNTER("MatrixQuaternion C",1);
trace = 1.0f + matrix[1][1] - matrix[0][0] - matrix[2][2];
q.x = (matrix[0][1] + matrix[1][0]);
q.y = trace;
q.z = (matrix[2][1] + matrix[1][2]);
q.w = (matrix[0][2] - matrix[2][0]);
}
else
{
// VPROF_INCREMENT_COUNTER("MatrixQuaternion D",1);
trace = 1.0f + matrix[2][2] - matrix[0][0] - matrix[1][1];
q.x = (matrix[0][2] + matrix[2][0]);
q.y = (matrix[2][1] + matrix[1][2]);
q.z = trace;
q.w = (matrix[1][0] - matrix[0][1]);
}
QuaternionNormalize(q);
#if 0
// check against the angle version
RadianEuler ang;
MatrixAngles(matrix, ang);
Quaternion test;
AngleQuaternion(ang, test);
float d = QuaternionDotProduct(q, test);
Assert(fabs(d) > 0.99 && fabs(d) < 1.01);
#endif
MatrixGetColumn(matrix, 3, pos);
}
void MatrixAngles(const matrix3x4_t& matrix, float* angles)
{
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("MatrixAngles", "Mathlib");
#endif
Assert(s_bMathlibInitialized);
float forward[3];
float left[3];
float up[3];
//
// Extract the basis vectors from the matrix. Since we only need the Z
// component of the up vector, we don't get X and Y.
//
forward[0] = matrix[0][0];
forward[1] = matrix[1][0];
forward[2] = matrix[2][0];
left[0] = matrix[0][1];
left[1] = matrix[1][1];
left[2] = matrix[2][1];
up[2] = matrix[2][2];
float xyDist = sqrtf(forward[0] * forward[0] + forward[1] * forward[1]);
// enough here to get angles?
if (xyDist > 0.001f)
{
// (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis
angles[1] = RAD2DEG(atan2f(forward[1], forward[0]));
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG(atan2f(-forward[2], xyDist));
// (roll) z = ATAN( left.z, up.z );
angles[2] = RAD2DEG(atan2f(left[2], up[2]));
}
else // forward is mostly Z, gimbal lock-
{
// (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw
angles[1] = RAD2DEG(atan2f(-left[0], left[1]));
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG(atan2f(-forward[2], xyDist));
// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll)
angles[2] = 0;
}
}
Vector3D MatrixNormalize(const matrix3x4_t& in, matrix3x4_t& out)
{
Vector3D vScale;
vScale.x = sqrt(in[0][0] * in[0][0] + in[1][0] * in[1][0] + in[2][0] * in[2][0]);
vScale.y = sqrt(in[0][1] * in[0][1] + in[1][1] * in[1][1] + in[2][1] * in[2][1]);
vScale.z = sqrt(in[0][2] * in[0][2] + in[1][2] * in[1][2] + in[2][2] * in[2][2]);
matrix3x4_t norm;
float flInvScaleX = 1.0f / vScale.x;
float flInvScaleY = 1.0f / vScale.y;
float flInvScaleZ = 1.0f / vScale.z;
out[0][0] = in[0][0] * flInvScaleX; out[1][0] = in[1][0] * flInvScaleX; out[2][0] = in[2][0] * flInvScaleX;
out[0][1] = in[0][1] * flInvScaleY; out[1][1] = in[1][1] * flInvScaleY; out[2][1] = in[2][1] * flInvScaleY;
out[0][2] = in[0][2] * flInvScaleZ; out[1][2] = in[1][2] * flInvScaleZ; out[2][2] = in[2][2] * flInvScaleZ;
out[0][3] = in[0][3]; out[1][3] = in[1][3]; out[2][3] = in[2][3];
return vScale;
}
#if !defined(__SPU__)
// transform in1 by the matrix in2
void VectorTransform(const float* RESTRICT in1, const matrix3x4_t& in2, float* RESTRICT out)
{
Assert(s_bMathlibInitialized);
float x = DotProduct(in1, in2[0]) + in2[0][3];
float y = DotProduct(in1, in2[1]) + in2[1][3];
float z = DotProduct(in1, in2[2]) + in2[2][3];
out[0] = x;
out[1] = y;
out[2] = z;
}
// assuming the matrix is orthonormal, transform in1 by the transpose (also the inverse in this case) of in2.
void VectorITransform(const float* in1, const matrix3x4_t& in2, float* out)
{
Assert(s_bMathlibInitialized);
float in1t[3];
in1t[0] = in1[0] - in2[0][3];
in1t[1] = in1[1] - in2[1][3];
in1t[2] = in1[2] - in2[2][3];
float x = in1t[0] * in2[0][0] + in1t[1] * in2[1][0] + in1t[2] * in2[2][0];
float y = in1t[0] * in2[0][1] + in1t[1] * in2[1][1] + in1t[2] * in2[2][1];
float z = in1t[0] * in2[0][2] + in1t[1] * in2[1][2] + in1t[2] * in2[2][2];
out[0] = x;
out[1] = y;
out[2] = z;
}
#endif // #if !defined(__SPU__)
// assume in2 is a rotation and rotate the input vector
void VectorRotate(const float* RESTRICT in1, const matrix3x4_t& in2, float* RESTRICT out)
{
Assert(s_bMathlibInitialized);
float x = DotProduct(in1, in2[0]);
float y = DotProduct(in1, in2[1]);
float z = DotProduct(in1, in2[2]);
out[0] = x;
out[1] = y;
out[2] = z;
}
#if !defined(__SPU__)
// assume in2 is a rotation and rotate the input vector
void VectorRotate(const Vector3D& in1, const QAngle& in2, Vector3D& out)
{
matrix3x4_t matRotate;
AngleMatrix(in2, matRotate);
VectorRotate(in1, matRotate, out);
}
// assume in2 is a rotation and rotate the input vector
void VectorRotate(const Vector3D& in1, const Quaternion& in2, Vector3D& out)
{
#if WE_WANT_OUR_CODE_TO_BE_POINTLESSLY_SLOW
matrix3x4_t matRotate;
QuaternionMatrix(in2, matRotate);
VectorRotate(in1, matRotate, out);
#else
// rotation is q * v * q^-1
Quaternion conjugate = in2.Conjugate();
// do the rotation as unrolled flop code ( QuaternionMult is a function call, which murders instruction scheduling )
// first q*v
Quaternion temp;
temp.x = in2.y * in1.z - in2.z * in1.y + in2.w * in1.x;
temp.y = -in2.x * in1.z + in2.z * in1.x + in2.w * in1.y;
temp.z = in2.x * in1.y - in2.y * in1.x + in2.w * in1.z;
temp.w = -in2.x * in1.x - in2.y * in1.y - in2.z * in1.z;
// now (qv)(q*)
out.x = temp.x * conjugate.w + temp.y * conjugate.z - temp.z * conjugate.y + temp.w * conjugate.x;
out.y = -temp.x * conjugate.z + temp.y * conjugate.w + temp.z * conjugate.x + temp.w * conjugate.y;
out.z = temp.x * conjugate.y - temp.y * conjugate.x + temp.z * conjugate.w + temp.w * conjugate.z;
Assert(fabs(-temp.x * conjugate.x - temp.y * conjugate.y - temp.z * conjugate.z + temp.w * conjugate.w) < 0.0001);
#endif
}
// rotate by the inverse of the matrix
void VectorIRotate(const float* RESTRICT in1, const matrix3x4_t& in2, float* RESTRICT out)
{
Assert(s_bMathlibInitialized);
Assert(in1 != 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];
}
#ifndef VECTOR_NO_SLOW_OPERATIONS
// transform a set of angles in the output space of parentMatrix to the input space
QAngle TransformAnglesToLocalSpace(const QAngle& angles, const matrix3x4_t& parentMatrix)
{
matrix3x4_t angToWorld, worldToParent, localMatrix;
MatrixInvert(parentMatrix, worldToParent);
AngleMatrix(angles, angToWorld);
ConcatTransforms(worldToParent, angToWorld, localMatrix);
QAngle out;
MatrixAngles(localMatrix, out);
return out;
}
// transform a set of angles in the input space of parentMatrix to the output space
QAngle TransformAnglesToWorldSpace(const QAngle& angles, const matrix3x4_t& parentMatrix)
{
matrix3x4_t angToParent, angToWorld;
AngleMatrix(angles, angToParent);
ConcatTransforms(parentMatrix, angToParent, angToWorld);
QAngle out;
MatrixAngles(angToWorld, out);
return out;
}
#endif // VECTOR_NO_SLOW_OPERATIONS
void MatrixInitialize(matrix3x4_t& mat, const Vector3D& vecOrigin, const Vector3D& vecXAxis, const Vector3D& vecYAxis, const Vector3D& vecZAxis)
{
MatrixSetColumn(vecXAxis, 0, mat);
MatrixSetColumn(vecYAxis, 1, mat);
MatrixSetColumn(vecZAxis, 2, mat);
MatrixSetColumn(vecOrigin, 3, mat);
}
void MatrixCopy(const matrix3x4_t& in, matrix3x4_t& out)
{
Assert(s_bMathlibInitialized);
memcpy(out.Base(), in.Base(), sizeof(float) * 3 * 4);
}
//-----------------------------------------------------------------------------
// Matrix equality test
//-----------------------------------------------------------------------------
bool MatricesAreEqual(const matrix3x4_t& src1, const matrix3x4_t& src2, float flTolerance)
{
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 4; ++j)
{
if (fabs(src1[i][j] - src2[i][j]) > flTolerance)
return false;
}
}
return true;
}
#endif // #if !defined(__SPU__)
// NOTE: This is just the transpose not a general inverse
void MatrixInvert(const matrix3x4_t& in, matrix3x4_t& out)
{
Assert(s_bMathlibInitialized);
if (&in == &out)
{
V_swap(out[0][1], out[1][0]);
V_swap(out[0][2], out[2][0]);
V_swap(out[1][2], out[2][1]);
}
else
{
// transpose the matrix
out[0][0] = in[0][0];
out[0][1] = in[1][0];
out[0][2] = in[2][0];
out[1][0] = in[0][1];
out[1][1] = in[1][1];
out[1][2] = in[2][1];
out[2][0] = in[0][2];
out[2][1] = in[1][2];
out[2][2] = in[2][2];
}
// now fix up the translation to be in the other space
float tmp[3];
tmp[0] = in[0][3];
tmp[1] = in[1][3];
tmp[2] = in[2][3];
out[0][3] = -DotProduct(tmp, out[0]);
out[1][3] = -DotProduct(tmp, out[1]);
out[2][3] = -DotProduct(tmp, out[2]);
}
void MatrixGetColumn(const matrix3x4_t& in, int column, Vector3D& out)
{
out.x = in[0][column];
out.y = in[1][column];
out.z = in[2][column];
}
void MatrixSetColumn(const Vector3D& in, int column, matrix3x4_t& out)
{
out[0][column] = in.x;
out[1][column] = in.y;
out[2][column] = in.z;
}
#if !defined(__SPU__)
int VectorCompare(const float* v1, const float* v2)
{
Assert(s_bMathlibInitialized);
int i;
for (i = 0; i < 3; i++)
if (v1[i] != v2[i])
return 0;
return 1;
}
void CrossProduct(const float* v1, const float* v2, float* cross)
{
Assert(s_bMathlibInitialized);
Assert(v1 != cross);
Assert(v2 != 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];
}
size_t Q_log2(unsigned int val)
{
#ifdef _X360 // use hardware
// both zero and one return zero (per old implementation)
return (val == 0) ? 0 : 31 - _CountLeadingZeros(val);
#else // use N. Compoop's algorithm ( inherited from days of yore )
int answer = 0;
while (val >>= 1)
answer++;
return answer;
#endif
}
// Matrix is right-handed x=forward, y=left, z=up. We a left-handed convention for vectors in the game code (forward, right, up)
void MatrixVectorsFLU(const matrix3x4_t& matrix, Vector3D* pForward, Vector3D* pLeft, Vector3D* pUp)
{
MatrixGetColumn(matrix, FORWARD_AXIS, *pForward);
MatrixGetColumn(matrix, LEFT_AXIS, *pLeft);
MatrixGetColumn(matrix, UP_AXIS, *pUp);
}
// Matrix is right-handed x=forward, y=left, z=up. We a left-handed convention for vectors in the game code (forward, right, up)
void MatrixVectors(const matrix3x4_t& matrix, Vector3D* pForward, Vector3D* pRight, Vector3D* pUp)
{
MatrixGetColumn(matrix, 0, *pForward);
MatrixGetColumn(matrix, 1, *pRight);
MatrixGetColumn(matrix, 2, *pUp);
*pRight *= -1.0f;
}
void VectorVectors(const Vector3D& forward, Vector3D& right, Vector3D& up)
{
Assert(s_bMathlibInitialized);
Vector3D tmp;
if (fabs(forward[0]) < 1e-6 && fabs(forward[1]) < 1e-6)
{
// pitch 90 degrees up/down from identity
right[0] = 0;
right[1] = -1;
right[2] = 0;
up[0] = -forward[2];
up[1] = 0;
up[2] = 0;
}
else
{
tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0;
CrossProduct(forward, tmp, right);
VectorNormalize(right);
CrossProduct(right, forward, up);
VectorNormalize(up);
}
}
void VectorMatrix(const Vector3D& forward, matrix3x4_t& matrix)
{
Assert(s_bMathlibInitialized);
Vector3D right, up;
VectorVectors(forward, right, up);
MatrixSetColumn(forward, 0, matrix);
MatrixSetColumn(-right, 1, matrix);
MatrixSetColumn(up, 2, matrix);
}
void VectorPerpendicularToVector(Vector3D const& in, Vector3D* pvecOut)
{
float flY = in.y * in.y;
pvecOut->x = RemapVal(flY, 0, 1, in.z, 1);
pvecOut->y = 0;
pvecOut->z = -in.x;
pvecOut->NormalizeInPlace();
float flDot = DotProduct(*pvecOut, in);
*pvecOut -= flDot * in;
pvecOut->NormalizeInPlace();
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Basis Vectors. Each vector is optional
//-----------------------------------------------------------------------------
void AngleVectorsFLU(const QAngle& angles, Vector3D* pForward, Vector3D* pLeft, Vector3D* pUp)
{
Assert(s_bMathlibInitialized);
float sr, sp, sy, cr, cp, cy;
#ifdef _X360
fltx4 radians, scale, sine, cosine;
radians = LoadUnaligned3SIMD(angles.Base());
scale = ReplicateX4(M_PI_F / 180.f);
radians = MulSIMD(radians, scale);
SinCos3SIMD(sine, cosine, radians);
sp = SubFloat(sine, 0); sy = SubFloat(sine, 1); sr = SubFloat(sine, 2);
cp = SubFloat(cosine, 0); cy = SubFloat(cosine, 1); cr = SubFloat(cosine, 2);
#else
SinCos(DEG2RAD(angles[YAW]), &sy, &cy);
SinCos(DEG2RAD(angles[PITCH]), &sp, &cp);
SinCos(DEG2RAD(angles[ROLL]), &sr, &cr);
#endif
if (pForward)
{
(*pForward)[FORWARD_AXIS] = cp * cy;
(*pForward)[LEFT_AXIS] = cp * sy;
(*pForward)[UP_AXIS] = -sp;
}
if (pLeft)
{
(*pLeft)[FORWARD_AXIS] = (sr * sp * cy + cr * -sy);
(*pLeft)[LEFT_AXIS] = (sr * sp * sy + cr * cy);
(*pLeft)[UP_AXIS] = sr * cp;
}
if (pUp)
{
(*pUp)[FORWARD_AXIS] = (cr * sp * cy + -sr * -sy);
(*pUp)[LEFT_AXIS] = (cr * sp * sy + -sr * cy);
(*pUp)[UP_AXIS] = cr * cp;
}
}
void VectorAngles(const float* forward, float* angles)
{
Assert(s_bMathlibInitialized);
float tmp, yaw, pitch;
if (forward[1] == 0 && forward[0] == 0)
{
yaw = 0;
if (forward[2] > 0)
pitch = 270;
else
pitch = 90;
}
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;
}
angles[0] = pitch;
angles[1] = yaw;
angles[2] = 0;
}
/*
================
R_ConcatRotations
================
*/
void ConcatRotations(const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out)
{
Assert(s_bMathlibInitialized);
Assert(in1 != out);
Assert(in2 != out);
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[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[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];
}
#endif // #if !defined(__SPU__)
void ConcatTransforms_Aligned(const matrix3x4a_t& m0, const matrix3x4a_t& m1, matrix3x4a_t& out)
{
//AssertAligned(&m0);
//AssertAligned(&m1);
//AssertAligned(&out);
fltx4 lastMask = *(fltx4*)(&g_SIMD_ComponentMask[3]);
fltx4 rowA0 = LoadAlignedSIMD(m0.m_flMatVal[0]);
fltx4 rowA1 = LoadAlignedSIMD(m0.m_flMatVal[1]);
fltx4 rowA2 = LoadAlignedSIMD(m0.m_flMatVal[2]);
fltx4 rowB0 = LoadAlignedSIMD(m1.m_flMatVal[0]);
fltx4 rowB1 = LoadAlignedSIMD(m1.m_flMatVal[1]);
fltx4 rowB2 = LoadAlignedSIMD(m1.m_flMatVal[2]);
// now we have the rows of m0 and the columns of m1
// first output row
fltx4 A0 = SplatXSIMD(rowA0);
fltx4 A1 = SplatYSIMD(rowA0);
fltx4 A2 = SplatZSIMD(rowA0);
fltx4 mul00 = MulSIMD(A0, rowB0);
fltx4 mul01 = MulSIMD(A1, rowB1);
fltx4 mul02 = MulSIMD(A2, rowB2);
fltx4 out0 = AddSIMD(mul00, AddSIMD(mul01, mul02));
// second output row
A0 = SplatXSIMD(rowA1);
A1 = SplatYSIMD(rowA1);
A2 = SplatZSIMD(rowA1);
fltx4 mul10 = MulSIMD(A0, rowB0);
fltx4 mul11 = MulSIMD(A1, rowB1);
fltx4 mul12 = MulSIMD(A2, rowB2);
fltx4 out1 = AddSIMD(mul10, AddSIMD(mul11, mul12));
// third output row
A0 = SplatXSIMD(rowA2);
A1 = SplatYSIMD(rowA2);
A2 = SplatZSIMD(rowA2);
fltx4 mul20 = MulSIMD(A0, rowB0);
fltx4 mul21 = MulSIMD(A1, rowB1);
fltx4 mul22 = MulSIMD(A2, rowB2);
fltx4 out2 = AddSIMD(mul20, AddSIMD(mul21, mul22));
// add in translation vector
A0 = AndSIMD(rowA0, lastMask);
A1 = AndSIMD(rowA1, lastMask);
A2 = AndSIMD(rowA2, lastMask);
out0 = AddSIMD(out0, A0);
out1 = AddSIMD(out1, A1);
out2 = AddSIMD(out2, A2);
StoreAlignedSIMD(out.m_flMatVal[0], out0);
StoreAlignedSIMD(out.m_flMatVal[1], out1);
StoreAlignedSIMD(out.m_flMatVal[2], out2);
}
/*
================
R_ConcatTransforms
================
*/
void ConcatTransforms(const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out)
{
#if 0
// test for ones that'll be 2x faster
if ((((size_t)&in1) % 16) == 0 && (((size_t)&in2) % 16) == 0 && (((size_t)&out) % 16) == 0)
{
ConcatTransforms_Aligned(in1, in2, out);
return;
}
#endif
fltx4 lastMask = *(fltx4*)(&g_SIMD_ComponentMask[3]);
fltx4 rowA0 = LoadUnalignedSIMD(in1.m_flMatVal[0]);
fltx4 rowA1 = LoadUnalignedSIMD(in1.m_flMatVal[1]);
fltx4 rowA2 = LoadUnalignedSIMD(in1.m_flMatVal[2]);
fltx4 rowB0 = LoadUnalignedSIMD(in2.m_flMatVal[0]);
fltx4 rowB1 = LoadUnalignedSIMD(in2.m_flMatVal[1]);
fltx4 rowB2 = LoadUnalignedSIMD(in2.m_flMatVal[2]);
// now we have the rows of m0 and the columns of m1
// first output row
fltx4 A0 = SplatXSIMD(rowA0);
fltx4 A1 = SplatYSIMD(rowA0);
fltx4 A2 = SplatZSIMD(rowA0);
fltx4 mul00 = MulSIMD(A0, rowB0);
fltx4 mul01 = MulSIMD(A1, rowB1);
fltx4 mul02 = MulSIMD(A2, rowB2);
fltx4 out0 = AddSIMD(mul00, AddSIMD(mul01, mul02));
// second output row
A0 = SplatXSIMD(rowA1);
A1 = SplatYSIMD(rowA1);
A2 = SplatZSIMD(rowA1);
fltx4 mul10 = MulSIMD(A0, rowB0);
fltx4 mul11 = MulSIMD(A1, rowB1);
fltx4 mul12 = MulSIMD(A2, rowB2);
fltx4 out1 = AddSIMD(mul10, AddSIMD(mul11, mul12));
// third output row
A0 = SplatXSIMD(rowA2);
A1 = SplatYSIMD(rowA2);
A2 = SplatZSIMD(rowA2);
fltx4 mul20 = MulSIMD(A0, rowB0);
fltx4 mul21 = MulSIMD(A1, rowB1);
fltx4 mul22 = MulSIMD(A2, rowB2);
fltx4 out2 = AddSIMD(mul20, AddSIMD(mul21, mul22));
// add in translation vector
A0 = AndSIMD(rowA0, lastMask);
A1 = AndSIMD(rowA1, lastMask);
A2 = AndSIMD(rowA2, lastMask);
out0 = AddSIMD(out0, A0);
out1 = AddSIMD(out1, A1);
out2 = AddSIMD(out2, A2);
// write to output
StoreUnalignedSIMD(out.m_flMatVal[0], out0);
StoreUnalignedSIMD(out.m_flMatVal[1], out1);
StoreUnalignedSIMD(out.m_flMatVal[2], out2);
}
/*
===================
FloorDivMod
Returns mathematically correct (floor-based) quotient and remainder for
numer and denom, both of which should contain no fractional part. The
quotient must fit in 32 bits.
====================
*/
#if !defined(__SPU__)
void FloorDivMod(double numer, double denom, int* quotient,
int* rem)
{
Assert(s_bMathlibInitialized);
int q, r;
double x;
#ifdef PARANOID
if (denom <= 0.0)
Sys_Error("FloorDivMod: bad denominator %d\n", denom);
// if ((floor(numer) != numer) || (floor(denom) != denom))
// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
// numer, denom);
#endif
if (numer >= 0.0)
{
x = floor(numer / denom);
q = (int)x;
r = Floor2Int(numer - (x * denom));
}
else
{
//
// perform operations with positive values, and fix mod to make floor-based
//
x = floor(-numer / denom);
q = -(int)x;
r = Floor2Int(-numer - (x * denom));
if (r != 0)
{
q--;
r = (int)denom - r;
}
}
*quotient = q;
*rem = r;
}
/*
===================
GreatestCommonDivisor
====================
*/
int GreatestCommonDivisor(int i1, int i2)
{
Assert(s_bMathlibInitialized);
if (i1 > i2)
{
if (i2 == 0)
return (i1);
return GreatestCommonDivisor(i2, i1 % i2);
}
else
{
if (i1 == 0)
return (i2);
return GreatestCommonDivisor(i1, i2 % i1);
}
}
bool IsDenormal(const float& val)
{
const int x = *reinterpret_cast <const int*> (&val); // needs 32-bit int
const int abs_mantissa = x & 0x007FFFFF;
const int biased_exponent = x & 0x7F800000;
return (biased_exponent == 0 && abs_mantissa != 0);
}
int SignbitsForPlane(cplane_t* out)
{
Assert(s_bMathlibInitialized);
int bits, j;
// for fast box on planeside test
bits = 0;
for (j = 0; j < 3; j++)
{
if (out->normal[j] < 0)
bits |= 1 << j;
}
return bits;
}
/*
==================
BoxOnPlaneSide
Returns 1, 2, or 1 + 2
==================
*/
int __cdecl BoxOnPlaneSide(const float* emins, const float* emaxs, const cplane_t* p)
{
Assert(s_bMathlibInitialized);
float dist1, dist2;
int sides;
// fast axial cases
if (p->type < 3)
{
if (p->dist <= emins[p->type])
return 1;
if (p->dist >= emaxs[p->type])
return 2;
return 3;
}
// general case
switch (p->signbits)
{
case 0:
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
break;
case 1:
dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
break;
case 2:
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
break;
case 3:
dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
break;
case 4:
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
break;
case 5:
dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emins[2];
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emaxs[2];
break;
case 6:
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
break;
case 7:
dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] + p->normal[2] * emins[2];
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] + p->normal[2] * emaxs[2];
break;
default:
dist1 = dist2 = 0; // shut up compiler
Assert(0);
break;
}
sides = 0;
if (dist1 >= p->dist)
sides = 1;
if (dist2 < p->dist)
sides |= 2;
Assert(sides != 0);
return sides;
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Euler QAngle Composed
//-----------------------------------------------------------------------------
void AngleCompose(const QAngle& a1, const QAngle& a2, QAngle& out)
{
Quaternion q1, q2, q3;
AngleQuaternion(a1, q1);
AngleQuaternion(a2, q2);
QuaternionMult(q1, q2, q3);
QuaternionAngles(q3, out);
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Euler QAngle Lerped
//-----------------------------------------------------------------------------
void AngleLerp(const QAngle& a1, const QAngle& a2, float t, QAngle& out)
{
Quaternion q1, q2, q3;
AngleQuaternion(a1, q1);
AngleQuaternion(a2, q2);
QuaternionSlerp(q1, q2, t, q3);
QuaternionAngles(q3, out);
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Euler QAngle Inverted
//-----------------------------------------------------------------------------
void AngleInverse(const QAngle& angles, QAngle& out)
{
Quaternion q1, q2;
AngleQuaternion(angles, q1);
QuaternionInvert(q1, q2);
QuaternionAngles(q2, out);
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Basis Vectors
//-----------------------------------------------------------------------------
void AngleVectors(const QAngle& angles, Vector3D* forward)
{
Assert(s_bMathlibInitialized);
Assert(forward);
float sp, sy, cp, cy;
SinCos(DEG2RAD(angles[YAW]), &sy, &cy);
SinCos(DEG2RAD(angles[PITCH]), &sp, &cp);
forward->x = cp * cy;
forward->y = cp * sy;
forward->z = -sp;
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Basis Vectors. Each vector is optional
//-----------------------------------------------------------------------------
void AngleVectors(const QAngle& angles, Vector3D* forward, Vector3D* right, Vector3D* up)
{
Assert(s_bMathlibInitialized);
float sr, sp, sy, cr, cp, cy;
#ifdef _X360
fltx4 radians, scale, sine, cosine;
radians = LoadUnaligned3SIMD(angles.Base());
scale = ReplicateX4(M_PI_F / 180.f);
radians = MulSIMD(radians, scale);
SinCos3SIMD(sine, cosine, radians);
sp = SubFloat(sine, 0); sy = SubFloat(sine, 1); sr = SubFloat(sine, 2);
cp = SubFloat(cosine, 0); cy = SubFloat(cosine, 1); cr = SubFloat(cosine, 2);
#else
SinCos(DEG2RAD(angles[YAW]), &sy, &cy);
SinCos(DEG2RAD(angles[PITCH]), &sp, &cp);
SinCos(DEG2RAD(angles[ROLL]), &sr, &cr);
#endif
if (forward)
{
forward->x = cp * cy;
forward->y = cp * sy;
forward->z = -sp;
}
if (right)
{
right->x = (-1 * sr * sp * cy + -1 * cr * -sy);
right->y = (-1 * sr * sp * sy + -1 * cr * cy);
right->z = -1 * sr * cp;
}
if (up)
{
up->x = (cr * sp * cy + -sr * -sy);
up->y = (cr * sp * sy + -sr * cy);
up->z = cr * cp;
}
}
//-----------------------------------------------------------------------------
// Euler QAngle -> Basis Vectors transposed
//-----------------------------------------------------------------------------
void AngleVectorsTranspose(const QAngle& angles, Vector3D* forward, Vector3D* right, Vector3D* up)
{
Assert(s_bMathlibInitialized);
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->x = cp * cy;
forward->y = (sr * sp * cy + cr * -sy);
forward->z = (cr * sp * cy + -sr * -sy);
}
if (right)
{
right->x = cp * sy;
right->y = (sr * sp * sy + cr * cy);
right->z = (cr * sp * sy + -sr * cy);
}
if (up)
{
up->x = -sp;
up->y = sr * cp;
up->z = cr * cp;
}
}
//-----------------------------------------------------------------------------
// Forward direction vector -> Euler angles
//-----------------------------------------------------------------------------
void VectorAngles(const Vector3D& forward, QAngle& angles)
{
Assert(s_bMathlibInitialized);
float tmp, yaw, pitch;
if (forward[1] == 0 && forward[0] == 0)
{
yaw = 0;
if (forward[2] > 0)
pitch = 270;
else
pitch = 90;
}
else
{
yaw = (atan2(forward[1], forward[0]) * 180 / M_PI);
if (yaw < 0)
yaw += 360;
tmp = FastSqrt(forward[0] * forward[0] + forward[1] * forward[1]);
pitch = (atan2(-forward[2], tmp) * 180 / M_PI);
if (pitch < 0)
pitch += 360;
}
angles[0] = pitch;
angles[1] = yaw;
angles[2] = 0;
}
//-----------------------------------------------------------------------------
// Forward direction vector with a reference up vector -> Euler angles
//-----------------------------------------------------------------------------
void VectorAngles(const Vector3D& forward, const Vector3D& pseudoup, QAngle& angles)
{
Assert(s_bMathlibInitialized);
Vector3D left;
CrossProduct(pseudoup, forward, left);
VectorNormalizeFast(left);
float xyDist = sqrtf(forward[0] * forward[0] + forward[1] * forward[1]);
// enough here to get angles?
if (xyDist > 0.001f)
{
// (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis
angles[1] = RAD2DEG(atan2f(forward[1], forward[0]));
// The engine does pitch inverted from this, but we always end up negating it in the DLL
// UNDONE: Fix the engine to make it consistent
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG(atan2f(-forward[2], xyDist));
float up_z = (left[1] * forward[0]) - (left[0] * forward[1]);
// (roll) z = ATAN( left.z, up.z );
angles[2] = RAD2DEG(atan2f(left[2], up_z));
}
else // forward is mostly Z, gimbal lock-
{
// (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw
angles[1] = RAD2DEG(atan2f(-left[0], left[1])); //This was originally copied from the "void MatrixAngles( const matrix3x4_t& matrix, float *angles )" code, and it's 180 degrees off, negated the values and it all works now (Dave Kircher)
// The engine does pitch inverted from this, but we always end up negating it in the DLL
// UNDONE: Fix the engine to make it consistent
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG(atan2f(-forward[2], xyDist));
// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll)
angles[2] = 0;
}
}
#endif // #if !defined(__SPU__)
void SetIdentityMatrix(matrix3x4_t& matrix)
{
memset(matrix.Base(), 0, sizeof(float) * 3 * 4);
matrix[0][0] = 1.0;
matrix[1][1] = 1.0;
matrix[2][2] = 1.0;
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Builds a scale matrix
//-----------------------------------------------------------------------------
void SetScaleMatrix(float x, float y, float z, matrix3x4_t& dst)
{
dst[0][0] = x; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f;
dst[1][0] = 0.0f; dst[1][1] = y; dst[1][2] = 0.0f; dst[1][3] = 0.0f;
dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = z; dst[2][3] = 0.0f;
}
//-----------------------------------------------------------------------------
// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis.
//
// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ |
// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ |
// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ |
//
// Input : mat -
// vAxisOrRot -
// angle -
//-----------------------------------------------------------------------------
void MatrixBuildRotationAboutAxis(const Vector3D& vAxisOfRot, float angleDegrees, matrix3x4_t& dst)
{
float radians;
float axisXSquared;
float axisYSquared;
float axisZSquared;
float fSin;
float fCos;
radians = angleDegrees * (M_PI / 180.0);
fSin = sin(radians);
fCos = cos(radians);
axisXSquared = vAxisOfRot[0] * vAxisOfRot[0];
axisYSquared = vAxisOfRot[1] * vAxisOfRot[1];
axisZSquared = vAxisOfRot[2] * vAxisOfRot[2];
// Column 0:
dst[0][0] = axisXSquared + (1 - axisXSquared) * fCos;
dst[1][0] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) + vAxisOfRot[2] * fSin;
dst[2][0] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) - vAxisOfRot[1] * fSin;
// Column 1:
dst[0][1] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) - vAxisOfRot[2] * fSin;
dst[1][1] = axisYSquared + (1 - axisYSquared) * fCos;
dst[2][1] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) + vAxisOfRot[0] * fSin;
// Column 2:
dst[0][2] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) + vAxisOfRot[1] * fSin;
dst[1][2] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) - vAxisOfRot[0] * fSin;
dst[2][2] = axisZSquared + (1 - axisZSquared) * fCos;
// Column 3:
dst[0][3] = 0;
dst[1][3] = 0;
dst[2][3] = 0;
}
//-----------------------------------------------------------------------------
// Computes the transpose
//-----------------------------------------------------------------------------
void MatrixTranspose(matrix3x4_t& mat)
{
vec_t tmp;
tmp = mat[0][1]; mat[0][1] = mat[1][0]; mat[1][0] = tmp;
tmp = mat[0][2]; mat[0][2] = mat[2][0]; mat[2][0] = tmp;
tmp = mat[1][2]; mat[1][2] = mat[2][1]; mat[2][1] = tmp;
}
void MatrixTranspose(const matrix3x4_t& src, matrix3x4_t& dst)
{
dst[0][0] = src[0][0]; dst[0][1] = src[1][0]; dst[0][2] = src[2][0]; dst[0][3] = 0.0f;
dst[1][0] = src[0][1]; dst[1][1] = src[1][1]; dst[1][2] = src[2][1]; dst[1][3] = 0.0f;
dst[2][0] = src[0][2]; dst[2][1] = src[1][2]; dst[2][2] = src[2][2]; dst[2][3] = 0.0f;
}
#endif // #if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: converts engine euler angles into a matrix
// Input : vec3_t angles - PITCH, YAW, ROLL
// Output : *matrix - left-handed column matrix
// the basis vectors for the rotations will be in the columns as follows:
// matrix[][0] is forward
// matrix[][1] is left
// matrix[][2] is up
//-----------------------------------------------------------------------------
void AngleMatrix(RadianEuler const& angles, const Vector3D& position, matrix3x4_t& matrix)
{
AngleMatrix(angles, matrix);
MatrixSetColumn(position, 3, matrix);
}
void AngleMatrix(const RadianEuler& angles, matrix3x4_t& matrix)
{
QAngle quakeEuler(RAD2DEG(angles.y), RAD2DEG(angles.z), RAD2DEG(angles.x));
AngleMatrix(quakeEuler, matrix);
}
void AngleMatrix(const QAngle& angles, const Vector3D& position, matrix3x4_t& matrix)
{
AngleMatrix(angles, matrix);
MatrixSetColumn(position, 3, matrix);
}
void AngleMatrix(const QAngle& angles, matrix3x4_t& matrix)
{
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("AngleMatrix", "Mathlib");
#endif
Assert(s_bMathlibInitialized);
float sr, sp, sy, cr, cp, cy;
#ifdef _X360
fltx4 radians, scale, sine, cosine;
radians = LoadUnaligned3SIMD(angles.Base());
scale = ReplicateX4(M_PI_F / 180.f);
radians = MulSIMD(radians, scale);
SinCos3SIMD(sine, cosine, radians);
sp = SubFloat(sine, 0); sy = SubFloat(sine, 1); sr = SubFloat(sine, 2);
cp = SubFloat(cosine, 0); cy = SubFloat(cosine, 1); cr = SubFloat(cosine, 2);
#else
SinCos(DEG2RAD(angles[YAW]), &sy, &cy);
SinCos(DEG2RAD(angles[PITCH]), &sp, &cp);
SinCos(DEG2RAD(angles[ROLL]), &sr, &cr);
#endif
// matrix = (YAW * PITCH) * ROLL
matrix[0][0] = cp * cy;
matrix[1][0] = cp * sy;
matrix[2][0] = -sp;
// NOTE: Do not optimize this to reduce multiplies! optimizer bug will screw this up.
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.0f;
matrix[1][3] = 0.0f;
matrix[2][3] = 0.0f;
}
#if !defined(__SPU__)
void AngleIMatrix(const RadianEuler& angles, matrix3x4_t& matrix)
{
QAngle quakeEuler(RAD2DEG(angles.y), RAD2DEG(angles.z), RAD2DEG(angles.x));
AngleIMatrix(quakeEuler, matrix);
}
void AngleIMatrix(const QAngle& angles, matrix3x4_t& matrix)
{
Assert(s_bMathlibInitialized);
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);
// matrix = (YAW * PITCH) * ROLL
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.f;
matrix[1][3] = 0.f;
matrix[2][3] = 0.f;
}
void AngleIMatrix(const QAngle& angles, const Vector3D& position, matrix3x4_t& mat)
{
AngleIMatrix(angles, mat);
Vector3D vecTranslation;
VectorRotate(position, mat, vecTranslation);
vecTranslation *= -1.0f;
MatrixSetColumn(vecTranslation, 3, mat);
}
#endif // #if !defined(__SPU__)
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Bounding box construction methods
//-----------------------------------------------------------------------------
void ClearBounds(Vector3D& mins, Vector3D& maxs)
{
Assert(s_bMathlibInitialized);
mins[0] = mins[1] = mins[2] = FLT_MAX;
maxs[0] = maxs[1] = maxs[2] = -FLT_MAX;
}
void AddPointToBounds(const Vector3D& v, Vector3D& mins, Vector3D& maxs)
{
Assert(s_bMathlibInitialized);
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;
}
}
bool AreBoundsValid(const Vector3D& vMin, const Vector3D& vMax)
{
for (int i = 0; i < 3; ++i)
{
if (vMin[i] > vMax[i])
{
return false;
}
}
return true;
}
bool IsPointInBounds(const Vector3D& vPoint, const Vector3D& vMin, const Vector3D& vMax)
{
for (int i = 0; i < 3; ++i)
{
if (vPoint[i] < vMin[i] || vPoint[i] > vMax[i])
{
return false;
}
}
return true;
}
// solve a x^2 + b x + c = 0
bool SolveQuadratic(float a, float b, float c, float& root1, float& root2)
{
Assert(s_bMathlibInitialized);
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;
}
bool SolveInverseQuadraticMonotonic(float x1, float y1, float x2, float y2, float x3, float y3,
float& a, float& b, float& c)
{
// use SolveInverseQuadratic, but if the sigm of the derivative at the start point is the wrong
// sign, displace the mid point
// first, sort parameters
if (x1 > x2)
{
V_swap(x1, x2);
V_swap(y1, y2);
}
if (x2 > x3)
{
V_swap(x2, x3);
V_swap(y2, y3);
}
if (x1 > x2)
{
V_swap(x1, x2);
V_swap(y1, y2);
}
// this code is not fast. what it does is when the curve would be non-monotonic, slowly shifts
// the center point closer to the linear line between the endpoints. Should anyone need this
// function to be actually fast, it would be fairly easy to change it to be so.
for (float blend_to_linear_factor = 0.0f; blend_to_linear_factor <= 1.0f; blend_to_linear_factor += 0.05f)
{
float tempy2 = (1 - blend_to_linear_factor) * y2 + blend_to_linear_factor * FLerp(y1, y3, x1, x3, x2);
if (!SolveInverseQuadratic(x1, y1, x2, tempy2, x3, y3, a, b, c))
return false;
float derivative = 2.0f * a + b;
if ((y1 < y2) && (y2 < y3)) // monotonically increasing
{
if (derivative >= 0.0f)
return true;
}
else
{
if ((y1 > y2) && (y2 > y3)) // monotonically decreasing
{
if (derivative <= 0.0f)
return true;
}
else
return true;
}
}
return true;
}
// solves for "a, b, c" where "1/(a x^2 + b x + c ) = y", return true if solution exists
bool SolveInverseReciprocalQuadratic(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) * y1 * y2 * y3;
// FIXME: check with some sort of epsilon
if (det == 0.0f)
return false;
a = (x1 * y1 * (y2 - y3) + x3 * (y1 - y2) * y3 + x2 * y2 * (-y1 + y3)) / det;
b = (x2 * x2 * y2 * (y1 - y3) + x3 * x3 * (-y1 + y2) * y3 + x1 * x1 * y1 * (-y2 + y3)) / det;
c = (x2 * (x2 - x3) * x3 * y2 * y3 + x1 * x1 * y1 * (x2 * y2 - x3 * y3) + x1 * (-(x2 * x2 * y1 * y2) + x3 * x3 * y1 * y3)) / det;
return true;
}
// Rotate a vector around the Z axis (YAW)
void VectorYawRotate(const Vector3D& in, float flYaw, Vector3D& out)
{
Assert(s_bMathlibInitialized);
if (&in == &out)
{
Vector3D tmp;
tmp = in;
VectorYawRotate(tmp, flYaw, out);
return;
}
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;
}
float Bias(float x, float biasAmt)
{
// WARNING: not thread safe
static float lastAmt = -1;
static float lastExponent = 0;
if (lastAmt != biasAmt)
{
lastExponent = log(biasAmt) * -1.4427f; // (-1.4427 = 1 / log(0.5))
}
return pow(x, lastExponent);
}
float Gain(float x, float biasAmt)
{
// WARNING: not thread safe
if (x < 0.5)
return 0.5f * Bias(2 * x, 1 - biasAmt);
else
return 1 - 0.5f * Bias(2 - 2 * x, 1 - biasAmt);
}
float SmoothCurve(float x)
{
return (1 - cos(x * M_PI)) * 0.5f;
}
inline float MovePeak(float x, float flPeakPos)
{
// Todo: make this higher-order?
if (x < flPeakPos)
return x * 0.5f / flPeakPos;
else
return 0.5f + 0.5f * (x - flPeakPos) / (1 - flPeakPos);
}
float SmoothCurve_Tweak(float x, float flPeakPos, float flPeakSharpness)
{
float flMovedPeak = MovePeak(x, flPeakPos);
float flSharpened = Gain(flMovedPeak, flPeakSharpness);
return SmoothCurve(flSharpened);
}
#endif // !defined(__SPU__)
//-----------------------------------------------------------------------------
// make sure quaternions are within 180 degrees of one another, if not, reverse q
//-----------------------------------------------------------------------------
void QuaternionAlign(const Quaternion& p, const Quaternion& q, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
// FIXME: can this be done with a quat dot product?
int i;
// 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++)
{
qt[i] = -q[i];
}
}
else if (&qt != &q)
{
for (i = 0; i < 4; i++)
{
qt[i] = q[i];
}
}
}
//-----------------------------------------------------------------------------
// Do a piecewise addition of the quaternion elements. This actually makes little
// mathematical sense, but it's a cheap way to simulate a slerp.
//-----------------------------------------------------------------------------
void QuaternionBlend(const Quaternion& p, const Quaternion& q, float t, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
#if ALLOW_SIMD_QUATERNION_MATH
fltx4 psimd, qsimd, qtsimd;
psimd = LoadUnalignedSIMD(p.Base());
qsimd = LoadUnalignedSIMD(q.Base());
qtsimd = QuaternionBlendSIMD(psimd, qsimd, t);
StoreUnalignedSIMD(qt.Base(), qtsimd);
#else
// decide if one of the quaternions is backwards
Quaternion q2;
QuaternionAlign(p, q, q2);
QuaternionBlendNoAlign(p, q2, t, qt);
#endif
}
void QuaternionBlendNoAlign(const Quaternion& p, const Quaternion& q, float t, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
float sclp, sclq;
int i;
// 0.0 returns p, 1.0 return q.
sclp = 1.0f - t;
sclq = t;
for (i = 0; i < 4; i++) {
qt[i] = sclp * p[i] + sclq * q[i];
}
QuaternionNormalize(qt);
}
void QuaternionIdentityBlend(const Quaternion& p, float t, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
float sclp;
sclp = 1.0f - t;
qt.x = p.x * sclp;
qt.y = p.y * sclp;
qt.z = p.z * sclp;
if (qt.w < 0.0)
{
qt.w = p.w * sclp - t;
}
else
{
qt.w = p.w * sclp + t;
}
QuaternionNormalize(qt);
}
//-----------------------------------------------------------------------------
// Quaternion spherical linear interpolation
//-----------------------------------------------------------------------------
void QuaternionSlerp(const Quaternion& p, const Quaternion& q, float t, Quaternion& qt)
{
Quaternion 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);
}
void QuaternionSlerpNoAlign(const Quaternion& p, const Quaternion& q, float t, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
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 {
// TODO: add short circuit for cosom == 1.0f?
sclp = 1.0f - t;
sclq = t;
}
for (i = 0; i < 4; i++) {
qt[i] = sclp * p[i] + sclq * q[i];
}
}
else {
Assert(&qt != &q);
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];
}
}
Assert(qt.IsValid());
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: Returns the angular delta between the two normalized quaternions in degrees.
//-----------------------------------------------------------------------------
float QuaternionAngleDiff(const Quaternion& p, const Quaternion& q)
{
#if 1
// this code path is here for 2 reasons:
// 1 - acos maps 1-epsilon to values much larger than epsilon (vs asin, which maps epsilon to itself)
// this means that in floats, anything below ~0.05 degrees truncates to 0
// 2 - normalized quaternions are frequently slightly non-normalized due to float precision issues,
// and the epsilon off of normalized can be several percents of a degree
Quaternion qInv, diff;
QuaternionConjugate(q, qInv);
QuaternionMult(p, qInv, diff);
// Note if the quaternion is slightly non-normalized the square root below may be more than 1,
// the value is clamped to one otherwise it may result in asin() returning an undefined result.
float sinang = MIN(1.0f, sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z));
float angle = RAD2DEG(2 * asin(sinang));
return angle;
#else
Quaternion q2;
QuaternionAlign(p, q, q2);
Assert(s_bMathlibInitialized);
float cosom = p.x * q2.x + p.y * q2.y + p.z * q2.z + p.w * q2.w;
if (cosom > -1.0f)
{
if (cosom < 1.0f)
{
float omega = 2 * fabs(acos(cosom));
return RAD2DEG(omega);
}
return 0.0f;
}
return 180.0f;
#endif
}
void QuaternionConjugate(const Quaternion& p, Quaternion& q)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
q.x = -p.x;
q.y = -p.y;
q.z = -p.z;
q.w = p.w;
}
void QuaternionInvert(const Quaternion& p, Quaternion& q)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
QuaternionConjugate(p, q);
float magnitudeSqr = QuaternionDotProduct(p, p);
Assert(magnitudeSqr);
if (magnitudeSqr)
{
float inv = 1.0f / magnitudeSqr;
q.x *= inv;
q.y *= inv;
q.z *= inv;
q.w *= inv;
}
}
void QuaternionMultiply(const Quaternion& q, const Vector3D& v, Vector3D& result)
{
Vector3D t, t2;
CrossProduct(q.ImaginaryPart(), v, t);
t *= 2.0f;
VectorMA(v, q.RealPart(), t, result);
CrossProduct(q.ImaginaryPart(), t, t2);
result += t2;
}
#endif // #if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Make sure the quaternion is of unit length
//-----------------------------------------------------------------------------
float QuaternionNormalize(Quaternion& q)
{
Assert(s_bMathlibInitialized);
float radius, iradius;
Assert(q.IsValid());
radius = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
if (radius) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON))
{
radius = sqrt(radius);
iradius = 1.0f / radius;
q[3] *= iradius;
q[2] *= iradius;
q[1] *= iradius;
q[0] *= iradius;
}
return radius;
}
void QuaternionScale(const Quaternion& p, float t, Quaternion& q)
{
Assert(s_bMathlibInitialized);
#if 0
Quaternion p0;
Quaternion q;
p0.Init(0.0, 0.0, 0.0, 1.0);
// slerp in "reverse order" so that p doesn't get realigned
QuaternionSlerp(p, p0, 1.0 - fabs(t), q);
if (t < 0.0)
{
q.w = -q.w;
}
#else
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.
float sinom = sqrt(DotProduct(&p.x, &p.x));
sinom = MIN(sinom, 1.f);
float sinsom = sin(asin(sinom) * t);
t = sinsom / (sinom + FLT_EPSILON);
VectorScale(&p.x, t, &q.x);
// rescale rotation
r = 1.0f - sinsom * sinsom;
// Assert( r >= 0 );
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;
#endif
Assert(q.IsValid());
return;
}
void QuaternionAdd(const Quaternion& p, const Quaternion& q, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
Assert(p.IsValid());
Assert(q.IsValid());
// decide if one of the quaternions is backwards
Quaternion q2;
QuaternionAlign(p, q, q2);
// is this right???
qt[0] = p[0] + q2[0];
qt[1] = p[1] + q2[1];
qt[2] = p[2] + q2[2];
qt[3] = p[3] + q2[3];
return;
}
float QuaternionDotProduct(const Quaternion& p, const Quaternion& q)
{
Assert(s_bMathlibInitialized);
Assert(p.IsValid());
Assert(q.IsValid());
return p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w;
}
// qt = p * q
void QuaternionMult(const Quaternion& p, const Quaternion& q, Quaternion& qt)
{
Assert(s_bMathlibInitialized);
Assert(p.IsValid());
Assert(q.IsValid());
if (&p == &qt)
{
Quaternion p2 = p;
QuaternionMult(p2, q, qt);
return;
}
// decide if one of the quaternions is backwards
Quaternion 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;
}
#if !defined(__SPU__)
void QuaternionExp(const Quaternion& p, Quaternion& q)
{
float r = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
float et = exp(p[3]);
float s = r >= 0.00001f ? et * sin(r) / r : 0.f;
q.Init(s * p[0], s * p[1], s * p[2], et * cos(r));
}
void QuaternionLn(const Quaternion& p, Quaternion& q)
{
float r = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
float t = r > 0.00001f ? atan2(r, p[3]) / r : 0.f;
float norm = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3];
q.Init(t * p[0], t * p[1], t * p[2], 0.5 * log(norm));
}
// Average using exponential method
// Qave = exp( 1 / n * log( Q1 ) + ... + 1 / n * log( Qn ) ) where
// if pflWeights passed in 1/n is replaced by normalized weighting
void QuaternionAverageExponential(Quaternion& q, int nCount, const Quaternion* pQuaternions, const float* pflWeights /*=NULL*/)
{
Assert(nCount >= 1);
Assert(pQuaternions);
// Nothing to do if only one input quaternions
if (nCount == 1)
{
q = pQuaternions[0];
return;
}
float ooWeightSum = 1.0f;
float flWeightSum = 0.0f;
for (int i = 0; i < nCount; ++i)
{
if (pflWeights)
{
flWeightSum += pflWeights[i];
}
else
{
flWeightSum += 1.0f;
}
}
if (flWeightSum > 0.0f)
{
ooWeightSum = 1.0f / flWeightSum;
}
Quaternion sum(0, 0, 0, 0);
// Now sum the ln of the quaternions
for (int i = 0; i < nCount; ++i)
{
float weight = ooWeightSum;
if (pflWeights)
{
weight *= pflWeights[i];
}
// Make sure all quaternions are aligned with the
// first to avoid blending the wrong direction.
Quaternion alignedQuat;
QuaternionAlign(pQuaternions[0], pQuaternions[i], alignedQuat);
Quaternion qLn;
QuaternionLn(alignedQuat, qLn);
for (int j = 0; j < 4; ++j)
{
sum[j] += (qLn[j] * weight);
}
}
// then exponentiate to get final value
QuaternionExp(sum, q);
}
// Given a vector and a pseudo-up reference vector, create a quaternion which represents
// the orientation of the forward vector. Note, will be unstable if vecForward is close
// to referenceUp
void QuaternionLookAt(const Vector3D& vecForward, const Vector3D& referenceUp, Quaternion& q)
{
Vector3D forward = vecForward;
forward.NormalizeInPlace();
float ratio = DotProduct(forward, referenceUp);
Vector3D up = referenceUp - (forward * ratio);
up.NormalizeInPlace();
Vector3D right = forward.Cross(up);
right.NormalizeInPlace();
const Vector3D& x = right;
const Vector3D& y = forward;
const Vector3D& z = up;
float tr = x.x + y.y + z.z;
q.Init(y.z - z.y, z.x - x.z, x.y - y.x, tr + 1.0f);
QuaternionNormalize(q);
/*
Vector z = vecForward;
z.NormalizeInPlace();
Vector x = referenceUp.Cross( z );
x.NormalizeInPlace();
Vector y = z.Cross( x );
y.NormalizeInPlace();
float tr = x.x + y.y + z.z;
q.Init( y.z - z.y , z.x - x.z, x.y - y.x, tr + 1.0f );
QuaternionNormalize( q );
*/
}
#endif // !defined(__SPU__)
void QuaternionMatrix(const Quaternion& q, const Vector3D& pos, matrix3x4_t& matrix)
{
Assert(pos.IsValid());
QuaternionMatrix(q, matrix);
matrix[0][3] = pos.x;
matrix[1][3] = pos.y;
matrix[2][3] = pos.z;
}
void QuaternionMatrix(const Quaternion& q, const Vector3D& pos, const Vector3D& vScale, matrix3x4_t& mat)
{
Assert(pos.IsValid());
Assert(q.IsValid());
Assert(vScale.IsValid());
QuaternionMatrix(q, mat);
mat[0][0] *= vScale.x; mat[1][0] *= vScale.x; mat[2][0] *= vScale.x;
mat[0][1] *= vScale.y; mat[1][1] *= vScale.y; mat[2][1] *= vScale.y;
mat[0][2] *= vScale.z; mat[1][2] *= vScale.z; mat[2][2] *= vScale.z;
mat[0][3] = pos.x; mat[1][3] = pos.y; mat[2][3] = pos.z;
}
void QuaternionMatrix(const Quaternion& q, matrix3x4_t& matrix)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("QuaternionMatrix", "Mathlib");
#endif
// Original code
// This should produce the same code as below with optimization, but looking at the assembly,
// it doesn't. There are 7 extra multiplies in the release build of this, go figure.
#if 1
matrix[0][0] = 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z;
matrix[1][0] = 2.0 * q.x * q.y + 2.0 * q.w * q.z;
matrix[2][0] = 2.0 * q.x * q.z - 2.0 * q.w * q.y;
matrix[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
matrix[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z;
matrix[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
matrix[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y;
matrix[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
matrix[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y;
matrix[0][3] = 0.0f;
matrix[1][3] = 0.0f;
matrix[2][3] = 0.0f;
#else
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
// precalculate common multiplitcations
x2 = q.x + q.x;
y2 = q.y + q.y;
z2 = q.z + q.z;
xx = q.x * x2;
xy = q.x * y2;
xz = q.x * z2;
yy = q.y * y2;
yz = q.y * z2;
zz = q.z * z2;
wx = q.w * x2;
wy = q.w * y2;
wz = q.w * z2;
matrix[0][0] = 1.0 - (yy + zz);
matrix[0][1] = xy - wz;
matrix[0][2] = xz + wy;
matrix[0][3] = 0.0f;
matrix[1][0] = xy + wz;
matrix[1][1] = 1.0 - (xx + zz);
matrix[1][2] = yz - wx;
matrix[1][3] = 0.0f;
matrix[2][0] = xz - wy;
matrix[2][1] = yz + wx;
matrix[2][2] = 1.0 - (xx + yy);
matrix[2][3] = 0.0f;
#endif
}
const Vector3D Quaternion::GetForward()const
{
Vector3D vAxisX;
vAxisX.x = 1.0 - 2.0 * y * y - 2.0 * z * z;
vAxisX.y = 2.0 * x * y + 2.0 * w * z;
vAxisX.z = 2.0 * x * z - 2.0 * w * y;
return vAxisX;
}
const Vector3D Quaternion::GetLeft()const
{
Vector3D vAxisY;
vAxisY.x = 2.0f * x * y - 2.0f * w * z;
vAxisY.y = 1.0f - 2.0f * x * x - 2.0f * z * z;
vAxisY.z = 2.0f * y * z + 2.0f * w * x;
return vAxisY;
}
const Vector3D Quaternion::GetUp()const
{
Vector3D vAxisZ;
vAxisZ.x = 2.0f * x * z + 2.0f * w * y;
vAxisZ.y = 2.0f * y * z - 2.0f * w * x;
vAxisZ.z = 1.0f - 2.0f * x * x - 2.0f * y * y;
return vAxisZ;
}
const Quaternion RotateBetween(const Vector3D& v1, const Vector3D& v2)
{
// Find quaternion that rotates v1 into v2
Quaternion qOut;
Vector3D vBisector = 0.5f * (v1 + v2);
if (vBisector.LengthSqr() > 1e-9f)
{
qOut.Init(CrossProduct(v1, vBisector), DotProduct(v1, vBisector));
}
else
{
// Anti-parallel: Use a perpendicular vector
if (fabsf(v1.x) > 0.5f)
{
qOut.x = v1.y;
qOut.y = -v1.x;
qOut.z = 0.0f;
}
else
{
qOut.x = 0.0f;
qOut.y = v1.z;
qOut.z = -v1.y;
}
qOut.w = 0.0f;
}
// The algorithm is simplified and made more accurate by normalizing at the end
QuaternionNormalize(qOut);
Assert((VectorTransform(v1, QuaternionMatrix(qOut)) - v2).Length() < 2e-3f);
return qOut;
}
void UnitTestQuatExpLog()
{
for (int i = 0; i < 300000; ++i)
{
Quaternion q = RandomQuaternion();
Vector3D l = QuaternionLog(q);
Quaternion q2 = Exp(l);
Assert(QuaternionLength(q - q2) < 0.0001f);
}
}
void UnitTestRotateBetween()
{
RandomSeed(1);
float flMaxError = 0;
int nMaxError;
for (int i = 0; i < 3000000; ++i)
{
Vector3D u = RandomVectorOnUnitSphere(), v = RandomVectorOnUnitSphere();
Quaternion q = RotateBetween(u, v);
float flError = (VectorTransform(u, QuaternionMatrix(q)) - v).Length();
if (flMaxError < flError)
{
flMaxError = flError;
nMaxError = i;
}
}
Assert(flMaxError < 0.001f);
NOTE_UNUSED(nMaxError);
}
//-----------------------------------------------------------------------------
// Purpose: Converts a quaternion into engine angles
// Input : *quaternion - q3 + q0.i + q1.j + q2.k
// *outAngles - PITCH, YAW, ROLL
//-----------------------------------------------------------------------------
void QuaternionAngles(const Quaternion& q, QAngle& angles)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("QuaternionAngles", "Mathlib");
#endif
#if 1
// FIXME: doing it this way calculates too much data, needs to do an optimized version...
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
MatrixAngles(matrix, angles);
#else
float m11, m12, m13, m23, m33;
m11 = (2.0f * q.w * q.w) + (2.0f * q.x * q.x) - 1.0f;
m12 = (2.0f * q.x * q.y) + (2.0f * q.w * q.z);
m13 = (2.0f * q.x * q.z) - (2.0f * q.w * q.y);
m23 = (2.0f * q.y * q.z) + (2.0f * q.w * q.x);
m33 = (2.0f * q.w * q.w) + (2.0f * q.z * q.z) - 1.0f;
// FIXME: this code has a singularity near PITCH +-90
angles[YAW] = RAD2DEG(atan2(m12, m11));
angles[PITCH] = RAD2DEG(asin(-m13));
angles[ROLL] = RAD2DEG(atan2(m23, m33));
#endif
Assert(angles.IsValid());
}
float QuaternionionGetYaw(const Quaternion& q)
{
// FIXME: doing it this way calculates too much data, need to do an optimized version...
QAngle angles;
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
MatrixAngles(matrix, angles);
return angles[YAW];
}
float QuaternionionGetPitch(const Quaternion& q)
{
// FIXME: doing it this way calculates too much data, need to do an optimized version...
QAngle angles;
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
MatrixAngles(matrix, angles);
return angles[PITCH];
}
float QuaternionionGetRoll(const Quaternion& q)
{
// FIXME: doing it this way calculates too much data, need to do an optimized version...
QAngle angles;
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
MatrixAngles(matrix, angles);
return angles[ROLL];
}
//-----------------------------------------------------------------------------
// Purpose: Converts a quaternion into FLU vectors
// Input : *quaternion - q3 + q0.i + q1.j + q2.k
// basis vectors, each vector is optional
//-----------------------------------------------------------------------------
void QuaternionVectorsFLU(Quaternion const& q, Vector3D* pForward, Vector3D* pLeft, Vector3D* pUp)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
#ifdef _VPROF_MATHLIB
// @TODO: VPROF_BUDGET( "QuaternionVectorsFLU", "Mathlib" );
#endif
// Note: it's pretty much identical to just computing the quaternion matrix and assigning its columns to the vectors
* pForward = q.GetForward();
*pLeft = q.GetLeft();
*pUp = q.GetUp();
#ifdef DBGFLAG_ASSERT
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
Vector3D forward, left, up;
MatrixVectorsFLU(matrix, &forward, &left, &up);
Assert((forward - *pForward).Length() + (left - *pLeft).Length() + (up - *pUp).Length() < 1e-4f);
#endif
}
void QuaternionVectorsForward(const Quaternion& q, Vector3D* pForward)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
#ifdef _VPROF_MATHLIB
// @TODO: VPROF_BUDGET( "QuaternionVectorsForward", "Mathlib" );
#endif
* pForward = q.GetForward();
#ifdef DBGFLAG_ASSERT
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
Assert((MatrixGetColumn(matrix, FORWARD_AXIS) - *pForward).Length() < 1e-4f);
#endif
}
void UnitTestVectorFLU()
{
for (int i = 0; i < 100000; ++i)
{
Quaternion q = RandomQuaternion();
Vector3D forward, left, up;
QuaternionVectorsForward(q, &forward);
QuaternionVectorsFLU(q, &forward, &left, &up);
}
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: Converts a quaternion to an axis / angle in degrees
// (exponential map)
//-----------------------------------------------------------------------------
void QuaternionAxisAngle(const Quaternion& q, Vector3D& axis, float& angle)
{
angle = RAD2DEG(2 * acos(q.w));
if (angle > 180)
{
angle -= 360;
}
axis.x = q.x;
axis.y = q.y;
axis.z = q.z;
VectorNormalize(axis);
}
//-----------------------------------------------------------------------------
// Purpose: Converts an exponential map (ang/axis) to a quaternion
//-----------------------------------------------------------------------------
void AxisAngleQuaternion(const Vector3D& axis, float angle, Quaternion& 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;
}
#endif // #if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: Converts radian-euler axis aligned angles to a quaternion
// Input : *pfAngles - Right-handed Euler angles in radians
// *outQuat - quaternion of form (i,j,k,real)
//-----------------------------------------------------------------------------
void AngleQuaternion(const RadianEuler& angles, Quaternion& outQuat)
{
Assert(s_bMathlibInitialized);
// Assert( angles.IsValid() );
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("AngleQuaternion", "Mathlib");
#endif
float sr, sp, sy, cr, cp, cy;
#ifdef _X360
fltx4 radians, scale, sine, cosine;
radians = LoadUnaligned3SIMD(&angles.x);
scale = ReplicateX4(0.5f);
radians = MulSIMD(radians, scale);
SinCos3SIMD(sine, cosine, radians);
// NOTE: The ordering here is *different* from the AngleQuaternion below
// because p, y, r are not in the same locations in QAngle + RadianEuler. Yay!
sr = SubFloat(sine, 0); sp = SubFloat(sine, 1); sy = SubFloat(sine, 2);
cr = SubFloat(cosine, 0); cp = SubFloat(cosine, 1); cy = SubFloat(cosine, 2);
#else
SinCos(angles.z * 0.5f, &sy, &cy);
SinCos(angles.y * 0.5f, &sp, &cp);
SinCos(angles.x * 0.5f, &sr, &cr);
#endif
// NJS: for some reason VC6 wasn't recognizing the common subexpressions:
float srXcp = sr * cp, crXsp = cr * sp;
outQuat.x = srXcp * cy - crXsp * sy; // X
outQuat.y = crXsp * cy + srXcp * sy; // Y
float crXcp = cr * cp, srXsp = sr * sp;
outQuat.z = crXcp * sy - srXsp * cy; // Z
outQuat.w = crXcp * cy + srXsp * sy; // W (real component)
}
#ifdef _X360
//-----------------------------------------------------------------------------
// Purpose: Converts radian-euler axis aligned angles to a quaternion, returning
// it on a vector register.
// Input : *vAngles - Right-handed Euler angles in radians (roll pitch yaw)
//
// Algorithm based on that found in the XDK (which really uses RPY order, as
// opposed to this which takes the parameters in RPY order but catenates them
// in PYR order).
//-----------------------------------------------------------------------------
fltx4 AngleQuaternionSIMD(FLTX4 vAngles)
{
Assert(s_bMathlibInitialized);
// Assert( angles.IsValid() );
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("AngleQuaternion", "Mathlib");
#endif
// we compute the sin and cos of half all the angles.
// in the comments I'll call these components
// sr = sin(r/2), cp = cos(p/2), sy = sin(y/2), etc.
fltx4 OneHalf = __vspltisw(1);
OneHalf = __vcfsx(OneHalf, 1);
fltx4 HalfAngles = MulSIMD(vAngles, OneHalf);
fltx4 sine, cosine;
SinCos3SIMD(sine, cosine, HalfAngles);
fltx4 SignMask = __vspltisw(-1);
fltx4 Zero = __vspltisw(0);
SignMask = __vslw(SignMask, SignMask); // shift left so 1 is only in the sign bit
SignMask = __vrlimi(SignMask, Zero, 0x5, 0); // { -1, 0, -1, 0 }
fltx4 Rc, Pc, Yc, Rs, Ps, Ys, retsum, retval;
Rc = __vspltw(cosine, 0); // cr cr cr cr
Pc = __vspltw(cosine, 1); // cp cp cp cp
Yc = __vspltw(cosine, 2); // cy cy cy cy
Rs = __vspltw(sine, 0); // sr sr sr sr
Ps = __vspltw(sine, 1); // sp sp sp sp
Ys = __vspltw(sine, 2); // sy sy sy sy
Rc = __vrlimi(Rc, sine, 0x8, 0); // sr cr cr cr
Rs = __vrlimi(Rs, cosine, 0x8, 0); // cr sr sr sr
Pc = __vrlimi(Pc, sine, 0x4, 0); // cp sp cp cp
Ps = __vrlimi(Ps, cosine, 0x4, 0); // sp cp sp sp
Yc = __vrlimi(Yc, sine, 0x2, 0); // cy cy sy cy
Ys = __vrlimi(Ys, cosine, 0x2, 0); // sy sy cy sy
retsum = __vxor(Rs, SignMask); // -cr sr -sr sr
retval = __vmulfp(Pc, Yc); // cp*cy sp*cy cp*sy cp*cy
retsum = __vmulfp(retsum, Ys); // -cr*sy sr*sy -sr*cy sr*sy
retval = __vmulfp(retval, Rc); // cp*cy*sr sp*cy*cr cp*sy*cr cp*cy*cr
retval = __vmaddfp(retsum, Ps, retval); // cp*cy*sr + -cr*sy*sp ...
return retval;
}
inline fltx4 AngleQuaternionSIMD(const RadianEuler& angles)
{
return AngleQuaternionSIMD(LoadUnaligned3SIMD(angles.Base()));
}
#endif
//-----------------------------------------------------------------------------
// Purpose: Converts engine-format euler angles to a quaternion
// Input : angles - Right-handed Euler angles in degrees as follows:
// [0]: PITCH: Clockwise rotation around the Y axis.
// [1]: YAW: Counterclockwise rotation around the Z axis.
// [2]: ROLL: Counterclockwise rotation around the X axis.
// *outQuat - quaternion of form (i,j,k,real)
//-----------------------------------------------------------------------------
void AngleQuaternion(const QAngle& angles, Quaternion& outQuat)
{
#ifdef _VPROF_MATHLIB
VPROF_BUDGET("AngleQuaternion", "Mathlib");
#endif
float sr, sp, sy, cr, cp, cy;
#ifdef _X360
fltx4 radians, scale, sine, cosine;
radians = LoadUnaligned3SIMD(angles.Base());
scale = ReplicateX4(0.5f * M_PI_F / 180.f);
radians = MulSIMD(radians, scale);
SinCos3SIMD(sine, cosine, radians);
// NOTE: The ordering here is *different* from the AngleQuaternion above
// because p, y, r are not in the same locations in QAngle + RadianEuler. Yay!
sp = SubFloat(sine, 0); sy = SubFloat(sine, 1); sr = SubFloat(sine, 2);
cp = SubFloat(cosine, 0); cy = SubFloat(cosine, 1); cr = SubFloat(cosine, 2);
#else
SinCos(DEG2RAD(angles.y) * 0.5f, &sy, &cy);
SinCos(DEG2RAD(angles.x) * 0.5f, &sp, &cp);
SinCos(DEG2RAD(angles.z) * 0.5f, &sr, &cr);
#endif
// NJS: for some reason VC6 wasn't recognizing the common subexpressions:
float srXcp = sr * cp, crXsp = cr * sp;
outQuat.x = srXcp * cy - crXsp * sy; // X
outQuat.y = crXsp * cy + srXcp * sy; // Y
float crXcp = cr * cp, srXsp = sr * sp;
outQuat.z = crXcp * sy - srXsp * cy; // Z
outQuat.w = crXcp * cy + srXsp * sy; // W (real component)
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: Converts a basis to a quaternion
//-----------------------------------------------------------------------------
void BasisToQuaternion(const Vector3D& vecForward, const Vector3D& vecRight, const Vector3D& vecUp, Quaternion& q)
{
Assert(fabs(vecForward.LengthSqr() - 1.0f) < 1e-3);
Assert(fabs(vecRight.LengthSqr() - 1.0f) < 1e-3);
Assert(fabs(vecUp.LengthSqr() - 1.0f) < 1e-3);
Vector3D vecLeft;
VectorMultiply(vecRight, -1.0f, vecLeft);
// FIXME: Don't know why, but this doesn't match at all with other result
// so we can't use this super-fast way.
/*
// Find the trace of the matrix:
float flTrace = vecForward.x + vecLeft.y + vecUp.z + 1.0f;
if ( flTrace > 1e-6 )
{
float flSqrtTrace = FastSqrt( flTrace );
float s = 0.5f / flSqrtTrace;
q.x = ( vecUp.y - vecLeft.z ) * s;
q.y = ( vecForward.z - vecUp.x ) * s;
q.z = ( vecLeft.x - vecForward.y ) * s;
q.w = 0.5f * flSqrtTrace;
}
else
{
if (( vecForward.x > vecLeft.y ) && ( vecForward.x > vecUp.z ) )
{
float flSqrtTrace = FastSqrt( 1.0f + vecForward.x - vecLeft.y - vecUp.z );
float s = 0.5f / flSqrtTrace;
q.x = 0.5f * flSqrtTrace;
q.y = ( vecForward.y + vecLeft.x ) * s;
q.z = ( vecUp.x + vecForward.z ) * s;
q.w = ( vecUp.y - vecLeft.z ) * s;
}
else if ( vecLeft.y > vecUp.z )
{
float flSqrtTrace = FastSqrt( 1.0f + vecLeft.y - vecForward.x - vecUp.z );
float s = 0.5f / flSqrtTrace;
q.x = ( vecForward.y + vecLeft.x ) * s;
q.y = 0.5f * flSqrtTrace;
q.z = ( vecUp.y + vecLeft.z ) * s;
q.w = ( vecForward.z - vecUp.x ) * s;
}
else
{
float flSqrtTrace = FastSqrt( 1.0 + vecUp.z - vecForward.x - vecLeft.y );
float s = 0.5f / flSqrtTrace;
q.x = ( vecUp.x + vecForward.z ) * s;
q.y = ( vecUp.y + vecLeft.z ) * s;
q.z = 0.5f * flSqrtTrace;
q.w = ( vecLeft.x - vecForward.y ) * s;
}
}
QuaternionNormalize( q );
*/
// Version 2: Go through angles
matrix3x4_t mat;
MatrixSetColumn(vecForward, 0, mat);
MatrixSetColumn(vecLeft, 1, mat);
MatrixSetColumn(vecUp, 2, mat);
QAngle angles;
MatrixAngles(mat, angles);
// Quaternion q2;
AngleQuaternion(angles, q);
// Assert( fabs(q.x - q2.x) < 1e-3 );
// Assert( fabs(q.y - q2.y) < 1e-3 );
// Assert( fabs(q.z - q2.z) < 1e-3 );
// Assert( fabs(q.w - q2.w) < 1e-3 );
}
// FIXME: Optimize!
void MatrixQuaternion(const matrix3x4_t& mat, Quaternion& q)
{
QAngle angles;
MatrixAngles(mat, angles);
AngleQuaternion(angles, q);
}
#endif // #if !defined(__SPU__)
void MatrixQuaternionFast(const matrix3x4_t& mat, Quaternion& q)
{
float t;
if (mat[2][2] < 0)
{
if (mat[0][0] > mat[1][1])
{
t = 1 + mat[0][0] - mat[1][1] - mat[2][2];
q.Init(t, mat[0][1] + mat[1][0], mat[2][0] + mat[0][2], mat[2][1] - mat[1][2]);
}
else
{
t = 1 - mat[0][0] + mat[1][1] - mat[2][2];
q.Init(mat[0][1] + mat[1][0], t, mat[1][2] + mat[2][1], mat[0][2] - mat[2][0]);
}
}
else
{
if (mat[0][0] < -mat[1][1])
{
t = 1 - mat[0][0] - mat[1][1] + mat[2][2];
q.Init(mat[2][0] + mat[0][2], mat[1][2] + mat[2][1], t, mat[1][0] - mat[0][1]);
}
else
{
t = 1 + mat[0][0] + mat[1][1] + mat[2][2];
q.Init(mat[2][1] - mat[1][2], mat[0][2] - mat[2][0], mat[1][0] - mat[0][1], t);
}
}
q = q * (0.5f / sqrtf(t));
}
float MatrixQuaternionTest(uint nCount)
{
float flMaxError = 0, flSumError = 0;
for (uint i = 0; i < nCount; ++i)
{
Quaternion q = RandomQuaternion(), r;
Assert(fabsf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1) < 1e-5f);
matrix3x4_t mat;
QuaternionMatrix(q, mat);
MatrixQuaternion(mat, r);
if (QuaternionDotProduct(q, r) < 0)
{
r = -r;
}
float flError = Sqr(q.x - r.x) + Sqr(q.y - r.y) + Sqr(q.z - r.z) + Sqr(q.w - r.w);
flSumError += flError;
if (flError > flMaxError)
{
flMaxError = flError;
}
}
NOTE_UNUSED(flMaxError); NOTE_UNUSED(flSumError);
return flSumError / nCount;
}
float MatrixQuaternionFastTest(uint nCount)
{
float flMaxError = 0, flSumError = 0;
for (uint i = 0; i < nCount; ++i)
{
Quaternion q = RandomQuaternion(), r;
Assert(fabsf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1) < 1e-5f);
matrix3x4_t mat;
QuaternionMatrix(q, mat);
MatrixQuaternionFast(mat, r);
if (QuaternionDotProduct(q, r) < 0)
{
r = -r;
}
float flError = Sqr(q.x - r.x) + Sqr(q.y - r.y) + Sqr(q.z - r.z) + Sqr(q.w - r.w);
flSumError += flError;
if (flError > flMaxError)
{
flMaxError = flError;
}
}
NOTE_UNUSED(flMaxError); NOTE_UNUSED(flSumError);
return flSumError / nCount;
}
// the same as MatrixQuaternionTest, but uses inline helper functions that return matrix and quaternion instead of using return-by-reference versions
// on MSVC10, this generates the same code as MatrixQuaternionTest, but it's easier to read, write and maintain code
float MatrixQuaternionTest2(uint nCount)
{
float flMaxError = 0, flSumError = 0;
for (uint i = 0; i < nCount; ++i)
{
Quaternion q = RandomQuaternion(), r;
Assert(fabsf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1) < 1e-5f);
matrix3x4_t mat = QuaternionMatrix(q);
r = MatrixQuaternion(mat);
if (QuaternionDotProduct(q, r) < 0)
{
r = -r;
}
float flError = Sqr(q.x - r.x) + Sqr(q.y - r.y) + Sqr(q.z - r.z) + Sqr(q.w - r.w);
flSumError += flError;
if (flError > flMaxError)
{
flMaxError = flError;
}
}
NOTE_UNUSED(flMaxError); NOTE_UNUSED(flSumError);
return flSumError / nCount;
}
//-----------------------------------------------------------------------------
// Purpose: Converts a quaternion into engine angles
// Input : *quaternion - q3 + q0.i + q1.j + q2.k
// *outAngles - PITCH, YAW, ROLL
//-----------------------------------------------------------------------------
void QuaternionAngles(const Quaternion& q, RadianEuler& angles)
{
Assert(s_bMathlibInitialized);
Assert(q.IsValid());
// FIXME: doing it this way calculates too much data, needs to do an optimized version...
matrix3x4_t matrix;
QuaternionMatrix(q, matrix);
MatrixAngles(matrix, angles);
Assert(angles.IsValid());
}
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: A helper function to normalize p2.x->p1.x and p3.x->p4.x to
// be the same length as p2.x->p3.x
// Input : &p2 -
// &p4 -
// p4n -
//-----------------------------------------------------------------------------
void Spline_Normalize(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
Vector3D& p1n,
Vector3D& p4n)
{
float dt = p3.x - p2.x;
p1n = p1;
p4n = p4;
if (dt != 0.0)
{
if (p1.x != p2.x)
{
// Equivalent to p1n = p2 - (p2 - p1) * (dt / (p2.x - p1.x));
VectorLerp(p2, p1, dt / (p2.x - p1.x), p1n);
}
if (p4.x != p3.x)
{
// Equivalent to p4n = p3 + (p4 - p3) * (dt / (p4.x - p3.x));
VectorLerp(p3, p4, dt / (p4.x - p3.x), p4n);
}
}
}
#endif // #if !defined(__SPU__)
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose:
// Input :
//-----------------------------------------------------------------------------
void Catmull_Rom_Spline(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float tSqr = t * t * 0.5f;
float tSqrSqr = t * tSqr;
t *= 0.5f;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
output.Init();
Vector3D a, b, c, d;
// matrix row 1
VectorScale(p1, -tSqrSqr, a); // 0.5 t^3 * [ (-1*p1) + ( 3*p2) + (-3*p3) + p4 ]
VectorScale(p2, tSqrSqr * 3, b);
VectorScale(p3, tSqrSqr * -3, c);
VectorScale(p4, tSqrSqr, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 2
VectorScale(p1, tSqr * 2, a); // 0.5 t^2 * [ ( 2*p1) + (-5*p2) + ( 4*p3) - p4 ]
VectorScale(p2, tSqr * -5, b);
VectorScale(p3, tSqr * 4, c);
VectorScale(p4, -tSqr, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 3
VectorScale(p1, -t, a); // 0.5 t * [ (-1*p1) + p3 ]
VectorScale(p3, t, b);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
// matrix row 4
VectorAdd(p2, output, output); // p2
}
void Catmull_Rom_Spline_Tangent(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float tOne = 3 * t * t * 0.5f;
float tTwo = 2 * t * 0.5f;
float tThree = 0.5;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
output.Init();
Vector3D a, b, c, d;
// matrix row 1
VectorScale(p1, -tOne, a); // 0.5 t^3 * [ (-1*p1) + ( 3*p2) + (-3*p3) + p4 ]
VectorScale(p2, tOne * 3, b);
VectorScale(p3, tOne * -3, c);
VectorScale(p4, tOne, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 2
VectorScale(p1, tTwo * 2, a); // 0.5 t^2 * [ ( 2*p1) + (-5*p2) + ( 4*p3) - p4 ]
VectorScale(p2, tTwo * -5, b);
VectorScale(p3, tTwo * 4, c);
VectorScale(p4, -tTwo, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 3
VectorScale(p1, -tThree, a); // 0.5 t * [ (-1*p1) + p3 ]
VectorScale(p3, tThree, b);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
}
// area under the curve [0..t]
void Catmull_Rom_Spline_Integral(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
output = p2 * t
- 0.25f * (p1 - p3) * t * t
+ (1.0f / 6.0f) * (2.0f * p1 - 5.0f * p2 + 4.0f * p3 - p4) * t * t * t
- 0.125f * (p1 - 3.0f * p2 + 3.0f * p3 - p4) * t * t * t * t;
}
// area under the curve [0..1]
void Catmull_Rom_Spline_Integral(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
Vector3D& output)
{
output = (-0.25f * p1 + 3.25f * p2 + 3.25f * p3 - 0.25f * p4) * (1.0f / 6.0f);
}
void Catmull_Rom_Spline_Normalize(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3
float dt = p3.DistTo(p2);
Vector3D p1n, p4n;
VectorSubtract(p1, p2, p1n);
VectorSubtract(p4, p3, p4n);
VectorNormalize(p1n);
VectorNormalize(p4n);
VectorMA(p2, dt, p1n, p1n);
VectorMA(p3, dt, p4n, p4n);
Catmull_Rom_Spline(p1n, p2, p3, p4n, t, output);
}
void Catmull_Rom_Spline_Integral_Normalize(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3
float dt = p3.DistTo(p2);
Vector3D p1n, p4n;
VectorSubtract(p1, p2, p1n);
VectorSubtract(p4, p3, p4n);
VectorNormalize(p1n);
VectorNormalize(p4n);
VectorMA(p2, dt, p1n, p1n);
VectorMA(p3, dt, p4n, p4n);
Catmull_Rom_Spline_Integral(p1n, p2, p3, p4n, t, output);
}
void Catmull_Rom_Spline_NormalizeX(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Vector3D p1n, p4n;
Spline_Normalize(p1, p2, p3, p4, p1n, p4n);
Catmull_Rom_Spline(p1n, p2, p3, p4n, t, output);
}
#endif // !defined(__SPU__)
//-----------------------------------------------------------------------------
// Purpose: basic hermite spline. t = 0 returns p1, t = 1 returns p2,
// d1 and d2 are used to entry and exit slope of curve
// Input :
//-----------------------------------------------------------------------------
void Hermite_Spline(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& d1,
const Vector3D& d2,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float tSqr = t * t;
float tCube = t * tSqr;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &d1);
Assert(&output != &d2);
float b1 = 2.0f * tCube - 3.0f * tSqr + 1.0f;
float b2 = 1.0f - b1; // -2*tCube+3*tSqr;
float b3 = tCube - 2 * tSqr + t;
float b4 = tCube - tSqr;
VectorScale(p1, b1, output);
VectorMA(output, b2, p2, output);
VectorMA(output, b3, d1, output);
VectorMA(output, b4, d2, output);
}
float Hermite_Spline(
float p1,
float p2,
float d1,
float d2,
float t)
{
Assert(s_bMathlibInitialized);
float output;
float tSqr = t * t;
float tCube = t * tSqr;
float b1 = 2.0f * tCube - 3.0f * tSqr + 1.0f;
float b2 = 1.0f - b1; // -2*tCube+3*tSqr;
float b3 = tCube - 2 * tSqr + t;
float b4 = tCube - tSqr;
output = p1 * b1;
output += p2 * b2;
output += d1 * b3;
output += d2 * b4;
return output;
}
void Hermite_SplineBasis(float t, float basis[4])
{
float tSqr = t * t;
float tCube = t * tSqr;
basis[0] = 2.0f * tCube - 3.0f * tSqr + 1.0f;
basis[1] = 1.0f - basis[0]; // -2*tCube+3*tSqr;
basis[2] = tCube - 2 * tSqr + t;
basis[3] = tCube - tSqr;
}
//-----------------------------------------------------------------------------
// Purpose: simple three data point hermite spline.
// t = 0 returns p1, t = 1 returns p2,
// slopes are generated from the p0->p1 and p1->p2 segments
// this is reasonable C1 method when there's no "p3" data yet.
// Input :
//-----------------------------------------------------------------------------
// BUG: the VectorSubtract()'s calls go away if the global optimizer is enabled
#if !defined(__SPU__)
#pragma optimize( "g", off )
#endif
void Hermite_Spline(const Vector3D& p0, const Vector3D& p1, const Vector3D& p2, float t, Vector3D& output)
{
Vector3D e10, e21;
VectorSubtract(p1, p0, e10);
VectorSubtract(p2, p1, e21);
Hermite_Spline(p1, p2, e10, e21, t, output);
}
#if !defined(__SPU__)
#pragma optimize( "", on )
#endif
float Hermite_Spline(float p0, float p1, float p2, float t)
{
return Hermite_Spline(p1, p2, p1 - p0, p2 - p1, t);
}
void Hermite_Spline(const Quaternion& q0, const Quaternion& q1, const Quaternion& q2, float t, Quaternion& output)
{
// cheap, hacked version of quaternions
Quaternion q0a;
Quaternion q1a;
QuaternionAlign(q2, q0, q0a);
QuaternionAlign(q2, q1, q1a);
output.x = Hermite_Spline(q0a.x, q1a.x, q2.x, t);
output.y = Hermite_Spline(q0a.y, q1a.y, q2.y, t);
output.z = Hermite_Spline(q0a.z, q1a.z, q2.z, t);
output.w = Hermite_Spline(q0a.w, q1a.w, q2.w, t);
QuaternionNormalize(output);
}
#if !defined(__SPU__)
// See http://en.wikipedia.org/wiki/Kochanek-Bartels_curves
//
// Tension: -1 = Round -> 1 = Tight
// Bias: -1 = Pre-shoot (bias left) -> 1 = Post-shoot (bias right)
// Continuity: -1 = Box corners -> 1 = Inverted corners
//
// If T=B=C=0 it's the same matrix as Catmull-Rom.
// If T=1 & B=C=0 it's the same as Cubic.
// If T=B=0 & C=-1 it's just linear interpolation
//
// See http://news.povray.org/povray.binaries.tutorials/attachment/%3CXns91B880592482seed7@povray.org%3E/Splines.bas.txt
// for example code and descriptions of various spline types...
//
void Kochanek_Bartels_Spline(
float tension,
float bias,
float continuity,
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float ffa, ffb, ffc, ffd;
ffa = (1.0f - tension) * (1.0f + continuity) * (1.0f + bias);
ffb = (1.0f - tension) * (1.0f - continuity) * (1.0f - bias);
ffc = (1.0f - tension) * (1.0f - continuity) * (1.0f + bias);
ffd = (1.0f - tension) * (1.0f + continuity) * (1.0f - bias);
float tSqr = t * t * 0.5f;
float tSqrSqr = t * tSqr;
t *= 0.5f;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
output.Init();
Vector3D a, b, c, d;
// matrix row 1
VectorScale(p1, tSqrSqr * -ffa, a);
VectorScale(p2, tSqrSqr * (4.0f + ffa - ffb - ffc), b);
VectorScale(p3, tSqrSqr * (-4.0f + ffb + ffc - ffd), c);
VectorScale(p4, tSqrSqr * ffd, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 2
VectorScale(p1, tSqr * 2 * ffa, a);
VectorScale(p2, tSqr * (-6 - 2 * ffa + 2 * ffb + ffc), b);
VectorScale(p3, tSqr * (6 - 2 * ffb - ffc + ffd), c);
VectorScale(p4, tSqr * -ffd, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 3
VectorScale(p1, t * -ffa, a);
VectorScale(p2, t * (ffa - ffb), b);
VectorScale(p3, t * ffb, c);
// p4 unchanged
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
// matrix row 4
// p1, p3, p4 unchanged
// p2 is multiplied by 1 and added, so just added it directly
VectorAdd(p2, output, output);
}
void Kochanek_Bartels_Spline_NormalizeX(
float tension,
float bias,
float continuity,
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Vector3D p1n, p4n;
Spline_Normalize(p1, p2, p3, p4, p1n, p4n);
Kochanek_Bartels_Spline(tension, bias, continuity, p1n, p2, p3, p4n, t, output);
}
void Cubic_Spline(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float tSqr = t * t;
float tSqrSqr = t * tSqr;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
#ifdef NDEBUG
NOTE_UNUSED(p1);
NOTE_UNUSED(p2);
NOTE_UNUSED(p3);
NOTE_UNUSED(p4);
#endif // NDEBUG
output.Init();
Vector3D a, b, c, d;
// matrix row 1
VectorScale(p2, tSqrSqr * 2, b);
VectorScale(p3, tSqrSqr * -2, c);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
// matrix row 2
VectorScale(p2, tSqr * -3, b);
VectorScale(p3, tSqr * 3, c);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
// matrix row 3
// no influence
// p4 unchanged
// matrix row 4
// p1, p3, p4 unchanged
VectorAdd(p2, output, output);
}
void Cubic_Spline_NormalizeX(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Vector3D p1n, p4n;
Spline_Normalize(p1, p2, p3, p4, p1n, p4n);
Cubic_Spline(p1n, p2, p3, p4n, t, output);
}
void BSpline(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float oneOver6 = 1.0f / 6.0f;
float tSqr = t * t * oneOver6;
float tSqrSqr = t * tSqr;
t *= oneOver6;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
output.Init();
Vector3D a, b, c, d;
// matrix row 1
VectorScale(p1, -tSqrSqr, a);
VectorScale(p2, tSqrSqr * 3.0f, b);
VectorScale(p3, tSqrSqr * -3.0f, c);
VectorScale(p4, tSqrSqr, d);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
VectorAdd(d, output, output);
// matrix row 2
VectorScale(p1, tSqr * 3.0f, a);
VectorScale(p2, tSqr * -6.0f, b);
VectorScale(p3, tSqr * 3.0f, c);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
// matrix row 3
VectorScale(p1, t * -3.0f, a);
VectorScale(p3, t * 3.0f, c);
// p4 unchanged
VectorAdd(a, output, output);
VectorAdd(c, output, output);
// matrix row 4
// p1 and p3 scaled by 1.0f, so done below
VectorScale(p1, oneOver6, a);
VectorScale(p2, 4.0f * oneOver6, b);
VectorScale(p3, oneOver6, c);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
}
void BSpline_NormalizeX(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Vector3D p1n, p4n;
Spline_Normalize(p1, p2, p3, p4, p1n, p4n);
BSpline(p1n, p2, p3, p4n, t, output);
}
void Parabolic_Spline(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Assert(s_bMathlibInitialized);
float tSqr = t * t * 0.5f;
t *= 0.5f;
Assert(&output != &p1);
Assert(&output != &p2);
Assert(&output != &p3);
Assert(&output != &p4);
#ifdef NDEBUG
NOTE_UNUSED(p4);
#endif // NDEBUG
output.Init();
Vector3D a, b, c, d;
// matrix row 1
// no influence from t cubed
// matrix row 2
VectorScale(p1, tSqr, a);
VectorScale(p2, tSqr * -2.0f, b);
VectorScale(p3, tSqr, c);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
VectorAdd(c, output, output);
// matrix row 3
VectorScale(p1, t * -2.0f, a);
VectorScale(p2, t * 2.0f, b);
// p4 unchanged
VectorAdd(a, output, output);
VectorAdd(b, output, output);
// matrix row 4
VectorScale(p1, 0.5f, a);
VectorScale(p2, 0.5f, b);
VectorAdd(a, output, output);
VectorAdd(b, output, output);
}
void Parabolic_Spline_NormalizeX(
const Vector3D& p1,
const Vector3D& p2,
const Vector3D& p3,
const Vector3D& p4,
float t,
Vector3D& output)
{
Vector3D p1n, p4n;
Spline_Normalize(p1, p2, p3, p4, p1n, p4n);
Parabolic_Spline(p1n, p2, p3, p4n, t, output);
}
//-----------------------------------------------------------------------------
// Cubic Bernstein basis functions
// http://mathworld.wolfram.com/BernsteinPolynomial.html
//
// Purpose: Evaluate the cubic Bernstein basis for the input parametric coordinate.
// Output is the coefficient for that basis polynomial.
//-----------------------------------------------------------------------------
float CubicBasis0(float t)
{
float invT = 1.0f - t;
return invT * invT * invT;
}
float CubicBasis1(float t)
{
float invT = 1.0f - t;
return 3.0f * t * invT * invT;
}
float CubicBasis2(float t)
{
float invT = 1.0f - t;
return 3.0f * t * t * invT;
}
float CubicBasis3(float t)
{
return t * t * t;
}
//-----------------------------------------------------------------------------
// Purpose: Compress the input values for a ranged result such that from 75% to 200% smoothly of the range maps
//-----------------------------------------------------------------------------
float RangeCompressor(float flValue, float flMin, float flMax, float flBase)
{
// clamp base
if (flBase < flMin)
flBase = flMin;
if (flBase > flMax)
flBase = flMax;
flValue += flBase;
// convert to 0 to 1 value
float flMid = (flValue - flMin) / (flMax - flMin);
// convert to -1 to 1 value
float flTarget = flMid * 2 - 1;
if (fabs(flTarget) > 0.75)
{
float t = (fabs(flTarget) - 0.75) / (1.25);
if (t < 1.0)
{
if (flTarget > 0)
{
flTarget = Hermite_Spline(0.75, 1, 0.75, 0, t);
}
else
{
flTarget = -Hermite_Spline(0.75, 1, 0.75, 0, t);
}
}
else
{
flTarget = (flTarget > 0) ? 1.0f : -1.0f;
}
}
flMid = (flTarget + 1) / 2.0;
flValue = flMin * (1 - flMid) + flMax * flMid;
flValue -= flBase;
return flValue;
}
//#pragma optimize( "", on )
//-----------------------------------------------------------------------------
// Transforms a AABB into another space; which will inherently grow the box.
//-----------------------------------------------------------------------------
void TransformAABB(const matrix3x4_t& transform, const Vector3D& vecMinsIn, const Vector3D& vecMaxsIn, Vector3D& vecMinsOut, Vector3D& vecMaxsOut)
{
Vector3D localCenter;
VectorAdd(vecMinsIn, vecMaxsIn, localCenter);
localCenter *= 0.5f;
Vector3D localExtents;
VectorSubtract(vecMaxsIn, localCenter, localExtents);
Vector3D worldCenter;
VectorTransform(localCenter, transform, worldCenter);
Vector3D worldExtents;
worldExtents.x = DotProductAbs(localExtents, transform[0]);
worldExtents.y = DotProductAbs(localExtents, transform[1]);
worldExtents.z = DotProductAbs(localExtents, transform[2]);
VectorSubtract(worldCenter, worldExtents, vecMinsOut);
VectorAdd(worldCenter, worldExtents, vecMaxsOut);
// sanity check
Assert(vecMinsOut.LengthSqr() + vecMaxsOut.LengthSqr() < 1e+12);
}
//-----------------------------------------------------------------------------
// Uses the inverse transform of in1
//-----------------------------------------------------------------------------
void ITransformAABB(const matrix3x4_t& transform, const Vector3D& vecMinsIn, const Vector3D& vecMaxsIn, Vector3D& vecMinsOut, Vector3D& vecMaxsOut)
{
Vector3D worldCenter;
VectorAdd(vecMinsIn, vecMaxsIn, worldCenter);
worldCenter *= 0.5f;
Vector3D worldExtents;
VectorSubtract(vecMaxsIn, worldCenter, worldExtents);
Vector3D localCenter;
VectorITransform(worldCenter, transform, localCenter);
Vector3D localExtents;
localExtents.x = FloatMakePositive(worldExtents.x * transform[0][0]) +
FloatMakePositive(worldExtents.y * transform[1][0]) +
FloatMakePositive(worldExtents.z * transform[2][0]);
localExtents.y = FloatMakePositive(worldExtents.x * transform[0][1]) +
FloatMakePositive(worldExtents.y * transform[1][1]) +
FloatMakePositive(worldExtents.z * transform[2][1]);
localExtents.z = FloatMakePositive(worldExtents.x * transform[0][2]) +
FloatMakePositive(worldExtents.y * transform[1][2]) +
FloatMakePositive(worldExtents.z * transform[2][2]);
VectorSubtract(localCenter, localExtents, vecMinsOut);
VectorAdd(localCenter, localExtents, vecMaxsOut);
}
//-----------------------------------------------------------------------------
// Rotates a AABB into another space; which will inherently grow the box.
// (same as TransformAABB, but doesn't take the translation into account)
//-----------------------------------------------------------------------------
void RotateAABB(const matrix3x4_t& transform, const Vector3D& vecMinsIn, const Vector3D& vecMaxsIn, Vector3D& vecMinsOut, Vector3D& vecMaxsOut)
{
Vector3D localCenter;
VectorAdd(vecMinsIn, vecMaxsIn, localCenter);
localCenter *= 0.5f;
Vector3D localExtents;
VectorSubtract(vecMaxsIn, localCenter, localExtents);
Vector3D newCenter;
VectorRotate(localCenter, transform, newCenter);
Vector3D newExtents;
newExtents.x = DotProductAbs(localExtents, transform[0]);
newExtents.y = DotProductAbs(localExtents, transform[1]);
newExtents.z = DotProductAbs(localExtents, transform[2]);
VectorSubtract(newCenter, newExtents, vecMinsOut);
VectorAdd(newCenter, newExtents, vecMaxsOut);
}
//-----------------------------------------------------------------------------
// Uses the inverse transform of in1
//-----------------------------------------------------------------------------
void IRotateAABB(const matrix3x4_t& transform, const Vector3D& vecMinsIn, const Vector3D& vecMaxsIn, Vector3D& vecMinsOut, Vector3D& vecMaxsOut)
{
Vector3D oldCenter;
VectorAdd(vecMinsIn, vecMaxsIn, oldCenter);
oldCenter *= 0.5f;
Vector3D oldExtents;
VectorSubtract(vecMaxsIn, oldCenter, oldExtents);
Vector3D newCenter;
VectorIRotate(oldCenter, transform, newCenter);
Vector3D newExtents;
newExtents.x = FloatMakePositive(oldExtents.x * transform[0][0]) +
FloatMakePositive(oldExtents.y * transform[1][0]) +
FloatMakePositive(oldExtents.z * transform[2][0]);
newExtents.y = FloatMakePositive(oldExtents.x * transform[0][1]) +
FloatMakePositive(oldExtents.y * transform[1][1]) +
FloatMakePositive(oldExtents.z * transform[2][1]);
newExtents.z = FloatMakePositive(oldExtents.x * transform[0][2]) +
FloatMakePositive(oldExtents.y * transform[1][2]) +
FloatMakePositive(oldExtents.z * transform[2][2]);
VectorSubtract(newCenter, newExtents, vecMinsOut);
VectorAdd(newCenter, newExtents, vecMaxsOut);
}
float CalcSqrDistanceToAABB(const Vector3D& mins, const Vector3D& maxs, const Vector3D& 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;
}
void CalcClosestPointOnAABB(const Vector3D& mins, const Vector3D& maxs, const Vector3D& point, Vector3D& closestOut)
{
closestOut.x = clamp(point.x, mins.x, maxs.x);
closestOut.y = clamp(point.y, mins.y, maxs.y);
closestOut.z = clamp(point.z, mins.z, maxs.z);
}
void CalcSqrDistAndClosestPointOnAABB(const Vector3D& mins, const Vector3D& maxs, const Vector3D& point, Vector3D& closestOut, float& distSqrOut)
{
distSqrOut = 0.0f;
for (int i = 0; i < 3; i++)
{
if (point[i] < mins[i])
{
closestOut[i] = mins[i];
float flDelta = closestOut[i] - mins[i];
distSqrOut += flDelta * flDelta;
}
else if (point[i] > maxs[i])
{
closestOut[i] = maxs[i];
float flDelta = closestOut[i] - maxs[i];
distSqrOut += flDelta * flDelta;
}
else
{
closestOut[i] = point[i];
}
}
}
float CalcClosestPointToLineT(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, Vector3D& vDir)
{
Assert(s_bMathlibInitialized);
VectorSubtract(vLineB, vLineA, vDir);
// D dot [P - (A + D*t)] = 0
// t = ( DP - DA) / DD
float div = vDir.Dot(vDir);
if (div < 0.00001f)
{
return 0;
}
else
{
return (vDir.Dot(P) - vDir.Dot(vLineA)) / div;
}
}
void CalcClosestPointOnLine(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, Vector3D& vClosest, float* outT)
{
Assert(s_bMathlibInitialized);
Vector3D vDir;
float t = CalcClosestPointToLineT(P, vLineA, vLineB, vDir);
if (outT) *outT = t;
vClosest.MulAdd(vLineA, vDir, t);
}
float CalcDistanceToLine(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector3D vClosest;
CalcClosestPointOnLine(P, vLineA, vLineB, vClosest, outT);
return P.DistTo(vClosest);
}
float CalcDistanceSqrToLine(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector3D vClosest;
CalcClosestPointOnLine(P, vLineA, vLineB, vClosest, outT);
return P.DistToSqr(vClosest);
}
void CalcClosestPointOnLineSegment(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, Vector3D& vClosest, float* outT)
{
Vector3D vDir;
float t = CalcClosestPointToLineT(P, vLineA, vLineB, vDir);
t = clamp(static_cast<int>(t), 0, 1);
if (outT)
{
*outT = t;
}
vClosest.MulAdd(vLineA, vDir, t);
}
float CalcDistanceToLineSegment(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector3D vClosest;
CalcClosestPointOnLineSegment(P, vLineA, vLineB, vClosest, outT);
return P.DistTo(vClosest);
}
float CalcDistanceSqrToLineSegment(const Vector3D& P, const Vector3D& vLineA, const Vector3D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector3D vClosest;
CalcClosestPointOnLineSegment(P, vLineA, vLineB, vClosest, outT);
return P.DistToSqr(vClosest);
}
float CalcClosestPointToLineT2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, Vector2D& vDir)
{
Assert(s_bMathlibInitialized);
Vector2DSubtract(vLineB, vLineA, vDir);
// D dot [P - (A + D*t)] = 0
// t = (DP - DA) / DD
float div = vDir.Dot(vDir);
if (div < 0.00001f)
{
return 0;
}
else
{
return (vDir.Dot(P) - vDir.Dot(vLineA)) / div;
}
}
void CalcClosestPointOnLine2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, Vector2D& vClosest, float* outT)
{
Assert(s_bMathlibInitialized);
Vector2D vDir;
float t = CalcClosestPointToLineT2D(P, vLineA, vLineB, vDir);
if (outT) *outT = t;
vClosest.MulAdd(vLineA, vDir, t);
}
float CalcDistanceToLine2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector2D vClosest;
CalcClosestPointOnLine2D(P, vLineA, vLineB, vClosest, outT);
return P.DistTo(vClosest);
}
float CalcDistanceSqrToLine2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector2D vClosest;
CalcClosestPointOnLine2D(P, vLineA, vLineB, vClosest, outT);
return P.DistToSqr(vClosest);
}
void CalcClosestPointOnLineSegment2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, Vector2D& vClosest, float* outT)
{
Vector2D vDir;
float t = CalcClosestPointToLineT2D(P, vLineA, vLineB, vDir);
t = clamp(static_cast<int>(t), 0, 1);
if (outT)
{
*outT = t;
}
vClosest.MulAdd(vLineA, vDir, t);
}
float CalcDistanceToLineSegment2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector2D vClosest;
CalcClosestPointOnLineSegment2D(P, vLineA, vLineB, vClosest, outT);
return P.DistTo(vClosest);
}
float CalcDistanceSqrToLineSegment2D(const Vector2D& P, const Vector2D& vLineA, const Vector2D& vLineB, float* outT)
{
Assert(s_bMathlibInitialized);
Vector2D vClosest;
CalcClosestPointOnLineSegment2D(P, vLineA, vLineB, vClosest, outT);
return P.DistToSqr(vClosest);
}
// Do we have another epsilon we could use
#define LINE_EPS ( 0.000001f )
//-----------------------------------------------------------------------------
// Purpose: Given lines p1->p2 and p3->p4, computes a line segment (pa->pb) and returns the parameters 0->1 multipliers
// along each segment for the returned points
// Input : p1 -
// p2 -
// p3 -
// p4 -
// *s1 -
// *s2 -
// Output : Returns true on success, false on failure.
//-----------------------------------------------------------------------------
bool CalcLineToLineIntersectionSegment(
const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p4, Vector3D* s1, Vector3D* s2,
float* t1, float* t2)
{
Vector3D p13, p43, p21;
float d1343, d4321, d1321, d4343, d2121;
float numer, denom;
p13.x = p1.x - p3.x;
p13.y = p1.y - p3.y;
p13.z = p1.z - p3.z;
p43.x = p4.x - p3.x;
p43.y = p4.y - p3.y;
p43.z = p4.z - p3.z;
if (fabs(p43.x) < LINE_EPS && fabs(p43.y) < LINE_EPS && fabs(p43.z) < LINE_EPS)
return false;
p21.x = p2.x - p1.x;
p21.y = p2.y - p1.y;
p21.z = p2.z - p1.z;
if (fabs(p21.x) < LINE_EPS && fabs(p21.y) < LINE_EPS && fabs(p21.z) < LINE_EPS)
return false;
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
denom = d2121 * d4343 - d4321 * d4321;
if (fabs(denom) < LINE_EPS)
return false;
numer = d1343 * d4321 - d1321 * d4343;
*t1 = numer / denom;
*t2 = (d1343 + d4321 * (*t1)) / d4343;
if (s1 != NULL && s2 != NULL)
{
s1->x = p1.x + *t1 * p21.x;
s1->y = p1.y + *t1 * p21.y;
s1->z = p1.z + *t1 * p21.z;
s2->x = p3.x + *t2 * p43.x;
s2->y = p3.y + *t2 * p43.y;
s2->z = p3.z + *t2 * p43.z;
}
return true;
}
#pragma optimize( "", off )
#ifndef EXCEPTION_EXECUTE_HANDLER
#define EXCEPTION_EXECUTE_HANDLER 1
#endif
#pragma optimize( "", on )
#ifndef NDEBUG
volatile static char const* pDebugString;
#endif
void MathLib_Init(float gamma, float texGamma, float brightness, int overbright)
{
if (s_bMathlibInitialized)
return;
#ifdef _WIN32
Assert(_rotl(0xC7654321, 1) == 0x8ECA8643);
Assert(_rotl64(0xC7654321ABCDEF00ull, 1) == 0x8ECA8643579BDE01ull);
#endif
#ifndef NDEBUG
pDebugString = "mathlib.lib built debug!";
#endif
// FIXME: Hook SSE into VectorAligned + Vector4DAligned
#if !defined( _GAMECONSOLE )
// Grab the processor information:
const CPUInformation& pi = GetCPUInformation();
if (!(pi.m_bSSE && pi.m_bSSE2))
{
Assert(0);
if (MessageBoxA(NULL, "SSE and SSE2 are required.", "Unsupported CPU", MB_ICONERROR | MB_OK))
{
TerminateProcess(GetCurrentProcess(), EXIT_FAILURE);
}
}
#endif //!360
s_bMathlibInitialized = true;
InitSinCosTable();
BuildGammaTable(gamma, texGamma, brightness, overbright);
SeedRandSIMD(0x31415926);
}
bool MathLib_MMXEnabled(void)
{
Assert(s_bMathlibInitialized);
return true;
}
bool MathLib_SSEEnabled(void)
{
Assert(s_bMathlibInitialized);
return true;
}
bool MathLib_SSE2Enabled(void)
{
Assert(s_bMathlibInitialized);
return true;
}
// BUGBUG: Why doesn't this call angle diff?!?!?
float ApproachAngle(float target, float value, float speed)
{
target = anglemod(target);
value = anglemod(value);
float delta = target - value;
// Speed is assumed to be positive
if (speed < 0)
speed = -speed;
if (delta < -180)
delta += 360;
else if (delta > 180)
delta -= 360;
if (delta > speed)
value += speed;
else if (delta < -speed)
value -= speed;
else
value = target;
return value;
}
// BUGBUG: Why do we need both of these?
float AngleDiff(float destAngle, float srcAngle)
{
float delta;
delta = fmodf(destAngle - srcAngle, 360.0f);
if (destAngle > srcAngle)
{
if (delta >= 180)
delta -= 360;
}
else
{
if (delta <= -180)
delta += 360;
}
return delta;
}
float AngleDistance(float next, float cur)
{
float delta = next - cur;
if (delta < -180)
delta += 360;
else if (delta > 180)
delta -= 360;
return delta;
}
float AngleNormalize(float angle)
{
angle = fmodf(angle, 360.0f);
if (angle > 180)
{
angle -= 360;
}
if (angle < -180)
{
angle += 360;
}
return angle;
}
//--------------------------------------------------------------------------------------------------------------
// ensure that 0 <= angle <= 360
float AngleNormalizePositive(float angle)
{
angle = fmodf(angle, 360.0f);
if (angle < 0.0f)
{
angle += 360.0f;
}
return angle;
}
//--------------------------------------------------------------------------------------------------------------
bool AnglesAreEqual(float a, float b, float tolerance)
{
return (fabs(AngleDiff(a, b)) < tolerance);
}
void RotationDeltaAxisAngle(const QAngle& srcAngles, const QAngle& destAngles, Vector3D& deltaAxis, float& deltaAngle)
{
Quaternion srcQuat, destQuat, srcQuatInv, out;
AngleQuaternion(srcAngles, srcQuat);
AngleQuaternion(destAngles, destQuat);
QuaternionScale(srcQuat, -1, srcQuatInv);
QuaternionMult(destQuat, srcQuatInv, out);
QuaternionNormalize(out);
QuaternionAxisAngle(out, deltaAxis, deltaAngle);
}
void RotationDelta(const QAngle& srcAngles, const QAngle& destAngles, QAngle* out)
{
matrix3x4_t src, srcInv;
matrix3x4_t dest;
AngleMatrix(srcAngles, src);
AngleMatrix(destAngles, dest);
// xform = src(-1) * dest
MatrixInvert(src, srcInv);
matrix3x4_t xform;
ConcatTransforms(dest, srcInv, xform);
QAngle xformAngles;
MatrixAngles(xform, xformAngles);
if (out)
{
*out = xformAngles;
}
}
void ClipLineSegmentToPlane(const Vector3D& vNormal, const Vector3D& vPlanePoint, Vector3D* p1, Vector3D* p2, float flBias)
{
float flDot1, flDot2;
flDot1 = (*p1 - vPlanePoint).Dot(vNormal) + flBias;
flDot2 = (*p2 - vPlanePoint).Dot(vNormal) + flBias;
if (flDot1 >= 0 && flDot2 >= 0)
{
return;
}
if (flDot1 >= 0)
{
Vector3D vRay = *p2 - *p1;
*p2 = *p1 + vRay * flDot1 / (flDot1 - flDot2);
}
else if (flDot2 >= 0)
{
Vector3D vRay = *p1 - *p2;
*p1 = *p2 + vRay * flDot2 / (flDot2 - flDot1);
}
else
{
*p1 = vec3_invalid;
*p2 = vec3_invalid;
}
}
//-----------------------------------------------------------------------------
// Purpose: Computes a triangle normal
//-----------------------------------------------------------------------------
void ComputeTrianglePlane(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3, Vector3D& normal, float& intercept)
{
Vector3D e1, e2;
VectorSubtract(v2, v1, e1);
VectorSubtract(v3, v1, e2);
CrossProduct(e1, e2, normal);
VectorNormalize(normal);
intercept = DotProduct(normal, v1);
}
//-----------------------------------------------------------------------------
// Purpose: Calculate the volume of a tetrahedron with these vertices
// Input : p0 - points of tetrahedron
// p1 -
// p2 -
// p3 -
// Output : float (volume in units^3)
//-----------------------------------------------------------------------------
float TetrahedronVolume(const Vector3D& p0, const Vector3D& p1, const Vector3D& p2, const Vector3D& p3)
{
Vector3D a, b, c, cross;
float volume = 1.0f / 6.0f;
a = p1 - p0;
b = p2 - p0;
c = p3 - p0;
cross = CrossProduct(b, c);
volume *= DotProduct(a, cross);
if (volume < 0)
return -volume;
return volume;
}
// computes the area of a triangle given three verts
float TriangleArea(const Vector3D& v0, const Vector3D& v1, const Vector3D& v2)
{
Vector3D vecEdge0, vecEdge1, vecCross;
VectorSubtract(v1, v0, vecEdge0);
VectorSubtract(v2, v0, vecEdge1);
CrossProduct(vecEdge0, vecEdge1, vecCross);
return (VectorLength(vecCross) * 0.5f);
}
//-----------------------------------------------------------------------------
// Purpose: This is a clone of BaseWindingForPlane()
// Input : *pOutVerts - an array of preallocated verts to build the polygon in
// normal - the plane normal
// dist - the plane constant
// Output : int - vert count (always 4)
//-----------------------------------------------------------------------------
int PolyFromPlane(Vector3D* pOutVerts, const Vector3D& normal, float dist, float fHalfScale)
{
int i, x;
vec_t max, v;
Vector3D org, vright, vup;
// find the major axis
max = -16384; //MAX_COORD_INTEGER
x = -1;
for (i = 0; i < 3; i++)
{
v = fabs(normal[i]);
if (v > max)
{
x = i;
max = v;
}
}
if (x == -1)
return 0;
// Build a unit vector along something other than the major axis
VectorCopy(vec3_origin, vup);
switch (x)
{
case 0:
case 1:
vup[2] = 1;
break;
case 2:
vup[0] = 1;
break;
}
// Remove the component of this vector along the normal
v = DotProduct(vup, normal);
VectorMA(vup, -v, normal, vup);
// Make it a unit (perpendicular)
VectorNormalize(vup);
// Center of the poly is at normal * dist
VectorScale(normal, dist, org);
// Calculate the third orthonormal basis vector for our plane space (this one and vup are in the plane)
CrossProduct(vup, normal, vright);
// Make the plane's basis vectors big (these are the half-sides of the polygon we're making)
VectorScale(vup, fHalfScale, vup);
VectorScale(vright, fHalfScale, vright);
// Move diagonally away from org to create the corner verts
VectorSubtract(org, vright, pOutVerts[0]); // left
VectorAdd(pOutVerts[0], vup, pOutVerts[0]); // up
VectorAdd(org, vright, pOutVerts[1]); // right
VectorAdd(pOutVerts[1], vup, pOutVerts[1]); // up
VectorAdd(org, vright, pOutVerts[2]); // right
VectorSubtract(pOutVerts[2], vup, pOutVerts[2]); // down
VectorSubtract(org, vright, pOutVerts[3]); // left
VectorSubtract(pOutVerts[3], vup, pOutVerts[3]); // down
// The four corners form a planar quadrilateral normal to "normal"
return 4;
}
// Returns void as it was impossible for the function to returns anything other than 4.
// Any absolute of a floating value will always return a number greater than -16384. That test seemed bogus.
void PolyFromPlane_SIMD(fltx4* pOutVerts, const fltx4& plane, float fHalfScale)
{
// So we need to find the biggest component of all three,
// And depending of the value, we need to build a unit vector along something that is not the major axis.
fltx4 f4Abs = AbsSIMD(plane);
fltx4 x = SplatXSIMD(f4Abs);
fltx4 y = SplatYSIMD(f4Abs);
fltx4 z = SplatZSIMD(f4Abs);
fltx4 max = MaxSIMD(x, y);
max = MaxSIMD(max, z);
// Simplify the code, if Z is the biggest component, we will use 1 0 0.
// If X or Y are the biggest, we will use 0 0 1.
bi32x4 fIsMax = CmpEqSIMD(max, f4Abs); // isMax will be set for the components that are the max
fltx4 fIsZMax = SplatZSIMD((fltx4)fIsMax); // 0 if Z is not the max, 0xffffffff is Z is the max
// And depending if Z is max or not, we are going to select one unit vector or the other
fltx4 vup = MaskedAssign((bi32x4)fIsZMax, g_SIMD_Identity[0], g_SIMD_Identity[2]);
fltx4 normal = SetWToZeroSIMD(plane);
fltx4 dist = SplatWSIMD(plane);
// Remove the component of this vector along the normal
fltx4 v = Dot3SIMD(vup, normal);
vup = MaddSIMD(-v, normal, vup);
// Make it a unit (perpendicular)
vup = Normalized3SIMD(vup);
// Center of the poly is at normal * dist
fltx4 org = MulSIMD(dist, normal);
// Calculate the third orthonormal basis vector for our plane space (this one and vup are in the plane)
fltx4 vright = CrossProductSIMD(vup, normal);
// Make the plane's basis vectors big (these are the half-sides of the polygon we're making)
fltx4 f4HalfScale = ReplicateX4(fHalfScale);
vup = MulSIMD(f4HalfScale, vup);
vright = MulSIMD(f4HalfScale, vright);
// Move diagonally away from org to create the corner verts
fltx4 vleft = SubSIMD(org, vright);
vright = AddSIMD(org, vright);
pOutVerts[0] = AddSIMD(vleft, vup); // left + up
pOutVerts[1] = AddSIMD(vright, vup); // right + up
pOutVerts[2] = SubSIMD(vright, vup); // right + down
pOutVerts[3] = SubSIMD(vleft, vup); // left + down
}
//-----------------------------------------------------------------------------
// Purpose: clip a poly to the plane and return the poly on the front side of the plane
// Input : *inVerts - input polygon
// vertCount - # verts in input poly
// *outVerts - destination poly
// normal - plane normal
// dist - plane constant
// Output : int - # verts in output poly
//-----------------------------------------------------------------------------
int ClipPolyToPlane(Vector3D* inVerts, int vertCount, Vector3D* outVerts, const Vector3D& normal, float dist, float fOnPlaneEpsilon)
{
vec_t* dists = (vec_t*)stackalloc(sizeof(vec_t) * vertCount * 4); //4x vertcount should cover all cases
int* sides = (int*)stackalloc(sizeof(vec_t) * vertCount * 4);
int counts[3];
vec_t dot;
int i, j;
Vector3D mid = vec3_origin;
int outCount;
counts[0] = counts[1] = counts[2] = 0;
// determine sides for each point
for (i = 0; i < vertCount; i++)
{
dot = DotProduct(inVerts[i], normal) - dist;
dists[i] = dot;
if (dot > fOnPlaneEpsilon)
{
sides[i] = SIDE_FRONT;
}
else if (dot < -fOnPlaneEpsilon)
{
sides[i] = SIDE_BACK;
}
else
{
sides[i] = SIDE_ON;
}
counts[sides[i]]++;
}
sides[i] = sides[0];
dists[i] = dists[0];
if (!counts[0])
return 0;
if (!counts[1])
{
// Copy to output verts
for (i = 0; i < vertCount; i++)
{
VectorCopy(inVerts[i], outVerts[i]);
}
return vertCount;
}
outCount = 0;
for (i = 0; i < vertCount; i++)
{
Vector3D& p1 = inVerts[i];
if (sides[i] == SIDE_ON)
{
VectorCopy(p1, outVerts[outCount]);
outCount++;
continue;
}
if (sides[i] == SIDE_FRONT)
{
VectorCopy(p1, outVerts[outCount]);
outCount++;
}
if (sides[i + 1] == SIDE_ON || sides[i + 1] == sides[i])
continue;
// generate a split point
Vector3D& p2 = inVerts[(i + 1) % vertCount];
dot = dists[i] / (dists[i] - dists[i + 1]);
for (j = 0; j < 3; j++)
{ // avoid round off error when possible
if (normal[j] == 1)
mid[j] = dist;
else if (normal[j] == -1)
mid[j] = -dist;
else
mid[j] = p1[j] + dot * (p2[j] - p1[j]);
}
VectorCopy(mid, outVerts[outCount]);
outCount++;
}
return outCount;
}
int ClipPolyToPlane_SIMD(fltx4* pInVerts, int nVertCount, fltx4* pOutVerts, const fltx4& plane, float fOnPlaneEpsilon)
{
vec_t* dists = (vec_t*)stackalloc(sizeof(vec_t) * nVertCount * 4); //4* nVertCount should cover all cases
uint8* sides = (uint8*)stackalloc(sizeof(uint8) * nVertCount * 4);
int i;
/*
* It seems something could be done here... Especially in relation with the code below i, i + 1, etc...
fltx4 f4OnPlaneEpsilonP = ReplicateX4( fOnPlaneEpsilon );
fltx4 f4OnPlaneEpsilonM = -f4OnPlaneEpsilonP;
Also we could store the full fltx4 instead of a single float. It would avoid doing a SubFloat() here,
and a ReplicateX4() later. Trading off potential LHS against L2 cache misses?
*/
// determine sides for each point
int nAllSides = 0;
fltx4 f4Dist = SplatWSIMD(plane);
for (i = 0; i < nVertCount; i++)
{
// dot = DotProduct( pInVerts[i], normal) - dist;
fltx4 dot = Dot3SIMD(pInVerts[i], plane);
dot = SubSIMD(dot, f4Dist);
float fDot = SubFloat(dot, 0);
dists[i] = fDot;
// Look how to update sides with a branch-less version
int nSide = OR_SIDE_ON;
if (fDot > fOnPlaneEpsilon)
{
nSide = OR_SIDE_FRONT;
}
else if (fDot < -fOnPlaneEpsilon)
{
nSide = OR_SIDE_BACK;
}
sides[i] = nSide;
nAllSides |= nSide;
}
sides[i] = sides[0];
dists[i] = dists[0];
// Shortcuts (either completely clipped or not clipped at all)
if ((nAllSides & OR_SIDE_FRONT) == 0)
{
return 0; // Completely clipped
}
if ((nAllSides & OR_SIDE_BACK) == 0)
{
// Not clipped at all, copy to output verts
Assert(i == nVertCount);
int nIndex = 0;
while (i >= 4)
{
pOutVerts[nIndex] = pInVerts[nIndex];
pOutVerts[nIndex + 1] = pInVerts[nIndex + 1];
pOutVerts[nIndex + 2] = pInVerts[nIndex + 2];
pOutVerts[nIndex + 3] = pInVerts[nIndex + 3];
nIndex += 4;
i -= 4;
}
while (i > 0)
{
pOutVerts[nIndex] = pInVerts[nIndex];
++nIndex;
--i;
}
return nVertCount;
}
fltx4 f4one = Four_Ones;
fltx4 f4MOne = -f4one;
fltx4 f4OneMask = (fltx4)CmpEqSIMD(plane, f4one);
fltx4 f4mOneMask = (fltx4)CmpEqSIMD(plane, f4MOne);
fltx4 f4AllMask = OrSIMD(f4OneMask, f4mOneMask); // 0xffffffff where normal was 1 or -1, 0 otherwise
f4OneMask = AndSIMD(f4OneMask, f4Dist); // Dist where normal.* was 1
f4mOneMask = AndSIMD(f4mOneMask, -f4Dist); // -Dist where normal.* was -1
fltx4 f4AllValue = OrSIMD(f4OneMask, f4mOneMask); // Dist and -Dist where normal.* was 1 and -1
// f4AllMask and f4AllValue will be used together (to override the default calculation).
int nOutCount = 0;
for (i = 0; i < nVertCount; i++)
{
const fltx4& p1 = pInVerts[i];
if (sides[i] == OR_SIDE_ON)
{
pOutVerts[nOutCount++] = p1;
continue;
}
if (sides[i] == OR_SIDE_FRONT)
{
pOutVerts[nOutCount++] = p1;
}
if (sides[i + 1] == OR_SIDE_ON || sides[i + 1] == sides[i])
continue;
// generate a split point
fltx4& p2 = pInVerts[(i + 1) % nVertCount];
float fDot = dists[i] / (dists[i] - dists[i + 1]);
fltx4 f4Dot = ReplicateX4(fDot);
// mid[j] = v1[j] + dot*(v2[j]-v1[j]); - For j=0...2
fltx4 f4Result = MaddSIMD(f4Dot, SubSIMD(p2, p1), p1);
// If normal.* is 1, it should be dist, if -1, it should be -dist, otherwise it should be mid[j] = v1[j] + dot*(v2[j]-v1[j]);
fltx4 mid = MaskedAssign((bi32x4)f4AllMask, f4AllValue, f4Result);
pOutVerts[nOutCount++] = mid;
}
return nOutCount;
}
int ClipPolyToPlane_Precise(double* inVerts, int vertCount, double* outVerts, const double* normal, double dist, double fOnPlaneEpsilon)
{
double* dists = (double*)stackalloc(sizeof(double) * vertCount * 4); //4x vertcount should cover all cases
int* sides = (int*)stackalloc(sizeof(double) * vertCount * 4);
int counts[3];
double dot;
int i, j;
//Vector mid = vec3_origin;
double mid[3];
mid[0] = 0.0;
mid[1] = 0.0;
mid[2] = 0.0;
int outCount;
counts[0] = counts[1] = counts[2] = 0;
// determine sides for each point
for (i = 0; i < vertCount; i++)
{
//dot = DotProduct( inVerts[i], normal) - dist;
dot = ((inVerts[i * 3 + 0] * normal[0]) + (inVerts[i * 3 + 1] * normal[1]) + (inVerts[i * 3 + 2] * normal[2])) - dist;
dists[i] = dot;
if (dot > fOnPlaneEpsilon)
{
sides[i] = SIDE_FRONT;
}
else if (dot < -fOnPlaneEpsilon)
{
sides[i] = SIDE_BACK;
}
else
{
sides[i] = SIDE_ON;
}
counts[sides[i]]++;
}
sides[i] = sides[0];
dists[i] = dists[0];
if (!counts[0])
return 0;
if (!counts[1])
{
// Copy to output verts
//for ( i = 0; i < vertCount; i++ )
for (i = 0; i < vertCount * 3; i++)
{
//VectorCopy( inVerts[i], outVerts[i] );
outVerts[i] = inVerts[i];
}
return vertCount;
}
outCount = 0;
for (i = 0; i < vertCount; i++)
{
//Vector& p1 = inVerts[i];
double* p1 = &inVerts[i * 3];
//p1[0] = inVerts[i*3 + 0];
//p1[1] = inVerts[i*3 + 1];
//p1[2] = inVerts[i*3 + 2];
if (sides[i] == SIDE_ON)
{
//VectorCopy( p1, outVerts[outCount]);
outVerts[outCount * 3 + 0] = p1[0];
outVerts[outCount * 3 + 1] = p1[1];
outVerts[outCount * 3 + 2] = p1[2];
outCount++;
continue;
}
if (sides[i] == SIDE_FRONT)
{
//VectorCopy( p1, outVerts[outCount]);
outVerts[outCount * 3 + 0] = p1[0];
outVerts[outCount * 3 + 1] = p1[1];
outVerts[outCount * 3 + 2] = p1[2];
outCount++;
}
if (sides[i + 1] == SIDE_ON || sides[i + 1] == sides[i])
continue;
// generate a split point
//Vector& p2 = inVerts[(i+1)%vertCount];
int wrappedindex = (i + 1) % vertCount;
double* p2 = &inVerts[wrappedindex * 3];
//p2[0] = inVerts[wrappedindex*3 + 0];
//p2[1] = inVerts[wrappedindex*3 + 1];
//p2[2] = inVerts[wrappedindex*3 + 2];
dot = dists[i] / (dists[i] - dists[i + 1]);
for (j = 0; j < 3; j++)
{
mid[j] = (double)p1[j] + dot * ((double)p2[j] - (double)p1[j]);
}
//VectorCopy (mid, outVerts[outCount]);
outVerts[outCount * 3 + 0] = mid[0];
outVerts[outCount * 3 + 1] = mid[1];
outVerts[outCount * 3 + 2] = mid[2];
outCount++;
}
return outCount;
}
int CeilPow2(int in)
{
int retval;
retval = 1;
while (retval < in)
retval <<= 1;
return retval;
}
int FloorPow2(int in)
{
int retval;
retval = 1;
while (retval < in)
retval <<= 1;
return retval >> 1;
}
//-----------------------------------------------------------------------------
// Computes Y fov from an X fov and a screen aspect ratio
//-----------------------------------------------------------------------------
float CalcFovY(float flFovX, float flAspect)
{
if (flFovX < 1 || flFovX > 179)
{
flFovX = 90; // error, set to 90
}
// The long, but illustrative version (more closely matches CShaderAPIDX8::PerspectiveX, which
// is what it's based on).
//
//float width = 2 * zNear * tan( DEG2RAD( fov_x / 2.0 ) );
//float height = width / screenaspect;
//float yRadians = atan( (height/2.0) / zNear );
//return RAD2DEG( yRadians ) * 2;
// The short and sweet version.
float val = atan(tan(DEG2RAD(flFovX) * 0.5f) / flAspect);
val = RAD2DEG(val) * 2.0f;
return val;
}
float CalcFovX(float flFovY, float flAspect)
{
return RAD2DEG(atan(tan(DEG2RAD(flFovY) * 0.5f) * flAspect)) * 2.0f;
}
#endif // !defined(__SPU__)
#if !defined(__SPU__)
//-----------------------------------------------------------------------------
// Generate a frustum based on perspective view parameters
//-----------------------------------------------------------------------------
void GeneratePerspectiveFrustum(const Vector3D& origin, const Vector3D& forward,
const Vector3D& right, const Vector3D& up, float flZNear, float flZFar,
float flFovX, float flFovY, VPlane* pPlanesOut)
{
float flIntercept = DotProduct(origin, forward);
// Setup the near and far planes.
pPlanesOut[FRUSTUM_FARZ].Init(-forward, -flZFar - flIntercept);
pPlanesOut[FRUSTUM_NEARZ].Init(forward, flZNear + flIntercept);
flFovX *= 0.5f;
flFovY *= 0.5f;
float flTanX = tan(DEG2RAD(flFovX));
float flTanY = tan(DEG2RAD(flFovY));
// OPTIMIZE: Normalizing these planes is not necessary for culling
Vector3D normalPos, normalNeg;
VectorMA(right, flTanX, forward, normalPos);
VectorMA(normalPos, -2.0f, right, normalNeg);
VectorNormalize(normalPos);
VectorNormalize(normalNeg);
pPlanesOut[FRUSTUM_LEFT].Init(normalPos, normalPos.Dot(origin));
pPlanesOut[FRUSTUM_RIGHT].Init(normalNeg, normalNeg.Dot(origin));
VectorMA(up, flTanY, forward, normalPos);
VectorMA(normalPos, -2.0f, up, normalNeg);
VectorNormalize(normalPos);
VectorNormalize(normalNeg);
pPlanesOut[FRUSTUM_BOTTOM].Init(normalPos, normalPos.Dot(origin));
pPlanesOut[FRUSTUM_TOP].Init(normalNeg, normalNeg.Dot(origin));
}
//-----------------------------------------------------------------------------
// Generate a frustum based on orthographic parameters
//-----------------------------------------------------------------------------
void GenerateOrthoFrustum(const Vector3D& origin, const Vector3D& forward, const Vector3D& right, const Vector3D& up, float flLeft, float flRight, float flBottom, float flTop, float flZNear, float flZFar, VPlane* pPlanesOut)
{
float flIntercept = DotProduct(origin, forward);
pPlanesOut[FRUSTUM_NEARZ].Init(forward, flZNear + flIntercept);
pPlanesOut[FRUSTUM_FARZ].Init(-forward, -flZFar - flIntercept);
flIntercept = DotProduct(origin, right);
pPlanesOut[FRUSTUM_RIGHT].Init(-right, -flRight - flIntercept);
pPlanesOut[FRUSTUM_LEFT].Init(right, flLeft + flIntercept);
flIntercept = DotProduct(origin, up);
pPlanesOut[FRUSTUM_BOTTOM].Init(up, flBottom + flIntercept);
pPlanesOut[FRUSTUM_TOP].Init(-up, -flTop - flIntercept);
}
//-----------------------------------------------------------------------------
// Version that accepts angles instead of vectors
//-----------------------------------------------------------------------------
void GeneratePerspectiveFrustum(const Vector3D& origin, const QAngle& angles, float flZNear, float flZFar, float flFovX, float flAspectRatio, Frustum_t& frustum)
{
VPlane planes[FRUSTUM_NUMPLANES];
Vector3D vecForward, vecRight, vecUp;
AngleVectors(angles, &vecForward, &vecRight, &vecUp);
float flFovY = CalcFovY(flFovX, flAspectRatio);
GeneratePerspectiveFrustum(origin, vecForward, vecRight, vecUp, flZNear, flZFar, flFovX, flFovY, planes);
frustum.SetPlanes(planes);
}
void fourplanes_t::ComputeSignbits()
{
xSign = CmpLtSIMD(nX, Four_Zeros);
ySign = CmpLtSIMD(nY, Four_Zeros);
zSign = CmpLtSIMD(nZ, Four_Zeros);
nXAbs = fabs(nX);
nYAbs = fabs(nY);
nZAbs = fabs(nZ);
}
void fourplanes_t::GetPlane(int index, Vector3D* pNormalOut, float* pDistOut) const
{
pNormalOut->x = SubFloat(nX, index);
pNormalOut->y = SubFloat(nY, index);
pNormalOut->z = SubFloat(nZ, index);
*pDistOut = SubFloat(dist, index);
}
void fourplanes_t::SetPlane(int index, const Vector3D& vecNormal, float planeDist)
{
SubFloat(nX, index) = vecNormal.x;
SubFloat(nY, index) = vecNormal.y;
SubFloat(nZ, index) = vecNormal.z;
SubFloat(dist, index) = planeDist;
ComputeSignbits();
}
void fourplanes_t::Set4Planes(const VPlane* pPlanes)
{
nX = LoadUnalignedSIMD(&pPlanes[0].m_Normal.x);
nY = LoadUnalignedSIMD(&pPlanes[1].m_Normal.x);
nZ = LoadUnalignedSIMD(&pPlanes[2].m_Normal.x);
dist = LoadUnalignedSIMD(&pPlanes[3].m_Normal.x);
TransposeSIMD(nX, nY, nZ, dist);
ComputeSignbits();
}
void fourplanes_t::Set2Planes(const VPlane* pPlanes)
{
nX = LoadUnalignedSIMD(&pPlanes[0].m_Normal.x);
nY = LoadUnalignedSIMD(&pPlanes[1].m_Normal.x);
nZ = Four_Zeros;
dist = Four_Zeros;
TransposeSIMD(nX, nY, nZ, dist);
ComputeSignbits();
}
void fourplanes_t::Get4Planes(VPlane* pPlanesOut) const
{
fltx4 p0 = nX;
fltx4 p1 = nY;
fltx4 p2 = nZ;
fltx4 p3 = dist;
TransposeSIMD(p0, p1, p2, p3);
StoreUnalignedSIMD(&pPlanesOut[0].m_Normal.x, p0);
StoreUnalignedSIMD(&pPlanesOut[1].m_Normal.x, p1);
StoreUnalignedSIMD(&pPlanesOut[2].m_Normal.x, p2);
StoreUnalignedSIMD(&pPlanesOut[3].m_Normal.x, p3);
}
void fourplanes_t::Get2Planes(VPlane* pPlanesOut) const
{
fltx4 p0 = nX;
fltx4 p1 = nY;
fltx4 p2 = nZ;
fltx4 p3 = dist;
TransposeSIMD(p0, p1, p2, p3);
StoreUnalignedSIMD(&pPlanesOut[0].m_Normal.x, p0);
StoreUnalignedSIMD(&pPlanesOut[1].m_Normal.x, p1);
}
Frustum_t::Frustum_t()
{
memset(this, 0, sizeof(*this));
}
void Frustum_t::SetPlane(int i, const Vector3D& vecNormal, float dist)
{
if (i < 4)
{
planes[0].SetPlane(i, vecNormal, dist);
}
else
{
planes[1].SetPlane(i - 4, vecNormal, dist);
}
}
void Frustum_t::GetPlane(int i, Vector3D* pNormalOut, float* pDistOut) const
{
if (i < 4)
{
planes[0].GetPlane(i, pNormalOut, pDistOut);
}
else
{
planes[1].GetPlane(i - 4, pNormalOut, pDistOut);
}
}
void Frustum_t::SetPlanes(const VPlane* pPlanes)
{
planes[0].Set4Planes(pPlanes);
planes[1].Set2Planes(pPlanes + 4);
}
void Frustum_t::GetPlanes(VPlane* pPlanesOut) const
{
planes[0].Get4Planes(pPlanesOut);
planes[1].Get2Planes(pPlanesOut + 4);
}
bool Frustum_t::CullBox(const Vector3D& mins, const Vector3D& maxs) const
{
fltx4 mins4 = LoadUnalignedSIMD(&mins.x);
fltx4 minx = SplatXSIMD(mins4);
fltx4 miny = SplatYSIMD(mins4);
fltx4 minz = SplatZSIMD(mins4);
fltx4 maxs4 = LoadUnalignedSIMD(&maxs.x);
fltx4 maxx = SplatXSIMD(maxs4);
fltx4 maxy = SplatYSIMD(maxs4);
fltx4 maxz = SplatZSIMD(maxs4);
// compute the dot product of the normal and the farthest corner
// dotBack0 = DotProduct( normal, normals.x < 0 ? mins.x : maxs.x );
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = MulSIMD(planes[i].nX, MaskedAssign(planes[i].xSign, minx, maxx));
fltx4 yTotalBack = MulSIMD(planes[i].nY, MaskedAssign(planes[i].ySign, miny, maxy));
fltx4 zTotalBack = MulSIMD(planes[i].nZ, MaskedAssign(planes[i].zSign, minz, maxz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
if (IsVector4LessThan(dotBack, planes[i].dist))
return true;
}
return false;
}
bool Frustum_t::CullBox(const fltx4& mins4, const fltx4& maxs4) const
{
fltx4 minx = SplatXSIMD(mins4);
fltx4 miny = SplatYSIMD(mins4);
fltx4 minz = SplatZSIMD(mins4);
fltx4 maxx = SplatXSIMD(maxs4);
fltx4 maxy = SplatYSIMD(maxs4);
fltx4 maxz = SplatZSIMD(maxs4);
// compute the dot product of the normal and the farthest corner
// dotBack0 = DotProduct( normal, normals.x < 0 ? mins.x : maxs.x );
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = MulSIMD(planes[i].nX, MaskedAssign(planes[i].xSign, minx, maxx));
fltx4 yTotalBack = MulSIMD(planes[i].nY, MaskedAssign(planes[i].ySign, miny, maxy));
fltx4 zTotalBack = MulSIMD(planes[i].nZ, MaskedAssign(planes[i].zSign, minz, maxz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
if (IsVector4LessThan(dotBack, planes[i].dist))
return true;
}
return false;
}
bool Frustum_t::CullBoxCenterExtents(const Vector3D& center, const Vector3D& extents) const
{
fltx4 center4 = LoadUnalignedSIMD(&center.x);
fltx4 centerx = SplatXSIMD(center4);
fltx4 centery = SplatYSIMD(center4);
fltx4 centerz = SplatZSIMD(center4);
fltx4 extents4 = LoadUnalignedSIMD(&extents.x);
fltx4 extx = SplatXSIMD(extents4);
fltx4 exty = SplatYSIMD(extents4);
fltx4 extz = SplatZSIMD(extents4);
// compute the dot product of the normal and the farthest corner
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = AddSIMD(MulSIMD(planes[i].nX, centerx), MulSIMD(planes[i].nXAbs, extx));
fltx4 yTotalBack = AddSIMD(MulSIMD(planes[i].nY, centery), MulSIMD(planes[i].nYAbs, exty));
fltx4 zTotalBack = AddSIMD(MulSIMD(planes[i].nZ, centerz), MulSIMD(planes[i].nZAbs, extz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
if (IsVector4LessThan(dotBack, planes[i].dist))
return true;
}
return false;
}
bool Frustum_t::CullBoxCenterExtents(const fltx4& fl4Center, const fltx4& fl4Extents) const
{
fltx4 centerx = SplatXSIMD(fl4Center);
fltx4 centery = SplatYSIMD(fl4Center);
fltx4 centerz = SplatZSIMD(fl4Center);
fltx4 extx = SplatXSIMD(fl4Extents);
fltx4 exty = SplatYSIMD(fl4Extents);
fltx4 extz = SplatZSIMD(fl4Extents);
// compute the dot product of the normal and the farthest corner
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = AddSIMD(MulSIMD(planes[i].nX, centerx), MulSIMD(planes[i].nXAbs, extx));
fltx4 yTotalBack = AddSIMD(MulSIMD(planes[i].nY, centery), MulSIMD(planes[i].nYAbs, exty));
fltx4 zTotalBack = AddSIMD(MulSIMD(planes[i].nZ, centerz), MulSIMD(planes[i].nZAbs, extz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
if (IsVector4LessThan(dotBack, planes[i].dist))
return true;
}
return false;
}
// Return true if this bounding volume is contained in the frustum, false if it is not
// TODO SIMDIFY
bool Frustum_t::Contains(const Vector3D& mins, const Vector3D& maxs) const
{
// Get box corners
Vector3D vCorners[8];
vCorners[0] = mins;
vCorners[1] = Vector3D(mins.x, mins.y, maxs.z);
vCorners[2] = Vector3D(mins.x, maxs.y, mins.z);
vCorners[3] = Vector3D(mins.x, maxs.y, maxs.z);
vCorners[4] = Vector3D(maxs.x, mins.y, mins.z);
vCorners[5] = Vector3D(maxs.x, mins.y, maxs.z);
vCorners[6] = Vector3D(maxs.x, maxs.y, mins.z);
vCorners[7] = maxs;
// if we are in with all points, then we are fully in
for (int j = 0; j < FRUSTUM_NUMPLANES; ++j)
{
for (int i = 0; i < 8; ++i)
{
// compute the dot product of the normal and the corner
Vector3D vNormal;
float dist;
GetPlane(i, &vNormal, &dist);
if (DotProduct(vCorners[j], vNormal) <= 0)
{
return false;
}
}
}
return true; // all pts were inside
}
// Brute force SAT frustum intersection between two frustums
bool Frustum_t::Intersects(Frustum_t& otherFrustum) const
{
Vector3D pPointsA[8];
bool bResult = false;
bResult = GetCorners(pPointsA);
Assert(bResult);
VPlane pPlanesA[FRUSTUM_NUMPLANES];
GetPlanes(pPlanesA);
Vector3D pPointsB[8];
bResult = otherFrustum.GetCorners(pPointsB);
Assert(bResult);
VPlane pPlanesB[FRUSTUM_NUMPLANES];
otherFrustum.GetPlanes(pPlanesB);
NOTE_UNUSED(bResult);
// See if all points in B are on one side of any plane in A
for (int p = 0; p < 6; ++p)
{
bool bPointsOnOutside = true;
for (int i = 0; i < 8; ++i)
{
float flDist = pPlanesA[p].DistTo(pPointsB[i]);
// If dist is pos, we are not on the outside
if (flDist > 0)
{
bPointsOnOutside = false;
break;
}
}
// We never hit a negative case, we have a separating axis
if (bPointsOnOutside)
{
return false;
}
}
// See if all points in A are on one side of any plane in B
for (int p = 0; p < 6; ++p)
{
bool bPointsOnOutside = true;
for (int i = 0; i < 8; ++i)
{
float flDist = pPlanesB[p].DistTo(pPointsA[i]);
// If dist is pos, we are not on the outside
if (flDist > 0)
{
bPointsOnOutside = false;
break;
}
}
// We never hit a negative case, we have a separating axis
if (bPointsOnOutside)
{
return false;
}
}
// They intersect
return true;
}
// Return true if this bounding volume intersects the frustum, false if it is outside
bool Frustum_t::Intersects(const Vector3D& mins, const Vector3D& maxs) const
{
fltx4 mins4 = LoadUnalignedSIMD(&mins.x);
fltx4 minx = SplatXSIMD(mins4);
fltx4 miny = SplatYSIMD(mins4);
fltx4 minz = SplatZSIMD(mins4);
fltx4 maxs4 = LoadUnalignedSIMD(&maxs.x);
fltx4 maxx = SplatXSIMD(maxs4);
fltx4 maxy = SplatYSIMD(maxs4);
fltx4 maxz = SplatZSIMD(maxs4);
// compute the dot product of the normal and the farthest corner
// dotBack0 = DotProduct( normal, normals.x < 0 ? mins.x : maxs.x );
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = MulSIMD(planes[i].nX, MaskedAssign(planes[i].xSign, minx, maxx));
fltx4 yTotalBack = MulSIMD(planes[i].nY, MaskedAssign(planes[i].ySign, miny, maxy));
fltx4 zTotalBack = MulSIMD(planes[i].nZ, MaskedAssign(planes[i].zSign, minz, maxz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
#if _X360
if (!XMVector3GreaterOrEqual(dotBack, planes[i].dist))
return false;
#elif defined( _PS3 )
bi32x4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#else
fltx4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#endif
}
return true;
}
bool Frustum_t::Intersects(const fltx4& mins4, const fltx4& maxs4) const
{
fltx4 minx = SplatXSIMD(mins4);
fltx4 miny = SplatYSIMD(mins4);
fltx4 minz = SplatZSIMD(mins4);
fltx4 maxx = SplatXSIMD(maxs4);
fltx4 maxy = SplatYSIMD(maxs4);
fltx4 maxz = SplatZSIMD(maxs4);
// compute the dot product of the normal and the farthest corner
// dotBack0 = DotProduct( normal, normals.x < 0 ? mins.x : maxs.x );
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = MulSIMD(planes[i].nX, MaskedAssign(planes[i].xSign, minx, maxx));
fltx4 yTotalBack = MulSIMD(planes[i].nY, MaskedAssign(planes[i].ySign, miny, maxy));
fltx4 zTotalBack = MulSIMD(planes[i].nZ, MaskedAssign(planes[i].zSign, minz, maxz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
#if _X360
if (!XMVector4GreaterOrEqual(dotBack, planes[i].dist))
return false;
#elif defined( _PS3 )
bi32x4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#else
fltx4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#endif
}
return true;
}
bool Frustum_t::IntersectsCenterExtents(const Vector3D& center, const Vector3D& extents) const
{
fltx4 center4 = LoadUnalignedSIMD(&center.x);
fltx4 centerx = SplatXSIMD(center4);
fltx4 centery = SplatYSIMD(center4);
fltx4 centerz = SplatZSIMD(center4);
fltx4 extents4 = LoadUnalignedSIMD(&extents.x);
fltx4 extx = SplatXSIMD(extents4);
fltx4 exty = SplatYSIMD(extents4);
fltx4 extz = SplatZSIMD(extents4);
// compute the dot product of the normal and the farthest corner
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = AddSIMD(MulSIMD(planes[i].nX, centerx), MulSIMD(planes[i].nXAbs, extx));
fltx4 yTotalBack = AddSIMD(MulSIMD(planes[i].nY, centery), MulSIMD(planes[i].nYAbs, exty));
fltx4 zTotalBack = AddSIMD(MulSIMD(planes[i].nZ, centerz), MulSIMD(planes[i].nZAbs, extz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
#if _X360
if (!XMVector4GreaterOrEqual(dotBack, planes[i].dist))
return false;
#elif defined( _PS3 )
bi32x4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#else
fltx4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#endif
}
return true;
}
bool Frustum_t::IntersectsCenterExtents(const fltx4& fl4Center, const fltx4& fl4Extents) const
{
fltx4 centerx = SplatXSIMD(fl4Center);
fltx4 centery = SplatYSIMD(fl4Center);
fltx4 centerz = SplatZSIMD(fl4Center);
fltx4 extx = SplatXSIMD(fl4Extents);
fltx4 exty = SplatYSIMD(fl4Extents);
fltx4 extz = SplatZSIMD(fl4Extents);
// compute the dot product of the normal and the farthest corner
for (int i = 0; i < 2; i++)
{
fltx4 xTotalBack = AddSIMD(MulSIMD(planes[i].nX, centerx), MulSIMD(planes[i].nXAbs, extx));
fltx4 yTotalBack = AddSIMD(MulSIMD(planes[i].nY, centery), MulSIMD(planes[i].nYAbs, exty));
fltx4 zTotalBack = AddSIMD(MulSIMD(planes[i].nZ, centerz), MulSIMD(planes[i].nZAbs, extz));
fltx4 dotBack = AddSIMD(xTotalBack, AddSIMD(yTotalBack, zTotalBack));
// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
#if _X360
if (!XMVector3GreaterOrEqual(dotBack, planes[i].dist))
return false;
#elif defined( _PS3 )
bi32x4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#else
fltx4 isOut = CmpLtSIMD(dotBack, planes[i].dist);
if (IsAnyNegative(isOut))
return false;
#endif
}
return true;
}
//-----------------------------------------------------------------------------
// Generate a frustum based on orthographic parameters
//-----------------------------------------------------------------------------
void GenerateOrthoFrustumFLU(const Vector3D& origin, const Vector3D& forward, const Vector3D& vLeft, const Vector3D& up, float flLeft, float flRight, float flBottom, float flTop, float flZNear, float flZFar, VPlane* pPlanesOut)
{
// YUP_ACTIVE: FIXME : This is actually producing incorrect planes (see the VectorMA below)
Vector3D vRight = vLeft;
vRight *= -1.0f;
float flIntercept = DotProduct(origin, forward);
pPlanesOut[FRUSTUM_NEARZ].Init(forward, flZNear + flIntercept);
pPlanesOut[FRUSTUM_FARZ].Init(-forward, -flZFar - flIntercept);
flIntercept = DotProduct(origin, vRight);
pPlanesOut[FRUSTUM_RIGHT].Init(-vRight, -flRight - flIntercept);
pPlanesOut[FRUSTUM_LEFT].Init(vRight, flLeft + flIntercept);
flIntercept = DotProduct(origin, up);
pPlanesOut[FRUSTUM_BOTTOM].Init(up, flBottom + flIntercept);
pPlanesOut[FRUSTUM_TOP].Init(-up, -flTop - flIntercept);
}
//-----------------------------------------------------------------------------
// Generate a frustum based on perspective view parameters
//-----------------------------------------------------------------------------
void GeneratePerspectiveFrustumFLU(const Vector3D& origin, const Vector3D& forward,
const Vector3D& vLeft, const Vector3D& up, float flZNear, float flZFar,
float flFovX, float flAspect, VPlane* pPlanesOut)
{
// YUP_ACTIVE: FIXME : This is actually producing incorrect planes (see the VectorMA below)
Vector3D vRight = vLeft;
vRight *= -1.0f;
float flIntercept = DotProduct(origin, forward);
// Setup the near and far planes.
pPlanesOut[FRUSTUM_FARZ].Init(-forward, -flZFar - flIntercept);
pPlanesOut[FRUSTUM_NEARZ].Init(forward, flZNear + flIntercept);
flFovX *= 0.5f;
float flTanX = tan(DEG2RAD(flFovX));
float flTanY = flTanX / flAspect;
// OPTIMIZE: Normalizing these planes is not necessary for culling
Vector3D normalPos, normalNeg;
// NOTE: This should be using left and not right to produce correct planes, not changing it quite yet
// because I'm not able to test whether fixing this breaks anything.
VectorMA(vRight, flTanX, forward, normalPos);
VectorMA(normalPos, -2.0f, vRight, normalNeg);
VectorNormalize(normalPos);
VectorNormalize(normalNeg);
pPlanesOut[FRUSTUM_LEFT].Init(normalPos, normalPos.Dot(origin));
pPlanesOut[FRUSTUM_RIGHT].Init(normalNeg, normalNeg.Dot(origin));
VectorMA(up, flTanY, forward, normalPos);
VectorMA(normalPos, -2.0f, up, normalNeg);
VectorNormalize(normalPos);
VectorNormalize(normalNeg);
pPlanesOut[FRUSTUM_BOTTOM].Init(normalPos, normalPos.Dot(origin));
pPlanesOut[FRUSTUM_TOP].Init(normalNeg, normalNeg.Dot(origin));
}
// Generate a frustum based on perspective view parameters
void Frustum_t::CreatePerspectiveFrustumFLU(const Vector3D& vOrigin, const Vector3D& vForward,
const Vector3D& vLeft, const Vector3D& vUp, float flZNear, float flZFar,
float flFovX, float flAspect)
{
VPlane frustumPlanes[FRUSTUM_NUMPLANES];
GeneratePerspectiveFrustumFLU(vOrigin, vForward, vLeft, vUp, flZNear, flZFar, flFovX, flAspect, frustumPlanes);
SetPlanes(frustumPlanes);
}
//#ifndef YUP_ACTIVE
void Frustum_t::CreatePerspectiveFrustum(const Vector3D& origin, const Vector3D& forward,
const Vector3D& right, const Vector3D& up, float flZNear, float flZFar,
float flFovX, float flAspect)
{
Vector3D vLeft = right;
vLeft *= -1.0f;
CreatePerspectiveFrustumFLU(origin, forward, vLeft, up, flZNear, flZFar, flFovX, flAspect);
}
//#endif
// Version that accepts angles instead of vectors
void Frustum_t::CreatePerspectiveFrustum(const Vector3D& origin, const QAngle& angles, float flZNear, float flZFar, float flFovX, float flAspectRatio)
{
VPlane frustumPlanes[FRUSTUM_NUMPLANES];
Vector3D vecForward, vecLeft, vecUp;
AngleVectorsFLU(angles, &vecForward, &vecLeft, &vecUp);
GeneratePerspectiveFrustumFLU(origin, vecForward, vecLeft, vecUp, flZNear, flZFar, flFovX, flAspectRatio, frustumPlanes);
SetPlanes(frustumPlanes);
}
// Generate a frustum based on orthographic parameters
void Frustum_t::CreateOrthoFrustumFLU(const Vector3D& origin, const Vector3D& forward, const Vector3D& vLeft, const Vector3D& up, float flLeft, float flRight, float flBottom, float flTop, float flZNear, float flZFar)
{
VPlane frustumPlanes[FRUSTUM_NUMPLANES];
GenerateOrthoFrustumFLU(origin, forward, vLeft, up, flLeft, flRight, flBottom, flTop, flZNear, flZFar, frustumPlanes);
SetPlanes(frustumPlanes);
}
//#ifndef YUP_ACTIVE
void Frustum_t::CreateOrthoFrustum(const Vector3D& origin, const Vector3D& forward, const Vector3D& right, const Vector3D& up, float flLeft, float flRight, float flBottom, float flTop, float flZNear, float flZFar)
{
Vector3D vLeft = right;
vLeft *= -1.0f;
CreateOrthoFrustumFLU(origin, forward, vLeft, up, flLeft, flRight, flBottom, flTop, flZNear, flZFar);
}
// The points returned correspond to the corners of the frustum faces
// Points 0 to 3 correspond to the near face
// Points 4 to 7 correspond to the far face
// Returns points in a face in this order:
// 2--3
// | |
// 0--1
bool Frustum_t::GetCorners(Vector3D* pPoints) const
{
VPlane frustumPlanes[FRUSTUM_NUMPLANES];
GetPlanes(frustumPlanes);
// Near face
// Bottom Left
if (!PlaneIntersection(frustumPlanes[FRUSTUM_NEARZ], frustumPlanes[FRUSTUM_LEFT], frustumPlanes[FRUSTUM_BOTTOM], pPoints[0]))
return false;
// Bottom right
if (!PlaneIntersection(frustumPlanes[FRUSTUM_NEARZ], frustumPlanes[FRUSTUM_RIGHT], frustumPlanes[FRUSTUM_BOTTOM], pPoints[1]))
return false;
// Upper Left
if (!PlaneIntersection(frustumPlanes[FRUSTUM_NEARZ], frustumPlanes[FRUSTUM_LEFT], frustumPlanes[FRUSTUM_TOP], pPoints[2]))
return false;
// Upper right
if (!PlaneIntersection(frustumPlanes[FRUSTUM_NEARZ], frustumPlanes[FRUSTUM_RIGHT], frustumPlanes[FRUSTUM_TOP], pPoints[3]))
return false;
// Far face
// Bottom Left
if (!PlaneIntersection(frustumPlanes[FRUSTUM_FARZ], frustumPlanes[FRUSTUM_LEFT], frustumPlanes[FRUSTUM_BOTTOM], pPoints[4]))
return false;
// Bottom right
if (!PlaneIntersection(frustumPlanes[FRUSTUM_FARZ], frustumPlanes[FRUSTUM_RIGHT], frustumPlanes[FRUSTUM_BOTTOM], pPoints[5]))
return false;
// Upper Left
if (!PlaneIntersection(frustumPlanes[FRUSTUM_FARZ], frustumPlanes[FRUSTUM_LEFT], frustumPlanes[FRUSTUM_TOP], pPoints[6]))
return false;
// Upper right
if (!PlaneIntersection(frustumPlanes[FRUSTUM_FARZ], frustumPlanes[FRUSTUM_RIGHT], frustumPlanes[FRUSTUM_TOP], pPoints[7]))
return false;
return true;
}
// NOTE: This routine was taken (and modified) from NVidia's BlinnReflection demo
// Creates basis vectors, based on a vertex and index list.
// See the NVidia white paper 'GDC2K PerPixel Lighting' for a description
// of how this computation works
#define SMALL_FLOAT 1e-12
void CalcTriangleTangentSpace(const Vector3D& p0, const Vector3D& p1, const Vector3D& p2,
const Vector2D& t0, const Vector2D& t1, const Vector2D& t2,
Vector3D& sVect, Vector3D& tVect)
{
/* Compute the partial derivatives of X, Y, and Z with respect to S and T. */
sVect.Init(0.0f, 0.0f, 0.0f);
tVect.Init(0.0f, 0.0f, 0.0f);
// x, s, t
Vector3D edge01(p1.x - p0.x, t1.x - t0.x, t1.y - t0.y);
Vector3D edge02(p2.x - p0.x, t2.x - t0.x, t2.y - t0.y);
Vector3D cross;
CrossProduct(edge01, edge02, cross);
if (fabs(cross.x) > SMALL_FLOAT)
{
sVect.x += -cross.y / cross.x;
tVect.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);
CrossProduct(edge01, edge02, cross);
if (fabs(cross.x) > SMALL_FLOAT)
{
sVect.y += -cross.y / cross.x;
tVect.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);
CrossProduct(edge01, edge02, cross);
if (fabs(cross.x) > SMALL_FLOAT)
{
sVect.z += -cross.y / cross.x;
tVect.z += -cross.z / cross.x;
}
// Normalize sVect and tVect
VectorNormalize(sVect);
VectorNormalize(tVect);
}
//-----------------------------------------------------------------------------
// Convert RGB to HSV
//-----------------------------------------------------------------------------
void RGBtoHSV(const Vector3D& rgb, Vector3D& hsv)
{
float flMax = MAX(rgb.x, rgb.y);
flMax = MAX(flMax, rgb.z);
float flMin = MIN(rgb.x, rgb.y);
flMin = MIN(flMin, rgb.z);
// hsv.z is the value
hsv.z = flMax;
// hsv.y is the saturation
if (flMax != 0.0F)
{
hsv.y = (flMax - flMin) / flMax;
}
else
{
hsv.y = 0.0F;
}
// hsv.x is the hue
if (hsv.y == 0.0F)
{
hsv.x = -1.0f;
}
else
{
float32 d = flMax - flMin;
if (rgb.x == flMax)
{
hsv.x = (rgb.y - rgb.z) / d;
}
else if (rgb.y == flMax)
{
hsv.x = 2.0F + (rgb.z - rgb.x) / d;
}
else
{
hsv.x = 4.0F + (rgb.x - rgb.y) / d;
}
hsv.x *= 60.0F;
if (hsv.x < 0.0F)
{
hsv.x += 360.0F;
}
}
}
//-----------------------------------------------------------------------------
// Convert HSV to RGB
//-----------------------------------------------------------------------------
void HSVtoRGB(const Vector3D& hsv, Vector3D& rgb)
{
if (hsv.y == 0.0F)
{
rgb.Init(hsv.z, hsv.z, hsv.z);
return;
}
float32 hue = hsv.x;
if (hue == 360.0F)
{
hue = 0.0F;
}
hue /= 60.0F;
int i = Float2Int(hue); // integer part
float32 f = hue - i; // fractional part
float32 p = hsv.z * (1.0F - hsv.y);
float32 q = hsv.z * (1.0F - hsv.y * f);
float32 t = hsv.z * (1.0F - hsv.y * (1.0F - f));
switch (i)
{
case 0: rgb.Init(hsv.z, t, p); break;
case 1: rgb.Init(q, hsv.z, p); break;
case 2: rgb.Init(p, hsv.z, t); break;
case 3: rgb.Init(p, q, hsv.z); break;
case 4: rgb.Init(t, p, hsv.z); break;
case 5: rgb.Init(hsv.z, p, q); break;
}
}
void GetInterpolationData(float const* pKnotPositions,
float const* pKnotValues,
int nNumValuesinList,
int nInterpolationRange,
float flPositionToInterpolateAt,
bool bWrap,
float* pValueA,
float* pValueB,
float* pInterpolationValue)
{
// first, find the bracketting knots by looking for the first knot >= our index
int idx;
for (idx = 0; idx < nNumValuesinList; idx++)
{
if (pKnotPositions[idx] >= flPositionToInterpolateAt)
break;
}
int nKnot1, nKnot2;
float flOffsetFromStartOfGap, flSizeOfGap;
if (idx == 0)
{
if (bWrap)
{
nKnot1 = nNumValuesinList - 1;
nKnot2 = 0;
flSizeOfGap =
(pKnotPositions[nKnot2] + (nInterpolationRange - pKnotPositions[nKnot1]));
flOffsetFromStartOfGap =
flPositionToInterpolateAt + (nInterpolationRange - pKnotPositions[nKnot1]);
}
else
{
*pValueA = *pValueB = pKnotValues[0];
*pInterpolationValue = 1.0;
return;
}
}
else if (idx == nNumValuesinList) // ran out of values
{
if (bWrap)
{
nKnot1 = nNumValuesinList - 1;
nKnot2 = 0;
flSizeOfGap = (pKnotPositions[nKnot2] +
(nInterpolationRange - pKnotPositions[nKnot1]));
flOffsetFromStartOfGap = flPositionToInterpolateAt - pKnotPositions[nKnot1];
}
else
{
*pValueA = *pValueB = pKnotValues[nNumValuesinList - 1];
*pInterpolationValue = 1.0;
return;
}
}
else
{
nKnot1 = idx - 1;
nKnot2 = idx;
flSizeOfGap = pKnotPositions[nKnot2] - pKnotPositions[nKnot1];
flOffsetFromStartOfGap = flPositionToInterpolateAt - pKnotPositions[nKnot1];
}
*pValueA = pKnotValues[nKnot1];
*pValueB = pKnotValues[nKnot2];
*pInterpolationValue = FLerp(0, 1, 0, flSizeOfGap, flOffsetFromStartOfGap);
return;
}
static Vector3D RandomVectorOnUnitSphere(float u, float v)
{
float flPhi = acos(1 - 2 * u);
float flTheta = 2 * M_PI * v;
float flSinPhi, flCosPhi;
float flSinTheta, flCosTheta;
SinCos(flPhi, &flSinPhi, &flCosPhi);
SinCos(flTheta, &flSinTheta, &flCosTheta);
return Vector3D(flSinPhi * flCosTheta, flSinPhi * flSinTheta, flCosPhi);
}
Vector3D RandomVectorOnUnitSphere()
{
// Guarantee uniform random distribution on a sphere
// Graphics gems III contains this algorithm ("Nonuniform random point sets via warping")
float u = RandomFloat(0., 1.);
float v = RandomFloat(0., 1.);
return RandomVectorOnUnitSphere(u, v);
}
Vector3D RandomVectorOnUnitSphere(IUniformRandomStream* pRnd)
{
return RandomVectorOnUnitSphere(pRnd->RandomFloat(), pRnd->RandomFloat());
}
float RandomVectorInUnitSphere(Vector3D* pVector)
{
// Guarantee uniform random distribution within a sphere
// Graphics gems III contains this algorithm ("Nonuniform random point sets via warping")
float u = ((float)rand() / VALVE_RAND_MAX);
float v = ((float)rand() / VALVE_RAND_MAX);
float w = ((float)rand() / VALVE_RAND_MAX);
float flPhi = acos(1 - 2 * u);
float flTheta = 2 * M_PI * v;
float flRadius = powf(w, 1.0f / 3.0f);
float flSinPhi, flCosPhi;
float flSinTheta, flCosTheta;
SinCos(flPhi, &flSinPhi, &flCosPhi);
SinCos(flTheta, &flSinTheta, &flCosTheta);
pVector->x = flRadius * flSinPhi * flCosTheta;
pVector->y = flRadius * flSinPhi * flSinTheta;
pVector->z = flRadius * flCosPhi;
return flRadius;
}
Vector3D RandomVectorInUnitSphere()
{
Vector3D vOut;
RandomVectorInUnitSphere(&vOut);
return vOut;
}
Vector3D RandomVectorInUnitSphere(IUniformRandomStream* pRnd)
{
float w = pRnd->RandomFloat();
float flRadius = powf(w, 1.0f / 3.0f);
Vector3D v = RandomVectorOnUnitSphere(pRnd) * flRadius;
return v;
}
float RandomVectorInUnitCircle(Vector2D* pVector)
{
// Guarantee uniform random distribution within a sphere
// Graphics gems III contains this algorithm ("Nonuniform random point sets via warping")
float u = ((float)rand() / VALVE_RAND_MAX);
float v = ((float)rand() / VALVE_RAND_MAX);
float flTheta = 2 * M_PI * v;
float flRadius = powf(u, 1.0f / 2.0f);
float flSinTheta, flCosTheta;
SinCos(flTheta, &flSinTheta, &flCosTheta);
pVector->x = flRadius * flCosTheta;
pVector->y = flRadius * flSinTheta;
return flRadius;
}
const Quaternion RandomQuaternion()
{
// Guarantee uniform distribution within S^3. Found on the internet, looked through the proof very briefly, looks sound enough to tentatively trust it before testing or checking the proof for real.
// http://mathproofs.blogspot.com/2005/05/uniformly-distributed-random-unit.html
float u = RandomFloat(0, float(2 * M_PI)), flSinU = sinf(u);
float v = acosf(RandomFloat(-1, 1)), flSinV = sinf(v);
float w = 0.5f * (RandomFloat(0, float(M_PI)) + acosf(RandomFloat(0, 1)) + M_PI / 2), flSinW = sinf(w);
return Quaternion(cosf(u), flSinU * cosf(v), flSinU * flSinV * cosf(w), flSinU * flSinV * flSinW);
}
const Quaternion RandomQuaternion(IUniformRandomStream* pRnd)
{
// Guarantee uniform distribution within S^3. Found on the internet, looked through the proof very briefly, looks sound enough to tentatively trust it before testing or checking the proof for real.
// http://mathproofs.blogspot.com/2005/05/uniformly-distributed-random-unit.html
float u = pRnd->RandomFloat(0, float(2 * M_PI)), flSinU = sinf(u);
float v = acosf(pRnd->RandomFloat(-1, 1)), flSinV = sinf(v);
float w = 0.5f * (pRnd->RandomFloat(0, float(M_PI)) + acosf(pRnd->RandomFloat(0, 1)) + M_PI / 2), flSinW = sinf(w);
return Quaternion(cosf(u), flSinU * cosf(v), flSinU * flSinV * cosf(w), flSinU * flSinV * flSinW);
}
// Originally from hammer_mathlib.cpp
//
// Generate the corner points of a box:
// +y _+z
// ^ /|
// | /
// | 3---7
// /| /|
// / | / |
// 2---6 |
// | 1|--5
// | / | /
// |/ |/
// 0---4 --> +x
void PointsFromBox(const Vector3D& mins, const Vector3D& maxs, Vector3D* points)
{
points[0][0] = mins[0];
points[0][1] = mins[1];
points[0][2] = mins[2];
points[1][0] = mins[0];
points[1][1] = mins[1];
points[1][2] = maxs[2];
points[2][0] = mins[0];
points[2][1] = maxs[1];
points[2][2] = mins[2];
points[3][0] = mins[0];
points[3][1] = maxs[1];
points[3][2] = maxs[2];
points[4][0] = maxs[0];
points[4][1] = mins[1];
points[4][2] = mins[2];
points[5][0] = maxs[0];
points[5][1] = mins[1];
points[5][2] = maxs[2];
points[6][0] = maxs[0];
points[6][1] = maxs[1];
points[6][2] = mins[2];
points[7][0] = maxs[0];
points[7][1] = maxs[1];
points[7][2] = maxs[2];
}
void BuildTransformedBox(Vector3D* v2, Vector3D const& bbmin, Vector3D const& bbmax, const matrix3x4_t& m)
{
Vector3D v[8];
PointsFromBox(bbmin, bbmax, v);
VectorTransform(v[0], m, v2[0]);
VectorTransform(v[1], m, v2[1]);
VectorTransform(v[2], m, v2[2]);
VectorTransform(v[3], m, v2[3]);
VectorTransform(v[4], m, v2[4]);
VectorTransform(v[5], m, v2[5]);
VectorTransform(v[6], m, v2[6]);
VectorTransform(v[7], m, v2[7]);
}
// Generate the corner points of a angled box:
// +y*r _+z*u
// ^ /|
// | /
// | 3---7
// /| /|
// / | / |
// 2---6 |
// | 1|--5
// | / | /
// |/ |/
// 0---4 --> +x*f
void PointsFromAngledBox(const QAngle& angles, const Vector3D& mins, const Vector3D& maxs, Vector3D* points)
{
Vector3D forward;
Vector3D up;
Vector3D right;
AngleVectors(angles, &forward, &right, &up);
points[0] = (forward * mins.x) + (right * maxs.y) + (up * maxs.z);
points[1] = (forward * mins.x) + (right * mins.y) + (up * maxs.z);
points[2] = (forward * maxs.x) + (right * mins.y) + (up * maxs.z);
points[3] = (forward * maxs.x) + (right * maxs.y) + (up * maxs.z);
points[4] = (forward * mins.x) + (right * maxs.y) + (up * mins.z);
points[5] = (forward * mins.x) + (right * mins.y) + (up * mins.z);
points[6] = (forward * maxs.x) + (right * mins.y) + (up * mins.z);
points[7] = (forward * maxs.x) + (right * maxs.y) + (up * mins.z);
}
void BuildTransformedAngledBox(Vector3D* v2, const QAngle& a, Vector3D const& bbmin, Vector3D const& bbmax, const matrix3x4_t& m)
{
Vector3D v[8];
PointsFromAngledBox(a, bbmin, bbmax, v);
VectorTransform(v[0], m, v2[0]);
VectorTransform(v[1], m, v2[1]);
VectorTransform(v[2], m, v2[2]);
VectorTransform(v[3], m, v2[3]);
VectorTransform(v[4], m, v2[4]);
VectorTransform(v[5], m, v2[5]);
VectorTransform(v[6], m, v2[6]);
VectorTransform(v[7], m, v2[7]);
}
#endif // !defined(__SPU__)
Reference in New Issue View Git Blame Copy Permalink