diff options
Diffstat (limited to 'external/vpc/public/mathlib')
| -rw-r--r-- | external/vpc/public/mathlib/fltx4.h | 97 | ||||
| -rw-r--r-- | external/vpc/public/mathlib/math_pfns.h | 283 | ||||
| -rw-r--r-- | external/vpc/public/mathlib/mathlib.h | 2425 | ||||
| -rw-r--r-- | external/vpc/public/mathlib/vector.h | 2633 | ||||
| -rw-r--r-- | external/vpc/public/mathlib/vector2d.h | 670 |
5 files changed, 6108 insertions, 0 deletions
diff --git a/external/vpc/public/mathlib/fltx4.h b/external/vpc/public/mathlib/fltx4.h new file mode 100644 index 0000000..a32877b --- /dev/null +++ b/external/vpc/public/mathlib/fltx4.h @@ -0,0 +1,97 @@ +//===== Copyright 1996-2010, Valve Corporation, All rights reserved. ======// +// +// Purpose: - defines the type fltx4 - Avoid cyclic includion. +// +//===========================================================================// + +#ifndef FLTX4_H +#define FLTX4_H + +#if defined(GNUC) +#define USE_STDC_FOR_SIMD 0 +#else +#define USE_STDC_FOR_SIMD 0 +#endif + +#if (!defined(PLATFORM_PPC) && (USE_STDC_FOR_SIMD == 0)) +#define _SSE1 1 +#endif + +// I thought about defining a class/union for the SIMD packed floats instead of using fltx4, +// but decided against it because (a) the nature of SIMD code which includes comparisons is to blur +// the relationship between packed floats and packed integer types and (b) not sure that the +// compiler would handle generating good code for the intrinsics. + +#if USE_STDC_FOR_SIMD + +#error "hello" +typedef union +{ + float m128_f32[4]; + uint32 m128_u32[4]; +} fltx4; + +typedef fltx4 i32x4; +typedef fltx4 u32x4; + +#ifdef _PS3 +typedef fltx4 u32x4; +typedef fltx4 i32x4; +#endif +typedef fltx4 bi32x4; + +#elif ( defined( _PS3 ) ) + +typedef union +{ + // This union allows float/int access (which generally shouldn't be done in inner loops) + + vec_float4 vmxf; + vec_int4 vmxi; + vec_uint4 vmxui; + __vector bool vmxbi; + + struct + { + float x; + float y; + float z; + float w; + }; + + float m128_f32[4]; + uint32 m128_u32[4]; + int32 m128_i32[4]; + +} fltx4_union; + +typedef vec_float4 fltx4; +typedef vec_uint4 u32x4; +typedef vec_int4 i32x4; +typedef __vector bool bi32x4; +#define DIFFERENT_NATIVE_VECTOR_TYPES // true if the compiler has different types for float4, uint4, int4, etc + +#elif ( defined( _X360 ) ) + +typedef union +{ + // This union allows float/int access (which generally shouldn't be done in inner loops) + __vector4 vmx; + float m128_f32[4]; + uint32 m128_u32[4]; +} fltx4_union; + +typedef __vector4 fltx4; +typedef __vector4 i32x4; // a VMX register; just a way of making it explicit that we're doing integer ops. +typedef __vector4 u32x4; // a VMX register; just a way of making it explicit that we're doing unsigned integer ops. +typedef fltx4 bi32x4; +#else + +typedef __m128 fltx4; +typedef __m128 i32x4; +typedef __m128 u32x4; +typedef fltx4 bi32x4; + +#endif + +#endif diff --git a/external/vpc/public/mathlib/math_pfns.h b/external/vpc/public/mathlib/math_pfns.h new file mode 100644 index 0000000..23a4f6b --- /dev/null +++ b/external/vpc/public/mathlib/math_pfns.h @@ -0,0 +1,283 @@ +//========= Copyright � 1996-2005, Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=====================================================================================// + +#ifndef _MATH_PFNS_H_ +#define _MATH_PFNS_H_ + +#include <limits> + +#if defined( _X360 ) +#include <xboxmath.h> +#elif defined(_PS3) + +#ifndef SPU +#include <ppu_asm_intrinsics.h> +#endif + +// Note that similar defines exist in ssemath.h +// Maybe we should consolidate in one place for all platforms. + +#define _VEC_0x7ff (vec_int4){0x7ff,0x7ff,0x7ff,0x7ff} +#define _VEC_0x3ff (vec_int4){0x3ff,0x3ff,0x3ff,0x3ff} +#define _VEC_22L (vector unsigned int){22,22,22,22} +#define _VEC_11L (vector unsigned int){11,11,11,11} +#define _VEC_0L (vector unsigned int){0,0,0,0} +#define _VEC_255F (vector float){255.0f,255.0f,255.0f,255.0f} +#define _VEC_NEGONEF (vector float){-1.0f,-1.0f,-1.0f,-1.0f} +#define _VEC_ONEF (vector float){1.0f,1.0f,1.0f,1.0f} +#define _VEC_ZEROF (vector float){0.0f,0.0f,0.0f,0.0f} +#define _VEC_ZEROxyzONEwF (vector float){0.0f,0.0f,0.0f,1.0f} +#define _VEC_HALFF (vector float){0.5f,0.5f,0.5f,0.5f} +#define _VEC_HALFxyzZEROwF (vector float){0.5f,0.5f,0.5f,0.0f} +#define _VEC_PERMUTE_XYZ0W1 (vector unsigned char){0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x1c,0x1d,0x1e,0x1f} + +#define _VEC_IEEEHACK (vector float){(float)(1 << 23),(float)(1 << 23),(float)(1 << 23),(float)(1 << 23)} +#define _VEC_PERMUTE_FASTFTOC (vector unsigned char){0,0,0,0,0,0,0,0,0,0,0,0,0x03,0x07,0x0b,0x0f} + +// AngleQuaternion +#define _VEC_PERMUTE_AQsxsxcxcx (vector unsigned char) {0x00,0x01,0x02,0x03,0x00,0x01,0x02,0x03,0x10,0x11,0x12,0x13,0x10,0x11,0x12,0x13} +#define _VEC_PERMUTE_AQczszszcz (vector unsigned char) {0x18,0x19,0x1a,0x1b,0x08,0x09,0x0a,0x0b,0x08,0x09,0x0a,0x0b,0x18,0x19,0x1a,0x1b} +#define _VEC_PERMUTE_AQcxcxsxsx (vector unsigned char) {0x10,0x11,0x12,0x13,0x10,0x11,0x12,0x13,0x00,0x01,0x02,0x03,0x00,0x01,0x02,0x03} +#define _VEC_PERMUTE_AQszczczsz (vector unsigned char) {0x08,0x09,0x0a,0x0b,0x18,0x19,0x1a,0x1b,0x18,0x19,0x1a,0x1b,0x08,0x09,0x0a,0x0b} +#define _VEC_PERMUTE_ANGLEQUAT (vector unsigned char) {0x10,0x11,0x12,0x13,0x04,0x05,0x06,0x07,0x18,0x19,0x1a,0x1b,0x0c,0x0d,0x0e,0x0f} + +#define _VEC_EPSILONF (__vector float) {FLT_EPSILON,FLT_EPSILON,FLT_EPSILON,FLT_EPSILON} + +#endif + +#if !(defined( PLATFORM_PPC ) || defined(SPU)) +// If we are not PPC based or SPU based, then assumes it is SSE2. We should make this code cleaner. + +#include <xmmintrin.h> + +// These globals are initialized by mathlib and redirected based on available fpu features + +// The following are not declared as macros because they are often used in limiting situations, +// and sometimes the compiler simply refuses to inline them for some reason +FORCEINLINE float FastSqrt( float x ) +{ + __m128 root = _mm_sqrt_ss( _mm_load_ss( &x ) ); + return *( reinterpret_cast<float *>( &root ) ); +} + +FORCEINLINE float FastRSqrtFast( float x ) +{ + // use intrinsics + __m128 rroot = _mm_rsqrt_ss( _mm_load_ss( &x ) ); + return *( reinterpret_cast<float *>( &rroot ) ); +} +// Single iteration NewtonRaphson reciprocal square root: +// 0.5 * rsqrtps * (3 - x * rsqrtps(x) * rsqrtps(x)) +// Very low error, and fine to use in place of 1.f / sqrtf(x). +FORCEINLINE float FastRSqrt( float x ) +{ + float rroot = FastRSqrtFast( x ); + return (0.5f * rroot) * (3.f - (x * rroot) * rroot); +} + +void FastSinCos( float x, float* s, float* c ); // any x +float FastCos( float x ); + + + +inline float FastRecip(float x) {return 1.0f / x;} +// Simple SSE rsqrt. Usually accurate to around 6 (relative) decimal places +// or so, so ok for closed transforms. (ie, computing lighting normals) +inline float FastSqrtEst(float x) { return FastRSqrtFast(x) * x; } + + +#else // !defined( PLATFORM_PPC ) && !defined(_SPU) + +#ifndef SPU +// We may not need this for SPU, so let's not bother for now + +FORCEINLINE float _VMX_Sqrt( float x ) +{ + return __fsqrts( x ); +} + +FORCEINLINE double _VMX_RSqrt( double x ) +{ + double rroot = __frsqrte( x ); + + // Single iteration NewtonRaphson on reciprocal square root estimate + return (0.5f * rroot) * (3.0f - (x * rroot) * rroot); +} + +FORCEINLINE double _VMX_RSqrtFast( double x ) +{ + return __frsqrte( x ); +} + +#ifdef _X360 +FORCEINLINE void _VMX_SinCos( float a, float *pS, float *pC ) +{ + XMScalarSinCos( pS, pC, a ); +} + +FORCEINLINE float _VMX_Cos( float a ) +{ + return XMScalarCos( a ); +} +#endif + +// the 360 has fixed hw and calls directly +#define FastSqrt(x) _VMX_Sqrt(x) +#define FastRSqrt(x) _VMX_RSqrt(x) +#define FastRSqrtFast(x) _VMX_RSqrtFast(x) +#define FastSinCos(x,s,c) _VMX_SinCos(x,s,c) +#define FastCos(x) _VMX_Cos(x) + +inline double FastRecip(double x) {return __fres(x);} +inline double FastSqrtEst(double x) { return __frsqrte(x) * x; } + +#endif // !defined( PLATFORM_PPC ) && !defined(_SPU) + +// if x is infinite, return FLT_MAX +inline float FastClampInfinity( float x ) +{ +#ifdef PLATFORM_PPC + return fsel( std::numeric_limits<float>::infinity() - x, x, FLT_MAX ); +#else + return ( x > FLT_MAX ? FLT_MAX : x ); +#endif +} + +#if defined (_PS3) && !defined(SPU) + +// extern float cosvf(float); /* single precision cosine */ +// extern float sinvf(float); /* single precision sine */ +// TODO: need a faster single precision equivalent +#define cosvf cosf +#define sinvf sinf + +inline int _rotl( int x, int c ) +{ + return __rlwimi(x,x,c,0,31); +} + +inline int64 _rotl64( int64 x, int c ) +{ + return __rldicl( x, c, 0 ); +} + +//----------------------------------------------------------------- +// Vector Unions +//----------------------------------------------------------------- + +//----------------------------------------------------------------- +// Floats +//----------------------------------------------------------------- +typedef union +{ + vector float vf; + float f[4]; +} vector_float_union; + +//----------------------------------------------------------------- +// Ints +//----------------------------------------------------------------- +typedef union +{ + vector int vi; + int i[4]; +} vector_int4_union; + +typedef union +{ + vector unsigned int vui; + unsigned int ui[4]; +} vector_uint4_union; + +//----------------------------------------------------------------- +// Shorts +//----------------------------------------------------------------- +typedef union +{ + vector signed short vs; + signed short s[8]; +} vector_short8_union; + +typedef union +{ + vector unsigned short vus; + unsigned short us[8]; +} vector_ushort8_union; + +//----------------------------------------------------------------- +// Chars +//----------------------------------------------------------------- +typedef union +{ + vector signed char vc; + signed char c[16]; +} vector_char16_union; + +typedef union +{ + vector unsigned char vuc; + unsigned char uc[16]; +} vector_uchar16_union; + +/* +FORCEINLINE float _VMX_Sqrt( float x ) +{ + vector_float_union vIn, vOut; + + vIn.f[0] = x; + + vOut.vf = sqrtf4(vIn.vf); + + return vOut.f[0]; +} + +FORCEINLINE float _VMX_RSqrt( float x ) +{ + vector_float_union vIn, vOut; + + vIn.f[0] = x; + + vOut.vf = rsqrtf4(vIn.vf); + + return vOut.f[0]; +} + +FORCEINLINE float _VMX_RSqrtFast( float x ) +{ + vector_float_union vIn, vOut; + + vIn.f[0] = x; + + vOut.vf = rsqrtf4fast(vIn.vf); + + return vOut.f[0]; +} +*/ + +FORCEINLINE void _VMX_SinCos( float a, float *pS, float *pC ) +{ + *pS=sinvf(a); + *pC=cosvf(a); +} + +FORCEINLINE float _VMX_Cos( float a ) +{ + return cosvf(a); +} + + +// the 360 has fixed hw and calls directly +/* +#define FastSqrt(x) _VMX_Sqrt(x) +#define FastRSqrt(x) _VMX_RSqrt(x) +#define FastRSqrtFast(x) _VMX_RSqrtFast(x) +#define FastSinCos(x,s,c) _VMX_SinCos(x,s,c) +#define FastCos(x) _VMX_Cos(x) +*/ +#endif // _PS3 +#endif // #ifndef SPU + +#endif // _MATH_PFNS_H_ diff --git a/external/vpc/public/mathlib/mathlib.h b/external/vpc/public/mathlib/mathlib.h new file mode 100644 index 0000000..db1848f --- /dev/null +++ b/external/vpc/public/mathlib/mathlib.h @@ -0,0 +1,2425 @@ +//===== Copyright � 1996-2005, Valve Corporation, All rights reserved. ======// +// +// Purpose: +// +//===========================================================================// + +#ifndef MATH_LIB_H +#define MATH_LIB_H + +#include <math.h> +#include "tier0/basetypes.h" +#include "mathlib/vector.h" +#include "mathlib/vector2d.h" +#include "tier0/dbg.h" + +#include "mathlib/math_pfns.h" +#include "mathlib/fltx4.h" + +#ifndef ALIGN8_POST +#define ALIGN8_POST +#endif + +#if defined(_PS3) + +#include <ppu_intrinsics.h> +#include <altivec.h> +#include <vectormath/c/vectormath_soa.h> + +#endif + +// plane_t structure +// !!! if this is changed, it must be changed in asm code too !!! +// FIXME: does the asm code even exist anymore? +// FIXME: this should move to a different file +struct cplane_t +{ + Vector normal; + float dist; + byte type; // for fast side tests + byte signbits; // signx + (signy<<1) + (signz<<1) + byte pad[2]; + +#ifdef VECTOR_NO_SLOW_OPERATIONS + cplane_t() {} + +private: + // No copy constructors allowed if we're in optimal mode + cplane_t(const cplane_t& vOther); +#endif +}; + +// structure offset for asm code +#define CPLANE_NORMAL_X 0 +#define CPLANE_NORMAL_Y 4 +#define CPLANE_NORMAL_Z 8 +#define CPLANE_DIST 12 +#define CPLANE_TYPE 16 +#define CPLANE_SIGNBITS 17 +#define CPLANE_PAD0 18 +#define CPLANE_PAD1 19 + +// 0-2 are axial planes +#define PLANE_X 0 +#define PLANE_Y 1 +#define PLANE_Z 2 + +// 3-5 are non-axial planes snapped to the nearest +#define PLANE_ANYX 3 +#define PLANE_ANYY 4 +#define PLANE_ANYZ 5 + + +//----------------------------------------------------------------------------- +// Frustum plane indices. +// WARNING: there is code that depends on these values +//----------------------------------------------------------------------------- + +enum +{ + FRUSTUM_RIGHT = 0, + FRUSTUM_LEFT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_NEARZ = 4, + FRUSTUM_FARZ = 5, + FRUSTUM_NUMPLANES = 6 +}; + +extern int SignbitsForPlane( cplane_t *out ); +class Frustum_t; + +// Computes Y fov from an X fov and a screen aspect ratio + X from Y +float CalcFovY( float flFovX, float flScreenAspect ); +float CalcFovX( float flFovY, float flScreenAspect ); + +// Generate a frustum based on perspective view parameters +// NOTE: FOV is specified in degrees, as the *full* view angle (not half-angle) +class VPlane; +void GeneratePerspectiveFrustum( const Vector& origin, const QAngle &angles, float flZNear, float flZFar, float flFovX, float flAspectRatio, Frustum_t &frustum ); +void GeneratePerspectiveFrustum( const Vector& origin, const Vector &forward, const Vector &right, const Vector &up, float flZNear, float flZFar, float flFovX, float flFovY, VPlane *pPlanesOut ); +// Cull the world-space bounding box to the specified frustum. +// bool R_CullBox( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ); +// bool R_CullBoxSkipNear( const Vector& mins, const Vector& maxs, const Frustum_t &frustum ); +void GenerateOrthoFrustum( const Vector &origin, const Vector &forward, const Vector &right, const Vector &up, float flLeft, float flRight, float flBottom, float flTop, float flZNear, float flZFar, VPlane *pPlanesOut ); + +class matrix3x4a_t; + +struct matrix3x4_t +{ + matrix3x4_t() {} + matrix3x4_t( + float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23 ) + { + m_flMatVal[0][0] = m00; m_flMatVal[0][1] = m01; m_flMatVal[0][2] = m02; m_flMatVal[0][3] = m03; + m_flMatVal[1][0] = m10; m_flMatVal[1][1] = m11; m_flMatVal[1][2] = m12; m_flMatVal[1][3] = m13; + m_flMatVal[2][0] = m20; m_flMatVal[2][1] = m21; m_flMatVal[2][2] = m22; m_flMatVal[2][3] = m23; + } + + //----------------------------------------------------------------------------- + // Creates a matrix where the X axis = forward + // the Y axis = left, and the Z axis = up + //----------------------------------------------------------------------------- + void Init( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) + { + m_flMatVal[0][0] = xAxis.x; m_flMatVal[0][1] = yAxis.x; m_flMatVal[0][2] = zAxis.x; m_flMatVal[0][3] = vecOrigin.x; + m_flMatVal[1][0] = xAxis.y; m_flMatVal[1][1] = yAxis.y; m_flMatVal[1][2] = zAxis.y; m_flMatVal[1][3] = vecOrigin.y; + m_flMatVal[2][0] = xAxis.z; m_flMatVal[2][1] = yAxis.z; m_flMatVal[2][2] = zAxis.z; m_flMatVal[2][3] = vecOrigin.z; + } + + //----------------------------------------------------------------------------- + // Creates a matrix where the X axis = forward + // the Y axis = left, and the Z axis = up + //----------------------------------------------------------------------------- + matrix3x4_t( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) + { + Init( xAxis, yAxis, zAxis, vecOrigin ); + } + + inline void SetOrigin( Vector const & p ) + { + m_flMatVal[0][3] = p.x; + m_flMatVal[1][3] = p.y; + m_flMatVal[2][3] = p.z; + } + + inline void Invalidate( void ) + { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 4; j++) + { + m_flMatVal[i][j] = VEC_T_NAN; + } + } + } + + float *operator[]( int i ) { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } + const float *operator[]( int i ) const { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } + float *Base() { return &m_flMatVal[0][0]; } + const float *Base() const { return &m_flMatVal[0][0]; } + + float m_flMatVal[3][4]; +}; + +class ALIGN16 matrix3x4a_t : public matrix3x4_t +{ +public: + /* + matrix3x4a_t() { if (((size_t)Base()) % 16 != 0) { Error( "matrix3x4a_t missaligned" ); } } + */ + matrix3x4a_t& operator=( const matrix3x4_t& src ) { memcpy( Base(), src.Base(), sizeof( float ) * 3 * 4 ); return *this; }; +} ALIGN16_POST; + +#ifndef M_PI + #define M_PI 3.14159265358979323846 // matches value in gcc v2 math.h +#endif + +#define M_PI_F ((float)(M_PI)) // Shouldn't collide with anything. + +// NJS: Inlined to prevent floats from being autopromoted to doubles, as with the old system. +#ifndef RAD2DEG + #define RAD2DEG( x ) ( (float)(x) * (float)(180.f / M_PI_F) ) +#endif + +#ifndef DEG2RAD + #define DEG2RAD( x ) ( (float)(x) * (float)(M_PI_F / 180.f) ) +#endif + +// Used to represent sides of things like planes. +#define SIDE_FRONT 0 +#define SIDE_BACK 1 +#define SIDE_ON 2 +#define SIDE_CROSS -2 // necessary for polylib.c + +// Use different side values (1, 2, 4) instead of (0, 1, 2) so we can '|' and '&' them, and quickly determine overall clipping +// without having to maintain counters and read / write memory. +enum Sides +{ + OR_SIDE_FRONT = 1, + OR_SIDE_BACK = 2, + OR_SIDE_ON = 4, +}; + +#define ON_VIS_EPSILON 0.01 // necessary for vvis (flow.c) -- again look into moving later! +#define EQUAL_EPSILON 0.001 // necessary for vbsp (faces.c) -- should look into moving it there? + +extern bool s_bMathlibInitialized; + +extern const Vector vec3_origin; +extern const QAngle vec3_angle; +extern const Quaternion quat_identity; +extern const Vector vec3_invalid; +extern const int nanmask; + +#define IS_NAN(x) (((*(int *)&x)&nanmask)==nanmask) + +FORCEINLINE vec_t DotProduct(const vec_t *v1, const vec_t *v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} +FORCEINLINE void VectorSubtract(const vec_t *a, const vec_t *b, vec_t *c) +{ + c[0]=a[0]-b[0]; + c[1]=a[1]-b[1]; + c[2]=a[2]-b[2]; +} +FORCEINLINE void VectorAdd(const vec_t *a, const vec_t *b, vec_t *c) +{ + c[0]=a[0]+b[0]; + c[1]=a[1]+b[1]; + c[2]=a[2]+b[2]; +} +FORCEINLINE void VectorCopy(const vec_t *a, vec_t *b) +{ + b[0]=a[0]; + b[1]=a[1]; + b[2]=a[2]; +} +FORCEINLINE void VectorClear(vec_t *a) +{ + a[0]=a[1]=a[2]=0; +} + +FORCEINLINE float VectorMaximum(const vec_t *v) +{ + return MAX( v[0], MAX( v[1], v[2] ) ); +} + +FORCEINLINE float VectorMaximum(const Vector& v) +{ + return MAX( v.x, MAX( v.y, v.z ) ); +} + +FORCEINLINE void VectorScale (const float* in, vec_t scale, float* out) +{ + out[0] = in[0]*scale; + out[1] = in[1]*scale; + out[2] = in[2]*scale; +} + + +// Cannot be forceinline as they have overloads: +inline void VectorFill(vec_t *a, float b) +{ + a[0]=a[1]=a[2]=b; +} + +inline void VectorNegate(vec_t *a) +{ + a[0]=-a[0]; + a[1]=-a[1]; + a[2]=-a[2]; +} + + +//#define VectorMaximum(a) ( max( (a)[0], max( (a)[1], (a)[2] ) ) ) +#define Vector2Clear(x) {(x)[0]=(x)[1]=0;} +#define Vector2Negate(x) {(x)[0]=-((x)[0]);(x)[1]=-((x)[1]);} +#define Vector2Copy(a,b) {(b)[0]=(a)[0];(b)[1]=(a)[1];} +#define Vector2Subtract(a,b,c) {(c)[0]=(a)[0]-(b)[0];(c)[1]=(a)[1]-(b)[1];} +#define Vector2Add(a,b,c) {(c)[0]=(a)[0]+(b)[0];(c)[1]=(a)[1]+(b)[1];} +#define Vector2Scale(a,b,c) {(c)[0]=(b)*(a)[0];(c)[1]=(b)*(a)[1];} + +// NJS: Some functions in VBSP still need to use these for dealing with mixing vec4's and shorts with vec_t's. +// remove when no longer needed. +#define VECTOR_COPY( A, B ) do { (B)[0] = (A)[0]; (B)[1] = (A)[1]; (B)[2]=(A)[2]; } while(0) +#define DOT_PRODUCT( A, B ) ( (A)[0]*(B)[0] + (A)[1]*(B)[1] + (A)[2]*(B)[2] ) + +FORCEINLINE void VectorMAInline( const float* start, float scale, const float* direction, float* dest ) +{ + dest[0]=start[0]+direction[0]*scale; + dest[1]=start[1]+direction[1]*scale; + dest[2]=start[2]+direction[2]*scale; +} + +FORCEINLINE void VectorMAInline( const Vector& start, float scale, const Vector& direction, Vector& dest ) +{ + dest.x=start.x+direction.x*scale; + dest.y=start.y+direction.y*scale; + dest.z=start.z+direction.z*scale; +} + +FORCEINLINE void VectorMA( const Vector& start, float scale, const Vector& direction, Vector& dest ) +{ + VectorMAInline(start, scale, direction, dest); +} + +FORCEINLINE void VectorMA( const float * start, float scale, const float *direction, float *dest ) +{ + VectorMAInline(start, scale, direction, dest); +} + + +int VectorCompare (const float *v1, const float *v2); + +inline float VectorLength(const float *v) +{ + return FastSqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + FLT_EPSILON ); +} + +void CrossProduct (const float *v1, const float *v2, float *cross); + +qboolean VectorsEqual( const float *v1, const float *v2 ); + +inline vec_t RoundInt (vec_t in) +{ + return floor(in + 0.5f); +} + +size_t Q_log2( unsigned int val ); + +// Math routines done in optimized assembly math package routines +void inline SinCos( float radians, float * RESTRICT sine, float * RESTRICT cosine ) +{ +#if defined( _X360 ) + XMScalarSinCos( sine, cosine, radians ); +#elif defined( _PS3 ) +#if ( __GNUC__ == 4 ) && ( __GNUC_MINOR__ == 1 ) && ( __GNUC_PATCHLEVEL__ == 1 ) + vector_float_union s; + vector_float_union c; + + vec_float4 rad = vec_splats( radians ); + vec_float4 sin; + vec_float4 cos; + + sincosf4( rad, &sin, &cos ); + + vec_st( sin, 0, s.f ); + vec_st( cos, 0, c.f ); + + *sine = s.f[0]; + *cosine = c.f[0]; +#else //__GNUC__ == 4 && __GNUC_MINOR__ == 1 && __GNUC_PATCHLEVEL__ == 1 + vector_float_union r; + vector_float_union s; + vector_float_union c; + + vec_float4 rad; + vec_float4 sin; + vec_float4 cos; + + r.f[0] = radians; + rad = vec_ld( 0, r.f ); + + sincosf4( rad, &sin, &cos ); + + vec_st( sin, 0, s.f ); + vec_st( cos, 0, c.f ); + + *sine = s.f[0]; + *cosine = c.f[0]; +#endif //__GNUC__ == 4 && __GNUC_MINOR__ == 1 && __GNUC_PATCHLEVEL__ == 1 +#elif defined( COMPILER_MSVC32 ) + _asm + { + fld DWORD PTR [radians] + fsincos + + mov edx, DWORD PTR [cosine] + mov eax, DWORD PTR [sine] + + fstp DWORD PTR [edx] + fstp DWORD PTR [eax] + } +#elif defined( GNUC ) + register double __cosr, __sinr; + __asm __volatile__ ("fsincos" : "=t" (__cosr), "=u" (__sinr) : "0" (radians)); + + *sine = __sinr; + *cosine = __cosr; +#else + *sine = sinf(radians); + *cosine = cosf(radians); +#endif +} + +#define SIN_TABLE_SIZE 256 +#define FTOIBIAS 12582912.f +extern float SinCosTable[SIN_TABLE_SIZE]; + +inline float TableCos( float theta ) +{ + union + { + int i; + float f; + } ftmp; + + // ideally, the following should compile down to: theta * constant + constant, changing any of these constants from defines sometimes fubars this. + ftmp.f = theta * ( float )( SIN_TABLE_SIZE / ( 2.0f * M_PI ) ) + ( FTOIBIAS + ( SIN_TABLE_SIZE / 4 ) ); + return SinCosTable[ ftmp.i & ( SIN_TABLE_SIZE - 1 ) ]; +} + +inline float TableSin( float theta ) +{ + union + { + int i; + float f; + } ftmp; + + // ideally, the following should compile down to: theta * constant + constant + ftmp.f = theta * ( float )( SIN_TABLE_SIZE / ( 2.0f * M_PI ) ) + FTOIBIAS; + return SinCosTable[ ftmp.i & ( SIN_TABLE_SIZE - 1 ) ]; +} + +template<class T> +FORCEINLINE T Square( T const &a ) +{ + return a * a; +} + +FORCEINLINE bool IsPowerOfTwo( uint x ) +{ + return ( x & ( x - 1 ) ) == 0; +} + +// return the smallest power of two >= x. +// returns 0 if x == 0 or x > 0x80000000 (ie numbers that would be negative if x was signed) +// NOTE: the old code took an int, and if you pass in an int of 0x80000000 casted to a uint, +// you'll get 0x80000000, which is correct for uints, instead of 0, which was correct for ints +FORCEINLINE uint SmallestPowerOfTwoGreaterOrEqual( uint x ) +{ + x -= 1; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + x |= x >> 8; + x |= x >> 16; + return x + 1; +} + +// return the largest power of two <= x. Will return 0 if passed 0 +FORCEINLINE uint LargestPowerOfTwoLessThanOrEqual( uint x ) +{ + if ( x >= 0x80000000 ) + return 0x80000000; + + return SmallestPowerOfTwoGreaterOrEqual( x + 1 ) >> 1; +} + + +// Math routines for optimizing division +void FloorDivMod (double numer, double denom, int *quotient, int *rem); +int GreatestCommonDivisor (int i1, int i2); + +// Test for FPU denormal mode +bool IsDenormal( const float &val ); + +// MOVEMENT INFO +enum +{ + PITCH = 0, // up / down + YAW, // left / right + ROLL // fall over +}; + +void MatrixAngles( const matrix3x4_t & matrix, float *angles ); // !!!! +void MatrixVectors( const matrix3x4_t &matrix, Vector* pForward, Vector *pRight, Vector *pUp ); +void VectorTransform (const float *in1, const matrix3x4_t & in2, float *out); +void VectorITransform (const float *in1, const matrix3x4_t & in2, float *out); +void VectorRotate( const float *in1, const matrix3x4_t & in2, float *out); +void VectorRotate( const Vector &in1, const QAngle &in2, Vector &out ); +void VectorRotate( const Vector &in1, const Quaternion &in2, Vector &out ); +void VectorIRotate( const float *in1, const matrix3x4_t & in2, float *out); + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +QAngle TransformAnglesToLocalSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ); +QAngle TransformAnglesToWorldSpace( const QAngle &angles, const matrix3x4_t &parentMatrix ); + +#endif + +void MatrixInitialize( matrix3x4_t &mat, const Vector &vecOrigin, const Vector &vecXAxis, const Vector &vecYAxis, const Vector &vecZAxis ); +void MatrixCopy( const matrix3x4_t &in, matrix3x4_t &out ); +void MatrixInvert( const matrix3x4_t &in, matrix3x4_t &out ); + +// Matrix equality test +bool MatricesAreEqual( const matrix3x4_t &src1, const matrix3x4_t &src2, float flTolerance = 1e-5 ); + +void MatrixGetColumn( const matrix3x4_t &in, int column, Vector &out ); +void MatrixSetColumn( const Vector &in, int column, matrix3x4_t &out ); + +//void DecomposeRotation( const matrix3x4_t &mat, float *out ); +void ConcatRotations (const matrix3x4_t &in1, const matrix3x4_t &in2, matrix3x4_t &out); +void ConcatTransforms (const matrix3x4_t &in1, const matrix3x4_t &in2, matrix3x4_t &out); +// faster version assumes m0, m1, out are 16-byte aligned addresses +void ConcatTransforms_Aligned( const matrix3x4a_t &m0, const matrix3x4a_t &m1, matrix3x4a_t &out ); + +// For identical interface w/ VMatrix +inline void MatrixMultiply ( const matrix3x4_t &in1, const matrix3x4_t &in2, matrix3x4_t &out ) +{ + ConcatTransforms( in1, in2, out ); +} + +void QuaternionExp( const Quaternion &p, Quaternion &q ); +void QuaternionLn( const Quaternion &p, Quaternion &q ); +void QuaternionAverageExponential( Quaternion &q, int nCount, const Quaternion *pQuaternions, const float *pflWeights = NULL ); +void QuaternionLookAt( const Vector &vecForward, const Vector &referenceUp, Quaternion &q ); +void QuaternionSlerp( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ); +void QuaternionSlerpNoAlign( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ); +void QuaternionBlend( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ); +void QuaternionBlendNoAlign( const Quaternion &p, const Quaternion &q, float t, Quaternion &qt ); +void QuaternionIdentityBlend( const Quaternion &p, float t, Quaternion &qt ); +float QuaternionAngleDiff( const Quaternion &p, const Quaternion &q ); +void QuaternionScale( const Quaternion &p, float t, Quaternion &q ); +void QuaternionAlign( const Quaternion &p, const Quaternion &q, Quaternion &qt ); +float QuaternionDotProduct( const Quaternion &p, const Quaternion &q ); +void QuaternionConjugate( const Quaternion &p, Quaternion &q ); +void QuaternionInvert( const Quaternion &p, Quaternion &q ); +float QuaternionNormalize( Quaternion &q ); +void QuaternionAdd( const Quaternion &p, const Quaternion &q, Quaternion &qt ); +void QuaternionMult( const Quaternion &p, const Quaternion &q, Quaternion &qt ); +void QuaternionMatrix( const Quaternion &q, matrix3x4_t &matrix ); +void QuaternionMatrix( const Quaternion &q, const Vector &pos, matrix3x4_t &matrix ); +void QuaternionAngles( const Quaternion &q, QAngle &angles ); +void AngleQuaternion( const QAngle& angles, Quaternion &qt ); +void QuaternionAngles( const Quaternion &q, RadianEuler &angles ); +void AngleQuaternion( RadianEuler const &angles, Quaternion &qt ); +void QuaternionAxisAngle( const Quaternion &q, Vector &axis, float &angle ); +void AxisAngleQuaternion( const Vector &axis, float angle, Quaternion &q ); +void BasisToQuaternion( const Vector &vecForward, const Vector &vecRight, const Vector &vecUp, Quaternion &q ); +void MatrixQuaternion( const matrix3x4_t &mat, Quaternion &q ); + +// A couple methods to find the dot product of a vector with a matrix row or column... +inline float MatrixRowDotProduct( const matrix3x4_t &in1, int row, const Vector& in2 ) +{ + Assert( (row >= 0) && (row < 3) ); + return DotProduct( in1[row], in2.Base() ); +} + +inline float MatrixColumnDotProduct( const matrix3x4_t &in1, int col, const Vector& in2 ) +{ + Assert( (col >= 0) && (col < 4) ); + return in1[0][col] * in2[0] + in1[1][col] * in2[1] + in1[2][col] * in2[2]; +} + +int __cdecl BoxOnPlaneSide (const float *emins, const float *emaxs, const cplane_t *plane); + +inline float anglemod(float a) +{ + a = (360.f/65536) * ((int)(a*(65536.f/360.0f)) & 65535); + return a; +} + +//// CLAMP +#if defined(__cplusplus) && defined(PLATFORM_PPC) + +#ifdef _X360 +#define __fsels __fsel +#endif + +template< > +inline double clamp( double const &val, double const &minVal, double const &maxVal ) +{ + float diffmin = val - minVal; + float diffmax = maxVal - val; + float r; + r = __fsel(diffmin, val, minVal); + r = __fsel(diffmax, r, maxVal); + return r; +} + +template< > +inline double clamp( double const &val, float const &minVal, float const &maxVal ) +{ + // these typecasts are actually free since all FPU regs are 64 bit on PPC anyway + return clamp ( val, (double) minVal, (double) maxVal ); +} +template< > +inline double clamp( double const &val, float const &minVal, double const &maxVal ) +{ + return clamp ( val, (double) minVal, (double) maxVal ); +} +template< > +inline double clamp( double const &val, double const &minVal, float const &maxVal ) +{ + return clamp ( val, (double) minVal, (double) maxVal ); +} + +template< > +inline float clamp( float const &val, float const &minVal, float const &maxVal ) +{ + float diffmin = val - minVal; + float diffmax = maxVal - val; + float r; + r = __fsels(diffmin, val, minVal); + r = __fsels(diffmax, r, maxVal); + return r; +} + +template< > +inline float clamp( float const &val, double const &minVal, double const &maxVal ) +{ + float diffmin = val - minVal; + float diffmax = maxVal - val; + float r; + r = __fsels(diffmin, val, minVal); + r = __fsels(diffmax, r, maxVal); + return r; +} +template< > +inline float clamp( float const &val, double const &minVal, float const &maxVal ) +{ + return clamp ( val, (float) minVal, maxVal ); +} +template< > +inline float clamp( float const &val, float const &minVal, double const &maxVal ) +{ + return clamp ( val, minVal, (float) maxVal ); +} + +#endif + +// Remap a value in the range [A,B] to [C,D]. +inline float RemapVal( float val, float A, float B, float C, float D) +{ + if ( A == B ) + return fsel( val - B , D , C ); + return C + (D - C) * (val - A) / (B - A); +} + +inline float RemapValClamped( float val, float A, float B, float C, float D) +{ + if ( A == B ) + return fsel( val - B , D , C ); + float cVal = (val - A) / (B - A); + cVal = clamp<float>( cVal, 0.0f, 1.0f ); + + return C + (D - C) * cVal; +} + +// Returns A + (B-A)*flPercent. +// float Lerp( float flPercent, float A, float B ); +template <class T> +FORCEINLINE T Lerp( float flPercent, T const &A, T const &B ) +{ + return A + (B - A) * flPercent; +} + +FORCEINLINE float Sqr( float f ) +{ + return f*f; +} + +// 5-argument floating point linear interpolation. +// FLerp(f1,f2,i1,i2,x)= +// f1 at x=i1 +// f2 at x=i2 +// smooth lerp between f1 and f2 at x>i1 and x<i2 +// extrapolation for x<i1 or x>i2 +// +// If you know a function f(x)'s value (f1) at position i1, and its value (f2) at position i2, +// the function can be linearly interpolated with FLerp(f1,f2,i1,i2,x) +// i2=i1 will cause a divide by zero. +static inline float FLerp(float f1, float f2, float i1, float i2, float x) +{ + return f1+(f2-f1)*(x-i1)/(i2-i1); +} + + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +// YWB: Specialization for interpolating euler angles via quaternions... +template<> FORCEINLINE QAngle Lerp<QAngle>( float flPercent, const QAngle& q1, const QAngle& q2 ) +{ + // Avoid precision errors + if ( q1 == q2 ) + return q1; + + Quaternion src, dest; + + // Convert to quaternions + AngleQuaternion( q1, src ); + AngleQuaternion( q2, dest ); + + Quaternion result; + + // Slerp + QuaternionSlerp( src, dest, flPercent, result ); + + // Convert to euler + QAngle output; + QuaternionAngles( result, output ); + return output; +} + +#else + +#pragma error + +// NOTE NOTE: I haven't tested this!! It may not work! Check out interpolatedvar.cpp in the client dll to try it +template<> FORCEINLINE QAngleByValue Lerp<QAngleByValue>( float flPercent, const QAngleByValue& q1, const QAngleByValue& q2 ) +{ + // Avoid precision errors + if ( q1 == q2 ) + return q1; + + Quaternion src, dest; + + // Convert to quaternions + AngleQuaternion( q1, src ); + AngleQuaternion( q2, dest ); + + Quaternion result; + + // Slerp + QuaternionSlerp( src, dest, flPercent, result ); + + // Convert to euler + QAngleByValue output; + QuaternionAngles( result, output ); + return output; +} + +#endif // VECTOR_NO_SLOW_OPERATIONS + + +// Swap two of anything. +template <class T> +FORCEINLINE void V_swap( T& x, T& y ) +{ + T temp = x; + x = y; + y = temp; +} + +template <class T> FORCEINLINE T AVG(T a, T b) +{ + return (a+b)/2; +} + +// number of elements in an array of static size +#define NELEMS(x) ((sizeof(x))/sizeof(x[0])) + +// XYZ macro, for printf type functions - ex printf("%f %f %f",XYZ(myvector)); +#define XYZ(v) (v).x,(v).y,(v).z + + + + +inline float Sign( float x ) +{ + return fsel( x, 1.0f, -1.0f ); // x >= 0 ? 1.0f : -1.0f + //return (x <0.0f) ? -1.0f : 1.0f; +} + +// +// Clamps the input integer to the given array bounds. +// Equivalent to the following, but without using any branches: +// +// if( n < 0 ) return 0; +// else if ( n > maxindex ) return maxindex; +// else return n; +// +// This is not always a clear performance win, but when you have situations where a clamped +// value is thrashing against a boundary this is a big win. (ie, valid, invalid, valid, invalid, ...) +// +// Note: This code has been run against all possible integers. +// +inline int ClampArrayBounds( int n, unsigned maxindex ) +{ + // mask is 0 if less than 4096, 0xFFFFFFFF if greater than + unsigned int inrangemask = 0xFFFFFFFF + (((unsigned) n) > maxindex ); + unsigned int lessthan0mask = 0xFFFFFFFF + ( n >= 0 ); + + // If the result was valid, set the result, (otherwise sets zero) + int result = (inrangemask & n); + + // if the result was out of range or zero. + result |= ((~inrangemask) & (~lessthan0mask)) & maxindex; + + return result; +} + + + +// Turn a number "inside out". +// See Recording Animation in Binary Order for Progressive Temporal Refinement +// by Paul Heckbert from "Graphics Gems". +// +// If you want to iterate something from 0 to n, you can use this to iterate non-sequentially, in +// such a way that you will start with widely separated values and then refine the gaps between +// them, as you would for progressive refinement. This works with non-power of two ranges. +int InsideOut( int nTotal, int nCounter ); + +#define BOX_ON_PLANE_SIDE(emins, emaxs, p) \ + (((p)->type < 3)? \ + ( \ + ((p)->dist <= (emins)[(p)->type])? \ + 1 \ + : \ + ( \ + ((p)->dist >= (emaxs)[(p)->type])?\ + 2 \ + : \ + 3 \ + ) \ + ) \ + : \ + BoxOnPlaneSide( (emins), (emaxs), (p))) + +//----------------------------------------------------------------------------- +// FIXME: Vector versions.... the float versions will go away hopefully soon! +//----------------------------------------------------------------------------- + +void AngleVectors (const QAngle& angles, Vector *forward); +void AngleVectors (const QAngle& angles, Vector *forward, Vector *right, Vector *up); +void AngleVectorsTranspose (const QAngle& angles, Vector *forward, Vector *right, Vector *up); +void AngleMatrix (const QAngle &angles, matrix3x4_t &mat ); +void AngleMatrix( const QAngle &angles, const Vector &position, matrix3x4_t &mat ); +void AngleMatrix (const RadianEuler &angles, matrix3x4_t &mat ); +void AngleMatrix( RadianEuler const &angles, const Vector &position, matrix3x4_t &mat ); +void AngleIMatrix (const QAngle &angles, matrix3x4_t &mat ); +void AngleIMatrix (const QAngle &angles, const Vector &position, matrix3x4_t &mat ); +void AngleIMatrix (const RadianEuler &angles, matrix3x4_t &mat ); +void VectorAngles( const Vector &forward, QAngle &angles ); +void VectorAngles( const Vector &forward, const Vector &pseudoup, QAngle &angles ); +void VectorMatrix( const Vector &forward, matrix3x4_t &mat ); +void VectorVectors( const Vector &forward, Vector &right, Vector &up ); +void SetIdentityMatrix( matrix3x4_t &mat ); +void SetScaleMatrix( float x, float y, float z, matrix3x4_t &dst ); +void MatrixBuildRotationAboutAxis( const Vector &vAxisOfRot, float angleDegrees, matrix3x4_t &dst ); + +inline bool MatrixIsIdentity( const matrix3x4_t &m ) +{ + return + m.m_flMatVal[0][0] == 1.0f && m.m_flMatVal[0][1] == 0.0f && m.m_flMatVal[0][2] == 0.0f && m.m_flMatVal[0][3] == 0.0f && + m.m_flMatVal[1][0] == 0.0f && m.m_flMatVal[1][1] == 1.0f && m.m_flMatVal[1][2] == 0.0f && m.m_flMatVal[1][3] == 0.0f && + m.m_flMatVal[2][0] == 0.0f && m.m_flMatVal[2][1] == 0.0f && m.m_flMatVal[2][2] == 1.0f && m.m_flMatVal[2][3] == 0.0f; +} + + +inline void SetScaleMatrix( float flScale, matrix3x4_t &dst ) +{ + SetScaleMatrix( flScale, flScale, flScale, dst ); +} + +inline void SetScaleMatrix( const Vector& scale, matrix3x4_t &dst ) +{ + SetScaleMatrix( scale.x, scale.y, scale.z, dst ); +} + +// Computes the inverse transpose +void MatrixTranspose( matrix3x4_t& mat ); +void MatrixTranspose( const matrix3x4_t& src, matrix3x4_t& dst ); +void MatrixInverseTranspose( const matrix3x4_t& src, matrix3x4_t& dst ); + +inline void PositionMatrix( const Vector &position, matrix3x4_t &mat ) +{ + MatrixSetColumn( position, 3, mat ); +} + +inline void MatrixPosition( const matrix3x4_t &matrix, Vector &position ) +{ + position[0] = matrix[0][3]; + position[1] = matrix[1][3]; + position[2] = matrix[2][3]; +} + +inline void VectorRotate( const Vector& in1, const matrix3x4_t &in2, Vector &out) +{ + VectorRotate( &in1.x, in2, &out.x ); +} + +inline void VectorIRotate( const Vector& in1, const matrix3x4_t &in2, Vector &out) +{ + VectorIRotate( &in1.x, in2, &out.x ); +} + +inline void MatrixAngles( const matrix3x4_t &matrix, QAngle &angles ) +{ + MatrixAngles( matrix, &angles.x ); +} + +inline void MatrixAngles( const matrix3x4_t &matrix, QAngle &angles, Vector &position ) +{ + MatrixAngles( matrix, angles ); + MatrixPosition( matrix, position ); +} + +inline void MatrixAngles( const matrix3x4_t &matrix, RadianEuler &angles ) +{ + MatrixAngles( matrix, &angles.x ); + + angles.Init( DEG2RAD( angles.z ), DEG2RAD( angles.x ), DEG2RAD( angles.y ) ); +} + +void MatrixAngles( const matrix3x4_t &mat, RadianEuler &angles, Vector &position ); + +void MatrixAngles( const matrix3x4_t &mat, Quaternion &q, Vector &position ); + +inline int VectorCompare (const Vector& v1, const Vector& v2) +{ + return v1 == v2; +} + +inline void VectorTransform (const Vector& in1, const matrix3x4_t &in2, Vector &out) +{ + VectorTransform( &in1.x, in2, &out.x ); +} + +inline void VectorITransform (const Vector& in1, const matrix3x4_t &in2, Vector &out) +{ + VectorITransform( &in1.x, in2, &out.x ); +} + +/* +inline void DecomposeRotation( const matrix3x4_t &mat, Vector &out ) +{ + DecomposeRotation( mat, &out.x ); +} +*/ + +inline int BoxOnPlaneSide (const Vector& emins, const Vector& emaxs, const cplane_t *plane ) +{ + return BoxOnPlaneSide( &emins.x, &emaxs.x, plane ); +} + +inline void VectorFill(Vector& a, float b) +{ + a[0]=a[1]=a[2]=b; +} + +inline void VectorNegate(Vector& a) +{ + a[0] = -a[0]; + a[1] = -a[1]; + a[2] = -a[2]; +} + +inline vec_t VectorAvg(Vector& a) +{ + return ( a[0] + a[1] + a[2] ) / 3; +} + +//----------------------------------------------------------------------------- +// Box/plane test (slow version) +//----------------------------------------------------------------------------- +inline int FASTCALL BoxOnPlaneSide2 (const Vector& emins, const Vector& emaxs, const cplane_t *p, float tolerance = 0.f ) +{ + Vector corners[2]; + + if (p->normal[0] < 0) + { + corners[0][0] = emins[0]; + corners[1][0] = emaxs[0]; + } + else + { + corners[1][0] = emins[0]; + corners[0][0] = emaxs[0]; + } + + if (p->normal[1] < 0) + { + corners[0][1] = emins[1]; + corners[1][1] = emaxs[1]; + } + else + { + corners[1][1] = emins[1]; + corners[0][1] = emaxs[1]; + } + + if (p->normal[2] < 0) + { + corners[0][2] = emins[2]; + corners[1][2] = emaxs[2]; + } + else + { + corners[1][2] = emins[2]; + corners[0][2] = emaxs[2]; + } + + int sides = 0; + + float dist1 = DotProduct (p->normal, corners[0]) - p->dist; + if (dist1 >= tolerance) + sides = 1; + + float dist2 = DotProduct (p->normal, corners[1]) - p->dist; + if (dist2 < -tolerance) + sides |= 2; + + return sides; +} + +//----------------------------------------------------------------------------- +// Helpers for bounding box construction +//----------------------------------------------------------------------------- + +void ClearBounds (Vector& mins, Vector& maxs); +void AddPointToBounds (const Vector& v, Vector& mins, Vector& maxs); + +//----------------------------------------------------------------------------- +// Ensures that the min and max bounds values are valid. +// (ClearBounds() sets min > max, which is clearly invalid.) +//----------------------------------------------------------------------------- +bool AreBoundsValid( const Vector &vMin, const Vector &vMax ); + +//----------------------------------------------------------------------------- +// Returns true if the provided point is in the AABB defined by vMin +// at the lower corner and vMax at the upper corner. +//----------------------------------------------------------------------------- +bool IsPointInBounds( const Vector &vPoint, const Vector &vMin, const Vector &vMax ); + +// +// COLORSPACE/GAMMA CONVERSION STUFF +// +void BuildGammaTable( float gamma, float texGamma, float brightness, int overbright ); + +// convert texture to linear 0..1 value +inline float TexLightToLinear( int c, int exponent ) +{ + extern float power2_n[256]; + Assert( exponent >= -128 && exponent <= 127 ); + return ( float )c * power2_n[exponent+128]; +} + + +// convert texture to linear 0..1 value +int LinearToTexture( float f ); +// converts 0..1 linear value to screen gamma (0..255) +int LinearToScreenGamma( float f ); +float TextureToLinear( int c ); + +// compressed color format +struct ColorRGBExp32 +{ + byte r, g, b; + signed char exponent; +}; + +void ColorRGBExp32ToVector( const ColorRGBExp32& in, Vector& out ); +void VectorToColorRGBExp32( const Vector& v, ColorRGBExp32 &c ); + +// solve for "x" where "a x^2 + b x + c = 0", return true if solution exists +bool SolveQuadratic( float a, float b, float c, float &root1, float &root2 ); + +// 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 ); + +// solves for a,b,c specified as above, except that it always creates a monotonically increasing or +// decreasing curve if the data is monotonically increasing or decreasing. In order to enforce the +// monoticity condition, it is possible that the resulting quadratic will only approximate the data +// instead of interpolating it. This code is not especially fast. +bool SolveInverseQuadraticMonotonic( float x1, float y1, float x2, float y2, + float x3, float y3, float &a, float &b, float &c ); + + + + +// 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 ); + +// rotate a vector around the Z axis (YAW) +void VectorYawRotate( const Vector& in, float flYaw, Vector &out); + + +// Bias takes an X value between 0 and 1 and returns another value between 0 and 1 +// The curve is biased towards 0 or 1 based on biasAmt, which is between 0 and 1. +// Lower values of biasAmt bias the curve towards 0 and higher values bias it towards 1. +// +// For example, with biasAmt = 0.2, the curve looks like this: +// +// 1 +// | * +// | * +// | * +// | ** +// | ** +// | **** +// |********* +// |___________________ +// 0 1 +// +// +// With biasAmt = 0.8, the curve looks like this: +// +// 1 +// | ************** +// | ** +// | * +// | * +// |* +// |* +// |* +// |___________________ +// 0 1 +// +// With a biasAmt of 0.5, Bias returns X. +float Bias( float x, float biasAmt ); + + +// Gain is similar to Bias, but biasAmt biases towards or away from 0.5. +// Lower bias values bias towards 0.5 and higher bias values bias away from it. +// +// For example, with biasAmt = 0.2, the curve looks like this: +// +// 1 +// | * +// | * +// | ** +// | *************** +// | ** +// | * +// |* +// |___________________ +// 0 1 +// +// +// With biasAmt = 0.8, the curve looks like this: +// +// 1 +// | ***** +// | *** +// | * +// | * +// | * +// | *** +// |***** +// |___________________ +// 0 1 +float Gain( float x, float biasAmt ); + + +// SmoothCurve maps a 0-1 value into another 0-1 value based on a cosine wave +// where the derivatives of the function at 0 and 1 (and 0.5) are 0. This is useful for +// any fadein/fadeout effect where it should start and end smoothly. +// +// The curve looks like this: +// +// 1 +// | ** +// | * * +// | * * +// | * * +// | * * +// | ** ** +// |*** *** +// |___________________ +// 0 1 +// +float SmoothCurve( float x ); + + +// This works like SmoothCurve, with two changes: +// +// 1. Instead of the curve peaking at 0.5, it will peak at flPeakPos. +// (So if you specify flPeakPos=0.2, then the peak will slide to the left). +// +// 2. flPeakSharpness is a 0-1 value controlling the sharpness of the peak. +// Low values blunt the peak and high values sharpen the peak. +float SmoothCurve_Tweak( float x, float flPeakPos=0.5, float flPeakSharpness=0.5 ); + + +//float ExponentialDecay( float halflife, float dt ); +//float ExponentialDecay( float decayTo, float decayTime, float dt ); + +// halflife is time for value to reach 50% +inline float ExponentialDecay( float halflife, float dt ) +{ + // log(0.5) == -0.69314718055994530941723212145818 + return expf( -0.69314718f / halflife * dt); +} + +// decayTo is factor the value should decay to in decayTime +inline float ExponentialDecay( float decayTo, float decayTime, float dt ) +{ + return expf( logf( decayTo ) / decayTime * dt); +} + +// Get the integrated distanced traveled +// decayTo is factor the value should decay to in decayTime +// dt is the time relative to the last velocity update +inline float ExponentialDecayIntegral( float decayTo, float decayTime, float dt ) +{ + return (powf( decayTo, dt / decayTime) * decayTime - decayTime) / logf( decayTo ); +} + +// hermite basis function for smooth interpolation +// Similar to Gain() above, but very cheap to call +// value should be between 0 & 1 inclusive +inline float SimpleSpline( float value ) +{ + float valueSquared = value * value; + + // Nice little ease-in, ease-out spline-like curve + return (3 * valueSquared - 2 * valueSquared * value); +} + +// remaps a value in [startInterval, startInterval+rangeInterval] from linear to +// spline using SimpleSpline +inline float SimpleSplineRemapVal( float val, float A, float B, float C, float D) +{ + if ( A == B ) + return val >= B ? D : C; + float cVal = (val - A) / (B - A); + return C + (D - C) * SimpleSpline( cVal ); +} + +// remaps a value in [startInterval, startInterval+rangeInterval] from linear to +// spline using SimpleSpline +inline float SimpleSplineRemapValClamped( float val, float A, float B, float C, float D ) +{ + if ( A == B ) + return val >= B ? D : C; + float cVal = (val - A) / (B - A); + cVal = clamp( cVal, 0.0f, 1.0f ); + return C + (D - C) * SimpleSpline( cVal ); +} + +FORCEINLINE int RoundFloatToInt(float f) +{ +#if defined( _X360 ) +#ifdef Assert + Assert( IsFPUControlWordSet() ); +#endif + union + { + double flResult; + int pResult[2]; + }; + flResult = __fctiw( f ); + return pResult[1]; +#elif defined ( _PS3 ) + return __fctiw( f ); +#else // !X360 + int nResult; +#if defined( COMPILER_MSVC32 ) + __asm + { + fld f + fistp nResult + } +#elif GNUC + __asm __volatile__ ( + "fistpl %0;": "=m" (nResult): "t" (f) : "st" + ); +#else + nResult = static_cast<int>(f); +#endif + return nResult; +#endif +} + +FORCEINLINE unsigned char RoundFloatToByte(float f) +{ +#if defined( _X360 ) +#ifdef Assert + Assert( IsFPUControlWordSet() ); +#endif + union + { + double flResult; + int pIntResult[2]; + unsigned char pResult[8]; + }; + flResult = __fctiw( f ); +#ifdef Assert + Assert( pIntResult[1] >= 0 && pIntResult[1] <= 255 ); +#endif + return pResult[7]; + +#elif defined ( _PS3 ) + return __fctiw( f ); +#else // !X360 + + int nResult; + +#if defined( COMPILER_MSVC32 ) + __asm + { + fld f + fistp nResult + } +#elif GNUC + __asm __volatile__ ( + "fistpl %0;": "=m" (nResult): "t" (f) : "st" + ); +#else + nResult = static_cast<unsigned int> (f) & 0xff; +#endif + +#ifdef Assert + Assert( nResult >= 0 && nResult <= 255 ); +#endif + return nResult; + +#endif +} + +FORCEINLINE unsigned long RoundFloatToUnsignedLong(float f) +{ +#if defined( _X360 ) +#ifdef Assert + Assert( IsFPUControlWordSet() ); +#endif + union + { + double flResult; + int pIntResult[2]; + unsigned long pResult[2]; + }; + flResult = __fctiw( f ); + Assert( pIntResult[1] >= 0 ); + return pResult[1]; +#elif defined ( _PS3 ) + return __fctiw( f ); +#else // !X360 + +#if defined( COMPILER_MSVC32 ) + unsigned char nResult[8]; + __asm + { + fld f + fistp qword ptr nResult + } + return *((unsigned long*)nResult); +#elif defined( COMPILER_GCC ) + unsigned char nResult[8]; + __asm __volatile__ ( + "fistpl %0;": "=m" (nResult): "t" (f) : "st" + ); + return *((unsigned long*)nResult); +#else + return static_cast<unsigned long>(f); +#endif + +#endif +} + +FORCEINLINE bool IsIntegralValue( float flValue, float flTolerance = 0.001f ) +{ + return fabs( RoundFloatToInt( flValue ) - flValue ) < flTolerance; +} + +// Fast, accurate ftol: +FORCEINLINE int Float2Int( float a ) +{ +#if defined( _X360 ) + union + { + double flResult; + int pResult[2]; + }; + flResult = __fctiwz( a ); + return pResult[1]; +#elif defined ( _PS3 ) + return __fctiwz( a ); +#else // !X360 + + int RetVal; + +#if defined( COMPILER_MSVC32 ) + int CtrlwdHolder; + int CtrlwdSetter; + __asm + { + fld a // push 'a' onto the FP stack + fnstcw CtrlwdHolder // store FPU control word + movzx eax, CtrlwdHolder // move and zero extend word into eax + and eax, 0xFFFFF3FF // set all bits except rounding bits to 1 + or eax, 0x00000C00 // set rounding mode bits to round towards zero + mov CtrlwdSetter, eax // Prepare to set the rounding mode -- prepare to enter plaid! + fldcw CtrlwdSetter // Entering plaid! + fistp RetVal // Store and converted (to int) result + fldcw CtrlwdHolder // Restore control word + } +#else + RetVal = static_cast<int>( a ); +#endif + + return RetVal; +#endif +} + +// Over 15x faster than: (int)floor(value) +inline int Floor2Int( float a ) +{ + int RetVal; + +#if defined( PLATFORM_PPC ) + RetVal = (int)floor( a ); +#elif defined( COMPILER_MSVC32 ) + int CtrlwdHolder; + int CtrlwdSetter; + __asm + { + fld a // push 'a' onto the FP stack + fnstcw CtrlwdHolder // store FPU control word + movzx eax, CtrlwdHolder // move and zero extend word into eax + and eax, 0xFFFFF3FF // set all bits except rounding bits to 1 + or eax, 0x00000400 // set rounding mode bits to round down + mov CtrlwdSetter, eax // Prepare to set the rounding mode -- prepare to enter plaid! + fldcw CtrlwdSetter // Entering plaid! + fistp RetVal // Store floored and converted (to int) result + fldcw CtrlwdHolder // Restore control word + } +#else + RetVal = static_cast<int>( floor(a) ); +#endif + + return RetVal; +} + +//----------------------------------------------------------------------------- +// Fast color conversion from float to unsigned char +//----------------------------------------------------------------------------- +FORCEINLINE unsigned char FastFToC( float c ) +{ + volatile float dc; + + // ieee trick + dc = c * 255.0f + (float)(1 << 23); + + // return the lsb +#if defined( _X360 ) || defined( _PS3 ) + return ((unsigned char*)&dc)[3]; +#else + return *(unsigned char*)&dc; +#endif +} + +//----------------------------------------------------------------------------- +// Purpose: Bound input float to .001 (millisecond) boundary +// Input : in - +// Output : inline float +//----------------------------------------------------------------------------- +inline float ClampToMsec( float in ) +{ + int msec = Floor2Int( in * 1000.0f + 0.5f ); + return msec / 1000.0f; +} + +// Over 15x faster than: (int)ceil(value) +inline int Ceil2Int( float a ) +{ + int RetVal; + +#if defined( PLATFORM_PPC ) + RetVal = (int)ceil( a ); +#elif defined( COMPILER_MSVC32 ) + int CtrlwdHolder; + int CtrlwdSetter; + __asm + { + fld a // push 'a' onto the FP stack + fnstcw CtrlwdHolder // store FPU control word + movzx eax, CtrlwdHolder // move and zero extend word into eax + and eax, 0xFFFFF3FF // set all bits except rounding bits to 1 + or eax, 0x00000800 // set rounding mode bits to round down + mov CtrlwdSetter, eax // Prepare to set the rounding mode -- prepare to enter plaid! + fldcw CtrlwdSetter // Entering plaid! + fistp RetVal // Store floored and converted (to int) result + fldcw CtrlwdHolder // Restore control word + } +#else + RetVal = static_cast<int>( ceil(a) ); +#endif + + return RetVal; +} + + +// Regular signed area of triangle +#define TriArea2D( A, B, C ) \ + ( 0.5f * ( ( B.x - A.x ) * ( C.y - A.y ) - ( B.y - A.y ) * ( C.x - A.x ) ) ) + +// This version doesn't premultiply by 0.5f, so it's the area of the rectangle instead +#define TriArea2DTimesTwo( A, B, C ) \ + ( ( ( B.x - A.x ) * ( C.y - A.y ) - ( B.y - A.y ) * ( C.x - A.x ) ) ) + + +// Get the barycentric coordinates of "pt" in triangle [A,B,C]. +inline void GetBarycentricCoords2D( + Vector2D const &A, + Vector2D const &B, + Vector2D const &C, + Vector2D const &pt, + float bcCoords[3] ) +{ + // Note, because to top and bottom are both x2, the issue washes out in the composite + float invTriArea = 1.0f / TriArea2DTimesTwo( A, B, C ); + + // NOTE: We assume here that the lightmap coordinate vertices go counterclockwise. + // If not, TriArea2D() is negated so this works out right. + bcCoords[0] = TriArea2DTimesTwo( B, C, pt ) * invTriArea; + bcCoords[1] = TriArea2DTimesTwo( C, A, pt ) * invTriArea; + bcCoords[2] = TriArea2DTimesTwo( A, B, pt ) * invTriArea; +} + + +// Return true of the sphere might touch the box (the sphere is actually treated +// like a box itself, so this may return true if the sphere's bounding box touches +// a corner of the box but the sphere itself doesn't). +inline bool QuickBoxSphereTest( + const Vector& vOrigin, + float flRadius, + const Vector& bbMin, + const Vector& bbMax ) +{ + return vOrigin.x - flRadius < bbMax.x && vOrigin.x + flRadius > bbMin.x && + vOrigin.y - flRadius < bbMax.y && vOrigin.y + flRadius > bbMin.y && + vOrigin.z - flRadius < bbMax.z && vOrigin.z + flRadius > bbMin.z; +} + + +// Return true of the boxes intersect (but not if they just touch). +inline bool QuickBoxIntersectTest( + const Vector& vBox1Min, + const Vector& vBox1Max, + const Vector& vBox2Min, + const Vector& vBox2Max ) +{ + return + vBox1Min.x < vBox2Max.x && vBox1Max.x > vBox2Min.x && + vBox1Min.y < vBox2Max.y && vBox1Max.y > vBox2Min.y && + vBox1Min.z < vBox2Max.z && vBox1Max.z > vBox2Min.z; +} + + +extern float GammaToLinearFullRange( float gamma ); +extern float LinearToGammaFullRange( float linear ); +extern float GammaToLinear( float gamma ); +extern float LinearToGamma( float linear ); + +extern float SrgbGammaToLinear( float flSrgbGammaValue ); +extern float SrgbLinearToGamma( float flLinearValue ); +extern float X360GammaToLinear( float fl360GammaValue ); +extern float X360LinearToGamma( float flLinearValue ); +extern float SrgbGammaTo360Gamma( float flSrgbGammaValue ); + +// linear (0..4) to screen corrected vertex space (0..1?) +FORCEINLINE float LinearToVertexLight( float f ) +{ + extern float lineartovertex[4096]; + + // Gotta clamp before the multiply; could overflow... + // assume 0..4 range + int i = RoundFloatToInt( f * 1024.f ); + + // Presumably the comman case will be not to clamp, so check that first: + if( (unsigned)i > 4095 ) + { + if ( i < 0 ) + i = 0; // Compare to zero instead of 4095 to save 4 bytes in the instruction stream + else + i = 4095; + } + + return lineartovertex[i]; +} + + +FORCEINLINE unsigned char LinearToLightmap( float f ) +{ + extern unsigned char lineartolightmap[4096]; + + // Gotta clamp before the multiply; could overflow... + int i = RoundFloatToInt( f * 1024.f ); // assume 0..4 range + + // Presumably the comman case will be not to clamp, so check that first: + if ( (unsigned)i > 4095 ) + { + if ( i < 0 ) + i = 0; // Compare to zero instead of 4095 to save 4 bytes in the instruction stream + else + i = 4095; + } + + return lineartolightmap[i]; +} + +FORCEINLINE void ColorClamp( Vector& color ) +{ + float maxc = MAX( color.x, MAX( color.y, color.z ) ); + if ( maxc > 1.0f ) + { + float ooMax = 1.0f / maxc; + color.x *= ooMax; + color.y *= ooMax; + color.z *= ooMax; + } + + if ( color[0] < 0.f ) color[0] = 0.f; + if ( color[1] < 0.f ) color[1] = 0.f; + if ( color[2] < 0.f ) color[2] = 0.f; +} + +inline void ColorClampTruncate( Vector& color ) +{ + if (color[0] > 1.0f) color[0] = 1.0f; else if (color[0] < 0.0f) color[0] = 0.0f; + if (color[1] > 1.0f) color[1] = 1.0f; else if (color[1] < 0.0f) color[1] = 0.0f; + if (color[2] > 1.0f) color[2] = 1.0f; else if (color[2] < 0.0f) color[2] = 0.0f; +} + +// Interpolate a Catmull-Rom spline. +// t is a [0,1] value and interpolates a curve between p2 and p3. +void Catmull_Rom_Spline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector &output ); + +// Interpolate a Catmull-Rom spline. +// Returns the tangent of the point at t of the spline +void Catmull_Rom_Spline_Tangent( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector &output ); + +// area under the curve [0..t] +void Catmull_Rom_Spline_Integral( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// area under the curve [0..1] +void Catmull_Rom_Spline_Integral( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + Vector& output ); + +// Interpolate a Catmull-Rom spline. +// Normalize p2->p1 and p3->p4 to be the same length as p2->p3 +void Catmull_Rom_Spline_Normalize( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector &output ); + +// area under the curve [0..t] +// Normalize p2->p1 and p3->p4 to be the same length as p2->p3 +void Catmull_Rom_Spline_Integral_Normalize( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// Interpolate a Catmull-Rom spline. +// Normalize p2.x->p1.x and p3.x->p4.x to be the same length as p2.x->p3.x +void Catmull_Rom_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector &output ); + +// area under the curve [0..t] +void Catmull_Rom_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// Interpolate a Hermite spline. +// t is a [0,1] value and interpolates a curve between p1 and p2 with the deltas d1 and d2. +void Hermite_Spline( + const Vector &p1, + const Vector &p2, + const Vector &d1, + const Vector &d2, + float t, + Vector& output ); + +float Hermite_Spline( + float p1, + float p2, + float d1, + float d2, + float t ); + +// t is a [0,1] value and interpolates a curve between p1 and p2 with the slopes p0->p1 and p1->p2 +void Hermite_Spline( + const Vector &p0, + const Vector &p1, + const Vector &p2, + float t, + Vector& output ); + +float Hermite_Spline( + float p0, + float p1, + float p2, + float t ); + + +void Hermite_SplineBasis( float t, float basis[] ); + +void Hermite_Spline( + const Quaternion &q0, + const Quaternion &q1, + const Quaternion &q2, + float t, + Quaternion &output ); + + +// 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/%[email protected]%3E/Splines.bas.txt +// for example code and descriptions of various spline types... +// +void Kochanek_Bartels_Spline( + float tension, + float bias, + float continuity, + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +void Kochanek_Bartels_Spline_NormalizeX( + float tension, + float bias, + float continuity, + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// See link at Kochanek_Bartels_Spline for info on the basis matrix used +void Cubic_Spline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +void Cubic_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// See link at Kochanek_Bartels_Spline for info on the basis matrix used +void BSpline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +void BSpline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// See link at Kochanek_Bartels_Spline for info on the basis matrix used +void Parabolic_Spline( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +void Parabolic_Spline_NormalizeX( + const Vector &p1, + const Vector &p2, + const Vector &p3, + const Vector &p4, + float t, + Vector& output ); + +// Evaluate the cubic Bernstein basis for the input parametric coordinate. +// Output is the coefficient for that basis polynomial. +float CubicBasis0( float t ); +float CubicBasis1( float t ); +float CubicBasis2( float t ); +float CubicBasis3( float t ); + +// quintic interpolating polynomial from Perlin. +// 0->0, 1->1, smooth-in between with smooth tangents +FORCEINLINE float QuinticInterpolatingPolynomial(float t) +{ + // 6t^5-15t^4+10t^3 + return t * t * t *( t * ( t* 6.0 - 15.0 ) + 10.0 ); +} + +// given a table of sorted tabulated positions, return the two indices and blendfactor to linear +// interpolate. Does a search. Can be used to find the blend value to interpolate between +// keyframes. +void GetInterpolationData( float const *pKnotPositions, + float const *pKnotValues, + int nNumValuesinList, + int nInterpolationRange, + float flPositionToInterpolateAt, + bool bWrap, + float *pValueA, + float *pValueB, + float *pInterpolationValue); +float RangeCompressor( float flValue, float flMin, float flMax, float flBase ); + +// Get the minimum distance from vOrigin to the bounding box defined by [mins,maxs] +// using voronoi regions. +// 0 is returned if the origin is inside the box. +float CalcSqrDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point ); +void CalcClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut ); +void CalcSqrDistAndClosestPointOnAABB( const Vector &mins, const Vector &maxs, const Vector &point, Vector &closestOut, float &distSqrOut ); + +inline float CalcDistanceToAABB( const Vector &mins, const Vector &maxs, const Vector &point ) +{ + float flDistSqr = CalcSqrDistanceToAABB( mins, maxs, point ); + return sqrt(flDistSqr); +} + +// Get the closest point from P to the (infinite) line through vLineA and vLineB and +// calculate the shortest distance from P to the line. +// If you pass in a value for t, it will tell you the t for (A + (B-A)t) to get the closest point. +// If the closest point lies on the segment between A and B, then 0 <= t <= 1. +void CalcClosestPointOnLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *t=0 ); +float CalcDistanceToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *t=0 ); +float CalcDistanceSqrToLine( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *t=0 ); + +// The same three functions as above, except now the line is closed between A and B. +void CalcClosestPointOnLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, Vector &vClosest, float *t=0 ); +float CalcDistanceToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *t=0 ); +float CalcDistanceSqrToLineSegment( const Vector &P, const Vector &vLineA, const Vector &vLineB, float *t=0 ); + +// A function to compute the closes line segment connnection two lines (or false if the lines are parallel, etc.) +bool CalcLineToLineIntersectionSegment( + const Vector& p1,const Vector& p2,const Vector& p3,const Vector& p4,Vector *s1,Vector *s2, + float *t1, float *t2 ); + +// The above functions in 2D +void CalcClosestPointOnLine2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, Vector2D &vClosest, float *t=0 ); +float CalcDistanceToLine2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, float *t=0 ); +float CalcDistanceSqrToLine2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, float *t=0 ); +void CalcClosestPointOnLineSegment2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, Vector2D &vClosest, float *t=0 ); +float CalcDistanceToLineSegment2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, float *t=0 ); +float CalcDistanceSqrToLineSegment2D( Vector2D const &P, Vector2D const &vLineA, Vector2D const &vLineB, float *t=0 ); + +// Init the mathlib +void MathLib_Init( float gamma = 2.2f, float texGamma = 2.2f, float brightness = 0.0f, int overbright = 2.0f, bool bAllow3DNow = true, bool bAllowSSE = true, bool bAllowSSE2 = true, bool bAllowMMX = true ); +bool MathLib_MMXEnabled( void ); +bool MathLib_SSEEnabled( void ); +bool MathLib_SSE2Enabled( void ); + +inline float Approach( float target, float value, float speed ); +float ApproachAngle( float target, float value, float speed ); +float AngleDiff( float destAngle, float srcAngle ); +float AngleDistance( float next, float cur ); +float AngleNormalize( float angle ); + +// ensure that 0 <= angle <= 360 +float AngleNormalizePositive( float angle ); + +bool AnglesAreEqual( float a, float b, float tolerance = 0.0f ); + + +void RotationDeltaAxisAngle( const QAngle &srcAngles, const QAngle &destAngles, Vector &deltaAxis, float &deltaAngle ); +void RotationDelta( const QAngle &srcAngles, const QAngle &destAngles, QAngle *out ); + +//----------------------------------------------------------------------------- +// Clips a line segment such that only the portion in the positive half-space +// of the plane remains. If the segment is entirely clipped, the vectors +// are set to vec3_invalid (all components are FLT_MAX). +// +// flBias is added to the dot product with the normal. A positive bias +// results in a more inclusive positive half-space, while a negative bias +// results in a more exclusive positive half-space. +//----------------------------------------------------------------------------- +void ClipLineSegmentToPlane( const Vector &vNormal, const Vector &vPlanePoint, Vector *p1, Vector *p2, float flBias = 0.0f ); + +void ComputeTrianglePlane( const Vector& v1, const Vector& v2, const Vector& v3, Vector& normal, float& intercept ); +int PolyFromPlane( Vector *pOutVerts, const Vector& normal, float dist, float fHalfScale = 9000.0f ); +void PolyFromPlane_SIMD( fltx4 *pOutVerts, const fltx4 & plane, float fHalfScale = 9000.0f ); +int ClipPolyToPlane( Vector *inVerts, int vertCount, Vector *outVerts, const Vector& normal, float dist, float fOnPlaneEpsilon = 0.1f ); +int ClipPolyToPlane_SIMD( fltx4 *pInVerts, int vertCount, fltx4 *pOutVerts, const fltx4& plane, float fOnPlaneEpsilon = 0.1f ); +int ClipPolyToPlane_Precise( double *inVerts, int vertCount, double *outVerts, const double *normal, double dist, double fOnPlaneEpsilon = 0.1 ); +float TetrahedronVolume( const Vector &p0, const Vector &p1, const Vector &p2, const Vector &p3 ); +float TriangleArea( const Vector &p0, const Vector &p1, const Vector &p2 ); + +//----------------------------------------------------------------------------- +// Computes a reasonable tangent space for a triangle +//----------------------------------------------------------------------------- +void CalcTriangleTangentSpace( const Vector &p0, const Vector &p1, const Vector &p2, + const Vector2D &t0, const Vector2D &t1, const Vector2D& t2, + Vector &sVect, Vector &tVect ); + +//----------------------------------------------------------------------------- +// Transforms a AABB into another space; which will inherently grow the box. +//----------------------------------------------------------------------------- +void TransformAABB( const matrix3x4_t &in1, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ); + +//----------------------------------------------------------------------------- +// Uses the inverse transform of in1 +//----------------------------------------------------------------------------- +void ITransformAABB( const matrix3x4_t &in1, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &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 &in1, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ); + +//----------------------------------------------------------------------------- +// Uses the inverse transform of in1 +//----------------------------------------------------------------------------- +void IRotateAABB( const matrix3x4_t &in1, const Vector &vecMinsIn, const Vector &vecMaxsIn, Vector &vecMinsOut, Vector &vecMaxsOut ); + +//----------------------------------------------------------------------------- +// Transform a plane +//----------------------------------------------------------------------------- +inline void MatrixTransformPlane( const matrix3x4_t &src, const cplane_t &inPlane, cplane_t &outPlane ) +{ + // What we want to do is the following: + // 1) transform the normal into the new space. + // 2) Determine a point on the old plane given by plane dist * plane normal + // 3) Transform that point into the new space + // 4) Plane dist = DotProduct( new normal, new point ) + + // An optimized version, which works if the plane is orthogonal. + // 1) Transform the normal into the new space + // 2) Realize that transforming the old plane point into the new space + // is given by [ d * n'x + Tx, d * n'y + Ty, d * n'z + Tz ] + // where d = old plane dist, n' = transformed normal, Tn = translational component of transform + // 3) Compute the new plane dist using the dot product of the normal result of #2 + + // For a correct result, this should be an inverse-transpose matrix + // but that only matters if there are nonuniform scale or skew factors in this matrix. + VectorRotate( inPlane.normal, src, outPlane.normal ); + outPlane.dist = inPlane.dist * DotProduct( outPlane.normal, outPlane.normal ); + outPlane.dist += outPlane.normal.x * src[0][3] + outPlane.normal.y * src[1][3] + outPlane.normal.z * src[2][3]; +} + +inline void MatrixITransformPlane( const matrix3x4_t &src, const cplane_t &inPlane, cplane_t &outPlane ) +{ + // The trick here is that Tn = translational component of transform, + // but for an inverse transform, Tn = - R^-1 * T + Vector vecTranslation; + MatrixGetColumn( src, 3, vecTranslation ); + + Vector vecInvTranslation; + VectorIRotate( vecTranslation, src, vecInvTranslation ); + + VectorIRotate( inPlane.normal, src, outPlane.normal ); + outPlane.dist = inPlane.dist * DotProduct( outPlane.normal, outPlane.normal ); + outPlane.dist -= outPlane.normal.x * vecInvTranslation[0] + outPlane.normal.y * vecInvTranslation[1] + outPlane.normal.z * vecInvTranslation[2]; +} + +int CeilPow2( int in ); +int FloorPow2( int in ); + +FORCEINLINE float * UnpackNormal_HEND3N( const unsigned int *pPackedNormal, float *pNormal ) +{ + int temp[3]; + temp[0] = ((*pPackedNormal >> 0L) & 0x7ff); + if ( temp[0] & 0x400 ) + { + temp[0] = 2048 - temp[0]; + } + temp[1] = ((*pPackedNormal >> 11L) & 0x7ff); + if ( temp[1] & 0x400 ) + { + temp[1] = 2048 - temp[1]; + } + temp[2] = ((*pPackedNormal >> 22L) & 0x3ff); + if ( temp[2] & 0x200 ) + { + temp[2] = 1024 - temp[2]; + } + pNormal[0] = (float)temp[0] * 1.0f/1023.0f; + pNormal[1] = (float)temp[1] * 1.0f/1023.0f; + pNormal[2] = (float)temp[2] * 1.0f/511.0f; + return pNormal; +} + + +FORCEINLINE unsigned int * PackNormal_HEND3N( const float *pNormal, unsigned int *pPackedNormal ) +{ + int temp[3]; + + temp[0] = Float2Int( pNormal[0] * 1023.0f ); + temp[1] = Float2Int( pNormal[1] * 1023.0f ); + temp[2] = Float2Int( pNormal[2] * 511.0f ); + + // the normal is out of bounds, determine the source and fix + // clamping would be even more of a slowdown here + Assert( temp[0] >= -1023 && temp[0] <= 1023 ); + Assert( temp[1] >= -1023 && temp[1] <= 1023 ); + Assert( temp[2] >= -511 && temp[2] <= 511 ); + + *pPackedNormal = ( ( temp[2] & 0x3ff ) << 22L ) | + ( ( temp[1] & 0x7ff ) << 11L ) | + ( ( temp[0] & 0x7ff ) << 0L ); + return pPackedNormal; +} + + +FORCEINLINE unsigned int * PackNormal_HEND3N( float nx, float ny, float nz, unsigned int *pPackedNormal ) +{ + int temp[3]; + + temp[0] = Float2Int( nx * 1023.0f ); + temp[1] = Float2Int( ny * 1023.0f ); + temp[2] = Float2Int( nz * 511.0f ); + + // the normal is out of bounds, determine the source and fix + // clamping would be even more of a slowdown here + Assert( temp[0] >= -1023 && temp[0] <= 1023 ); + Assert( temp[1] >= -1023 && temp[1] <= 1023 ); + Assert( temp[2] >= -511 && temp[2] <= 511 ); + + *pPackedNormal = ( ( temp[2] & 0x3ff ) << 22L ) | + ( ( temp[1] & 0x7ff ) << 11L ) | + ( ( temp[0] & 0x7ff ) << 0L ); + return pPackedNormal; +} + + + +FORCEINLINE float * UnpackNormal_SHORT2( const unsigned int *pPackedNormal, float *pNormal, bool bIsTangent = FALSE ) +{ + // Unpacks from Jason's 2-short format (fills in a 4th binormal-sign (+1/-1) value, if this is a tangent vector) + + // FIXME: short math is slow on 360 - use ints here instead (bit-twiddle to deal w/ the sign bits) + short iX = (*pPackedNormal & 0x0000FFFF); + short iY = (*pPackedNormal & 0xFFFF0000) >> 16; + + float zSign = +1; + if ( iX < 0 ) + { + zSign = -1; + iX = -iX; + } + float tSign = +1; + if ( iY < 0 ) + { + tSign = -1; + iY = -iY; + } + + pNormal[0] = ( iX - 16384.0f ) / 16384.0f; + pNormal[1] = ( iY - 16384.0f ) / 16384.0f; + float mag = ( pNormal[0]*pNormal[0] + pNormal[1]*pNormal[1] ); + if ( mag > 1.0f ) + { + mag = 1.0f; + } + pNormal[2] = zSign*sqrtf( 1.0f - mag ); + if ( bIsTangent ) + { + pNormal[3] = tSign; + } + + return pNormal; +} + +FORCEINLINE unsigned int * PackNormal_SHORT2( float nx, float ny, float nz, unsigned int *pPackedNormal, float binormalSign = +1.0f ) +{ + // Pack a vector (ASSUMED TO BE NORMALIZED) into Jason's 4-byte (SHORT2) format. + // This simply reconstructs Z from X & Y. It uses the sign bits of the X & Y coords + // to reconstruct the sign of Z and, if this is a tangent vector, the sign of the + // binormal (this is needed because tangent/binormal vectors are supposed to follow + // UV gradients, but shaders reconstruct the binormal from the tangent and normal + // assuming that they form a right-handed basis). + + nx += 1; // [-1,+1] -> [0,2] + ny += 1; + nx *= 16384.0f; // [ 0, 2] -> [0,32768] + ny *= 16384.0f; + + // '0' and '32768' values are invalid encodings + nx = MAX( nx, 1.0f ); // Make sure there are no zero values + ny = MAX( ny, 1.0f ); + nx = MIN( nx, 32767.0f ); // Make sure there are no 32768 values + ny = MIN( ny, 32767.0f ); + + if ( nz < 0.0f ) + nx = -nx; // Set the sign bit for z + + ny *= binormalSign; // Set the sign bit for the binormal (use when encoding a tangent vector) + + // FIXME: short math is slow on 360 - use ints here instead (bit-twiddle to deal w/ the sign bits), also use Float2Int() + short sX = (short)nx; // signed short [1,32767] + short sY = (short)ny; + + *pPackedNormal = ( sX & 0x0000FFFF ) | ( sY << 16 ); // NOTE: The mask is necessary (if sX is negative and cast to an int...) + + return pPackedNormal; +} + +FORCEINLINE unsigned int * PackNormal_SHORT2( const float *pNormal, unsigned int *pPackedNormal, float binormalSign = +1.0f ) +{ + return PackNormal_SHORT2( pNormal[0], pNormal[1], pNormal[2], pPackedNormal, binormalSign ); +} + +// Unpacks a UBYTE4 normal (for a tangent, the result's fourth component receives the binormal 'sign') +FORCEINLINE float * UnpackNormal_UBYTE4( const unsigned int *pPackedNormal, float *pNormal, bool bIsTangent = FALSE ) +{ + unsigned char cX, cY; + if ( bIsTangent ) + { + cX = *pPackedNormal >> 16; // Unpack Z + cY = *pPackedNormal >> 24; // Unpack W + } + else + { + cX = *pPackedNormal >> 0; // Unpack X + cY = *pPackedNormal >> 8; // Unpack Y + } + + float x = cX - 128.0f; + float y = cY - 128.0f; + float z; + + float zSignBit = x < 0 ? 1.0f : 0.0f; // z and t negative bits (like slt asm instruction) + float tSignBit = y < 0 ? 1.0f : 0.0f; + float zSign = -( 2*zSignBit - 1 ); // z and t signs + float tSign = -( 2*tSignBit - 1 ); + + x = x*zSign - zSignBit; // 0..127 + y = y*tSign - tSignBit; + x = x - 64; // -64..63 + y = y - 64; + + float xSignBit = x < 0 ? 1.0f : 0.0f; // x and y negative bits (like slt asm instruction) + float ySignBit = y < 0 ? 1.0f : 0.0f; + float xSign = -( 2*xSignBit - 1 ); // x and y signs + float ySign = -( 2*ySignBit - 1 ); + + x = ( x*xSign - xSignBit ) / 63.0f; // 0..1 range + y = ( y*ySign - ySignBit ) / 63.0f; + z = 1.0f - x - y; + + float oolen = 1.0f / sqrt( x*x + y*y + z*z ); // Normalize and + x *= oolen * xSign; // Recover signs + y *= oolen * ySign; + z *= oolen * zSign; + + pNormal[0] = x; + pNormal[1] = y; + pNormal[2] = z; + if ( bIsTangent ) + { + pNormal[3] = tSign; + } + + return pNormal; +} + +////////////////////////////////////////////////////////////////////////////// +// See: http://www.oroboro.com/rafael/docserv.php/index/programming/article/unitv2 +// +// UBYTE4 encoding, using per-octant projection onto x+y+z=1 +// Assume input vector is already unit length +// +// binormalSign specifies 'sign' of binormal, stored in t sign bit of tangent +// (lets the shader know whether norm/tan/bin form a right-handed basis) +// +// bIsTangent is used to specify which WORD of the output to store the data +// The expected usage is to call once with the normal and once with +// the tangent and binormal sign flag, bitwise OR'ing the returned DWORDs +FORCEINLINE unsigned int * PackNormal_UBYTE4( float nx, float ny, float nz, unsigned int *pPackedNormal, bool bIsTangent = false, float binormalSign = +1.0f ) +{ + float xSign = nx < 0.0f ? -1.0f : 1.0f; // -1 or 1 sign + float ySign = ny < 0.0f ? -1.0f : 1.0f; + float zSign = nz < 0.0f ? -1.0f : 1.0f; + float tSign = binormalSign; + Assert( ( binormalSign == +1.0f ) || ( binormalSign == -1.0f ) ); + + float xSignBit = 0.5f*( 1 - xSign ); // [-1,+1] -> [1,0] + float ySignBit = 0.5f*( 1 - ySign ); // 1 is negative bit (like slt instruction) + float zSignBit = 0.5f*( 1 - zSign ); + float tSignBit = 0.5f*( 1 - binormalSign ); + + float absX = xSign*nx; // 0..1 range (abs) + float absY = ySign*ny; + float absZ = zSign*nz; + + float xbits = absX / ( absX + absY + absZ ); // Project onto x+y+z=1 plane + float ybits = absY / ( absX + absY + absZ ); + + xbits *= 63; // 0..63 + ybits *= 63; + + xbits = xbits * xSign - xSignBit; // -64..63 range + ybits = ybits * ySign - ySignBit; + xbits += 64.0f; // 0..127 range + ybits += 64.0f; + + xbits = xbits * zSign - zSignBit; // Negate based on z and t + ybits = ybits * tSign - tSignBit; // -128..127 range + + xbits += 128.0f; // 0..255 range + ybits += 128.0f; + + unsigned char cX = (unsigned char) xbits; + unsigned char cY = (unsigned char) ybits; + + if ( !bIsTangent ) + *pPackedNormal = (cX << 0) | (cY << 8); // xy for normal + else + *pPackedNormal = (cX << 16) | (cY << 24); // zw for tangent + + return pPackedNormal; +} + +FORCEINLINE unsigned int * PackNormal_UBYTE4( const float *pNormal, unsigned int *pPackedNormal, bool bIsTangent = false, float binormalSign = +1.0f ) +{ + return PackNormal_UBYTE4( pNormal[0], pNormal[1], pNormal[2], pPackedNormal, bIsTangent, binormalSign ); +} + +FORCEINLINE void RGB2YUV( int &nR, int &nG, int &nB, float &fY, float &fU, float &fV, bool bApplySaturationCurve ) +{ + // YUV conversion: + // |Y| | 0.299f 0.587f 0.114f | |R| + // |U| = | -0.14713f -0.28886f 0.436f | x |G| + // |V| | 0.615f -0.51499f -0.10001f | |B| + // + // The coefficients in the first row sum to one, whereas the 2nd and 3rd rows each sum to zero (UV (0,0) means greyscale). + // Ranges are Y [0,1], U [-0.436,+0.436] and V [-0.615,+0.615]. + // We scale and offset to [0,1] and allow the caller to round as they please. + + fY = ( 0.29900f*nR + 0.58700f*nG + 0.11400f*nB ) / 255; + fU = ( -0.14713f*nR + -0.28886f*nG + 0.43600f*nB )*( 0.5f / 0.436f ) / 255 + 0.5f; + fV = ( 0.61500f*nR + -0.51499f*nG + -0.10001f*nB )*( 0.5f / 0.615f ) / 255 + 0.5f; + + if ( bApplySaturationCurve ) + { + // Apply a curve to saturation, and snap-to-grey for low saturations + const float SNAP_TO_GREY = 0;//0.0125f; Disabled, saturation curve seems sufficient + float dX, dY, sat, scale; + dX = 2*( fU - 0.5f ); + dY = 2*( fV - 0.5f ); + sat = sqrtf( dX*dX + dY*dY ); + sat = clamp( ( sat*( 1 + SNAP_TO_GREY ) - SNAP_TO_GREY ), 0, 1 ); + scale = ( sat == 0 ) ? 0 : MIN( ( sqrtf( sat ) / sat ), 4.0f ); + fU = 0.5f + scale*( fU - 0.5f ); + fV = 0.5f + scale*( fV - 0.5f ); + } +} + +#ifdef _X360 +// Used for direct CPU access to VB data on 360 (used by shaderapi, studiorender and engine) +struct VBCPU_AccessInfo_t +{ + // Points to the GPU data pointer in the CVertexBuffer struct (VB data can be relocated during level transitions) + const byte **ppBaseAddress; + // pBaseAddress should be computed from ppBaseAddress immediately before use + const byte *pBaseAddress; + int nStride; + int nPositionOffset; + int nTexCoord0_Offset; + int nNormalOffset; + int nBoneIndexOffset; + int nBoneWeightOffset; + int nCompressionType; + // TODO: if needed, add colour and tangents +}; +#endif + +//----------------------------------------------------------------------------- +// Convert RGB to HSV +//----------------------------------------------------------------------------- +void RGBtoHSV( const Vector &rgb, Vector &hsv ); + + +//----------------------------------------------------------------------------- +// Convert HSV to RGB +//----------------------------------------------------------------------------- +void HSVtoRGB( const Vector &hsv, Vector &rgb ); + + +//----------------------------------------------------------------------------- +// Fast version of pow and log +//----------------------------------------------------------------------------- +#ifndef _PS3 // these actually aren't fast (or correct) on the PS3 +float FastLog2(float i); // log2( i ) +float FastPow2(float i); // 2^i +float FastPow(float a, float b); // a^b +float FastPow10( float i ); // 10^i +#else +inline float FastLog2(float i) {return logbf(i);} // log2( i ) +inline float FastPow2(float i) {return exp2f(i);} // 2^i +inline float FastPow(float a, float b) {return powf(a,b);} // a^b +#define LOGBASE2OF10 3.3219280948873623478703194294893901758648313930 +inline float FastPow10( float i ) { return exp2f( i * LOGBASE2OF10 ); } // 10^i, transform to base two, so log2(10^y) = y log2(10) . log2(10) = 3.3219280948873623478703194294893901758648313930 +#endif + +//----------------------------------------------------------------------------- +// For testing float equality +//----------------------------------------------------------------------------- + +inline bool CloseEnough( float a, float b, float epsilon = EQUAL_EPSILON ) +{ + return fabs( a - b ) <= epsilon; +} + +inline bool CloseEnough( const Vector &a, const Vector &b, float epsilon = EQUAL_EPSILON ) +{ + return fabs( a.x - b.x ) <= epsilon && + fabs( a.y - b.y ) <= epsilon && + fabs( a.z - b.z ) <= epsilon; +} + +// Fast compare +// maxUlps is the maximum error in terms of Units in the Last Place. This +// specifies how big an error we are willing to accept in terms of the value +// of the least significant digit of the floating point number�s +// representation. maxUlps can also be interpreted in terms of how many +// representable floats we are willing to accept between A and B. +// This function will allow maxUlps-1 floats between A and B. +bool AlmostEqual(float a, float b, int maxUlps = 10); + +inline bool AlmostEqual( const Vector &a, const Vector &b, int maxUlps = 10) +{ + return AlmostEqual( a.x, b.x, maxUlps ) && + AlmostEqual( a.y, b.y, maxUlps ) && + AlmostEqual( a.z, b.z, maxUlps ); +} + +inline float Approach( float target, float value, float speed ) +{ + float delta = target - value; + +#if defined(_X360) || defined( _PS3 ) // use conditional move for speed on 360 + + return fsel( delta-speed, // delta >= speed ? + value + speed, // if delta == speed, then value + speed == value + delta == target + fsel( (-speed) - delta, // delta <= -speed + value - speed, + target ) + ); // delta < speed && delta > -speed + +#else + + if ( delta > speed ) + value += speed; + else if ( delta < -speed ) + value -= speed; + else + value = target; + + return value; + +#endif +} + +// on PPC we can do this truncate without converting to int +#if defined(_X360) || defined(_PS3) +inline double TruncateFloatToIntAsFloat( double flVal ) +{ +#if defined(_X360) + double flIntFormat = __fctiwz( flVal ); + return __fcfid( flIntFormat ); +#elif defined(_PS3) + double flIntFormat = __builtin_fctiwz( flVal ); + return __builtin_fcfid( flIntFormat ); +#endif +} +#endif + +inline double SubtractIntegerPart( double flVal ) +{ +#if defined(_X360) || defined(_PS3) + return flVal - TruncateFloatToIntAsFloat(flVal); +#else + return flVal - int(flVal); +#endif +} +#endif // MATH_BASE_H + diff --git a/external/vpc/public/mathlib/vector.h b/external/vpc/public/mathlib/vector.h new file mode 100644 index 0000000..dac51a4 --- /dev/null +++ b/external/vpc/public/mathlib/vector.h @@ -0,0 +1,2633 @@ +//====== Copyright 1996-2005, Valve Corporation, All rights reserved. =======// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// + +#ifndef VECTOR_H +#define VECTOR_H + +#ifdef _WIN32 +#pragma once +#endif + +#include <math.h> +#include <float.h> + +// For vec_t, put this somewhere else? +#include "tier0/basetypes.h" + +#if defined( _PS3 ) +//#include <ssemath.h> +#include <vectormath/c/vectormath_aos.h> +#include "platform.h" +#include "mathlib/math_pfns.h" +#endif + +#ifndef PLATFORM_PPC // we want our linux with xmm support +// For MMX intrinsics +#include <xmmintrin.h> +#endif + +#ifndef ALIGN16_POST +#define ALIGN16_POST +#endif + +#include "tier0/dbg.h" +#include "tier0/platform.h" +#include "tier0/threadtools.h" +#include "mathlib/vector2d.h" +#include "mathlib/math_pfns.h" +#include "tier0/memalloc.h" +#include "vstdlib/random.h" +// Uncomment this to add extra Asserts to check for NANs, uninitialized vecs, etc. +//#define VECTOR_PARANOIA 1 + +// Uncomment this to make sure we don't do anything slow with our vectors +//#define VECTOR_NO_SLOW_OPERATIONS 1 + + +// Used to make certain code easier to read. +#define X_INDEX 0 +#define Y_INDEX 1 +#define Z_INDEX 2 + + +#ifdef VECTOR_PARANOIA +#define CHECK_VALID( _v) Assert( (_v).IsValid() ) +#else +#ifdef GNUC +#define CHECK_VALID( _v) +#else +#define CHECK_VALID( _v) 0 +#endif +#endif + +#define VecToString(v) (static_cast<const char *>(CFmtStr("(%f, %f, %f)", (v).x, (v).y, (v).z))) // ** Note: this generates a temporary, don't hold reference! + +class VectorByValue; + +//========================================================= +// 3D Vector +//========================================================= +class Vector +{ +public: + // Members + vec_t x, y, z; + + // Construction/destruction: + Vector(void); + Vector(vec_t X, vec_t Y, vec_t Z); + + // Initialization + void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f); + // TODO (Ilya): Should there be an init that takes a single float for consistency? + + // Got any nasty NAN's? + bool IsValid() const; + void Invalidate(); + + // array access... + vec_t operator[](int i) const; + vec_t& operator[](int i); + + // Base address... + vec_t* Base(); + vec_t const* Base() const; + + // Cast to Vector2D... + Vector2D& AsVector2D(); + const Vector2D& AsVector2D() const; + + // Initialization methods + void Random( vec_t minVal, vec_t maxVal ); + inline void Zero(); ///< zero out a vector + + // equality + bool operator==(const Vector& v) const; + bool operator!=(const Vector& v) const; + + // arithmetic operations + FORCEINLINE Vector& operator+=(const Vector &v); + FORCEINLINE Vector& operator-=(const Vector &v); + FORCEINLINE Vector& operator*=(const Vector &v); + FORCEINLINE Vector& operator*=(float s); + FORCEINLINE Vector& operator/=(const Vector &v); + FORCEINLINE Vector& operator/=(float s); + FORCEINLINE Vector& operator+=(float fl) ; ///< broadcast add + FORCEINLINE Vector& operator-=(float fl) ; ///< broadcast sub + +// negate the vector components + void Negate(); + + // Get the vector's magnitude. + inline vec_t Length() const; + + // Get the vector's magnitude squared. + FORCEINLINE vec_t LengthSqr(void) const + { + CHECK_VALID(*this); + return (x*x + y*y + z*z); + } + + // Get one over the vector's length + // via fast hardware approximation + inline vec_t LengthRecipFast(void) const + { + return FastRSqrtFast( LengthSqr() ); + } + + // return true if this vector is (0,0,0) within tolerance + bool IsZero( float tolerance = 0.01f ) const + { + return (x > -tolerance && x < tolerance && + y > -tolerance && y < tolerance && + z > -tolerance && z < tolerance); + } + + + // return true if this vector is exactly (0,0,0) -- only fast if vector is coming from memory, not registers + inline bool IsZeroFast( ) const RESTRICT + { + COMPILE_TIME_ASSERT( sizeof(vec_t) == sizeof(int) ); + return ( *(const int *)(&x) == 0 && + *(const int *)(&y) == 0 && + *(const int *)(&z) == 0 ); + } + + vec_t NormalizeInPlace(); + Vector Normalized() const; + bool IsLengthGreaterThan( float val ) const; + bool IsLengthLessThan( float val ) const; + + // check if a vector is within the box defined by two other vectors + FORCEINLINE bool WithinAABox( Vector const &boxmin, Vector const &boxmax); + + // Get the distance from this vector to the other one. + vec_t DistTo(const Vector &vOther) const; + + // Get the distance from this vector to the other one squared. + // NJS: note, VC wasn't inlining it correctly in several deeply nested inlines due to being an 'out of line' inline. + // may be able to tidy this up after switching to VC7 + FORCEINLINE vec_t DistToSqr(const Vector &vOther) const + { + Vector delta; + + delta.x = x - vOther.x; + delta.y = y - vOther.y; + delta.z = z - vOther.z; + + return delta.LengthSqr(); + } + + // Copy + void CopyToArray(float* rgfl) const; + + // Multiply, add, and assign to this (ie: *this = a + b * scalar). This + // is about 12% faster than the actual vector equation (because it's done per-component + // rather than per-vector). + void MulAdd(const Vector& a, const Vector& b, float scalar); + + // Dot product. + vec_t Dot(const Vector& vOther) const; + + // assignment + Vector& operator=(const Vector &vOther); + + // returns 0, 1, 2 corresponding to the component with the largest absolute value + inline int LargestComponent() const; + + // 2d + vec_t Length2D(void) const; + vec_t Length2DSqr(void) const; + + /// get the component of this vector parallel to some other given vector + inline Vector ProjectOnto( const Vector& onto ); + + operator VectorByValue &() { return *((VectorByValue *)(this)); } + operator const VectorByValue &() const { return *((const VectorByValue *)(this)); } + +#ifndef VECTOR_NO_SLOW_OPERATIONS + // copy constructors +// Vector(const Vector &vOther); + + // arithmetic operations + Vector operator-(void) const; + + Vector operator+(const Vector& v) const; + Vector operator-(const Vector& v) const; + Vector operator*(const Vector& v) const; + Vector operator/(const Vector& v) const; + Vector operator*(float fl) const; + Vector operator/(float fl) const; + + // Cross product between two vectors. + Vector Cross(const Vector &vOther) const; + + // Returns a vector with the min or max in X, Y, and Z. + Vector Min(const Vector &vOther) const; + Vector Max(const Vector &vOther) const; + +#else + +private: + // No copy constructors allowed if we're in optimal mode + Vector(const Vector& vOther); +#endif +}; + + + +#define USE_M64S defined( PLATFORM_WINDOWS_PC ) + + + +//========================================================= +// 4D Short Vector (aligned on 8-byte boundary) +//========================================================= +class ALIGN8 ShortVector +{ +public: + + short x, y, z, w; + + // Initialization + void Init(short ix = 0, short iy = 0, short iz = 0, short iw = 0 ); + + +#if USE_M64S + __m64 &AsM64() { return *(__m64*)&x; } + const __m64 &AsM64() const { return *(const __m64*)&x; } +#endif + + // Setter + void Set( const ShortVector& vOther ); + void Set( const short ix, const short iy, const short iz, const short iw ); + + // array access... + short operator[](int i) const; + short& operator[](int i); + + // Base address... + short* Base(); + short const* Base() const; + + // equality + bool operator==(const ShortVector& v) const; + bool operator!=(const ShortVector& v) const; + + // Arithmetic operations + FORCEINLINE ShortVector& operator+=(const ShortVector &v); + FORCEINLINE ShortVector& operator-=(const ShortVector &v); + FORCEINLINE ShortVector& operator*=(const ShortVector &v); + FORCEINLINE ShortVector& operator*=(float s); + FORCEINLINE ShortVector& operator/=(const ShortVector &v); + FORCEINLINE ShortVector& operator/=(float s); + FORCEINLINE ShortVector operator*(float fl) const; + +private: + + // No copy constructors allowed if we're in optimal mode +// ShortVector(ShortVector const& vOther); + + // No assignment operators either... +// ShortVector& operator=( ShortVector const& src ); + +} ALIGN8_POST; + + + + + + +//========================================================= +// 4D Integer Vector +//========================================================= +class IntVector4D +{ +public: + + int x, y, z, w; + + // Initialization + void Init(int ix = 0, int iy = 0, int iz = 0, int iw = 0 ); + +#if USE_M64S + __m64 &AsM64() { return *(__m64*)&x; } + const __m64 &AsM64() const { return *(const __m64*)&x; } +#endif + + // Setter + void Set( const IntVector4D& vOther ); + void Set( const int ix, const int iy, const int iz, const int iw ); + + // array access... + int operator[](int i) const; + int& operator[](int i); + + // Base address... + int* Base(); + int const* Base() const; + + // equality + bool operator==(const IntVector4D& v) const; + bool operator!=(const IntVector4D& v) const; + + // Arithmetic operations + FORCEINLINE IntVector4D& operator+=(const IntVector4D &v); + FORCEINLINE IntVector4D& operator-=(const IntVector4D &v); + FORCEINLINE IntVector4D& operator*=(const IntVector4D &v); + FORCEINLINE IntVector4D& operator*=(float s); + FORCEINLINE IntVector4D& operator/=(const IntVector4D &v); + FORCEINLINE IntVector4D& operator/=(float s); + FORCEINLINE IntVector4D operator*(float fl) const; + +private: + + // No copy constructors allowed if we're in optimal mode + // IntVector4D(IntVector4D const& vOther); + + // No assignment operators either... + // IntVector4D& operator=( IntVector4D const& src ); + +}; + + + +//----------------------------------------------------------------------------- +// Allows us to specifically pass the vector by value when we need to +//----------------------------------------------------------------------------- +class VectorByValue : public Vector +{ +public: + // Construction/destruction: + VectorByValue(void) : Vector() {} + VectorByValue(vec_t X, vec_t Y, vec_t Z) : Vector( X, Y, Z ) {} + VectorByValue(const VectorByValue& vOther) { *this = vOther; } +}; + + +//----------------------------------------------------------------------------- +// Utility to simplify table construction. No constructor means can use +// traditional C-style initialization +//----------------------------------------------------------------------------- +class TableVector +{ +public: + vec_t x, y, z; + + operator Vector &() { return *((Vector *)(this)); } + operator const Vector &() const { return *((const Vector *)(this)); } + + // array access... + inline vec_t& operator[](int i) + { + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; + } + + inline vec_t operator[](int i) const + { + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; + } +}; + + +//----------------------------------------------------------------------------- +// Here's where we add all those lovely SSE optimized routines +//----------------------------------------------------------------------------- + +class ALIGN16 VectorAligned : public Vector +{ +public: + inline VectorAligned(void) {}; + inline VectorAligned(vec_t X, vec_t Y, vec_t Z) + { + Init(X,Y,Z); + } + +#ifdef VECTOR_NO_SLOW_OPERATIONS + +private: + // No copy constructors allowed if we're in optimal mode + VectorAligned(const VectorAligned& vOther); + VectorAligned(const Vector &vOther); + +#else +public: + explicit VectorAligned(const Vector &vOther) + { + Init(vOther.x, vOther.y, vOther.z); + } + + VectorAligned& operator=(const Vector &vOther) + { + Init(vOther.x, vOther.y, vOther.z); + return *this; + } + + VectorAligned& operator=(const VectorAligned &vOther) + { + // we know we're aligned, so use simd + // we can't use the convenient abstract interface coz it gets declared later +#ifdef _X360 + XMStoreVector4A(Base(), XMLoadVector4A(vOther.Base())); +#elif _WIN32 + _mm_store_ps(Base(), _mm_load_ps( vOther.Base() )); +#else + Init(vOther.x, vOther.y, vOther.z); +#endif + return *this; + } + + +#endif + float w; // this space is used anyway + +#if !defined(NO_MALLOC_OVERRIDE) + void* operator new[] ( size_t nSize) + { + return MemAlloc_AllocAligned(nSize, 16); + } + + void* operator new[] ( size_t nSize, const char *pFileName, int nLine) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + + void* operator new[] ( size_t nSize, int /*nBlockUse*/, const char *pFileName, int nLine) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + + void operator delete[] ( void* p) + { + MemAlloc_FreeAligned(p); + } + + void operator delete[] ( void* p, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + void operator delete[] ( void* p, int /*nBlockUse*/, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + // please don't allocate a single quaternion... + void* operator new ( size_t nSize ) + { + return MemAlloc_AllocAligned(nSize, 16); + } + void* operator new ( size_t nSize, const char *pFileName, int nLine ) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + void* operator new ( size_t nSize, int /*nBlockUse*/, const char *pFileName, int nLine ) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + void operator delete ( void* p) + { + MemAlloc_FreeAligned(p); + } + + void operator delete ( void* p, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + void operator delete ( void* p, int /*nBlockUse*/, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } +#endif +} ALIGN16_POST; + +//----------------------------------------------------------------------------- +// Vector related operations +//----------------------------------------------------------------------------- + +// Vector clear +FORCEINLINE void VectorClear( Vector& a ); + +// Copy +FORCEINLINE void VectorCopy( const Vector& src, Vector& dst ); + +// Vector arithmetic +FORCEINLINE void VectorAdd( const Vector& a, const Vector& b, Vector& result ); +FORCEINLINE void VectorSubtract( const Vector& a, const Vector& b, Vector& result ); +FORCEINLINE void VectorMultiply( const Vector& a, vec_t b, Vector& result ); +FORCEINLINE void VectorMultiply( const Vector& a, const Vector& b, Vector& result ); +FORCEINLINE void VectorDivide( const Vector& a, vec_t b, Vector& result ); +FORCEINLINE void VectorDivide( const Vector& a, const Vector& b, Vector& result ); +inline void VectorScale ( const Vector& in, vec_t scale, Vector& result ); +void VectorMA( const Vector& start, float scale, const Vector& direction, Vector& dest ); + +// Vector equality with tolerance +bool VectorsAreEqual( const Vector& src1, const Vector& src2, float tolerance = 0.0f ); + +#define VectorExpand(v) (v).x, (v).y, (v).z + + +// Normalization +// FIXME: Can't use quite yet +//vec_t VectorNormalize( Vector& v ); + +// Length +inline vec_t VectorLength( const Vector& v ); + +// Dot Product +FORCEINLINE vec_t DotProduct(const Vector& a, const Vector& b); + +// Cross product +void CrossProduct(const Vector& a, const Vector& b, Vector& result ); + +// Store the min or max of each of x, y, and z into the result. +void VectorMin( const Vector &a, const Vector &b, Vector &result ); +void VectorMax( const Vector &a, const Vector &b, Vector &result ); + +// Linearly interpolate between two vectors +void VectorLerp(const Vector& src1, const Vector& src2, vec_t t, Vector& dest ); +Vector VectorLerp(const Vector& src1, const Vector& src2, vec_t t ); + +FORCEINLINE Vector ReplicateToVector( float x ) +{ + return Vector( x, x, x ); +} + +FORCEINLINE bool PointWithinViewAngle( Vector const &vecSrcPosition, + Vector const &vecTargetPosition, + Vector const &vecLookDirection, float flCosHalfFOV ) +{ + Vector vecDelta = vecTargetPosition - vecSrcPosition; + float cosDiff = DotProduct( vecLookDirection, vecDelta ); + + if ( flCosHalfFOV <= 0 ) // >180 + { + // signs are different, answer is implicit + if ( cosDiff > 0 ) + return true; + + // a/sqrt(b) > c == a^2 < b * c ^2 + // IFF left and right sides are <= 0 + float flLen2 = vecDelta.LengthSqr(); + return ( cosDiff * cosDiff <= flLen2 * flCosHalfFOV * flCosHalfFOV ); + } + else // flCosHalfFOV > 0 + { + // signs are different, answer is implicit + if ( cosDiff < 0 ) + return false; + + // a/sqrt(b) > c == a^2 > b * c ^2 + // IFF left and right sides are >= 0 + float flLen2 = vecDelta.LengthSqr(); + return ( cosDiff * cosDiff >= flLen2 * flCosHalfFOV * flCosHalfFOV ); + } +} + + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +// Cross product +Vector CrossProduct( const Vector& a, const Vector& b ); + +// Random vector creation +Vector RandomVector( vec_t minVal, vec_t maxVal ); + +#endif + +float RandomVectorInUnitSphere( Vector *pVector ); +float RandomVectorInUnitCircle( Vector2D *pVector ); + + +//----------------------------------------------------------------------------- +// +// Inlined Vector methods +// +//----------------------------------------------------------------------------- + + +//----------------------------------------------------------------------------- +// constructors +//----------------------------------------------------------------------------- +inline Vector::Vector(void) +{ +#ifdef _DEBUG +#ifdef VECTOR_PARANOIA + // Initialize to NAN to catch errors + x = y = z = VEC_T_NAN; +#endif +#endif +} + +inline Vector::Vector(vec_t X, vec_t Y, vec_t Z) +{ + x = X; y = Y; z = Z; + CHECK_VALID(*this); +} + +//inline Vector::Vector(const float *pFloat) +//{ +// Assert( pFloat ); +// x = pFloat[0]; y = pFloat[1]; z = pFloat[2]; +// CHECK_VALID(*this); +//} + +#if 0 +//----------------------------------------------------------------------------- +// copy constructor +//----------------------------------------------------------------------------- + +inline Vector::Vector(const Vector &vOther) +{ + CHECK_VALID(vOther); + x = vOther.x; y = vOther.y; z = vOther.z; +} +#endif + +//----------------------------------------------------------------------------- +// initialization +//----------------------------------------------------------------------------- + +inline void Vector::Init( vec_t ix, vec_t iy, vec_t iz ) +{ + x = ix; y = iy; z = iz; + CHECK_VALID(*this); +} + +inline void Vector::Random( vec_t minVal, vec_t maxVal ) +{ + x = RandomFloat( minVal, maxVal ); + y = RandomFloat( minVal, maxVal ); + z = RandomFloat( minVal, maxVal ); + CHECK_VALID(*this); +} + +// This should really be a single opcode on the PowerPC (move r0 onto the vec reg) +inline void Vector::Zero() +{ + x = y = z = 0.0f; +} + +inline void VectorClear( Vector& a ) +{ + a.x = a.y = a.z = 0.0f; +} + +//----------------------------------------------------------------------------- +// assignment +//----------------------------------------------------------------------------- + +inline Vector& Vector::operator=(const Vector &vOther) +{ + CHECK_VALID(vOther); + x=vOther.x; y=vOther.y; z=vOther.z; + return *this; +} + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline vec_t& Vector::operator[](int i) +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + +inline vec_t Vector::operator[](int i) const +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + + +//----------------------------------------------------------------------------- +// Base address... +//----------------------------------------------------------------------------- +inline vec_t* Vector::Base() +{ + return (vec_t*)this; +} + +inline vec_t const* Vector::Base() const +{ + return (vec_t const*)this; +} + +//----------------------------------------------------------------------------- +// Cast to Vector2D... +//----------------------------------------------------------------------------- + +inline Vector2D& Vector::AsVector2D() +{ + return *(Vector2D*)this; +} + +inline const Vector2D& Vector::AsVector2D() const +{ + return *(const Vector2D*)this; +} + +//----------------------------------------------------------------------------- +// IsValid? +//----------------------------------------------------------------------------- + +inline bool Vector::IsValid() const +{ + return IsFinite(x) && IsFinite(y) && IsFinite(z); +} + +//----------------------------------------------------------------------------- +// Invalidate +//----------------------------------------------------------------------------- + +inline void Vector::Invalidate() +{ +//#ifdef _DEBUG +//#ifdef VECTOR_PARANOIA + x = y = z = VEC_T_NAN; +//#endif +//#endif +} + +//----------------------------------------------------------------------------- +// comparison +//----------------------------------------------------------------------------- + +inline bool Vector::operator==( const Vector& src ) const +{ + CHECK_VALID(src); + CHECK_VALID(*this); + return (src.x == x) && (src.y == y) && (src.z == z); +} + +inline bool Vector::operator!=( const Vector& src ) const +{ + CHECK_VALID(src); + CHECK_VALID(*this); + return (src.x != x) || (src.y != y) || (src.z != z); +} + + +//----------------------------------------------------------------------------- +// Copy +//----------------------------------------------------------------------------- + +FORCEINLINE void VectorCopy( const Vector& src, Vector& dst ) +{ + CHECK_VALID(src); + dst.x = src.x; + dst.y = src.y; + dst.z = src.z; +} + +inline void Vector::CopyToArray(float* rgfl) const +{ + Assert( rgfl ); + CHECK_VALID(*this); + rgfl[0] = x, rgfl[1] = y, rgfl[2] = z; +} + +//----------------------------------------------------------------------------- +// standard math operations +//----------------------------------------------------------------------------- +// #pragma message("TODO: these should be SSE") + +inline void Vector::Negate() +{ + CHECK_VALID(*this); + x = -x; y = -y; z = -z; +} + +FORCEINLINE Vector& Vector::operator+=(const Vector& v) +{ + CHECK_VALID(*this); + CHECK_VALID(v); + x+=v.x; y+=v.y; z += v.z; + return *this; +} + +FORCEINLINE Vector& Vector::operator-=(const Vector& v) +{ + CHECK_VALID(*this); + CHECK_VALID(v); + x-=v.x; y-=v.y; z -= v.z; + return *this; +} + +FORCEINLINE Vector& Vector::operator*=(float fl) +{ + x *= fl; + y *= fl; + z *= fl; + CHECK_VALID(*this); + return *this; +} + +FORCEINLINE Vector& Vector::operator*=(const Vector& v) +{ + CHECK_VALID(v); + x *= v.x; + y *= v.y; + z *= v.z; + CHECK_VALID(*this); + return *this; +} + +// this ought to be an opcode. +FORCEINLINE Vector& Vector::operator+=(float fl) +{ + x += fl; + y += fl; + z += fl; + CHECK_VALID(*this); + return *this; +} + +FORCEINLINE Vector& Vector::operator-=(float fl) +{ + x -= fl; + y -= fl; + z -= fl; + CHECK_VALID(*this); + return *this; +} + + + +FORCEINLINE Vector& Vector::operator/=(float fl) +{ + Assert( fl != 0.0f ); + float oofl = 1.0f / fl; + x *= oofl; + y *= oofl; + z *= oofl; + CHECK_VALID(*this); + return *this; +} + +FORCEINLINE Vector& Vector::operator/=(const Vector& v) +{ + CHECK_VALID(v); + Assert( v.x != 0.0f && v.y != 0.0f && v.z != 0.0f ); + x /= v.x; + y /= v.y; + z /= v.z; + CHECK_VALID(*this); + return *this; +} + + +// get the component of this vector parallel to some other given vector +inline Vector Vector::ProjectOnto( const Vector& onto ) +{ + return onto * ( this->Dot(onto) / ( onto.LengthSqr() ) ); +} + + +//----------------------------------------------------------------------------- +// +// Inlined Short Vector methods +// +//----------------------------------------------------------------------------- + + +inline void ShortVector::Init( short ix, short iy, short iz, short iw ) +{ + x = ix; y = iy; z = iz; w = iw; +} + +FORCEINLINE void ShortVector::Set( const ShortVector& vOther ) +{ + x = vOther.x; + y = vOther.y; + z = vOther.z; + w = vOther.w; +} + +FORCEINLINE void ShortVector::Set( const short ix, const short iy, const short iz, const short iw ) +{ + x = ix; + y = iy; + z = iz; + w = iw; +} + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline short ShortVector::operator[](int i) const +{ + Assert( (i >= 0) && (i < 4) ); + return ((short*)this)[i]; +} + +inline short& ShortVector::operator[](int i) +{ + Assert( (i >= 0) && (i < 4) ); + return ((short*)this)[i]; +} + +//----------------------------------------------------------------------------- +// Base address... +//----------------------------------------------------------------------------- +inline short* ShortVector::Base() +{ + return (short*)this; +} + +inline short const* ShortVector::Base() const +{ + return (short const*)this; +} + + +//----------------------------------------------------------------------------- +// comparison +//----------------------------------------------------------------------------- + +inline bool ShortVector::operator==( const ShortVector& src ) const +{ + return (src.x == x) && (src.y == y) && (src.z == z) && (src.w == w); +} + +inline bool ShortVector::operator!=( const ShortVector& src ) const +{ + return (src.x != x) || (src.y != y) || (src.z != z) || (src.w != w); +} + + + +//----------------------------------------------------------------------------- +// standard math operations +//----------------------------------------------------------------------------- + +FORCEINLINE ShortVector& ShortVector::operator+=(const ShortVector& v) +{ + x+=v.x; y+=v.y; z += v.z; w += v.w; + return *this; +} + +FORCEINLINE ShortVector& ShortVector::operator-=(const ShortVector& v) +{ + x-=v.x; y-=v.y; z -= v.z; w -= v.w; + return *this; +} + +FORCEINLINE ShortVector& ShortVector::operator*=(float fl) +{ + x = (short)(x * fl); + y = (short)(y * fl); + z = (short)(z * fl); + w = (short)(w * fl); + return *this; +} + +FORCEINLINE ShortVector& ShortVector::operator*=(const ShortVector& v) +{ + x = (short)(x * v.x); + y = (short)(y * v.y); + z = (short)(z * v.z); + w = (short)(w * v.w); + return *this; +} + +FORCEINLINE ShortVector& ShortVector::operator/=(float fl) +{ + Assert( fl != 0.0f ); + float oofl = 1.0f / fl; + x = (short)(x * oofl); + y = (short)(y * oofl); + z = (short)(z * oofl); + w = (short)(w * oofl); + return *this; +} + +FORCEINLINE ShortVector& ShortVector::operator/=(const ShortVector& v) +{ + Assert( v.x != 0 && v.y != 0 && v.z != 0 && v.w != 0 ); + x = (short)(x / v.x); + y = (short)(y / v.y); + z = (short)(z / v.z); + w = (short)(w / v.w); + return *this; +} + +FORCEINLINE void ShortVectorMultiply( const ShortVector& src, float fl, ShortVector& res ) +{ + Assert( IsFinite(fl) ); + res.x = (short)(src.x * fl); + res.y = (short)(src.y * fl); + res.z = (short)(src.z * fl); + res.w = (short)(src.w * fl); +} + +FORCEINLINE ShortVector ShortVector::operator*(float fl) const +{ + ShortVector res; + ShortVectorMultiply( *this, fl, res ); + return res; +} + + + + + + +//----------------------------------------------------------------------------- +// +// Inlined Integer Vector methods +// +//----------------------------------------------------------------------------- + + +inline void IntVector4D::Init( int ix, int iy, int iz, int iw ) +{ + x = ix; y = iy; z = iz; w = iw; +} + +FORCEINLINE void IntVector4D::Set( const IntVector4D& vOther ) +{ + x = vOther.x; + y = vOther.y; + z = vOther.z; + w = vOther.w; +} + +FORCEINLINE void IntVector4D::Set( const int ix, const int iy, const int iz, const int iw ) +{ + x = ix; + y = iy; + z = iz; + w = iw; +} + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline int IntVector4D::operator[](int i) const +{ + Assert( (i >= 0) && (i < 4) ); + return ((int*)this)[i]; +} + +inline int& IntVector4D::operator[](int i) +{ + Assert( (i >= 0) && (i < 4) ); + return ((int*)this)[i]; +} + +//----------------------------------------------------------------------------- +// Base address... +//----------------------------------------------------------------------------- +inline int* IntVector4D::Base() +{ + return (int*)this; +} + +inline int const* IntVector4D::Base() const +{ + return (int const*)this; +} + + +//----------------------------------------------------------------------------- +// comparison +//----------------------------------------------------------------------------- + +inline bool IntVector4D::operator==( const IntVector4D& src ) const +{ + return (src.x == x) && (src.y == y) && (src.z == z) && (src.w == w); +} + +inline bool IntVector4D::operator!=( const IntVector4D& src ) const +{ + return (src.x != x) || (src.y != y) || (src.z != z) || (src.w != w); +} + + + +//----------------------------------------------------------------------------- +// standard math operations +//----------------------------------------------------------------------------- + +FORCEINLINE IntVector4D& IntVector4D::operator+=(const IntVector4D& v) +{ + x+=v.x; y+=v.y; z += v.z; w += v.w; + return *this; +} + +FORCEINLINE IntVector4D& IntVector4D::operator-=(const IntVector4D& v) +{ + x-=v.x; y-=v.y; z -= v.z; w -= v.w; + return *this; +} + +FORCEINLINE IntVector4D& IntVector4D::operator*=(float fl) +{ + x = (int)(x * fl); + y = (int)(y * fl); + z = (int)(z * fl); + w = (int)(w * fl); + return *this; +} + +FORCEINLINE IntVector4D& IntVector4D::operator*=(const IntVector4D& v) +{ + x = (int)(x * v.x); + y = (int)(y * v.y); + z = (int)(z * v.z); + w = (int)(w * v.w); + return *this; +} + +FORCEINLINE IntVector4D& IntVector4D::operator/=(float fl) +{ + Assert( fl != 0.0f ); + float oofl = 1.0f / fl; + x = (int)(x * oofl); + y = (int)(y * oofl); + z = (int)(z * oofl); + w = (int)(w * oofl); + return *this; +} + +FORCEINLINE IntVector4D& IntVector4D::operator/=(const IntVector4D& v) +{ + Assert( v.x != 0 && v.y != 0 && v.z != 0 && v.w != 0 ); + x = (int)(x / v.x); + y = (int)(y / v.y); + z = (int)(z / v.z); + w = (int)(w / v.w); + return *this; +} + +FORCEINLINE void IntVector4DMultiply( const IntVector4D& src, float fl, IntVector4D& res ) +{ + Assert( IsFinite(fl) ); + res.x = (int)(src.x * fl); + res.y = (int)(src.y * fl); + res.z = (int)(src.z * fl); + res.w = (int)(src.w * fl); +} + +FORCEINLINE IntVector4D IntVector4D::operator*(float fl) const +{ + IntVector4D res; + IntVector4DMultiply( *this, fl, res ); + return res; +} + + + +// ======================= + + +FORCEINLINE void VectorAdd( const Vector& a, const Vector& b, Vector& c ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + c.x = a.x + b.x; + c.y = a.y + b.y; + c.z = a.z + b.z; +} + +FORCEINLINE void VectorSubtract( const Vector& a, const Vector& b, Vector& c ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + c.x = a.x - b.x; + c.y = a.y - b.y; + c.z = a.z - b.z; +} + +FORCEINLINE void VectorMultiply( const Vector& a, vec_t b, Vector& c ) +{ + CHECK_VALID(a); + Assert( IsFinite(b) ); + c.x = a.x * b; + c.y = a.y * b; + c.z = a.z * b; +} + +FORCEINLINE void VectorMultiply( const Vector& a, const Vector& b, Vector& c ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + c.x = a.x * b.x; + c.y = a.y * b.y; + c.z = a.z * b.z; +} + +// for backwards compatability +inline void VectorScale ( const Vector& in, vec_t scale, Vector& result ) +{ + VectorMultiply( in, scale, result ); +} + + +FORCEINLINE void VectorDivide( const Vector& a, vec_t b, Vector& c ) +{ + CHECK_VALID(a); + Assert( b != 0.0f ); + vec_t oob = 1.0f / b; + c.x = a.x * oob; + c.y = a.y * oob; + c.z = a.z * oob; +} + +FORCEINLINE void VectorDivide( const Vector& a, const Vector& b, Vector& c ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + Assert( (b.x != 0.0f) && (b.y != 0.0f) && (b.z != 0.0f) ); + c.x = a.x / b.x; + c.y = a.y / b.y; + c.z = a.z / b.z; +} + +// FIXME: Remove +// For backwards compatability +inline void Vector::MulAdd(const Vector& a, const Vector& b, float scalar) +{ + CHECK_VALID(a); + CHECK_VALID(b); + x = a.x + b.x * scalar; + y = a.y + b.y * scalar; + z = a.z + b.z * scalar; +} + +inline void VectorLerp(const Vector& src1, const Vector& src2, vec_t t, Vector& dest ) +{ + CHECK_VALID(src1); + CHECK_VALID(src2); + dest.x = src1.x + (src2.x - src1.x) * t; + dest.y = src1.y + (src2.y - src1.y) * t; + dest.z = src1.z + (src2.z - src1.z) * t; +} + +inline Vector VectorLerp(const Vector& src1, const Vector& src2, vec_t t ) +{ + Vector result; + VectorLerp( src1, src2, t, result ); + return result; +} + +//----------------------------------------------------------------------------- +// Temporary storage for vector results so const Vector& results can be returned +//----------------------------------------------------------------------------- +inline Vector &AllocTempVector() +{ + static Vector s_vecTemp[128]; + static CInterlockedInt s_nIndex; + + int nIndex; + for (;;) + { + int nOldIndex = s_nIndex; + nIndex = ( (nOldIndex + 0x10001) & 0x7F ); + + if ( s_nIndex.AssignIf( nOldIndex, nIndex ) ) + { + break; + } + ThreadPause(); + } + return s_vecTemp[nIndex & 0xffff]; +} + + + +//----------------------------------------------------------------------------- +// dot, cross +//----------------------------------------------------------------------------- +FORCEINLINE vec_t DotProduct(const Vector& a, const Vector& b) +{ + CHECK_VALID(a); + CHECK_VALID(b); + return( a.x*b.x + a.y*b.y + a.z*b.z ); +} + +// for backwards compatability +inline vec_t Vector::Dot( const Vector& vOther ) const +{ + CHECK_VALID(vOther); + return DotProduct( *this, vOther ); +} + +inline int Vector::LargestComponent() const +{ + float flAbsx = fabs(x); + float flAbsy = fabs(y); + float flAbsz = fabs(z); + if ( flAbsx > flAbsy ) + { + if ( flAbsx > flAbsz ) + return X_INDEX; + return Z_INDEX; + } + if ( flAbsy > flAbsz ) + return Y_INDEX; + return Z_INDEX; +} + +inline void CrossProduct(const Vector& a, const Vector& b, Vector& result ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + Assert( &a != &result ); + Assert( &b != &result ); + result.x = a.y*b.z - a.z*b.y; + result.y = a.z*b.x - a.x*b.z; + result.z = a.x*b.y - a.y*b.x; +} + +inline vec_t DotProductAbs( const Vector &v0, const Vector &v1 ) +{ + CHECK_VALID(v0); + CHECK_VALID(v1); + return FloatMakePositive(v0.x*v1.x) + FloatMakePositive(v0.y*v1.y) + FloatMakePositive(v0.z*v1.z); +} + +inline vec_t DotProductAbs( const Vector &v0, const float *v1 ) +{ + return FloatMakePositive(v0.x * v1[0]) + FloatMakePositive(v0.y * v1[1]) + FloatMakePositive(v0.z * v1[2]); +} + +//----------------------------------------------------------------------------- +// length +//----------------------------------------------------------------------------- + +inline vec_t VectorLength( const Vector& v ) +{ + CHECK_VALID(v); + return (vec_t)FastSqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} + + +inline vec_t Vector::Length(void) const +{ + CHECK_VALID(*this); + return VectorLength( *this ); +} + + +//----------------------------------------------------------------------------- +// Normalization +//----------------------------------------------------------------------------- + +/* +// FIXME: Can't use until we're un-macroed in mathlib.h +inline vec_t VectorNormalize( Vector& v ) +{ + Assert( v.IsValid() ); + vec_t l = v.Length(); + if (l != 0.0f) + { + v /= l; + } + else + { + // FIXME: + // Just copying the existing implemenation; shouldn't res.z == 0? + v.x = v.y = 0.0f; v.z = 1.0f; + } + return l; +} +*/ + + +// check a point against a box +bool Vector::WithinAABox( Vector const &boxmin, Vector const &boxmax) +{ + return ( + ( x >= boxmin.x ) && ( x <= boxmax.x) && + ( y >= boxmin.y ) && ( y <= boxmax.y) && + ( z >= boxmin.z ) && ( z <= boxmax.z) + ); +} + +//----------------------------------------------------------------------------- +// Get the distance from this vector to the other one +//----------------------------------------------------------------------------- +inline vec_t Vector::DistTo(const Vector &vOther) const +{ + Vector delta; + VectorSubtract( *this, vOther, delta ); + return delta.Length(); +} + + +//----------------------------------------------------------------------------- +// Vector equality with tolerance +//----------------------------------------------------------------------------- +inline bool VectorsAreEqual( const Vector& src1, const Vector& src2, float tolerance ) +{ + if (FloatMakePositive(src1.x - src2.x) > tolerance) + return false; + if (FloatMakePositive(src1.y - src2.y) > tolerance) + return false; + return (FloatMakePositive(src1.z - src2.z) <= tolerance); +} + + +//----------------------------------------------------------------------------- +// Computes the closest point to vecTarget no farther than flMaxDist from vecStart +//----------------------------------------------------------------------------- +inline void ComputeClosestPoint( const Vector& vecStart, float flMaxDist, const Vector& vecTarget, Vector *pResult ) +{ + Vector vecDelta; + VectorSubtract( vecTarget, vecStart, vecDelta ); + float flDistSqr = vecDelta.LengthSqr(); + if ( flDistSqr <= flMaxDist * flMaxDist ) + { + *pResult = vecTarget; + } + else + { + vecDelta /= FastSqrt( flDistSqr ); + VectorMA( vecStart, flMaxDist, vecDelta, *pResult ); + } +} + + +//----------------------------------------------------------------------------- +// Takes the absolute value of a vector +//----------------------------------------------------------------------------- +inline void VectorAbs( const Vector& src, Vector& dst ) +{ + dst.x = FloatMakePositive(src.x); + dst.y = FloatMakePositive(src.y); + dst.z = FloatMakePositive(src.z); +} + + +//----------------------------------------------------------------------------- +// +// Slow methods +// +//----------------------------------------------------------------------------- + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +//----------------------------------------------------------------------------- +// Returns a vector with the min or max in X, Y, and Z. +//----------------------------------------------------------------------------- +inline Vector Vector::Min(const Vector &vOther) const +{ + return Vector(x < vOther.x ? x : vOther.x, + y < vOther.y ? y : vOther.y, + z < vOther.z ? z : vOther.z); +} + +inline Vector Vector::Max(const Vector &vOther) const +{ + return Vector(x > vOther.x ? x : vOther.x, + y > vOther.y ? y : vOther.y, + z > vOther.z ? z : vOther.z); +} + + +//----------------------------------------------------------------------------- +// arithmetic operations +//----------------------------------------------------------------------------- + +inline Vector Vector::operator-(void) const +{ + return Vector(-x,-y,-z); +} + +inline Vector Vector::operator+(const Vector& v) const +{ + Vector res; + VectorAdd( *this, v, res ); + return res; +} + +inline Vector Vector::operator-(const Vector& v) const +{ + Vector res; + VectorSubtract( *this, v, res ); + return res; +} + +inline Vector Vector::operator*(float fl) const +{ + Vector res; + VectorMultiply( *this, fl, res ); + return res; +} + +inline Vector Vector::operator*(const Vector& v) const +{ + Vector res; + VectorMultiply( *this, v, res ); + return res; +} + +inline Vector Vector::operator/(float fl) const +{ + Vector res; + VectorDivide( *this, fl, res ); + return res; +} + +inline Vector Vector::operator/(const Vector& v) const +{ + Vector res; + VectorDivide( *this, v, res ); + return res; +} + +inline Vector operator*(float fl, const Vector& v) +{ + return v * fl; +} + +//----------------------------------------------------------------------------- +// cross product +//----------------------------------------------------------------------------- + +inline Vector Vector::Cross(const Vector& vOther) const +{ + Vector res; + CrossProduct( *this, vOther, res ); + return res; +} + +//----------------------------------------------------------------------------- +// 2D +//----------------------------------------------------------------------------- + +inline vec_t Vector::Length2D(void) const +{ + return (vec_t)FastSqrt(x*x + y*y); +} + +inline vec_t Vector::Length2DSqr(void) const +{ + return (x*x + y*y); +} + +inline Vector CrossProduct(const Vector& a, const Vector& b) +{ + return Vector( a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x ); +} + +inline void VectorMin( const Vector &a, const Vector &b, Vector &result ) +{ + result.x = fpmin(a.x, b.x); + result.y = fpmin(a.y, b.y); + result.z = fpmin(a.z, b.z); +} + +inline void VectorMax( const Vector &a, const Vector &b, Vector &result ) +{ + result.x = fpmax(a.x, b.x); + result.y = fpmax(a.y, b.y); + result.z = fpmax(a.z, b.z); +} + +// and when you want to return the vector rather than cause a LHS with it... +inline Vector VectorMin( const Vector &a, const Vector &b ) +{ + return Vector( fpmin(a.x, b.x), fpmin(a.y, b.y), fpmin(a.z, b.z) ); +} + +inline Vector VectorMax( const Vector &a, const Vector &b ) +{ + return Vector( fpmax(a.x, b.x), fpmax(a.y, b.y), fpmax(a.z, b.z) ); +} + +inline float ComputeVolume( const Vector &vecMins, const Vector &vecMaxs ) +{ + Vector vecDelta; + VectorSubtract( vecMaxs, vecMins, vecDelta ); + return DotProduct( vecDelta, vecDelta ); +} + +// Get a random vector. +inline Vector RandomVector( float minVal, float maxVal ) +{ + Vector random; + random.Random( minVal, maxVal ); + return random; +} + +#endif //slow + +//----------------------------------------------------------------------------- +// Helper debugging stuff.... +//----------------------------------------------------------------------------- + +inline bool operator==( float const* f, const Vector& v ) +{ + // AIIIEEEE!!!! + Assert(0); + return false; +} + +inline bool operator==( const Vector& v, float const* f ) +{ + // AIIIEEEE!!!! + Assert(0); + return false; +} + +inline bool operator!=( float const* f, const Vector& v ) +{ + // AIIIEEEE!!!! + Assert(0); + return false; +} + +inline bool operator!=( const Vector& v, float const* f ) +{ + // AIIIEEEE!!!! + Assert(0); + return false; +} + + +// return a vector perpendicular to another, with smooth variation. The difference between this and +// something like VectorVectors is that there are now discontinuities. _unlike_ VectorVectors, +// you won't get an "u +void VectorPerpendicularToVector( Vector const &in, Vector *pvecOut ); + +//----------------------------------------------------------------------------- +// AngularImpulse +//----------------------------------------------------------------------------- +// AngularImpulse are exponetial maps (an axis scaled by a "twist" angle in degrees) +typedef Vector AngularImpulse; + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +inline AngularImpulse RandomAngularImpulse( float minVal, float maxVal ) +{ + AngularImpulse angImp; + angImp.Random( minVal, maxVal ); + return angImp; +} + +#endif + + +//----------------------------------------------------------------------------- +// Quaternion +//----------------------------------------------------------------------------- + +class RadianEuler; + +class Quaternion // same data-layout as engine's vec4_t, +{ // which is a vec_t[4] +public: + inline Quaternion(void) { + + // Initialize to NAN to catch errors +#ifdef _DEBUG +#ifdef VECTOR_PARANOIA + x = y = z = w = VEC_T_NAN; +#endif +#endif + } + inline Quaternion(vec_t ix, vec_t iy, vec_t iz, vec_t iw) : x(ix), y(iy), z(iz), w(iw) { } + inline Quaternion(RadianEuler const &angle); // evil auto type promotion!!! + + inline void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f, vec_t iw=0.0f) { x = ix; y = iy; z = iz; w = iw; } + + bool IsValid() const; + void Invalidate(); + + bool operator==( const Quaternion &src ) const; + bool operator!=( const Quaternion &src ) const; + + inline Quaternion Conjugate() const { return Quaternion( -x, -y, -z, w ); } + + vec_t* Base() { return (vec_t*)this; } + const vec_t* Base() const { return (vec_t*)this; } + + // convenience for debugging + inline void Print() const; + + // array access... + vec_t operator[](int i) const; + vec_t& operator[](int i); + + vec_t x, y, z, w; +}; + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline vec_t& Quaternion::operator[](int i) +{ + Assert( (i >= 0) && (i < 4) ); + return ((vec_t*)this)[i]; +} + +inline vec_t Quaternion::operator[](int i) const +{ + Assert( (i >= 0) && (i < 4) ); + return ((vec_t*)this)[i]; +} + + +//----------------------------------------------------------------------------- +// Equality test +//----------------------------------------------------------------------------- +inline bool Quaternion::operator==( const Quaternion &src ) const +{ + return ( x == src.x ) && ( y == src.y ) && ( z == src.z ) && ( w == src.w ); +} + +inline bool Quaternion::operator!=( const Quaternion &src ) const +{ + return !operator==( src ); +} + + +//----------------------------------------------------------------------------- +// Debugging only +//----------------------------------------------------------------------------- +void Quaternion::Print() const +{ +#ifndef _CERT + Msg("q{ %.3fi + %.3fj + %.3fk + %.3f }", x, y, z, w ); +#endif +} + +//----------------------------------------------------------------------------- +// Quaternion equality with tolerance +//----------------------------------------------------------------------------- +inline bool QuaternionsAreEqual( const Quaternion& src1, const Quaternion& src2, float tolerance ) +{ + if (FloatMakePositive(src1.x - src2.x) > tolerance) + return false; + if (FloatMakePositive(src1.y - src2.y) > tolerance) + return false; + if (FloatMakePositive(src1.z - src2.z) > tolerance) + return false; + return (FloatMakePositive(src1.w - src2.w) <= tolerance); +} + + +//----------------------------------------------------------------------------- +// Here's where we add all those lovely SSE optimized routines +//----------------------------------------------------------------------------- +class ALIGN16 QuaternionAligned : public Quaternion +{ +public: + inline QuaternionAligned(void) {}; + inline QuaternionAligned(vec_t X, vec_t Y, vec_t Z, vec_t W) + { + Init(X,Y,Z,W); + } + + operator Quaternion * () { return this; } + operator const Quaternion * () { return this; } + +#ifdef VECTOR_NO_SLOW_OPERATIONS + +private: + // No copy constructors allowed if we're in optimal mode + QuaternionAligned(const QuaternionAligned& vOther); + QuaternionAligned(const Quaternion &vOther); + +#else +public: + explicit QuaternionAligned(const Quaternion &vOther) + { + Init(vOther.x, vOther.y, vOther.z, vOther.w); + } + + QuaternionAligned& operator=(const Quaternion &vOther) + { + Init(vOther.x, vOther.y, vOther.z, vOther.w); + return *this; + } + + QuaternionAligned& operator=(const QuaternionAligned &vOther) + { + // we know we're aligned, so use simd + // we can't use the convenient abstract interface coz it gets declared later +#ifdef _X360 + XMStoreVector4A(Base(), XMLoadVector4A(vOther.Base())); +#elif _WIN32 + _mm_store_ps(Base(), _mm_load_ps( vOther.Base() )); +#else + Init(vOther.x, vOther.y, vOther.z, vOther.w); +#endif + return *this; + } + +#endif + +#if !defined(NO_MALLOC_OVERRIDE) + void* operator new[] ( size_t nSize) + { + return MemAlloc_AllocAligned(nSize, 16); + } + + void* operator new[] ( size_t nSize, const char *pFileName, int nLine) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + + void* operator new[] ( size_t nSize, int /*nBlockUse*/, const char *pFileName, int nLine) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + + void operator delete[] ( void* p) + { + MemAlloc_FreeAligned(p); + } + + void operator delete[] ( void* p, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + void operator delete[] ( void* p, int /*nBlockUse*/, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + // please don't allocate a single quaternion... + void* operator new ( size_t nSize ) + { + return MemAlloc_AllocAligned(nSize, 16); + } + void* operator new ( size_t nSize, const char *pFileName, int nLine ) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + void* operator new ( size_t nSize, int /*nBlockUse*/, const char *pFileName, int nLine ) + { + return MemAlloc_AllocAlignedFileLine(nSize, 16, pFileName, nLine); + } + void operator delete ( void* p) + { + MemAlloc_FreeAligned(p); + } + + void operator delete ( void* p, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } + + void operator delete ( void* p, int /*nBlockUse*/, const char *pFileName, int nLine) + { + MemAlloc_FreeAligned(p, pFileName, nLine); + } +#endif +} ALIGN16_POST; + +//----------------------------------------------------------------------------- +// Radian Euler angle aligned to axis (NOT ROLL/PITCH/YAW) +//----------------------------------------------------------------------------- +class QAngle; +class RadianEuler +{ +public: + inline RadianEuler(void) { } + inline RadianEuler(vec_t X, vec_t Y, vec_t Z) { x = X; y = Y; z = Z; } + inline RadianEuler(Quaternion const &q); // evil auto type promotion!!! + inline RadianEuler(QAngle const &angles); // evil auto type promotion!!! + + // Initialization + inline void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f) { x = ix; y = iy; z = iz; } + + // conversion to qangle + QAngle ToQAngle( void ) const; + bool IsValid() const; + void Invalidate(); + + inline vec_t *Base() { return &x; } + inline const vec_t *Base() const { return &x; } + + // array access... + vec_t operator[](int i) const; + vec_t& operator[](int i); + + vec_t x, y, z; +}; + + +extern void AngleQuaternion( RadianEuler const &angles, Quaternion &qt ); +extern void QuaternionAngles( Quaternion const &q, RadianEuler &angles ); +inline Quaternion::Quaternion(RadianEuler const &angle) +{ + AngleQuaternion( angle, *this ); +} + +inline bool Quaternion::IsValid() const +{ + return IsFinite(x) && IsFinite(y) && IsFinite(z) && IsFinite(w); +} + +inline void Quaternion::Invalidate() +{ +//#ifdef _DEBUG +//#ifdef VECTOR_PARANOIA + x = y = z = w = VEC_T_NAN; +//#endif +//#endif +} + +inline RadianEuler::RadianEuler(Quaternion const &q) +{ + QuaternionAngles( q, *this ); +} + +inline void VectorCopy( RadianEuler const& src, RadianEuler &dst ) +{ + CHECK_VALID(src); + dst.x = src.x; + dst.y = src.y; + dst.z = src.z; +} + +inline void VectorScale( RadianEuler const& src, float b, RadianEuler &dst ) +{ + CHECK_VALID(src); + Assert( IsFinite(b) ); + dst.x = src.x * b; + dst.y = src.y * b; + dst.z = src.z * b; +} + +inline bool RadianEuler::IsValid() const +{ + return IsFinite(x) && IsFinite(y) && IsFinite(z); +} + +inline void RadianEuler::Invalidate() +{ +//#ifdef _DEBUG +//#ifdef VECTOR_PARANOIA + x = y = z = VEC_T_NAN; +//#endif +//#endif +} + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline vec_t& RadianEuler::operator[](int i) +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + +inline vec_t RadianEuler::operator[](int i) const +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + + +//----------------------------------------------------------------------------- +// Degree Euler QAngle pitch, yaw, roll +//----------------------------------------------------------------------------- +class QAngleByValue; + +class QAngle +{ +public: + // Members + vec_t x, y, z; + + // Construction/destruction + QAngle(void); + QAngle(vec_t X, vec_t Y, vec_t Z); +#ifndef _PS3 +// QAngle(RadianEuler const &angles); // evil auto type promotion!!! +#endif + + // Allow pass-by-value + operator QAngleByValue &() { return *((QAngleByValue *)(this)); } + operator const QAngleByValue &() const { return *((const QAngleByValue *)(this)); } + + // Initialization + void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f); + void Random( vec_t minVal, vec_t maxVal ); + + // Got any nasty NAN's? + bool IsValid() const; + void Invalidate(); + + // array access... + vec_t operator[](int i) const; + vec_t& operator[](int i); + + // Base address... + vec_t* Base(); + vec_t const* Base() const; + + // equality + bool operator==(const QAngle& v) const; + bool operator!=(const QAngle& v) const; + + // arithmetic operations + QAngle& operator+=(const QAngle &v); + QAngle& operator-=(const QAngle &v); + QAngle& operator*=(float s); + QAngle& operator/=(float s); + + // Get the vector's magnitude. + vec_t Length() const; + vec_t LengthSqr() const; + + // negate the QAngle components + //void Negate(); + + // No assignment operators either... + QAngle& operator=( const QAngle& src ); + +#ifndef VECTOR_NO_SLOW_OPERATIONS + // copy constructors + + // arithmetic operations + QAngle operator-(void) const; + + QAngle operator+(const QAngle& v) const; + QAngle operator-(const QAngle& v) const; + QAngle operator*(float fl) const; + QAngle operator/(float fl) const; +#else + +private: + // No copy constructors allowed if we're in optimal mode + QAngle(const QAngle& vOther); + +#endif +}; + +//----------------------------------------------------------------------------- +// Allows us to specifically pass the vector by value when we need to +//----------------------------------------------------------------------------- +class QAngleByValue : public QAngle +{ +public: + // Construction/destruction: + QAngleByValue(void) : QAngle() {} + QAngleByValue(vec_t X, vec_t Y, vec_t Z) : QAngle( X, Y, Z ) {} + QAngleByValue(const QAngleByValue& vOther) { *this = vOther; } +}; + + +inline void VectorAdd( const QAngle& a, const QAngle& b, QAngle& result ) +{ + CHECK_VALID(a); + CHECK_VALID(b); + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; +} + +inline void VectorMA( const QAngle &start, float scale, const QAngle &direction, QAngle &dest ) +{ + CHECK_VALID(start); + CHECK_VALID(direction); + dest.x = start.x + scale * direction.x; + dest.y = start.y + scale * direction.y; + dest.z = start.z + scale * direction.z; +} + + +//----------------------------------------------------------------------------- +// constructors +//----------------------------------------------------------------------------- +inline QAngle::QAngle(void) +{ +#ifdef _DEBUG +#ifdef VECTOR_PARANOIA + // Initialize to NAN to catch errors + x = y = z = VEC_T_NAN; +#endif +#endif +} + +inline QAngle::QAngle(vec_t X, vec_t Y, vec_t Z) +{ + x = X; y = Y; z = Z; + CHECK_VALID(*this); +} + + +//----------------------------------------------------------------------------- +// initialization +//----------------------------------------------------------------------------- +inline void QAngle::Init( vec_t ix, vec_t iy, vec_t iz ) +{ + x = ix; y = iy; z = iz; + CHECK_VALID(*this); +} + +inline void QAngle::Random( vec_t minVal, vec_t maxVal ) +{ + x = RandomFloat( minVal, maxVal ); + y = RandomFloat( minVal, maxVal ); + z = RandomFloat( minVal, maxVal ); + CHECK_VALID(*this); +} + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +inline QAngle RandomAngle( float minVal, float maxVal ) +{ + Vector random; + random.Random( minVal, maxVal ); + QAngle ret( random.x, random.y, random.z ); + return ret; +} + +#endif + +inline RadianEuler::RadianEuler(QAngle const &angles) +{ + Init( + angles.z * 3.14159265358979323846f / 180.f, + angles.x * 3.14159265358979323846f / 180.f, + angles.y * 3.14159265358979323846f / 180.f ); +} + + + + +inline QAngle RadianEuler::ToQAngle( void) const +{ + return QAngle( + y * 180.f / 3.14159265358979323846f, + z * 180.f / 3.14159265358979323846f, + x * 180.f / 3.14159265358979323846f ); +} + +//----------------------------------------------------------------------------- +// assignment +//----------------------------------------------------------------------------- +inline QAngle& QAngle::operator=(const QAngle &vOther) +{ + CHECK_VALID(vOther); + x=vOther.x; y=vOther.y; z=vOther.z; + return *this; +} + + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- +inline vec_t& QAngle::operator[](int i) +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + +inline vec_t QAngle::operator[](int i) const +{ + Assert( (i >= 0) && (i < 3) ); + return ((vec_t*)this)[i]; +} + + +//----------------------------------------------------------------------------- +// Base address... +//----------------------------------------------------------------------------- +inline vec_t* QAngle::Base() +{ + return (vec_t*)this; +} + +inline vec_t const* QAngle::Base() const +{ + return (vec_t const*)this; +} + + +//----------------------------------------------------------------------------- +// IsValid? +//----------------------------------------------------------------------------- +inline bool QAngle::IsValid() const +{ + return IsFinite(x) && IsFinite(y) && IsFinite(z); +} + +//----------------------------------------------------------------------------- +// Invalidate +//----------------------------------------------------------------------------- + +inline void QAngle::Invalidate() +{ +//#ifdef _DEBUG +//#ifdef VECTOR_PARANOIA + x = y = z = VEC_T_NAN; +//#endif +//#endif +} + +//----------------------------------------------------------------------------- +// comparison +//----------------------------------------------------------------------------- +inline bool QAngle::operator==( const QAngle& src ) const +{ + CHECK_VALID(src); + CHECK_VALID(*this); + return (src.x == x) && (src.y == y) && (src.z == z); +} + +inline bool QAngle::operator!=( const QAngle& src ) const +{ + CHECK_VALID(src); + CHECK_VALID(*this); + return (src.x != x) || (src.y != y) || (src.z != z); +} + + +//----------------------------------------------------------------------------- +// Copy +//----------------------------------------------------------------------------- +inline void VectorCopy( const QAngle& src, QAngle& dst ) +{ + CHECK_VALID(src); + dst.x = src.x; + dst.y = src.y; + dst.z = src.z; +} + + +//----------------------------------------------------------------------------- +// standard math operations +//----------------------------------------------------------------------------- +inline QAngle& QAngle::operator+=(const QAngle& v) +{ + CHECK_VALID(*this); + CHECK_VALID(v); + x+=v.x; y+=v.y; z += v.z; + return *this; +} + +inline QAngle& QAngle::operator-=(const QAngle& v) +{ + CHECK_VALID(*this); + CHECK_VALID(v); + x-=v.x; y-=v.y; z -= v.z; + return *this; +} + +inline QAngle& QAngle::operator*=(float fl) +{ + x *= fl; + y *= fl; + z *= fl; + CHECK_VALID(*this); + return *this; +} + +inline QAngle& QAngle::operator/=(float fl) +{ + Assert( fl != 0.0f ); + float oofl = 1.0f / fl; + x *= oofl; + y *= oofl; + z *= oofl; + CHECK_VALID(*this); + return *this; +} + + +//----------------------------------------------------------------------------- +// length +//----------------------------------------------------------------------------- +inline vec_t QAngle::Length( ) const +{ + CHECK_VALID(*this); + return (vec_t)FastSqrt( LengthSqr( ) ); +} + + +inline vec_t QAngle::LengthSqr( ) const +{ + CHECK_VALID(*this); + return x * x + y * y + z * z; +} + + +//----------------------------------------------------------------------------- +// Vector equality with tolerance +//----------------------------------------------------------------------------- +inline bool QAnglesAreEqual( const QAngle& src1, const QAngle& src2, float tolerance = 0.0f ) +{ + if (FloatMakePositive(src1.x - src2.x) > tolerance) + return false; + if (FloatMakePositive(src1.y - src2.y) > tolerance) + return false; + return (FloatMakePositive(src1.z - src2.z) <= tolerance); +} + + +//----------------------------------------------------------------------------- +// arithmetic operations (SLOW!!) +//----------------------------------------------------------------------------- +#ifndef VECTOR_NO_SLOW_OPERATIONS + +inline QAngle QAngle::operator-(void) const +{ + QAngle ret(-x,-y,-z); + return ret; +} + +inline QAngle QAngle::operator+(const QAngle& v) const +{ + QAngle res; + res.x = x + v.x; + res.y = y + v.y; + res.z = z + v.z; + return res; +} + +inline QAngle QAngle::operator-(const QAngle& v) const +{ + QAngle res; + res.x = x - v.x; + res.y = y - v.y; + res.z = z - v.z; + return res; +} + +inline QAngle QAngle::operator*(float fl) const +{ + QAngle res; + res.x = x * fl; + res.y = y * fl; + res.z = z * fl; + return res; +} + +inline QAngle QAngle::operator/(float fl) const +{ + QAngle res; + res.x = x / fl; + res.y = y / fl; + res.z = z / fl; + return res; +} + +inline QAngle operator*(float fl, const QAngle& v) +{ + QAngle ret( v * fl ); + return ret; +} + +#endif // VECTOR_NO_SLOW_OPERATIONS + + +//----------------------------------------------------------------------------- +// NOTE: These are not completely correct. The representations are not equivalent +// unless the QAngle represents a rotational impulse along a coordinate axis (x,y,z) +inline void QAngleToAngularImpulse( const QAngle &angles, AngularImpulse &impulse ) +{ + impulse.x = angles.z; + impulse.y = angles.x; + impulse.z = angles.y; +} + +inline void AngularImpulseToQAngle( const AngularImpulse &impulse, QAngle &angles ) +{ + angles.x = impulse.y; + angles.y = impulse.z; + angles.z = impulse.x; +} + +#if !defined( _X360 ) && !defined( _PS3 ) + +FORCEINLINE vec_t InvRSquared( const float* v ) +{ + return 1.0 / MAX( 1.0, v[0] * v[0] + v[1] * v[1] + v[2] * v[2] ); +} + +FORCEINLINE vec_t InvRSquared( const Vector &v ) +{ + return InvRSquared( v.Base() ); +} + +#else + +// call directly +FORCEINLINE float _VMX_InvRSquared( const Vector &v ) +{ +#if !defined (_PS3) + XMVECTOR xmV = XMVector3ReciprocalLength( XMLoadVector3( v.Base() ) ); + xmV = XMVector3Dot( xmV, xmV ); + return xmV.x; +#else //!_PS3 + vector_float_union vRet; + vec_float4 v0, v1, vIn, vOut; + vector unsigned char permMask; + v0 = vec_ld( 0, v.Base() ); + permMask = vec_lvsl( 0, v.Base() ); + v1 = vec_ld( 11, v.Base() ); + vIn = vec_perm(v0, v1, permMask); + vOut = vec_madd( vIn, vIn, _VEC_ZEROF ); + vec_float4 vTmp = vec_sld( vIn, vIn, 4 ); + vec_float4 vTmp2 = vec_sld( vIn, vIn, 8 ); + vOut = vec_madd( vTmp, vTmp, vOut ); + vOut = vec_madd( vTmp2, vTmp2, vOut ); + vOut = vec_re( vec_add(vOut, _VEC_EPSILONF) ); + vec_st(vOut,0,&vRet.vf); + float ret = vRet.f[0]; + return ret; +#endif //!_PS3 +} + +#define InvRSquared(x) _VMX_InvRSquared(x) + +#endif // _X360 + +#if !defined( _X360 ) && !defined( _PS3 ) + +// FIXME: Change this back to a #define once we get rid of the vec_t version +float VectorNormalize( Vector& v ); + +// FIXME: Obsolete version of VectorNormalize, once we remove all the friggin float*s +FORCEINLINE float VectorNormalize( float * v ) +{ + return VectorNormalize(*(reinterpret_cast<Vector *>(v))); +} + +#else +#if !defined( _PS3 ) +// modified version of Microsoft's XMVector3Length +// microsoft's version will return INF for very small vectors +// e.g. Vector vTest(7.98555446e-20,-6.85012984e-21,0); VectorNormalize( vTest ); +// so we clamp to epsilon instead of checking for zero +XMFINLINE XMVECTOR XMVector3Length_Fixed +( + FXMVECTOR V + ) +{ + // Returns a QNaN on infinite vectors. + static CONST XMVECTOR g_fl4SmallVectorEpsilon = {1e-24f,1e-24f,1e-24f,1e-24f}; + + XMVECTOR D; + XMVECTOR Rsq; + XMVECTOR Rcp; + XMVECTOR Zero; + XMVECTOR RT; + XMVECTOR Result; + XMVECTOR Length; + XMVECTOR H; + + H = __vspltisw(1); + D = __vmsum3fp(V, V); + H = __vcfsx(H, 1); + Rsq = __vrsqrtefp(D); + RT = __vmulfp(D, H); + Rcp = __vmulfp(Rsq, Rsq); + H = __vnmsubfp(RT, Rcp, H); + Rsq = __vmaddfp(Rsq, H, Rsq); + Zero = __vspltisw(0); + Result = __vcmpgefp( g_fl4SmallVectorEpsilon, D ); + Length = __vmulfp(D, Rsq); + Result = __vsel(Length, Zero, Result); + + return Result; +} +#endif + +// call directly +FORCEINLINE float _VMX_VectorNormalize( Vector &vec ) +{ +#if !defined _PS3 + float mag = XMVector3Length_Fixed( XMLoadVector3( vec.Base() ) ).x; + float den = 1.f / (mag + FLT_EPSILON ); + vec.x *= den; + vec.y *= den; + vec.z *= den; + return mag; +#else // !_PS3 + vec_float4 vIn; + vec_float4 v0, v1; + vector unsigned char permMask; + v0 = vec_ld( 0, vec.Base() ); + permMask = vec_lvsl( 0, vec.Base() ); + v1 = vec_ld( 11, vec.Base() ); + vIn = vec_perm(v0, v1, permMask); + float mag = vmathV3Length((VmathVector3 *)&vIn); + float den = 1.f / (mag + FLT_EPSILON ); + vec.x *= den; + vec.y *= den; + vec.z *= den; + return mag; +#endif // !_PS3 +} +// FIXME: Change this back to a #define once we get rid of the vec_t version +FORCEINLINE float VectorNormalize( Vector& v ) +{ + return _VMX_VectorNormalize( v ); +} +// FIXME: Obsolete version of VectorNormalize, once we remove all the friggin float*s +FORCEINLINE float VectorNormalize( float *pV ) +{ + return _VMX_VectorNormalize(*(reinterpret_cast<Vector*>(pV))); +} + +#endif // _X360 + +#if !defined( _X360 ) && !defined( _PS3 ) +FORCEINLINE void VectorNormalizeFast (Vector& vec) +{ + float ool = FastRSqrt( FLT_EPSILON + vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ); + + vec.x *= ool; + vec.y *= ool; + vec.z *= ool; +} +#else + +// call directly +FORCEINLINE void VectorNormalizeFast( Vector &vec ) +{ +#if !defined (_PS3) + XMVECTOR xmV = XMVector3LengthEst( XMLoadVector3( vec.Base() ) ); + float den = 1.f / (xmV.x + FLT_EPSILON); + vec.x *= den; + vec.y *= den; + vec.z *= den; +#else // !_PS3 + vector_float_union vVec; + + vec_float4 vIn, vOut, vOOLen, vDot; + + // load + vec_float4 v0, v1; + vector unsigned char permMask; + v0 = vec_ld( 0, vec.Base() ); + permMask = vec_lvsl( 0, vec.Base() ); + v1 = vec_ld( 11, vec.Base() ); + vIn = vec_perm(v0, v1, permMask); + + // vec.vec + vOut = vec_madd( vIn, vIn, _VEC_ZEROF ); + vec_float4 vTmp = vec_sld( vIn, vIn, 4 ); + vec_float4 vTmp2 = vec_sld( vIn, vIn, 8 ); + vOut = vec_madd( vTmp, vTmp, vOut ); + vOut = vec_madd( vTmp2, vTmp2, vOut ); + + // splat dot to all + vDot = vec_splat( vOut, 0 ); + + vOOLen = vec_rsqrte( vec_add( vDot, _VEC_EPSILONF ) ); + + // vec * 1.0/sqrt(vec.vec) + vOut = vec_madd( vIn, vOOLen, _VEC_ZEROF ); + + // store + vec_st(vOut,0,&vVec.vf); + + // store vec + vec.x = vVec.f[0]; + vec.y = vVec.f[1]; + vec.z = vVec.f[2]; + +#endif // !_PS3 +} + +#endif // _X360 + +inline vec_t Vector::NormalizeInPlace() +{ + return VectorNormalize( *this ); +} + +inline Vector Vector::Normalized() const +{ + Vector norm = *this; + VectorNormalize( norm ); + return norm; +} + +inline bool Vector::IsLengthGreaterThan( float val ) const +{ + return LengthSqr() > val*val; +} + +inline bool Vector::IsLengthLessThan( float val ) const +{ + return LengthSqr() < val*val; +} + +#endif + diff --git a/external/vpc/public/mathlib/vector2d.h b/external/vpc/public/mathlib/vector2d.h new file mode 100644 index 0000000..30fcc86 --- /dev/null +++ b/external/vpc/public/mathlib/vector2d.h @@ -0,0 +1,670 @@ +//========= Copyright � 1996-2005, Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// + +#ifndef VECTOR2D_H +#define VECTOR2D_H + +#ifdef _WIN32 +#pragma once +#endif + +#include <math.h> +#include <float.h> + +// For vec_t, put this somewhere else? +#include "tier0/basetypes.h" + +// For RandomFloat() +#include "vstdlib/random.h" + +#include "tier0/dbg.h" +#include "mathlib/math_pfns.h" + +//========================================================= +// 2D Vector2D +//========================================================= + +class Vector2D +{ +public: + // Members + vec_t x, y; + + // Construction/destruction + Vector2D(void); + Vector2D(vec_t X, vec_t Y); + Vector2D(const float *pFloat); + + // Initialization + void Init(vec_t ix=0.0f, vec_t iy=0.0f); + + // Got any nasty NAN's? + bool IsValid() const; + + // array access... + vec_t operator[](int i) const; + vec_t& operator[](int i); + + // Base address... + vec_t* Base(); + vec_t const* Base() const; + + // Initialization methods + void Random( float minVal, float maxVal ); + + // equality + bool operator==(const Vector2D& v) const; + bool operator!=(const Vector2D& v) const; + + // arithmetic operations + Vector2D& operator+=(const Vector2D &v); + Vector2D& operator-=(const Vector2D &v); + Vector2D& operator*=(const Vector2D &v); + Vector2D& operator*=(float s); + Vector2D& operator/=(const Vector2D &v); + Vector2D& operator/=(float s); + + // negate the Vector2D components + void Negate(); + + // Get the Vector2D's magnitude. + vec_t Length() const; + + // Get the Vector2D's magnitude squared. + vec_t LengthSqr(void) const; + + // return true if this vector is (0,0) within tolerance + bool IsZero( float tolerance = 0.01f ) const + { + return (x > -tolerance && x < tolerance && + y > -tolerance && y < tolerance); + } + + // Normalize in place and return the old length. + vec_t NormalizeInPlace(); + + // Compare length. + bool IsLengthGreaterThan( float val ) const; + bool IsLengthLessThan( float val ) const; + + // Get the distance from this Vector2D to the other one. + vec_t DistTo(const Vector2D &vOther) const; + + // Get the distance from this Vector2D to the other one squared. + vec_t DistToSqr(const Vector2D &vOther) const; + + // Copy + void CopyToArray(float* rgfl) const; + + // Multiply, add, and assign to this (ie: *this = a + b * scalar). This + // is about 12% faster than the actual Vector2D equation (because it's done per-component + // rather than per-Vector2D). + void MulAdd(const Vector2D& a, const Vector2D& b, float scalar); + + // Dot product. + vec_t Dot(const Vector2D& vOther) const; + + // assignment + Vector2D& operator=(const Vector2D &vOther); + +#ifndef VECTOR_NO_SLOW_OPERATIONS + // copy constructors + Vector2D(const Vector2D &vOther); + + // arithmetic operations + Vector2D operator-(void) const; + + Vector2D operator+(const Vector2D& v) const; + Vector2D operator-(const Vector2D& v) const; + Vector2D operator*(const Vector2D& v) const; + Vector2D operator/(const Vector2D& v) const; + Vector2D operator*(float fl) const; + Vector2D operator/(float fl) const; + + // Cross product between two vectors. + Vector2D Cross(const Vector2D &vOther) const; + + // Returns a Vector2D with the min or max in X, Y, and Z. + Vector2D Min(const Vector2D &vOther) const; + Vector2D Max(const Vector2D &vOther) const; + +#else + +private: + // No copy constructors allowed if we're in optimal mode + Vector2D(const Vector2D& vOther); +#endif +}; + +//----------------------------------------------------------------------------- + +const Vector2D vec2_origin(0,0); +const Vector2D vec2_invalid( FLT_MAX, FLT_MAX ); + +//----------------------------------------------------------------------------- +// Vector2D related operations +//----------------------------------------------------------------------------- + +// Vector2D clear +void Vector2DClear( Vector2D& a ); + +// Copy +void Vector2DCopy( const Vector2D& src, Vector2D& dst ); + +// Vector2D arithmetic +void Vector2DAdd( const Vector2D& a, const Vector2D& b, Vector2D& result ); +void Vector2DSubtract( const Vector2D& a, const Vector2D& b, Vector2D& result ); +void Vector2DMultiply( const Vector2D& a, vec_t b, Vector2D& result ); +void Vector2DMultiply( const Vector2D& a, const Vector2D& b, Vector2D& result ); +void Vector2DDivide( const Vector2D& a, vec_t b, Vector2D& result ); +void Vector2DDivide( const Vector2D& a, const Vector2D& b, Vector2D& result ); +void Vector2DMA( const Vector2D& start, float s, const Vector2D& dir, Vector2D& result ); + +// Store the min or max of each of x, y, and z into the result. +void Vector2DMin( const Vector2D &a, const Vector2D &b, Vector2D &result ); +void Vector2DMax( const Vector2D &a, const Vector2D &b, Vector2D &result ); + +#define Vector2DExpand( v ) (v).x, (v).y + +// Normalization +vec_t Vector2DNormalize( Vector2D& v ); + +// Length +vec_t Vector2DLength( const Vector2D& v ); + +// Dot Product +vec_t DotProduct2D(const Vector2D& a, const Vector2D& b); + +// Linearly interpolate between two vectors +void Vector2DLerp(const Vector2D& src1, const Vector2D& src2, vec_t t, Vector2D& dest ); + + +//----------------------------------------------------------------------------- +// +// Inlined Vector2D methods +// +//----------------------------------------------------------------------------- + + +//----------------------------------------------------------------------------- +// constructors +//----------------------------------------------------------------------------- + +inline Vector2D::Vector2D(void) +{ +#ifdef _DEBUG + // Initialize to NAN to catch errors + x = y = VEC_T_NAN; +#endif +} + +inline Vector2D::Vector2D(vec_t X, vec_t Y) +{ + x = X; y = Y; + Assert( IsValid() ); +} + +inline Vector2D::Vector2D(const float *pFloat) +{ + Assert( pFloat ); + x = pFloat[0]; y = pFloat[1]; + Assert( IsValid() ); +} + + +//----------------------------------------------------------------------------- +// copy constructor +//----------------------------------------------------------------------------- + +inline Vector2D::Vector2D(const Vector2D &vOther) +{ + Assert( vOther.IsValid() ); + x = vOther.x; y = vOther.y; +} + +//----------------------------------------------------------------------------- +// initialization +//----------------------------------------------------------------------------- + +inline void Vector2D::Init( vec_t ix, vec_t iy ) +{ + x = ix; y = iy; + Assert( IsValid() ); +} + +inline void Vector2D::Random( float minVal, float maxVal ) +{ + x = RandomFloat( minVal , maxVal ); + y = RandomFloat( minVal , maxVal ); +} + +inline void Vector2DClear( Vector2D& a ) +{ + a.x = a.y = 0.0f; +} + +//----------------------------------------------------------------------------- +// assignment +//----------------------------------------------------------------------------- + +inline Vector2D& Vector2D::operator=(const Vector2D &vOther) +{ + Assert( vOther.IsValid() ); + x=vOther.x; y=vOther.y; + return *this; +} + +//----------------------------------------------------------------------------- +// Array access +//----------------------------------------------------------------------------- + +inline vec_t& Vector2D::operator[](int i) +{ + Assert( (i >= 0) && (i < 2) ); + return ((vec_t*)this)[i]; +} + +inline vec_t Vector2D::operator[](int i) const +{ + Assert( (i >= 0) && (i < 2) ); + return ((vec_t*)this)[i]; +} + +//----------------------------------------------------------------------------- +// Base address... +//----------------------------------------------------------------------------- + +inline vec_t* Vector2D::Base() +{ + return (vec_t*)this; +} + +inline vec_t const* Vector2D::Base() const +{ + return (vec_t const*)this; +} + +//----------------------------------------------------------------------------- +// IsValid? +//----------------------------------------------------------------------------- + +inline bool Vector2D::IsValid() const +{ + return IsFinite(x) && IsFinite(y); +} + +//----------------------------------------------------------------------------- +// comparison +//----------------------------------------------------------------------------- + +inline bool Vector2D::operator==( const Vector2D& src ) const +{ + Assert( src.IsValid() && IsValid() ); + return (src.x == x) && (src.y == y); +} + +inline bool Vector2D::operator!=( const Vector2D& src ) const +{ + Assert( src.IsValid() && IsValid() ); + return (src.x != x) || (src.y != y); +} + + +//----------------------------------------------------------------------------- +// Copy +//----------------------------------------------------------------------------- + +inline void Vector2DCopy( const Vector2D& src, Vector2D& dst ) +{ + Assert( src.IsValid() ); + dst.x = src.x; + dst.y = src.y; +} + +inline void Vector2D::CopyToArray(float* rgfl) const +{ + Assert( IsValid() ); + Assert( rgfl ); + rgfl[0] = x; rgfl[1] = y; +} + +//----------------------------------------------------------------------------- +// standard math operations +//----------------------------------------------------------------------------- + +inline void Vector2D::Negate() +{ + Assert( IsValid() ); + x = -x; y = -y; +} + +inline Vector2D& Vector2D::operator+=(const Vector2D& v) +{ + Assert( IsValid() && v.IsValid() ); + x+=v.x; y+=v.y; + return *this; +} + +inline Vector2D& Vector2D::operator-=(const Vector2D& v) +{ + Assert( IsValid() && v.IsValid() ); + x-=v.x; y-=v.y; + return *this; +} + +inline Vector2D& Vector2D::operator*=(float fl) +{ + x *= fl; + y *= fl; + Assert( IsValid() ); + return *this; +} + +inline Vector2D& Vector2D::operator*=(const Vector2D& v) +{ + x *= v.x; + y *= v.y; + Assert( IsValid() ); + return *this; +} + +inline Vector2D& Vector2D::operator/=(float fl) +{ + Assert( fl != 0.0f ); + float oofl = 1.0f / fl; + x *= oofl; + y *= oofl; + Assert( IsValid() ); + return *this; +} + +inline Vector2D& Vector2D::operator/=(const Vector2D& v) +{ + Assert( v.x != 0.0f && v.y != 0.0f ); + x /= v.x; + y /= v.y; + Assert( IsValid() ); + return *this; +} + +inline void Vector2DAdd( const Vector2D& a, const Vector2D& b, Vector2D& c ) +{ + Assert( a.IsValid() && b.IsValid() ); + c.x = a.x + b.x; + c.y = a.y + b.y; +} + +inline void Vector2DSubtract( const Vector2D& a, const Vector2D& b, Vector2D& c ) +{ + Assert( a.IsValid() && b.IsValid() ); + c.x = a.x - b.x; + c.y = a.y - b.y; +} + +inline void Vector2DMultiply( const Vector2D& a, vec_t b, Vector2D& c ) +{ + Assert( a.IsValid() && IsFinite(b) ); + c.x = a.x * b; + c.y = a.y * b; +} + +inline void Vector2DMultiply( const Vector2D& a, const Vector2D& b, Vector2D& c ) +{ + Assert( a.IsValid() && b.IsValid() ); + c.x = a.x * b.x; + c.y = a.y * b.y; +} + + +inline void Vector2DDivide( const Vector2D& a, vec_t b, Vector2D& c ) +{ + Assert( a.IsValid() ); + Assert( b != 0.0f ); + vec_t oob = 1.0f / b; + c.x = a.x * oob; + c.y = a.y * oob; +} + +inline void Vector2DDivide( const Vector2D& a, const Vector2D& b, Vector2D& c ) +{ + Assert( a.IsValid() ); + Assert( (b.x != 0.0f) && (b.y != 0.0f) ); + c.x = a.x / b.x; + c.y = a.y / b.y; +} + +inline void Vector2DMA( const Vector2D& start, float s, const Vector2D& dir, Vector2D& result ) +{ + Assert( start.IsValid() && IsFinite(s) && dir.IsValid() ); + result.x = start.x + s*dir.x; + result.y = start.y + s*dir.y; +} + +// FIXME: Remove +// For backwards compatability +inline void Vector2D::MulAdd(const Vector2D& a, const Vector2D& b, float scalar) +{ + x = a.x + b.x * scalar; + y = a.y + b.y * scalar; +} + +inline void Vector2DLerp(const Vector2D& src1, const Vector2D& src2, vec_t t, Vector2D& dest ) +{ + dest[0] = src1[0] + (src2[0] - src1[0]) * t; + dest[1] = src1[1] + (src2[1] - src1[1]) * t; +} + +//----------------------------------------------------------------------------- +// dot, cross +//----------------------------------------------------------------------------- +inline vec_t DotProduct2D(const Vector2D& a, const Vector2D& b) +{ + Assert( a.IsValid() && b.IsValid() ); + return( a.x*b.x + a.y*b.y ); +} + +// for backwards compatability +inline vec_t Vector2D::Dot( const Vector2D& vOther ) const +{ + return DotProduct2D( *this, vOther ); +} + + +//----------------------------------------------------------------------------- +// length +//----------------------------------------------------------------------------- +inline vec_t Vector2DLength( const Vector2D& v ) +{ + Assert( v.IsValid() ); + return (vec_t)FastSqrt(v.x*v.x + v.y*v.y); +} + +inline vec_t Vector2D::LengthSqr(void) const +{ + Assert( IsValid() ); + return (x*x + y*y); +} + +inline vec_t Vector2D::NormalizeInPlace() +{ + return Vector2DNormalize( *this ); +} + +inline bool Vector2D::IsLengthGreaterThan( float val ) const +{ + return LengthSqr() > val*val; +} + +inline bool Vector2D::IsLengthLessThan( float val ) const +{ + return LengthSqr() < val*val; +} + +inline vec_t Vector2D::Length(void) const +{ + return Vector2DLength( *this ); +} + + +inline void Vector2DMin( const Vector2D &a, const Vector2D &b, Vector2D &result ) +{ + result.x = (a.x < b.x) ? a.x : b.x; + result.y = (a.y < b.y) ? a.y : b.y; +} + + +inline void Vector2DMax( const Vector2D &a, const Vector2D &b, Vector2D &result ) +{ + result.x = (a.x > b.x) ? a.x : b.x; + result.y = (a.y > b.y) ? a.y : b.y; +} + + +//----------------------------------------------------------------------------- +// Normalization +//----------------------------------------------------------------------------- +inline vec_t Vector2DNormalize( Vector2D& v ) +{ + Assert( v.IsValid() ); + vec_t l = v.Length(); + if (l != 0.0f) + { + v /= l; + } + else + { + v.x = v.y = 0.0f; + } + return l; +} + + +//----------------------------------------------------------------------------- +// Get the distance from this Vector2D to the other one +//----------------------------------------------------------------------------- +inline vec_t Vector2D::DistTo(const Vector2D &vOther) const +{ + Vector2D delta; + Vector2DSubtract( *this, vOther, delta ); + return delta.Length(); +} + +inline vec_t Vector2D::DistToSqr(const Vector2D &vOther) const +{ + Vector2D delta; + Vector2DSubtract( *this, vOther, delta ); + return delta.LengthSqr(); +} + + +//----------------------------------------------------------------------------- +// Computes the closest point to vecTarget no farther than flMaxDist from vecStart +//----------------------------------------------------------------------------- +inline void ComputeClosestPoint2D( const Vector2D& vecStart, float flMaxDist, const Vector2D& vecTarget, Vector2D *pResult ) +{ + Vector2D vecDelta; + Vector2DSubtract( vecTarget, vecStart, vecDelta ); + float flDistSqr = vecDelta.LengthSqr(); + if ( flDistSqr <= flMaxDist * flMaxDist ) + { + *pResult = vecTarget; + } + else + { + vecDelta /= FastSqrt( flDistSqr ); + Vector2DMA( vecStart, flMaxDist, vecDelta, *pResult ); + } +} + + + +//----------------------------------------------------------------------------- +// +// Slow methods +// +//----------------------------------------------------------------------------- + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +//----------------------------------------------------------------------------- +// Returns a Vector2D with the min or max in X, Y, and Z. +//----------------------------------------------------------------------------- + +inline Vector2D Vector2D::Min(const Vector2D &vOther) const +{ + return Vector2D(x < vOther.x ? x : vOther.x, + y < vOther.y ? y : vOther.y); +} + +inline Vector2D Vector2D::Max(const Vector2D &vOther) const +{ + return Vector2D(x > vOther.x ? x : vOther.x, + y > vOther.y ? y : vOther.y); +} + + +//----------------------------------------------------------------------------- +// arithmetic operations +//----------------------------------------------------------------------------- + +inline Vector2D Vector2D::operator-(void) const +{ + return Vector2D(-x,-y); +} + +inline Vector2D Vector2D::operator+(const Vector2D& v) const +{ + Vector2D res; + Vector2DAdd( *this, v, res ); + return res; +} + +inline Vector2D Vector2D::operator-(const Vector2D& v) const +{ + Vector2D res; + Vector2DSubtract( *this, v, res ); + return res; +} + +inline Vector2D Vector2D::operator*(float fl) const +{ + Vector2D res; + Vector2DMultiply( *this, fl, res ); + return res; +} + +inline Vector2D Vector2D::operator*(const Vector2D& v) const +{ + Vector2D res; + Vector2DMultiply( *this, v, res ); + return res; +} + +inline Vector2D Vector2D::operator/(float fl) const +{ + Vector2D res; + Vector2DDivide( *this, fl, res ); + return res; +} + +inline Vector2D Vector2D::operator/(const Vector2D& v) const +{ + Vector2D res; + Vector2DDivide( *this, v, res ); + return res; +} + +inline Vector2D operator*(float fl, const Vector2D& v) +{ + return v * fl; +} + +#endif //slow + +#endif // VECTOR2D_H + |