initial

mathlib/3dnow.cpp

@@ -0,0 +1,160 @@
+//========= Copyright � 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 3DNow Math primitives.
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+//#include <memory.h>
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+//#include "mathlib/amd3dx.h"
+#include "mathlib/vector.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+#ifdef COMPILER_MSVC
+#pragma warning(disable:4244)   // "conversion from 'const int' to 'float', possible loss of data"
+#pragma warning(disable:4730)	// "mixing _m64 and floating point expressions may result in incorrect code"
+#endif
+
+//-----------------------------------------------------------------------------
+// 3D Now Implementations of optimized routines:
+//-----------------------------------------------------------------------------
+float _3DNow_Sqrt(float x)
+{
+	Assert( s_bMathlibInitialized );
+	float	root = 0.f;
+#ifdef _WIN32
+	_asm
+	{
+		femms
+		movd		mm0, x
+		PFRSQRT		(mm1,mm0)
+		punpckldq	mm0, mm0
+		PFMUL		(mm0, mm1)
+		movd		root, mm0
+		femms
+	}
+#elif POSIX
+ 	__asm __volatile__( "femms" );
+ 	__asm __volatile__ ( "pfrsqrt    %y0, %y1 \n\t" "punpckldq   %y1, %y1 \n\t" "pfmul      %y1, %y0 \n\t" : "=y" (root), "=y" (x) :"0" (x));
+ 	__asm __volatile__( "femms" );
+#else
+#error
+#endif
+
+	return root;
+}
+
+// NJS FIXME: Need to test Recripricol squareroot performance and accuraccy
+// on AMD's before using the specialized instruction.
+float _3DNow_RSqrt(float x)
+{
+	Assert( s_bMathlibInitialized );
+
+	return 1.f / _3DNow_Sqrt(x);
+}
+
+
+float FASTCALL _3DNow_VectorNormalize (Vector& vec)
+{
+	Assert( s_bMathlibInitialized );
+	float *v = &vec[0];
+	float	radius = 0.f;
+
+	if ( v[0] || v[1] || v[2] )
+	{
+#ifdef _WIN32
+	_asm
+		{
+			mov			eax, v
+			femms
+			movq		mm0, QWORD PTR [eax]
+			movd		mm1, DWORD PTR [eax+8]
+			movq		mm2, mm0
+			movq		mm3, mm1
+			PFMUL		(mm0, mm0)
+			PFMUL		(mm1, mm1)
+			PFACC		(mm0, mm0)
+			PFADD		(mm1, mm0)
+			PFRSQRT		(mm0, mm1)
+			punpckldq	mm1, mm1
+			PFMUL		(mm1, mm0)
+			PFMUL		(mm2, mm0)
+			PFMUL		(mm3, mm0)
+			movq		QWORD PTR [eax], mm2
+			movd		DWORD PTR [eax+8], mm3
+			movd		radius, mm1
+			femms
+		}
+#elif POSIX	
+		long long a,c;
+    		int b,d;
+    		memcpy(&a,&vec[0],sizeof(a));
+    		memcpy(&b,&vec[2],sizeof(b));
+    		memcpy(&c,&vec[0],sizeof(c));
+    		memcpy(&d,&vec[2],sizeof(d));
+
+      		__asm __volatile__( "femms" );
+        	__asm __volatile__ ( "pfmul           %y3, %y3\n\t" "pfmul           %y0, %y0 \n\t" "pfacc           %y3, %y3 \n\t" "pfadd           %y3, %y0 \n\t" "pfrsqrt         %y0, %y3 \n\t" "punpckldq       %y0, %y0 \n\t" "pfmul           %y3, %y0 \n\t" "pfmul           %y3, %y2 \n\t" "pfmul           %y3, %y1 \n\t" : "=y" (radius), "=y" (c), "=y" (d) : "y" (a), "0" (b), "1" (c), "2" (d));
+      		memcpy(&vec[0],&c,sizeof(c));
+      		memcpy(&vec[2],&d,sizeof(d));		
+        	__asm __volatile__( "femms" );
+
+#else
+#error
+#endif
+	}
+    return radius;
+}
+
+
+void FASTCALL _3DNow_VectorNormalizeFast (Vector& vec)
+{
+	_3DNow_VectorNormalize( vec );
+}
+
+
+// JAY: This complains with the latest processor pack
+#pragma warning(disable: 4730)
+
+float _3DNow_InvRSquared(const float* v)
+{
+	Assert( s_bMathlibInitialized );
+	float	r2 = 1.f;
+#ifdef _WIN32
+	_asm { // AMD 3DNow only routine
+		mov			eax, v
+		femms
+		movq		mm0, QWORD PTR [eax]
+		movd		mm1, DWORD PTR [eax+8]
+		movd		mm2, [r2]
+		PFMUL		(mm0, mm0)
+		PFMUL		(mm1, mm1)
+		PFACC		(mm0, mm0)
+		PFADD		(mm1, mm0)
+		PFMAX		(mm1, mm2)
+		PFRCP		(mm0, mm1)
+		movd		[r2], mm0
+		femms
+	}
+#elif POSIX
+		long long a,c;
+    		int b;
+    		memcpy(&a,&v[0],sizeof(a));
+    		memcpy(&b,&v[2],sizeof(b));
+    		memcpy(&c,&v[0],sizeof(c));
+
+      		__asm __volatile__( "femms" );
+        	__asm __volatile__ ( "PFMUL          %y2, %y2 \n\t" "PFMUL          %y3, %y3 \n\t" "PFACC          %y2, %y2 \n\t" "PFADD          %y2, %y3 \n\t" "PFMAX          %y3, %y4 \n\t" "PFRCP          %y3, %y2 \n\t" "movq           %y2, %y0 \n\t" : "=y" (r2) : "0" (r2), "y" (a), "y" (b), "y" (c));
+        	__asm __volatile__( "femms" );
+#else
+#error
+#endif
+
+	return r2;
+}

mathlib/IceKey.cpp

@@ -0,0 +1,429 @@
+// Purpose: C++ implementation of the ICE encryption algorithm.
+//			Taken from public domain code, as written by Matthew Kwan - July 1996
+//			http://www.darkside.com.au/ice/
+
+#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)
+
+#include "mathlib/IceKey.H"
+#include "tier1/strtools.h"
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+#pragma warning(disable: 4244)
+
+
+	/* Structure of a single round subkey */
+class IceSubkey {
+    public:
+	unsigned long	val[3];
+};
+
+
+	/* The S-boxes */
+static unsigned long	ice_sbox[4][1024];
+static int		ice_sboxes_initialised = 0;
+
+
+	/* Modulo values for the S-boxes */
+static const int	ice_smod[4][4] = {
+				{333, 313, 505, 369},
+				{379, 375, 319, 391},
+				{361, 445, 451, 397},
+				{397, 425, 395, 505}};
+
+	/* XOR values for the S-boxes */
+static const int	ice_sxor[4][4] = {
+				{0x83, 0x85, 0x9b, 0xcd},
+				{0xcc, 0xa7, 0xad, 0x41},
+				{0x4b, 0x2e, 0xd4, 0x33},
+				{0xea, 0xcb, 0x2e, 0x04}};
+
+	/* Permutation values for the P-box */
+static const unsigned long	ice_pbox[32] = {
+		0x00000001, 0x00000080, 0x00000400, 0x00002000,
+		0x00080000, 0x00200000, 0x01000000, 0x40000000,
+		0x00000008, 0x00000020, 0x00000100, 0x00004000,
+		0x00010000, 0x00800000, 0x04000000, 0x20000000,
+		0x00000004, 0x00000010, 0x00000200, 0x00008000,
+		0x00020000, 0x00400000, 0x08000000, 0x10000000,
+		0x00000002, 0x00000040, 0x00000800, 0x00001000,
+		0x00040000, 0x00100000, 0x02000000, 0x80000000};
+
+	/* The key rotation schedule */
+static const int	ice_keyrot[16] = {
+				0, 1, 2, 3, 2, 1, 3, 0,
+				1, 3, 2, 0, 3, 1, 0, 2};
+
+
+/*
+ * 8-bit Galois Field multiplication of a by b, modulo m.
+ * Just like arithmetic multiplication, except that additions and
+ * subtractions are replaced by XOR.
+ */
+
+static unsigned int
+gf_mult (
+	register unsigned int	a,
+	register unsigned int	b,
+	register unsigned int	m
+) {
+	register unsigned int	res = 0;
+
+	while (b) {
+	    if (b & 1)
+		res ^= a;
+
+	    a <<= 1;
+	    b >>= 1;
+
+	    if (a >= 256)
+		a ^= m;
+	}
+
+	return (res);
+}
+
+
+/*
+ * Galois Field exponentiation.
+ * Raise the base to the power of 7, modulo m.
+ */
+
+static unsigned long
+gf_exp7 (
+	register unsigned int	b,
+	unsigned int		m
+) {
+	register unsigned int	x;
+
+	if (b == 0)
+	    return (0);
+
+	x = gf_mult (b, b, m);
+	x = gf_mult (b, x, m);
+	x = gf_mult (x, x, m);
+	return (gf_mult (b, x, m));
+}
+
+
+/*
+ * Carry out the ICE 32-bit P-box permutation.
+ */
+
+static unsigned long
+ice_perm32 (
+	register unsigned long	x
+) {
+	register unsigned long		res = 0;
+	register const unsigned long	*pbox = ice_pbox;
+
+	while (x) {
+	    if (x & 1)
+		res |= *pbox;
+	    pbox++;
+	    x >>= 1;
+	}
+
+	return (res);
+}
+
+
+/*
+ * Initialise the ICE S-boxes.
+ * This only has to be done once.
+ */
+
+static void
+ice_sboxes_init (void)
+{
+	register int	i;
+
+	for (i=0; i<1024; i++) {
+	    int			col = (i >> 1) & 0xff;
+	    int			row = (i & 0x1) | ((i & 0x200) >> 8);
+	    unsigned long	x;
+
+	    x = gf_exp7 (col ^ ice_sxor[0][row], ice_smod[0][row]) << 24;
+	    ice_sbox[0][i] = ice_perm32 (x);
+
+	    x = gf_exp7 (col ^ ice_sxor[1][row], ice_smod[1][row]) << 16;
+	    ice_sbox[1][i] = ice_perm32 (x);
+
+	    x = gf_exp7 (col ^ ice_sxor[2][row], ice_smod[2][row]) << 8;
+	    ice_sbox[2][i] = ice_perm32 (x);
+
+	    x = gf_exp7 (col ^ ice_sxor[3][row], ice_smod[3][row]);
+	    ice_sbox[3][i] = ice_perm32 (x);
+	}
+}
+
+
+/*
+ * Create a new ICE key.
+ */
+
+IceKey::IceKey (int n)
+{
+	if (!ice_sboxes_initialised) {
+	    ice_sboxes_init ();
+	    ice_sboxes_initialised = 1;
+	}
+
+	if (n < 1) {
+	    _size = 1;
+	    _rounds = 8;
+	} else {
+	    _size = n;
+	    _rounds = n * 16;
+	}
+
+	_keysched = new IceSubkey[_rounds];
+}
+
+
+/*
+ * Destroy an ICE key.
+ */
+
+IceKey::~IceKey ()
+{
+	int	i, j;
+
+	for (i=0; i<_rounds; i++)
+	    for (j=0; j<3; j++)
+		_keysched[i].val[j] = 0;
+
+	_rounds = _size = 0;
+
+	delete[] _keysched;
+}
+
+
+/*
+ * The single round ICE f function.
+ */
+
+static unsigned long
+ice_f (
+	register unsigned long	p,
+	const IceSubkey		*sk
+) {
+	unsigned long	tl, tr;		/* Expanded 40-bit values */
+	unsigned long	al, ar;		/* Salted expanded 40-bit values */
+
+					/* Left half expansion */
+	tl = ((p >> 16) & 0x3ff) | (((p >> 14) | (p << 18)) & 0xffc00);
+
+					/* Right half expansion */
+	tr = (p & 0x3ff) | ((p << 2) & 0xffc00);
+
+					/* Perform the salt permutation */
+			// al = (tr & sk->val[2]) | (tl & ~sk->val[2]);
+			// ar = (tl & sk->val[2]) | (tr & ~sk->val[2]);
+	al = sk->val[2] & (tl ^ tr);
+	ar = al ^ tr;
+	al ^= tl;
+
+	al ^= sk->val[0];		/* XOR with the subkey */
+	ar ^= sk->val[1];
+
+					/* S-box lookup and permutation */
+	return (ice_sbox[0][al >> 10] | ice_sbox[1][al & 0x3ff]
+		| ice_sbox[2][ar >> 10] | ice_sbox[3][ar & 0x3ff]);
+}
+
+
+/*
+ * Encrypt a block of 8 bytes of data with the given ICE key.
+ */
+
+void
+IceKey::encrypt (
+	const unsigned char	*ptext,
+	unsigned char		*ctext
+) const
+{
+	register int		i;
+	register unsigned long	l, r;
+
+	l = (((unsigned long) ptext[0]) << 24)
+				| (((unsigned long) ptext[1]) << 16)
+				| (((unsigned long) ptext[2]) << 8) | ptext[3];
+	r = (((unsigned long) ptext[4]) << 24)
+				| (((unsigned long) ptext[5]) << 16)
+				| (((unsigned long) ptext[6]) << 8) | ptext[7];
+
+	for (i = 0; i < _rounds; i += 2) {
+	    l ^= ice_f (r, &_keysched[i]);
+	    r ^= ice_f (l, &_keysched[i + 1]);
+	}
+
+	for (i = 0; i < 4; i++) {
+	    ctext[3 - i] = r & 0xff;
+	    ctext[7 - i] = l & 0xff;
+
+	    r >>= 8;
+	    l >>= 8;
+	}
+}
+
+
+/*
+ * Decrypt a block of 8 bytes of data with the given ICE key.
+ */
+
+void
+IceKey::decrypt (
+	const unsigned char	*ctext,
+	unsigned char		*ptext
+) const
+{
+	register int		i;
+	register unsigned long	l, r;
+
+	l = (((unsigned long) ctext[0]) << 24)
+				| (((unsigned long) ctext[1]) << 16)
+				| (((unsigned long) ctext[2]) << 8) | ctext[3];
+	r = (((unsigned long) ctext[4]) << 24)
+				| (((unsigned long) ctext[5]) << 16)
+				| (((unsigned long) ctext[6]) << 8) | ctext[7];
+
+	for (i = _rounds - 1; i > 0; i -= 2) {
+	    l ^= ice_f (r, &_keysched[i]);
+	    r ^= ice_f (l, &_keysched[i - 1]);
+	}
+
+	for (i = 0; i < 4; i++) {
+	    ptext[3 - i] = r & 0xff;
+	    ptext[7 - i] = l & 0xff;
+
+	    r >>= 8;
+	    l >>= 8;
+	}
+}
+
+
+/*
+ * Set 8 rounds [n, n+7] of the key schedule of an ICE key.
+ */
+
+void
+IceKey::scheduleBuild (
+	unsigned short	*kb,
+	int		n,
+	const int	*keyrot
+) {
+	int		i;
+
+	for (i=0; i<8; i++) {
+	    register int	j;
+	    register int	kr = keyrot[i];
+	    IceSubkey		*isk = &_keysched[n + i];
+
+	    for (j=0; j<3; j++)
+		isk->val[j] = 0;
+
+	    for (j=0; j<15; j++) {
+		register int	k;
+		unsigned long	*curr_sk = &isk->val[j % 3];
+
+		for (k=0; k<4; k++) {
+		    unsigned short	*curr_kb = &kb[(kr + k) & 3];
+		    register int	bit = *curr_kb & 1;
+
+		    *curr_sk = (*curr_sk << 1) | bit;
+		    *curr_kb = (*curr_kb >> 1) | ((bit ^ 1) << 15);
+		}
+	    }
+	}
+}
+
+
+/*
+ * Set the key schedule of an ICE key.
+ */
+
+void
+IceKey::set (
+	const unsigned char	*key
+) {
+	int		i;
+
+	if (_rounds == 8) {
+	    unsigned short	kb[4];
+
+	    for (i=0; i<4; i++)
+		kb[3 - i] = (key[i*2] << 8) | key[i*2 + 1];
+
+	    scheduleBuild (kb, 0, ice_keyrot);
+	    return;
+	}
+
+	for (i=0; i<_size; i++) {
+	    int			j;
+	    unsigned short	kb[4];
+
+	    for (j=0; j<4; j++)
+		kb[3 - j] = (key[i*8 + j*2] << 8) | key[i*8 + j*2 + 1];
+
+	    scheduleBuild (kb, i*8, ice_keyrot);
+	    scheduleBuild (kb, _rounds - 8 - i*8, &ice_keyrot[8]);
+	}
+}
+
+
+/*
+ * Return the key size, in bytes.
+ */
+
+int
+IceKey::keySize () const
+{
+	return (_size * 8);
+}
+
+
+/*
+ * Return the block size, in bytes.
+ */
+
+int
+IceKey::blockSize () const
+{
+	return (8);
+}
+
+
+// Valve-written routine  to decode a buffer
+void DecodeICE( unsigned char *pBuffer, int nSize, const unsigned char *pKey)
+{
+	if ( !pKey )
+		return;
+
+	IceKey ice( 0 ); // level 0 = 64bit key
+	ice.set( pKey ); // set key
+
+	int nBlockSize = ice.blockSize();
+
+	unsigned char *pTemp = (unsigned char *) stackalloc( PAD_NUMBER( nSize, nBlockSize ) );
+	unsigned char *p1 = pBuffer;
+	unsigned char *p2 = pTemp;
+
+	// encrypt data in 8 byte blocks
+	int nBytesLeft = nSize;
+	while ( nBytesLeft >= nBlockSize )
+	{
+		ice.decrypt( p1, p2 );
+		nBytesLeft -= nBlockSize;
+		p1+=nBlockSize;
+		p2+=nBlockSize;
+	}
+
+	// copy encrypted data back to original buffer
+	Q_memcpy( pBuffer, pTemp, nSize - nBytesLeft );
+}
+
+
+#endif // !_STATIC_LINKED || _SHARED_LIB

mathlib/_vpc_/manifest_mathlib/win32/manifest.txt

@@ -0,0 +1,208 @@
+// ----------------------------------------- //
+//           File generated by VPC           //
+// ----------------------------------------- //
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\almostequal.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\almostequal.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\almostequal.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\anorms.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\anorms.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\anorms.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\box_buoyancy.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\box_buoyancy.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\box_buoyancy.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\bumpvects.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\bumpvects.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\bumpvects.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\camera.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\camera.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\camera.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\capsule.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\capsule.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\capsule.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\cholesky.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\cholesky.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\cholesky.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\color_conversion.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\color_conversion.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\color_conversion.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Release output file:    F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\expressioncalculator.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\expressioncalculator.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\expressioncalculator.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\halton.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\halton.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\halton.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\IceKey.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\IceKey.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\IceKey.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\imagequant.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\imagequant.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\imagequant.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\kdop.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\kdop.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\kdop.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\lightdesc.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\lightdesc.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\lightdesc.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\mathlib_base.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\mathlib_base.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\mathlib_base.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\planefit.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\planefit.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\planefit.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\polygon.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\polygon.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\polygon.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\polyhedron.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\polyhedron.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\polyhedron.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\powsse.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\powsse.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\powsse.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\quantize.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\quantize.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\quantize.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\randsse.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\randsse.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\randsse.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\simplex.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\simplex.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\simplex.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\sparse_convolution_noise.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\sparse_convolution_noise.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\sparse_convolution_noise.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\sphere.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\sphere.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\sphere.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\spherical.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\spherical.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\spherical.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\sse.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\sse.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\sse.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\sseconst.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\sseconst.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\sseconst.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\ssenoise.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\ssenoise.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\ssenoise.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\vmatrix.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\vmatrix.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\vmatrix.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\volumeculler.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\volumeculler.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\volumeculler.cpp
+Containing unity file:  
+PCH file:               
+

mathlib/_vpc_/manifest_mathlib_extended/win32/manifest.txt

@@ -0,0 +1,82 @@
+// ----------------------------------------- //
+//           File generated by VPC           //
+// ----------------------------------------- //
+
+Source file:            F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Release output file:    F:\csgo_64\cstrike15_src\common\debug_lib_check.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\disjoint_set_forest.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\disjoint_set_forest.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\disjoint_set_forest.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\dynamictree.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\dynamictree.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\dynamictree.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\eigen.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\feagglomerator.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\feagglomerator.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\feagglomerator.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\femodel.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\femodel.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\femodel.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\femodelbuilder.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\femodelbuilder.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\femodelbuilder.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\femodeldesc.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\femodeldesc.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\femodeldesc.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\simdvectormatrix.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\softbody.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\softbody.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\softbody.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\softbodyenvironment.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\softbodyenvironment.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\softbodyenvironment.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\svd.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\svd.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\svd.cpp
+Containing unity file:  
+PCH file:               
+
+Source file:            F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Debug output file:      F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Release output file:    F:\csgo_64\cstrike15_src\mathlib\transform.cpp
+Containing unity file:  
+PCH file:               
+

mathlib/almostequal.cpp

@@ -0,0 +1,97 @@
+//========= Copyright © 1996-2008, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: Fast ways to compare equality of two floats.  Assumes 
+// sizeof(float) == sizeof(int) and we are using IEEE format.
+//
+// Source:  http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
+//=====================================================================================//
+
+#include <float.h>
+#include <math.h>
+
+#include "mathlib/mathlib.h"
+
+static inline bool AE_IsInfinite(float a)
+{
+    const int kInfAsInt = 0x7F800000;
+
+    // An infinity has an exponent of 255 (shift left 23 positions) and
+    // a zero mantissa. There are two infinities - positive and negative.
+    if ((*(int*)&a & 0x7FFFFFFF) == kInfAsInt)
+        return true;
+    return false;
+}
+
+static inline bool AE_IsNan(float a)
+{
+    // a NAN has an exponent of 255 (shifted left 23 positions) and
+    // a non-zero mantissa.
+    int exp = *(int*)&a & 0x7F800000;
+    int mantissa = *(int*)&a & 0x007FFFFF;
+    if (exp == 0x7F800000 && mantissa != 0)
+        return true;
+    return false;
+}
+
+static inline int AE_Sign(float a)
+{
+    // The sign bit of a number is the high bit.
+    return (*(int*)&a) & 0x80000000;
+}
+
+// This is the 'final' version of the AlmostEqualUlps function.
+// The optional checks are included for completeness, but in many
+// cases they are not necessary, or even not desirable.
+bool AlmostEqual(float a, float b, int maxUlps)
+{
+    // There are several optional checks that you can do, depending
+    // on what behavior you want from your floating point comparisons.
+    // These checks should not be necessary and they are included
+    // mainly for completeness.
+
+    // If a or b are infinity (positive or negative) then
+    // only return true if they are exactly equal to each other -
+    // that is, if they are both infinities of the same sign.
+    // This check is only needed if you will be generating
+    // infinities and you don't want them 'close' to numbers
+    // near FLT_MAX.
+    if (AE_IsInfinite(a) || AE_IsInfinite(b))
+        return a == b;
+
+    // If a or b are a NAN, return false. NANs are equal to nothing,
+    // not even themselves.
+    // This check is only needed if you will be generating NANs
+    // and you use a maxUlps greater than 4 million or you want to
+    // ensure that a NAN does not equal itself.
+    if (AE_IsNan(a) || AE_IsNan(b))
+        return false;
+
+    // After adjusting floats so their representations are lexicographically
+    // ordered as twos-complement integers a very small positive number
+    // will compare as 'close' to a very small negative number. If this is
+    // not desireable, and if you are on a platform that supports
+    // subnormals (which is the only place the problem can show up) then
+    // you need this check.
+    // The check for a == b is because zero and negative zero have different
+    // signs but are equal to each other.
+    if (AE_Sign(a) != AE_Sign(b))
+        return a == b;
+
+    int aInt = *(int*)&a;
+    // Make aInt lexicographically ordered as a twos-complement int
+    if (aInt < 0)
+        aInt = 0x80000000 - aInt;
+    // Make bInt lexicographically ordered as a twos-complement int
+    int bInt = *(int*)&b;
+    if (bInt < 0)
+        bInt = 0x80000000 - bInt;
+
+    // Now we can compare aInt and bInt to find out how far apart a and b
+    // are.
+    int intDiff = abs(aInt - bInt);
+    if (intDiff <= maxUlps)
+        return true;
+    return false;
+}
+
+

mathlib/anorms.cpp

@@ -0,0 +1,181 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+//=============================================================================//
+#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)
+
+
+#include "mathlib/vector.h"
+#include "mathlib/anorms.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+Vector g_anorms[NUMVERTEXNORMALS] =
+{
+	Vector(-0.525731, 0.000000, 0.850651), 
+	Vector(-0.442863, 0.238856, 0.864188), 
+	Vector(-0.295242, 0.000000, 0.955423), 
+	Vector(-0.309017, 0.500000, 0.809017), 
+	Vector(-0.162460, 0.262866, 0.951056), 
+	Vector(0.000000, 0.000000, 1.000000), 
+	Vector(0.000000, 0.850651, 0.525731), 
+	Vector(-0.147621, 0.716567, 0.681718), 
+	Vector(0.147621, 0.716567, 0.681718), 
+	Vector(0.000000, 0.525731, 0.850651), 
+	Vector(0.309017, 0.500000, 0.809017), 
+	Vector(0.525731, 0.000000, 0.850651), 
+	Vector(0.295242, 0.000000, 0.955423), 
+	Vector(0.442863, 0.238856, 0.864188), 
+	Vector(0.162460, 0.262866, 0.951056), 
+	Vector(-0.681718, 0.147621, 0.716567), 
+	Vector(-0.809017, 0.309017, 0.500000), 
+	Vector(-0.587785, 0.425325, 0.688191), 
+	Vector(-0.850651, 0.525731, 0.000000), 
+	Vector(-0.864188, 0.442863, 0.238856), 
+	Vector(-0.716567, 0.681718, 0.147621), 
+	Vector(-0.688191, 0.587785, 0.425325), 
+	Vector(-0.500000, 0.809017, 0.309017), 
+	Vector(-0.238856, 0.864188, 0.442863), 
+	Vector(-0.425325, 0.688191, 0.587785), 
+	Vector(-0.716567, 0.681718, -0.147621), 
+	Vector(-0.500000, 0.809017, -0.309017), 
+	Vector(-0.525731, 0.850651, 0.000000), 
+	Vector(0.000000, 0.850651, -0.525731), 
+	Vector(-0.238856, 0.864188, -0.442863), 
+	Vector(0.000000, 0.955423, -0.295242), 
+	Vector(-0.262866, 0.951056, -0.162460), 
+	Vector(0.000000, 1.000000, 0.000000), 
+	Vector(0.000000, 0.955423, 0.295242), 
+	Vector(-0.262866, 0.951056, 0.162460), 
+	Vector(0.238856, 0.864188, 0.442863), 
+	Vector(0.262866, 0.951056, 0.162460), 
+	Vector(0.500000, 0.809017, 0.309017), 
+	Vector(0.238856, 0.864188, -0.442863), 
+	Vector(0.262866, 0.951056, -0.162460), 
+	Vector(0.500000, 0.809017, -0.309017), 
+	Vector(0.850651, 0.525731, 0.000000), 
+	Vector(0.716567, 0.681718, 0.147621), 
+	Vector(0.716567, 0.681718, -0.147621), 
+	Vector(0.525731, 0.850651, 0.000000), 
+	Vector(0.425325, 0.688191, 0.587785), 
+	Vector(0.864188, 0.442863, 0.238856), 
+	Vector(0.688191, 0.587785, 0.425325), 
+	Vector(0.809017, 0.309017, 0.500000), 
+	Vector(0.681718, 0.147621, 0.716567), 
+	Vector(0.587785, 0.425325, 0.688191), 
+	Vector(0.955423, 0.295242, 0.000000), 
+	Vector(1.000000, 0.000000, 0.000000), 
+	Vector(0.951056, 0.162460, 0.262866), 
+	Vector(0.850651, -0.525731, 0.000000), 
+	Vector(0.955423, -0.295242, 0.000000), 
+	Vector(0.864188, -0.442863, 0.238856), 
+	Vector(0.951056, -0.162460, 0.262866), 
+	Vector(0.809017, -0.309017, 0.500000), 
+	Vector(0.681718, -0.147621, 0.716567), 
+	Vector(0.850651, 0.000000, 0.525731), 
+	Vector(0.864188, 0.442863, -0.238856), 
+	Vector(0.809017, 0.309017, -0.500000), 
+	Vector(0.951056, 0.162460, -0.262866), 
+	Vector(0.525731, 0.000000, -0.850651), 
+	Vector(0.681718, 0.147621, -0.716567), 
+	Vector(0.681718, -0.147621, -0.716567), 
+	Vector(0.850651, 0.000000, -0.525731), 
+	Vector(0.809017, -0.309017, -0.500000), 
+	Vector(0.864188, -0.442863, -0.238856), 
+	Vector(0.951056, -0.162460, -0.262866), 
+	Vector(0.147621, 0.716567, -0.681718), 
+	Vector(0.309017, 0.500000, -0.809017), 
+	Vector(0.425325, 0.688191, -0.587785), 
+	Vector(0.442863, 0.238856, -0.864188), 
+	Vector(0.587785, 0.425325, -0.688191), 
+	Vector(0.688191, 0.587785, -0.425325), 
+	Vector(-0.147621, 0.716567, -0.681718), 
+	Vector(-0.309017, 0.500000, -0.809017), 
+	Vector(0.000000, 0.525731, -0.850651), 
+	Vector(-0.525731, 0.000000, -0.850651), 
+	Vector(-0.442863, 0.238856, -0.864188), 
+	Vector(-0.295242, 0.000000, -0.955423), 
+	Vector(-0.162460, 0.262866, -0.951056), 
+	Vector(0.000000, 0.000000, -1.000000), 
+	Vector(0.295242, 0.000000, -0.955423), 
+	Vector(0.162460, 0.262866, -0.951056), 
+	Vector(-0.442863, -0.238856, -0.864188), 
+	Vector(-0.309017, -0.500000, -0.809017), 
+	Vector(-0.162460, -0.262866, -0.951056), 
+	Vector(0.000000, -0.850651, -0.525731), 
+	Vector(-0.147621, -0.716567, -0.681718), 
+	Vector(0.147621, -0.716567, -0.681718), 
+	Vector(0.000000, -0.525731, -0.850651), 
+	Vector(0.309017, -0.500000, -0.809017), 
+	Vector(0.442863, -0.238856, -0.864188), 
+	Vector(0.162460, -0.262866, -0.951056), 
+	Vector(0.238856, -0.864188, -0.442863), 
+	Vector(0.500000, -0.809017, -0.309017), 
+	Vector(0.425325, -0.688191, -0.587785), 
+	Vector(0.716567, -0.681718, -0.147621), 
+	Vector(0.688191, -0.587785, -0.425325), 
+	Vector(0.587785, -0.425325, -0.688191), 
+	Vector(0.000000, -0.955423, -0.295242), 
+	Vector(0.000000, -1.000000, 0.000000), 
+	Vector(0.262866, -0.951056, -0.162460), 
+	Vector(0.000000, -0.850651, 0.525731), 
+	Vector(0.000000, -0.955423, 0.295242), 
+	Vector(0.238856, -0.864188, 0.442863), 
+	Vector(0.262866, -0.951056, 0.162460), 
+	Vector(0.500000, -0.809017, 0.309017), 
+	Vector(0.716567, -0.681718, 0.147621), 
+	Vector(0.525731, -0.850651, 0.000000), 
+	Vector(-0.238856, -0.864188, -0.442863), 
+	Vector(-0.500000, -0.809017, -0.309017), 
+	Vector(-0.262866, -0.951056, -0.162460), 
+	Vector(-0.850651, -0.525731, 0.000000), 
+	Vector(-0.716567, -0.681718, -0.147621), 
+	Vector(-0.716567, -0.681718, 0.147621), 
+	Vector(-0.525731, -0.850651, 0.000000), 
+	Vector(-0.500000, -0.809017, 0.309017), 
+	Vector(-0.238856, -0.864188, 0.442863), 
+	Vector(-0.262866, -0.951056, 0.162460), 
+	Vector(-0.864188, -0.442863, 0.238856), 
+	Vector(-0.809017, -0.309017, 0.500000), 
+	Vector(-0.688191, -0.587785, 0.425325), 
+	Vector(-0.681718, -0.147621, 0.716567), 
+	Vector(-0.442863, -0.238856, 0.864188), 
+	Vector(-0.587785, -0.425325, 0.688191), 
+	Vector(-0.309017, -0.500000, 0.809017), 
+	Vector(-0.147621, -0.716567, 0.681718), 
+	Vector(-0.425325, -0.688191, 0.587785), 
+	Vector(-0.162460, -0.262866, 0.951056), 
+	Vector(0.442863, -0.238856, 0.864188), 
+	Vector(0.162460, -0.262866, 0.951056), 
+	Vector(0.309017, -0.500000, 0.809017), 
+	Vector(0.147621, -0.716567, 0.681718), 
+	Vector(0.000000, -0.525731, 0.850651), 
+	Vector(0.425325, -0.688191, 0.587785), 
+	Vector(0.587785, -0.425325, 0.688191), 
+	Vector(0.688191, -0.587785, 0.425325), 
+	Vector(-0.955423, 0.295242, 0.000000), 
+	Vector(-0.951056, 0.162460, 0.262866), 
+	Vector(-1.000000, 0.000000, 0.000000), 
+	Vector(-0.850651, 0.000000, 0.525731), 
+	Vector(-0.955423, -0.295242, 0.000000), 
+	Vector(-0.951056, -0.162460, 0.262866), 
+	Vector(-0.864188, 0.442863, -0.238856), 
+	Vector(-0.951056, 0.162460, -0.262866), 
+	Vector(-0.809017, 0.309017, -0.500000), 
+	Vector(-0.864188, -0.442863, -0.238856), 
+	Vector(-0.951056, -0.162460, -0.262866), 
+	Vector(-0.809017, -0.309017, -0.500000), 
+	Vector(-0.681718, 0.147621, -0.716567), 
+	Vector(-0.681718, -0.147621, -0.716567), 
+	Vector(-0.850651, 0.000000, -0.525731), 
+	Vector(-0.688191, 0.587785, -0.425325), 
+	Vector(-0.587785, 0.425325, -0.688191), 
+	Vector(-0.425325, 0.688191, -0.587785), 
+	Vector(-0.425325, -0.688191, -0.587785), 
+	Vector(-0.587785, -0.425325, -0.688191), 
+	Vector(-0.688191, -0.587785, -0.425325)
+};
+
+#endif // !_STATIC_LINKED || _SHARED_LIB

mathlib/bumpvects.cpp

@@ -0,0 +1,69 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $Workfile:     $
+// $Date:         $
+//
+//-----------------------------------------------------------------------------
+// $Log: $
+//
+// $NoKeywords: $
+//=============================================================================//
+
+#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)
+
+
+#ifdef QUIVER
+#include "r_local.h"
+#endif
+#include "mathlib/bumpvects.h"
+#include "mathlib/vector.h"
+#include <assert.h>
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+// z is coming out of the face.
+
+void GetBumpNormals( const Vector& sVect, const Vector& tVect, const Vector& flatNormal, 
+					 const Vector& phongNormal, Vector bumpNormals[NUM_BUMP_VECTS] )
+{
+	Vector tmpNormal;
+	bool leftHanded;
+	int i;
+
+	assert( NUM_BUMP_VECTS == 3 );
+	
+	// Are we left or right handed?
+	CrossProduct( sVect, tVect, tmpNormal );
+	if( DotProduct( flatNormal, tmpNormal ) < 0.0f )
+	{
+		leftHanded = true;
+	}
+	else
+	{
+		leftHanded = false;
+	}
+
+	// Build a basis for the face around the phong normal
+	matrix3x4_t smoothBasis;
+	CrossProduct( phongNormal.Base(), sVect.Base(), smoothBasis[1] );
+	VectorNormalize( smoothBasis[1] );
+	CrossProduct( smoothBasis[1], phongNormal.Base(), smoothBasis[0] );
+	VectorNormalize( smoothBasis[0] );
+	VectorCopy( phongNormal.Base(), smoothBasis[2] );
+	
+	if( leftHanded )
+	{
+		VectorNegate( smoothBasis[1] );
+	}
+	
+	// move the g_localBumpBasis into world space to create bumpNormals
+	for( i = 0; i < 3; i++ )
+	{
+		VectorIRotate( g_localBumpBasis[i], smoothBasis, bumpNormals[i] );
+	}
+}
+
+#endif // !_STATIC_LINKED || _SHARED_LIB

mathlib/camera.cpp

@@ -0,0 +1,733 @@
+//====== Copyright © 1996-2004, Valve Corporation, All rights reserved. =======
+//
+// Purpose: 
+//
+//=============================================================================
+#include "mathlib/camera.h"
+#include "tier0/dbg.h"
+#include "mathlib/vector.h"
+#include "mathlib/vmatrix.h"
+#include "tier2/tier2.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+#define FORWARD_AXIS 0
+#define LEFT_AXIS 1
+#define UP_AXIS 2
+
+/// matrix to align our Valve coordinate system camera space with a standard viewing coordinate system
+/// x-axis goes from left to right in view space (right in camera space)
+/// y-axis goes from bottom to top in view space (up in camera space)
+/// z-axis goes from far to near in view space (-forward in camera space)
+/// The constructor takes sequential matrix rows, which are basis vectors in view space (so transposed from the init)
+#ifdef YUP_ACTIVE
+#error YUP_ACTIVE is not supported on this branch.
+#endif
+
+/// g_ViewAlignMatrix.Init( Vector(0,-1,0), Vector(0,0,1), Vector(-1,0,0), vec3_origin );
+static matrix3x4_t g_ViewAlignMatrix( 0, 0, -1, 0,		-1, 0, 0, 0,	0, 1, 0, 0 );
+VMatrix g_matViewToCameraMatrix( 0, 0, -1, 0,	-1, 0, 0, 0,   0, 1, 0, 0,  0, 0, 0, 1 );
+VMatrix g_matCameraToViewMatrix( 0, -1, 0, 0,   0, 0, 1, 0,  -1, 0, 0, 0,  0, 0, 0, 1 );
+
+//--------------------------------------------------------------------------------------------------
+// Extract the direction vectors from the a matrix 
+//--------------------------------------------------------------------------------------------------
+void ExtractDirectionVectors( Vector *pForward, Vector *pLeft, Vector *pUp, const matrix3x4_t &mMatrix )
+{ 
+	MatrixGetColumn( mMatrix, FORWARD_AXIS, *pForward );
+	MatrixGetColumn( mMatrix, LEFT_AXIS, *pLeft );
+	MatrixGetColumn( mMatrix, UP_AXIS, *pUp );
+}
+
+
+// Returns points  in this order:
+//  2--3
+//	|  |
+//	0--1
+void CalcFarPlaneCameraRelativePoints( Vector *p4PointsOut, Vector &vForward, Vector &vUp, Vector &vLeft, float flFarPlane, 
+									   float flFovX, float flAspect,
+									   float flClipSpaceBottomLeftX /*= -1.0f*/, float flClipSpaceBottomLeftY /*= -1.0f*/,
+									   float flClipSpaceTopRightX /*= 1.0f*/, float flClipSpaceTopRightY /*= 1.0f*/ )
+{
+	Vector vFowardShift = flFarPlane * vForward;
+	Vector vUpShift;
+	Vector vRightShift;
+	if ( flFovX == -1 )
+	{
+		vUpShift = vUp;
+		vRightShift = -vLeft;
+	}
+	else
+	{
+		float flTanX = tanf( DEG2RAD( flFovX * 0.5f ) );
+		float flTanY = flTanX / flAspect;
+
+		vUpShift = flFarPlane * flTanY * vUp;
+		vRightShift = flFarPlane * flTanX * -vLeft;
+	}
+
+	p4PointsOut[0] = vFowardShift + flClipSpaceBottomLeftX * vRightShift + flClipSpaceBottomLeftY * vUpShift;
+	p4PointsOut[1] = vFowardShift + flClipSpaceTopRightX * vRightShift + flClipSpaceBottomLeftY * vUpShift;
+	p4PointsOut[2] = vFowardShift + flClipSpaceBottomLeftX * vRightShift + flClipSpaceTopRightY * vUpShift;
+	p4PointsOut[3] = vFowardShift + flClipSpaceTopRightX * vRightShift + flClipSpaceTopRightY * vUpShift;
+}
+
+//-----------------------------------------------------------------------------
+// accessors for generated matrices
+//-----------------------------------------------------------------------------
+void ComputeViewMatrix( matrix3x4_t *pWorldToView, matrix3x4_t *pCameraToWorld, const Camera_t &camera )
+{
+	AngleMatrix( camera.m_angles, camera.m_origin, *pCameraToWorld );
+	matrix3x4_t tmp;
+	ConcatTransforms( *pCameraToWorld, g_ViewAlignMatrix, tmp );
+	MatrixInvert( tmp, *pWorldToView );
+}
+
+void ComputeViewMatrix( matrix3x4_t *pWorldToView, matrix3x4_t *pCameraToWorld, 
+						Vector const &vecOrigin,
+						Vector const &vecForward, Vector const &vecLeft, Vector const &vecUp )
+{
+	MatrixSetColumn( vecForward, FORWARD_AXIS, *pCameraToWorld );
+	MatrixSetColumn( vecLeft, LEFT_AXIS, *pCameraToWorld );
+	MatrixSetColumn( vecUp, UP_AXIS, *pCameraToWorld );
+	MatrixSetColumn( vecOrigin, ORIGIN, *pCameraToWorld );
+	matrix3x4_t tmp;
+	ConcatTransforms( *pCameraToWorld, g_ViewAlignMatrix, tmp );
+	MatrixInvert( tmp, *pWorldToView );
+}
+
+
+void ComputeViewMatrix( matrix3x4_t *pWorldToView, const Camera_t &camera )
+{
+	matrix3x4_t cameraToWorld;
+	ComputeViewMatrix( pWorldToView, &cameraToWorld, camera );
+}
+
+void ComputeViewMatrix( VMatrix *pWorldToView, const Camera_t &camera )
+{
+	matrix3x4_t transform, invTransform;
+	AngleMatrix( camera.m_angles, camera.m_origin, transform );
+
+	VMatrix matRotate( transform );
+
+#ifndef YUP_ACTIVE
+	VMatrix matRotateZ;
+	MatrixBuildRotationAboutAxis( matRotateZ, Vector(0,0,1), -90 );
+	MatrixMultiply( matRotate, matRotateZ, matRotate );
+
+	VMatrix matRotateX;
+	MatrixBuildRotationAboutAxis( matRotateX, Vector(1,0,0), 90 );
+	MatrixMultiply( matRotate, matRotateX, matRotate );
+	transform = matRotate.As3x4();
+#else
+	VMatrix matRotateUp;
+	MatrixBuildRotationAboutAxis( matRotateUp, Vector(0,1,0), -180 );
+	transform = matRotate.As3x4();
+#endif
+
+	MatrixInvert( transform, invTransform );
+		
+	pWorldToView->Init( invTransform );
+}
+
+void ComputeViewMatrix( VMatrix *pViewMatrix, const Vector &origin, const QAngle &angles )
+{
+	static VMatrix baseRotation;
+	static bool bDidInit;
+
+	if ( !bDidInit )
+	{
+		MatrixBuildRotationAboutAxis( baseRotation, Vector( 1, 0, 0 ), -90 );
+		MatrixRotate( baseRotation, Vector( 0, 0, 1 ), 90 );
+		bDidInit = true;
+	}
+
+	*pViewMatrix = baseRotation;
+	MatrixRotate( *pViewMatrix, Vector( 1, 0, 0 ), -angles[2] );
+	MatrixRotate( *pViewMatrix, Vector( 0, 1, 0 ), -angles[0] );
+	MatrixRotate( *pViewMatrix, Vector( 0, 0, 1 ), -angles[1] );
+
+	MatrixTranslate( *pViewMatrix, -origin );
+}
+
+void ComputeViewMatrix( VMatrix *pViewMatrix, const matrix3x4_t &matGameCustom )
+{
+	//translate game coordinates to rendering coordinates. Basically does the same as baseRotation in the other version of ComputeViewMatrix()
+	pViewMatrix->m[0][0] = -matGameCustom.m_flMatVal[1][0];
+	pViewMatrix->m[0][1] = -matGameCustom.m_flMatVal[1][1];
+	pViewMatrix->m[0][2] = -matGameCustom.m_flMatVal[1][2];
+	pViewMatrix->m[0][3] = -matGameCustom.m_flMatVal[1][3];
+
+	pViewMatrix->m[1][0] = matGameCustom.m_flMatVal[2][0];
+	pViewMatrix->m[1][1] = matGameCustom.m_flMatVal[2][1];
+	pViewMatrix->m[1][2] = matGameCustom.m_flMatVal[2][2];
+	pViewMatrix->m[1][3] = matGameCustom.m_flMatVal[2][3];
+
+	pViewMatrix->m[2][0] = -matGameCustom.m_flMatVal[0][0];
+	pViewMatrix->m[2][1] = -matGameCustom.m_flMatVal[0][1];
+	pViewMatrix->m[2][2] = -matGameCustom.m_flMatVal[0][2];
+	pViewMatrix->m[2][3] = -matGameCustom.m_flMatVal[0][3];
+
+	//standard 4th row
+	pViewMatrix->m[3][0] = pViewMatrix->m[3][1] = pViewMatrix->m[3][2] = 0.0f;
+	pViewMatrix->m[3][3] = 1.0f;
+}
+
+void ComputeProjectionMatrix( VMatrix *pCameraToProjection, const Camera_t &camera, int width, int height )
+{
+	float flApsectRatio = (float)width / (float)height;
+	ComputeProjectionMatrix( pCameraToProjection, camera.m_flZNear, camera.m_flZFar, camera.m_flFOVX, flApsectRatio );
+}
+
+void ComputeProjectionMatrix( VMatrix *pCameraToProjection, float flZNear, float flZFar, float flFOVX, float flAspectRatio )
+{
+	float halfWidth = tan( flFOVX * M_PI / 360.0 );
+	float halfHeight = halfWidth / flAspectRatio;
+	memset( pCameraToProjection, 0, sizeof( VMatrix ) );
+	pCameraToProjection->m[0][0]  = 1.0f / halfWidth;
+	pCameraToProjection->m[1][1]  = 1.0f / halfHeight;
+	pCameraToProjection->m[2][2] = flZFar / ( flZNear - flZFar );
+	pCameraToProjection->m[3][2] = -1.0f;
+	pCameraToProjection->m[2][3] = flZNear * flZFar / ( flZNear - flZFar );
+}
+
+void ComputeProjectionMatrix( VMatrix *pCameraToProjection, float flZNear, float flZFar, float flFOVX, float flAspectRatio, 
+							  float flClipSpaceBottomLeftX, float flClipSpaceBottomLeftY,
+							  float flClipSpaceTopRightX, float flClipSpaceTopRightY )
+{
+	Vector pNearPoints[ 4 ];
+	Vector vForward( 0, 0, 1 );
+	Vector vUp( 0, 1, 0 );
+	Vector vLeft( -1, 0, 0 );
+	CalcFarPlaneCameraRelativePoints( pNearPoints, vForward, vUp, vLeft, flZNear, 
+									  flFOVX, flAspectRatio,
+									  flClipSpaceBottomLeftX, flClipSpaceBottomLeftY,
+									  flClipSpaceTopRightX, flClipSpaceTopRightY );
+
+	float l = pNearPoints[ 0 ].x;
+	float r = pNearPoints[ 1 ].x;
+	float b = pNearPoints[ 0 ].y;
+	float t = pNearPoints[ 2 ].y;
+	float zn = flZNear;
+	float zf = flZFar;
+
+	float flWidth = r - l;
+	float flHeight = t - b;
+	float flReverseDepth = zn - zf;
+
+	memset( pCameraToProjection, 0, sizeof( VMatrix ) );
+
+	pCameraToProjection->m[0][0] = ( 2.0f * zn ) / flWidth;
+	pCameraToProjection->m[1][1] = ( 2.0f * zn ) / flHeight;
+	pCameraToProjection->m[2][2] = zf / flReverseDepth;
+	pCameraToProjection->m[0][2] = ( l + r ) / flWidth;
+	pCameraToProjection->m[1][2] = ( t + b ) / flHeight;
+	pCameraToProjection->m[2][3] = ( zn * zf ) / flReverseDepth;
+	pCameraToProjection->m[3][2] = -1.0f;
+	/*
+	// this is the matrix we're going for here
+	2*zn/(r-l)   0            0                0
+	0            2*zn/(t-b)   0                0
+	(l+r)/(r-l)  (t+b)/(t-b)  zf/(zn-zf)      -1
+	0            0            zn*zf/(zn-zf)    0
+	*/
+}
+
+//-----------------------------------------------------------------------------
+// Computes the screen space position given a screen size
+//-----------------------------------------------------------------------------
+void ComputeScreenSpacePosition( Vector2D *pScreenPosition, const Vector &vecWorldPosition, 
+								 const Camera_t &camera, int width, int height )
+{
+	VMatrix view, proj, viewproj;
+	ComputeViewMatrix( &view, camera );
+	ComputeProjectionMatrix( &proj, camera, width, height );
+	MatrixMultiply( proj, view, viewproj );
+
+	Vector vecScreenPos;
+	Vector3DMultiplyPositionProjective( viewproj, vecWorldPosition, vecScreenPos );
+
+	pScreenPosition->x = ( vecScreenPos.x + 1.0f ) * width / 2.0f;
+	pScreenPosition->y = ( -vecScreenPos.y + 1.0f ) * height / 2.0f;
+}
+
+VMatrix ViewMatrixRH( Vector &vEye, Vector &vAt, Vector &vUp )
+{
+	Vector xAxis, yAxis;
+	Vector zAxis = vEye - vAt;
+	xAxis = CrossProduct( vUp, zAxis );
+	yAxis = CrossProduct( zAxis, xAxis );
+	xAxis.NormalizeInPlace();
+	yAxis.NormalizeInPlace();
+	zAxis.NormalizeInPlace();
+	float flDotX = -DotProduct( xAxis, vEye );
+	float flDotY = -DotProduct( yAxis, vEye );
+	float flDotZ = -DotProduct( zAxis, vEye );
+
+	// YUP_ACTIVE: This is ok
+	VMatrix mRet( 
+		xAxis.x, yAxis.x, zAxis.x, 0,
+		xAxis.y, yAxis.y, zAxis.y, 0,
+		xAxis.z, yAxis.z, zAxis.z, 0,
+		flDotX, flDotY, flDotZ, 1 );
+	return mRet.Transpose();
+}
+
+
+// Given populated camera params, generate view and proj matrices.
+void MatricesFromCamera( VMatrix &mWorldToView, VMatrix &mProjection, const Camera_t &camera,
+							   float flClipSpaceBottomLeftX, float flClipSpaceBottomLeftY,
+							   float flClipSpaceTopRightX, float flClipSpaceTopRightY )
+{
+	matrix3x4_t cameraToWorld;	
+	ComputeViewMatrix( &mWorldToView.As3x4(), &cameraToWorld, camera );
+
+	if ( camera.IsOrthographic() )
+	{
+		mProjection = OrthoMatrixRH( camera.m_flWidth, camera.m_flHeight, camera.m_flZNear, camera.m_flZFar );
+	}
+	else
+	{
+		ComputeProjectionMatrix( &mProjection, camera.m_flZNear, camera.m_flZFar, camera.m_flFOVX, camera.m_flAspect, 
+			flClipSpaceBottomLeftX, flClipSpaceBottomLeftY, flClipSpaceTopRightX, flClipSpaceTopRightY );
+	}
+}
+
+// Generate frustum planes from viewproj matrix
+void FrustumFromViewProj( Frustum_t *pFrustum, const VMatrix &mViewProj, const Vector &origin, bool bD3DClippingRange )
+{
+	VPlane planes[FRUSTUM_NUMPLANES];
+	ExtractClipPlanesFromNonTransposedMatrix( mViewProj, planes, bD3DClippingRange );
+
+	// Subtract the origin.
+	for ( int i = 0; i < FRUSTUM_NUMPLANES; ++i )
+	{
+		planes[i].m_Dist = planes[i].m_Dist + DotProduct( planes[i].m_Normal, -origin );
+	}
+
+	pFrustum->SetPlanes( planes );	
+}
+
+// Generate frustum planes given view and proj matrices
+void FrustumFromMatrices( Frustum_t *pFrustum, const VMatrix &mWorldToView, const VMatrix &mProjection, const Vector &origin, bool bD3DClippingRange )
+{
+	VMatrix viewProj;
+	viewProj = ( mProjection * mWorldToView ); 
+
+	FrustumFromViewProj( pFrustum, viewProj, origin, bD3DClippingRange );
+}
+
+VMatrix ViewProjFromVectors( const Vector &origin, float flNear, float flFar, float flFOV, float flAspect,
+						 Vector const &vecForward, Vector const &vecLeft, Vector const &vecUp )
+{
+	
+	matrix3x4_t	mCameraToWorld;
+	matrix3x4_t	mWorldToView;
+	ComputeViewMatrix( &mWorldToView, &mCameraToWorld, origin, vecForward, vecLeft, vecUp );
+	VMatrix mProjection;
+	ComputeProjectionMatrix( &mProjection, flNear, flFar, flFOV, flAspect );
+	VMatrix mViewProj;
+	mViewProj = (mProjection * VMatrix(mWorldToView));
+	return mViewProj;
+}
+
+
+
+int CFrustum::CheckBoxAgainstNearAndFarPlanes( const VectorAligned &minBounds, const VectorAligned &maxBounds ) const
+{
+	// !!speed!! not super fast. change to use simd, inlining
+	float flNear = 0;
+	float flFar = 0;
+	AABB_t aabb;
+	aabb.m_vMinBounds = minBounds;
+	aabb.m_vMaxBounds = maxBounds;
+	Vector vZero( 0, 0, 0 );
+	GetNearAndFarPlanesAroundBox( &flNear, &flFar, aabb, vZero );
+	int nRet = 0;
+	if ( flNear <= m_camera.m_flZNear )
+	{
+		nRet |= BOXCHECK_FLAGS_OVERLAPS_NEAR;
+	}
+	if ( flFar >= m_camera.m_flZFar )
+	{
+		nRet |= BOXCHECK_FLAGS_OVERLAPS_FAR;
+	}
+	return nRet;
+
+}
+
+
+void CFrustum::GetNearAndFarPlanesAroundBox( float *pNear, float *pFar, AABB_t const &inBox, Vector &vOriginShift ) const
+{
+	AABB_t box = inBox;
+	box.m_vMinBounds -= vOriginShift;
+	box.m_vMaxBounds -= vOriginShift;
+
+	Vector vCorners[8];
+	vCorners[0] = box.m_vMinBounds;
+	vCorners[1] = Vector( box.m_vMinBounds.x, box.m_vMinBounds.y, box.m_vMaxBounds.z );
+	vCorners[2] = Vector( box.m_vMinBounds.x, box.m_vMaxBounds.y, box.m_vMinBounds.z );
+	vCorners[3] = Vector( box.m_vMinBounds.x, box.m_vMaxBounds.y, box.m_vMaxBounds.z );
+
+	vCorners[4] = Vector( box.m_vMaxBounds.x, box.m_vMinBounds.y, box.m_vMinBounds.z );
+	vCorners[5] = Vector( box.m_vMaxBounds.x, box.m_vMinBounds.y, box.m_vMaxBounds.z );
+	vCorners[6] = Vector( box.m_vMaxBounds.x, box.m_vMaxBounds.y, box.m_vMinBounds.z );
+	vCorners[7] = box.m_vMaxBounds;
+
+	float flNear = FLT_MAX;//m_camera.m_flZNear;
+	float flFar = -FLT_MAX;//m_camera.m_flZFar;
+	for ( int i=0; i<8; ++i )
+	{
+		Vector vDelta = vCorners[i] - m_camera.m_origin;
+		float flDist = DotProduct( m_forward, vDelta );
+		flNear = MIN( flNear, flDist );
+		flFar = MAX( flFar, flDist );
+	}
+
+	*pNear = flNear;
+	*pFar = flFar;
+}
+
+
+void CFrustum::UpdateFrustumFromCamera()
+{
+	if ( !m_bDirty )
+		return;
+
+	ComputeViewMatrix( &m_worldToView, &m_cameraToWorld, m_camera );
+
+	if ( m_camera.IsOrthographic() )
+	{
+		MatrixBuildOrtho( m_projection,	
+				m_camera.m_flWidth  * m_flClipSpaceBottomLeftX * 0.5f,
+				m_camera.m_flHeight * m_flClipSpaceTopRightY * 0.5f,
+				m_camera.m_flWidth  * m_flClipSpaceTopRightX   * 0.5f,
+				m_camera.m_flHeight * m_flClipSpaceBottomLeftY   * 0.5f, 
+				m_camera.m_flZNear, m_camera.m_flZFar );
+	}
+	else
+	{
+		// Determine the extents
+		ComputeProjectionMatrix( &m_projection, m_camera.m_flZNear, m_camera.m_flZFar, m_camera.m_flFOVX, m_camera.m_flAspect, 
+								 m_flClipSpaceBottomLeftX, m_flClipSpaceBottomLeftY, m_flClipSpaceTopRightX, m_flClipSpaceTopRightY );	
+	}
+
+	CalcViewProj();
+	ExtractDirectionVectors( &m_forward, &m_left, &m_up, m_cameraToWorld );
+
+	FrustumFromViewProj( &m_frustumStruct, m_viewProj, m_camera.m_origin, true );
+
+	m_bDirty = false;
+}
+
+
+void CFrustum::BuildFrustumFromVectors( const Vector &origin, float flNear, float flFar, float flFOV, float flAspect,
+									   Vector const &vecForward, Vector const &vecLeft, Vector const &vecUp )
+{
+	InitCamera( origin, QAngle( 0, 0, 0 ), flNear, flFar, flFOV, flAspect );
+	ComputeViewMatrix( &m_worldToView, &m_cameraToWorld, origin, vecForward, vecLeft, vecUp );
+	ComputeProjectionMatrix( &m_projection, flNear, flFar, flFOV, flAspect );
+	m_viewProj = (m_projection * VMatrix(m_worldToView));
+	ExtractDirectionVectors( &m_forward, &m_left, &m_up, m_cameraToWorld );
+
+	m_frustumStruct.CreatePerspectiveFrustum( vec3_origin, m_forward, -m_left, m_up, 
+		flNear, flFar, flFOV, flAspect );
+
+	MatrixInverseGeneral( m_viewProj, m_invViewProj );
+	MatrixInverseGeneral( m_projection, m_invProjection );
+
+	VMatrix worldToView( m_worldToView );
+
+	VMatrix viewToWorld;
+	worldToView.InverseGeneral( viewToWorld );
+	m_cameraToWorld = viewToWorld.As3x4();
+
+	viewToWorld.GetTranslation( m_camera.m_origin );
+
+}
+
+
+/// Given only the world->view and an ortho view->proj matrices, this helper method computes
+/// the implied frustum values needed for orthographic shadow buffer rendering (but
+/// should work with perspective projections too).  This is slow and general, but
+/// it should guarantee a frustum in a consistent/sane state given any world->view and
+/// view->proj matrices.
+void CFrustum::BuildShadowFrustum( VMatrix &newWorldToView, VMatrix &newProj )
+{
+	SetView( newWorldToView );
+	SetProj( newProj );
+	CalcViewProj();
+
+	VMatrix &viewToProj = m_projection;
+	Assert( ( viewToProj.m[3][0] == 0.0f ) && ( viewToProj.m[3][1] == 0.0f )  && ( viewToProj.m[3][2] == 0.0f ) && ( viewToProj[3][3] == 1.0f ) );
+
+	VMatrix worldToView( m_worldToView );
+
+	VMatrix worldToCamera;
+	MatrixMultiply( g_matViewToCameraMatrix, worldToView, worldToCamera );
+	VMatrix cameraToWorld;
+	MatrixInverseGeneral( worldToCamera, cameraToWorld );
+	m_cameraToWorld = cameraToWorld.As3x4();
+
+	// Compute camera location in world space.
+
+	VMatrix viewToWorld;
+	MatrixInverseGeneral( worldToView, viewToWorld );
+	viewToWorld.GetTranslation( m_camera.m_origin );
+
+	cameraToWorld.GetTranslation( m_camera.m_origin );
+
+	MatrixToAngles( cameraToWorld, m_camera.m_angles );
+
+	// forward/left/up - world relative coordinates, assuming an FPS camera sitting on an XY plane, Z is up
+	MatrixGetRow( worldToCamera, FORWARD_AXIS, &m_forward );
+	MatrixGetRow( worldToCamera, LEFT_AXIS, &m_left );
+	MatrixGetRow( worldToCamera, UP_AXIS, &m_up );
+
+	// Compute near/far planes, assuming D3D-style clipping range of [0,1]
+	VMatrix projToView;
+	viewToProj.InverseGeneral( projToView );
+
+	Vector vNearPoint, vFarPoint; 
+	projToView.V3Mul( Vector( 0.0f, 0.0f, 0.0f ), vNearPoint );
+	projToView.V3Mul( Vector( 0.0f, 0.0f, 1.0f ), vFarPoint );
+	m_camera.m_flZNear = fabs( vNearPoint.z );
+	m_camera.m_flZFar = fabs( vFarPoint.z );
+	m_camera.m_flAspect = 1.0f;
+	m_camera.m_flFOVX = -1.0f;
+
+	Vector vCornerPoints[2];
+	
+	// Y's are negated here because MatrixBuildOrtho() flips top/bottom!
+	projToView.V3Mul( Vector( -1.0f,  1.0f, 0.0f ), vCornerPoints[0] );	// left/bottom
+	projToView.V3Mul( Vector(  1.0f, -1.0f, 0.0f ), vCornerPoints[1] );	// right/top
+	
+	m_flClipSpaceBottomLeftX = vCornerPoints[0].x; 
+	m_flClipSpaceBottomLeftY = vCornerPoints[0].y;
+	m_flClipSpaceTopRightX = vCornerPoints[1].x;
+	m_flClipSpaceTopRightY = vCornerPoints[1].y;
+	
+	m_camera.m_flWidth = 2.0f;
+	m_camera.m_flHeight = 2.0f;
+
+	// Now compute the frustum planes used for culling purposes. These planes are computed assuming the camera is already at the origin.
+	VMatrix worldToCamLocalWorld;
+	// This is confusing - vShadowCamPos is not negated here, because we need to compensate for the fact that the 
+	// frustum culling code makes the cam pos the origin before culling by subtracting the camera's origin - so undo it.
+	// Calc a viewproj matrix that has no g_ViewAlignMatrix matrix in it.
+	MatrixBuildTranslation( worldToCamLocalWorld, m_camera.m_origin.x, m_camera.m_origin.y, m_camera.m_origin.z );
+	VMatrix worldToCamLocalWorldToView( worldToView * worldToCamLocalWorld );
+	VMatrix shadowCamLocalWorldToViewProj( viewToProj * worldToCamLocalWorldToView );
+		
+	VPlane pSixPlanes[FRUSTUM_NUMPLANES];
+	
+#if 0
+	Vector actualOriginProjSpace( 0.0f, 0.0f, .5f );
+	Vector actualOriginWorldSpace;
+	VMatrix projToWorld;
+	m_viewProj.InverseGeneral( projToWorld );
+	projToWorld.V3Mul( actualOriginProjSpace, actualOriginWorldSpace );
+	
+	ExtractClipPlanesFromNonTransposedMatrix( m_viewProj, pSixPlanes, true );
+	// Testing
+	float flDots[6];
+	for (uint i = 0; i < 6; i++)
+	{
+		flDots[i] = pSixPlanes[i].DistTo( actualOriginWorldSpace );
+	}
+#endif
+
+	ExtractClipPlanesFromNonTransposedMatrix( shadowCamLocalWorldToViewProj, pSixPlanes, true );
+
+	m_frustumStruct.SetPlanes( pSixPlanes );
+
+	MatrixInverseGeneral( m_viewProj, m_invViewProj );
+	MatrixInverseGeneral( m_projection, m_invProjection );
+
+	m_bDirty = false;
+
+	// This should be a no-op (ignoring FP precision) if all the above stuff was done right.
+	//m_bDirty = true;
+	//UpdateFrustumFromCamera();
+}
+
+void CFrustum::CalcFarPlaneCameraRelativePoints( Vector *p4PointsOut, float flFarPlane, 
+												 float flClipSpaceBottomLeftX, float flClipSpaceBottomLeftY,
+												 float flClipSpaceTopRightX, float flClipSpaceTopRightY ) const
+{
+	Vector vForward = CameraForward();
+	Vector vUp = CameraUp();
+	Vector vLeft = CameraLeft();
+
+	float flFovX = GetCameraFOV();
+	::CalcFarPlaneCameraRelativePoints( p4PointsOut, vForward, vUp, vLeft, flFarPlane, 
+										flFovX, GetCameraAspect(),
+										flClipSpaceBottomLeftX, flClipSpaceBottomLeftY,
+										flClipSpaceTopRightX, flClipSpaceTopRightY );
+}
+
+
+// generates 8 vertices of the frustum
+void Camera_t::ComputeGeometry( Vector *pVertsOut8, const Vector &vForward, const Vector &vLeft, const Vector &vUp ) const
+{
+	Vector vNearLeft, vFarLeft;
+	Vector vNearUp, vFarUp;
+	Vector vNear = m_origin + m_flZNear * vForward;
+	Vector vFar = m_origin + m_flZFar * vForward;
+
+	if ( IsOrthographic() )
+	{
+		vNearLeft = vLeft * m_flWidth;
+		vNearUp = vUp * m_flHeight;
+		vFarLeft = vNearLeft;
+		vFarUp = vNearUp;
+	}
+	else
+	{
+		float flTanX = tan( DEG2RAD(m_flFOVX) * 0.5f );
+		float flooAspect = 1.0f / m_flAspect;
+		float flWidth = m_flZNear * flTanX;
+		float flHeight = flWidth * flooAspect;
+
+		vNearLeft = vLeft * flWidth;
+		vNearUp = vUp * flHeight;
+
+		float flFarWidth = m_flZFar * flTanX;
+		float flFarHeight = flFarWidth * flooAspect;
+		vFarLeft = vLeft * flFarWidth;
+		vFarUp = vUp * flFarHeight;
+	}
+
+	pVertsOut8[0] = vNear + vNearLeft - vNearUp;
+	pVertsOut8[1] = vNear - vNearLeft - vNearUp;
+	pVertsOut8[2] = vNear + vNearLeft + vNearUp;
+	pVertsOut8[3] = vNear - vNearLeft + vNearUp;
+
+	pVertsOut8[4] = vFar + vFarLeft - vFarUp;
+	pVertsOut8[5] = vFar - vFarLeft - vFarUp;
+	pVertsOut8[6] = vFar + vFarLeft + vFarUp;
+	pVertsOut8[7] = vFar - vFarLeft + vFarUp;
+}
+
+void Camera_t::ComputeGeometry( Vector *pVertsOut8 ) const
+{
+	Vector vForward, vLeft, vUp;
+	AngleVectorsFLU( m_angles, &vForward, &vLeft, &vUp );
+	ComputeGeometry( pVertsOut8, vForward, vLeft, vUp );
+}
+
+// generates 8 vertices of the frustum as bounds
+void CFrustum::ComputeBounds( Vector *pMins, Vector *pMaxs ) const
+{
+	ClearBounds( *pMins, *pMaxs );
+	Vector vPts[8];
+	m_camera.ComputeGeometry( vPts, m_forward, m_left, m_up );
+
+	for ( int i = 0; i < 8; i++ )
+	{
+		AddPointToBounds( vPts[i], *pMins, *pMaxs );
+	}
+}
+
+static inline void InvertVMatrix( const VMatrix &src, VMatrix &dst )
+{
+	src.InverseGeneral( dst );
+}
+
+void CFrustum::CalcViewProj() 
+{ 
+	m_viewProj = ( m_projection * VMatrix( m_worldToView ) ); 
+	InvertVMatrix( m_viewProj, m_invViewProj );
+	InvertVMatrix( m_projection, m_invProjection );
+}
+
+float CFrustum::ComputeScreenSize( Vector vecOrigin, float flRadius ) const
+{
+	vecOrigin -= GetCameraPosition();
+	float flDist = vecOrigin.Length();
+	if ( flDist < flRadius )
+	{
+		return 1.0;											// eye inside sphere
+	}
+	float flSin = sin( DEG2RAD( MIN( GetCameraFOV(), 90.0 ) ) );
+	return MIN( 1.0, flSin * ( flRadius / flDist ) );
+	
+}
+
+void CFrustum::ViewToWorld( const Vector2D &vViewMinusOneToOne, Vector *pOutWorld )
+{		
+	Vector vView3D;
+	vView3D.x = vViewMinusOneToOne.x;
+	vView3D.y = vViewMinusOneToOne.y;
+	vView3D.z = 0;
+
+	const VMatrix &invViewProjMatrix = GetInvViewProj();
+	Vector3DMultiplyPositionProjective( invViewProjMatrix, vView3D, *pOutWorld );	
+}
+
+void CFrustum::BuildRay( const Vector2D &vViewMinusOneToOne, Vector *pOutRayStart, Vector *pOutRayDirection )
+{
+	Vector vClickPoint;
+	ViewToWorld( vViewMinusOneToOne, &vClickPoint );
+
+	if ( !IsOrthographic() )
+	{
+		Camera_t camera = GetCameraStruct();
+
+		Vector vRay = vClickPoint - camera.m_origin;
+		VectorNormalize( vRay );
+
+		*pOutRayStart = camera.m_origin;
+		*pOutRayDirection = vRay;
+	}
+	else
+	{
+		*pOutRayStart = vClickPoint;
+		ViewForward( *pOutRayDirection );
+	}
+}
+
+void CFrustum::BuildFrustumFromParameters(
+	const Vector &origin, const QAngle &angles, 
+	float flNear, float flFar, float flFOV, float flAspect,
+	const VMatrix &worldToView, const VMatrix &viewToProj )
+{
+	InitCamera( origin, angles, flNear, flFar, flFOV, flAspect );
+
+	m_worldToView = worldToView.As3x4();
+
+	VMatrix worldToCamera;
+	MatrixMultiply( g_matViewToCameraMatrix, worldToView, worldToCamera );
+	VMatrix cameraToWorld;
+	MatrixInverseGeneral( worldToCamera, cameraToWorld );
+	m_cameraToWorld = cameraToWorld.As3x4();
+	
+	m_projection = viewToProj;
+	
+	CalcViewProj();
+						
+	// forward/left/up - world relative coordinates, assuming an FPS camera sitting on an XY plane, Z is up
+	MatrixGetRow( worldToCamera, FORWARD_AXIS, &m_forward );
+	MatrixGetRow( worldToCamera, LEFT_AXIS, &m_left );
+	MatrixGetRow( worldToCamera, UP_AXIS, &m_up );
+
+	// Now compute the frustum planes used for culling purposes. These planes are computed assuming the camera is already at the origin.
+	VMatrix worldToCamLocalWorld;
+	// This is confusing - vShadowCamPos is not negated here, because we need to compensate for the fact that the 
+	// frustum culling code makes the cam pos the origin before culling by subtracting the camera's origin - so undo it.
+	MatrixBuildTranslation( worldToCamLocalWorld, m_camera.m_origin.x, m_camera.m_origin.y, m_camera.m_origin.z );
+		
+	VMatrix worldToCamLocalWorldToView( worldToView * worldToCamLocalWorld );
+	VMatrix shadowCamLocalWorldToViewProj( viewToProj * worldToCamLocalWorldToView );
+	VPlane pSixPlanes[FRUSTUM_NUMPLANES];
+	ExtractClipPlanesFromNonTransposedMatrix( shadowCamLocalWorldToViewProj, pSixPlanes, true );
+
+	m_frustumStruct.SetPlanes( pSixPlanes );
+	
+	m_bDirty = false;
+
+	// This should be a no-op (ignoring FP precision) if all the above stuff was done right.
+	//m_bDirty = true;
+	//UpdateFrustumFromCamera();
+}

mathlib/capsule.cpp

@@ -0,0 +1,259 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+#include "capsule.h"
+#include "trace.h"
+//#include "body.h"
+//#include "mass.h"
+
+//#include "distance.h"
+//#include "gjk.h"
+//#include "sat.h"
+
+#define NUM_STACKS 8
+#define NUM_SLICES 16
+
+
+//--------------------------------------------------------------------------------------------------
+// Local utilities 
+//--------------------------------------------------------------------------------------------------
+struct CapsuleCast2D_t
+{
+	float m_flCapsule, m_flRay;
+};
+
+
+//--------------------------------------------------------------------------------------------------
+static void CastCapsuleRay2DCoaxialInternal( CapsuleCast2D_t &out, float mx, float dx, float h, float e )
+{
+	Assert( e >= 0 );
+	float mxProj = mx + e; // m.x - (-e)
+	if( mxProj < 0 )
+	{
+		// ray starts before the capsule cap
+		out.m_flCapsule = 0;
+		if( dx >= -mxProj ) // otherwise, ending before capsule starts: FLT_MAX
+		{
+			out.m_flRay = -mxProj / dx;
+		}
+	}
+	else if( mx < h + e ) // otherwise, starting after capsule ends : FLT_MAX
+	{
+		out.m_flCapsule = Clamp( mx, 0.0f, h );
+		out.m_flRay = 0;
+	}
+	else
+	{
+		// ray starts after the capsule cap
+		out.m_flCapsule = h;
+		float mxEnd = mx - ( h + e );
+		if( -dx >= mxEnd )
+		{
+			out.m_flRay = mxEnd / -dx;
+		}
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+static void CastCapsuleRay2DParallelInternal( CapsuleCast2D_t &out, const Vector2D &m, float dx, float h, float rr )
+{
+	float e2 = rr - Sqr( m.y );
+	if( e2 > 0 ) // otherwise, going parallel and outside : FLT_MAX
+	{
+		// going parallel and inside the infinite slab at level m.y, left to right
+		float e = sqrtf( e2 ); // -e..h+e is the extent 
+		CastCapsuleRay2DCoaxialInternal( out, m.x, dx, h, e );
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+// Intersect 2D ray with 2D capsule; capsule has radius r, length h, it starts at (0,0) and ends at (h,0)
+// ray goes from m, delta d
+// return: time of hit
+static void CastCapsuleRay2DInternal( CapsuleCast2D_t &out, const Vector2D &m, const Vector2D &d, float h, float rr )
+{
+	Assert( rr >= 0 );
+	Assert( d.y > -FLT_EPSILON );
+	Assert( d.y != 0.0f ); // otherwise it's going parallel
+	float my2 = Sqr( m.y );
+	out.m_flCapsule = Clamp( m.x, 0.0f, h );
+
+	// Easy case we'll have to check a few times if we delay: are we starting in solid?
+	// same idea as with box-box distance: cut out x=0..h, capsule becomes a circle, find distance to circle
+	// I'm sure there's more elegant way to handle it
+	if( Sqr( m.x - out.m_flCapsule ) + my2 < rr )
+	{
+		out.m_flRay = 0; // start-in-solid
+		return;
+	}
+	// well, we don't start inside the capsule. Good to know
+	float r = sqrtf( rr ), dd = Sqr( d.x ) + Sqr( d.y ), ddInv = 1.0f / dd, dymy = d.y * m.y;
+
+	// first, intersect the ray with the rectangle
+
+	float t = ( -r - m.y ) / d.y, t0 = fpmax( 0, t ), s0 = m.x + d.x * t0;
+
+	// solutions: -b0±sqrt(b0^2-c0) , -b1±sqrt(b1^2-c1) with	± controlled by d.x sign
+	// since we know we go left-bottom to right-top, we can just choose the circle we wanna hit
+	// since we know we don't start-in-solid, we know the first root (if any) will be t>=0
+	float mxh;
+	if( s0 < 0 )
+	{
+		// we're entering through the left cap
+		// if we hit, we hit left circle
+		out.m_flCapsule = 0;
+		mxh = m.x;
+	}
+	else if( s0 < h )
+	{
+		// we're entering through the side of the capsule
+		out.m_flCapsule = s0;
+		if( t >= 0 ) // only if we didn't enter before ray started; otherwise, since we didn't start-in-solid, we don't hit capsule at all
+		{
+			out.m_flRay = t; // the caller will sort out if it's >1 or not
+		}
+		return;
+	}
+	else
+	{
+		out.m_flCapsule = h;
+		mxh = m.x - h;
+	}
+
+	float b = ( d.x * mxh + dymy ) * ddInv, c = ( mxh * mxh + my2 - rr ) * ddInv, D = b * b - c;
+	if( D >= 0 )
+	{
+		float tc = -b - sqrtf( D );
+		Assert( tc - t >= -1e-4f );	// the ray should really enter the circle after it entered the stripe of halfspaces 
+		// if tc < 0, we entered capsule before ray began; since we didn't start-in-solid, it means we don't hit the capsule at all
+		if( tc >= 0 )
+		{
+			out.m_flRay = tc;
+		}
+	}
+}
+
+
+
+static void CastCapsuleShortRay( CShapeCastResult &out, const Vector &sUnit, float sLen, const Vector &m, const Vector &vRayStart, const Vector vCenter[], float flRadius )
+{
+	// the ray is too short, just compute the distance to the capsule and compare with radius
+	// if we really need both high precision and stability, we need to compute distance to capsule from both ends of the ray: the capsule curvature is very low in the vicinity of the ray and is o(d^2) effect
+	float flProjOnCapsule = DotProduct( sUnit, m );
+	Vector vDistance;
+	if( flProjOnCapsule < 0 )
+	{
+		vDistance = m;
+	}
+	else if( flProjOnCapsule > sLen )
+	{
+		vDistance = vRayStart - vCenter[ 1 ];
+		
+	}
+	else
+	{
+		vDistance = m - vCenter[ 0 ] * flProjOnCapsule ; 
+	}
+	
+	float flDistFromCapsuleSqr = vDistance.LengthSqr();
+	
+	if( flDistFromCapsuleSqr > flRadius )
+	{
+		// the ray is outside of the capsule
+		out.m_bStartInSolid = false;
+		out.m_flHitTime = 1.0f;
+	}
+	else
+	{
+		out.m_bStartInSolid = true;
+		out.m_flHitTime = 0;
+		out.m_vHitNormal = flDistFromCapsuleSqr > 1e-8f ? vDistance / sqrtf( flDistFromCapsuleSqr ) : VectorPerpendicularToVector( sUnit );
+		out.m_vHitPoint = vRayStart;
+	}
+}
+
+
+
+void CastSphereRay( CShapeCastResult& out, const Vector &m, const Vector& p, const Vector& d, float flRadius );
+
+//--------------------------------------------------------------------------------------------------
+void CastCapsuleRay( CShapeCastResult& out, const Vector& vRayStart, const Vector& vRayDelta, const Vector vCenter[], float flRadius ) 
+{
+	Vector m = vRayStart - vCenter[0], s = vCenter[1] - vCenter[0];
+	float sLen = s.Length();
+	
+	if( flRadius < 1e-5f )
+	{
+		return;
+	}
+
+	if( sLen < 1e-3f ) // note: we should filter out 0-length capsules somewhere outside of this function
+	{
+		CastSphereRay( out, m, vRayStart, vRayDelta, flRadius );
+		return;
+	}
+	Vector sUnit = s / sLen;
+	float dLen = vRayDelta.Length();
+	if( dLen > 1e-4f )
+	{
+		Vector dUnit = vRayDelta / dLen;
+		Vector z = CrossProduct( sUnit, dUnit );
+		float zLenSqr = z.LengthSqr();
+		float dsUnit = DotProduct( vRayDelta, sUnit );
+
+		CapsuleCast2D_t cast;
+		cast.m_flRay = FLT_MAX;
+
+		if( zLenSqr > 256*256 * FLT_EPSILON * FLT_EPSILON ) // the tolerance here is found experimentally, with the target of achieving minimal orthogonality of 1e-3 between z^s and z^d
+		{
+			float zLen = sqrtf( zLenSqr );
+			Vector zUnit = z / zLen;
+	#ifdef _DEBUG
+			// z must be orthogonal to capsule and ray (it's a cross product of the two); if it's not, we need to handle this case as parallel
+			float flOrthogonality[2] = { DotProduct( zUnit, s ), DotProduct( zUnit, vRayDelta ) };
+			Assert( fabsf( flOrthogonality[0] ) < 1e-3f * MAX( 1, MAX( zLen, sLen ) ) && fabsf( flOrthogonality[1] ) < 1e-3f * MAX( 1, MAX( zLen, dLen ) ) );
+	#endif
+			float mzUnit = DotProduct( m, zUnit ), rr = Sqr( flRadius ) - Sqr( mzUnit );
+			if( rr <= 0 )
+			{
+				out.m_flHitTime = FLT_MAX;
+				return;
+			}
+			else
+			{
+				Vector yUnit = CrossProduct( zUnit, sUnit );
+				Vector2D mProj( DotProduct( m, sUnit ), DotProduct( m, yUnit ) );
+				float dyUnit = DotProduct( vRayDelta, yUnit );
+				CastCapsuleRay2DInternal( cast, mProj, Vector2D( dsUnit, dyUnit ), sLen, rr );
+			}
+		}
+		else
+		{
+			// they're parallel..
+			float msUnit = DotProduct( m, sUnit );
+			Vector zAlt = m - sUnit * msUnit;
+			float zAltLenSqr = zAlt.LengthSqr();
+			if( zAltLenSqr < FLT_EPSILON * FLT_EPSILON )
+			{
+				// ray and capsule are coaxial...
+				CastCapsuleRay2DCoaxialInternal( cast, msUnit, dsUnit, sLen, flRadius ); // note: we're passing radius!
+			}
+			else
+			{
+				// ray and capsule are parallel
+				Vector zUnit = zAlt / sqrtf( zAltLenSqr ), yUnit = CrossProduct( zUnit, sUnit );
+				CastCapsuleRay2DParallelInternal( cast, Vector2D( DotProduct( m, sUnit ), DotProduct( m, yUnit ) ), dsUnit, sLen, Sqr( flRadius ) - zAltLenSqr ); // r^2 may be negative here - it'll just return no hit
+			}
+		}
+
+		Assert( cast.m_flRay >= 0 );
+		out.m_flHitTime = cast.m_flRay;
+		out.m_vHitPoint = vRayStart + vRayDelta * cast.m_flRay;
+		out.m_vHitNormal = ( out.m_vHitPoint - ( vCenter[0] + sUnit * cast.m_flCapsule ) ).Normalized();
+	}
+	else
+	{
+		CastCapsuleShortRay( out, sUnit, sLen, m, vRayStart, vCenter, flRadius );
+	}
+}
+

mathlib/cholesky.cpp

@@ -0,0 +1,143 @@
+//========= Copyright c 1996-2009, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: Cholesky and LDL' decomposition-related code
+//
+//=====================================================================================//
+#include "mathlib/cholesky.h"
+#include "mathlib/ssecholesky.h"
+
+inline float ClampNeg( float a )
+{
+	return a >= 0.0f ? a : 0.0f ; //( a + fabsf(a) ) * 0.5f;
+}
+
+inline float SafeSqrt( float a )
+{
+	return sqrtf( ClampNeg( a ) );
+}
+
+inline float SafeRecip( float a )
+{
+	return 1.0f / ( a > 1e-8f ? a : 1e-8f );
+}
+
+bool Cholesky3x3_t::IsValid( )
+{
+	return m_inv00 + m_inv11 + m_inv22 < 1e+7f;
+}
+
+const fltx4 Four_1e7 = { 1.0e+7, 1.0e+7, 1.0e+7, 1.0e+7 };
+
+bool SimdCholesky3x3_t::IsValid( )const 
+{
+	return IsAllGreaterThan( Four_1e7, m_inv00 + m_inv11 + m_inv22 );
+}
+
+fltx4 SimdCholesky3x3_t::GetValidMask( )const
+{
+	return CmpGtSIMD( Four_1e7, m_inv00 + m_inv11 + m_inv22 );
+}
+
+
+// initializes this decomposition; see formula at http://planetmath.org/encyclopedia/CholeskyDecomposition.html
+bool Cholesky3x3_t::Init( float a00, float a10, float a11, float a20, float a21, float a22 )
+{
+#ifdef _DEBUG
+	memset( this, 0xCD, sizeof(*this) ); // to see what's changing easily
+#endif
+	m_00 = SafeSqrt( a00 ); m_inv00 = SafeRecip( m_00 );
+	m_10 = ( a10 ) * m_inv00;
+	m_11 = SafeSqrt( a11 - Sqr( m_10 ) ); m_inv11 = SafeRecip( m_11 );
+	m_20 = ( a20 ) * m_inv00;
+	m_21 = ( a21 - m_20 * m_10) * m_inv11;
+	m_22 = SafeSqrt( a22 - Sqr( m_20 ) - Sqr( m_21 ) ); m_inv22 = SafeRecip( m_22 );
+
+#ifdef _DEBUG
+	if( IsValid() )
+	{
+		matrix3x4_t l,r, a;
+		FillLeft(l);
+		FillRight(r);
+		MatrixMultiply(l, r, a);
+		float flError = Sqr( a00 - a[0][0] ) + Sqr( a10 - a[1][0] ) + Sqr( a20 - a[2][0] ) + 
+			Sqr( a11 - a[1][1] ) + Sqr( a21 - a[2][1] ) + Sqr( a22 - a[2][2] );
+		Assert( flError < 1e-5f );
+	}
+#endif
+
+	return IsValid();
+}
+
+
+void Cholesky3x3_t::FillLeft( matrix3x4_t & l )
+{
+	l[0][0] = m_00;
+	l[0][1] = l[0][2] = l[0][3] = 0;
+	l[1][0] = m_10; l[1][1] = m_11;
+	l[1][2] = l[1][3] = 0;
+	l[2][0] = m_20;	l[2][1] = m_21; l[2][2] = m_22;
+	l[2][3] = 0;
+}
+
+void Cholesky3x3_t::FillRight( matrix3x4_t & r )
+{
+	r[0][0] = m_00;
+	r[1][0] = r[2][0] = 0;
+	r[0][1] = m_10; r[1][1] = m_11;
+	r[2][1] = 0;
+	r[0][2] = m_20;	r[1][2] = m_21; r[2][2] = m_22;
+	r[0][3] = r[1][3] = r[2][3] = 0;
+}
+
+
+// solve this : L x = b
+const Vector Cholesky3x3_t::SolveLeft( const Vector &b )
+{
+	Vector result;
+	result.x = m_inv00 * b.x;
+	result.y = m_inv11 * ( b.y - m_10 * result.x );
+	result.z = m_inv22 * ( b.z - m_20 * result.x - m_21 * result.y );
+	return result;
+}
+
+const Vector Cholesky3x3_t::SolveRight( const Vector &b )
+{
+	Vector result;
+	result.z = m_inv22 * b.z;
+	result.y = m_inv11 * ( b.y - m_21 * result.z );
+	result.x = m_inv00 * ( b.x - m_20 * result.z - m_10 * result.y );
+	return result;
+}
+
+
+
+// initializes this decomposition; see formula at http://planetmath.org/encyclopedia/CholeskyDecomposition.html
+void SimdCholesky3x3_t::Init( const fltx4 & a00, const fltx4 & a10, const fltx4 & a11, const fltx4 & a20, const fltx4 & a21, const fltx4 & a22 )
+{
+	m_inv00 = ReciprocalSqrtSIMD( a00 );
+	m_10 = ( a10 ) * m_inv00;
+	m_inv11 = ReciprocalSqrtSIMD( a11 - ( m_10 * m_10 ) );
+	m_20 = ( a20 ) * m_inv00;
+	m_21 = ( a21 - m_20 * m_10 ) * m_inv11;
+	m_inv22 = ReciprocalSqrtSIMD( a22 - ( m_20 * m_20 ) - ( m_21 * m_21 ) );
+}
+
+
+// solve this : L x = b
+const FourVectors SimdCholesky3x3_t::SolveLeft( const FourVectors &b )
+{
+	FourVectors result;
+	result.x = m_inv00 * b.x;
+	result.y = m_inv11 * ( b.y - m_10 * result.x );
+	result.z = m_inv22 * ( b.z - m_20 * result.x - m_21 * result.y );
+	return result;
+}
+
+const FourVectors SimdCholesky3x3_t::SolveRight( const FourVectors &b )
+{
+	FourVectors result;
+	result.z = m_inv22 * b.z;
+	result.y = m_inv11 * ( b.y - m_21 * result.z );
+	result.x = m_inv00 * ( b.x - m_20 * result.z - m_10 * result.y );
+	return result;
+}

mathlib/color_conversion.cpp

@@ -0,0 +1,639 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: Color conversion routines.
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#ifndef _PS3
+#include <memory.h>
+#endif
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+//-----------------------------------------------------------------------------
+// Gamma conversion support
+//-----------------------------------------------------------------------------
+static byte		texgammatable[256];	// palette is sent through this to convert to screen gamma
+
+static float	texturetolinear[256];	// texture (0..255) to linear (0..1)
+static int		lineartotexture[1024];	// linear (0..1) to texture (0..255)
+static int		lineartoscreen[1024];	// linear (0..1) to gamma corrected vertex light (0..255)
+
+// build a lightmap texture to combine with surface texture, adjust for src*dst+dst*src, ramp reprogramming, etc
+float			lineartovertex[4096];	// linear (0..4) to screen corrected vertex space (0..1?)
+unsigned char	lineartolightmap[4096];	// linear (0..4) to screen corrected texture value (0..255)
+
+static float	g_Mathlib_GammaToLinear[256];	// gamma (0..1) to linear (0..1)
+static float	g_Mathlib_LinearToGamma[256];	// linear (0..1) to gamma (0..1)
+
+// This is aligned to 16-byte boundaries so that we can load it
+// onto SIMD registers easily if needed (used by SSE version of lightmaps)
+// TODO: move this into the one DLL that actually uses it, instead of statically
+// linking it everywhere via mathlib.
+ALIGN128 float	power2_n[256] = 			// 2**(index - 128) / 255
+{ 
+	1.152445441982634800E-041, 2.304890883965269600E-041, 4.609781767930539200E-041, 9.219563535861078400E-041, 
+	1.843912707172215700E-040, 3.687825414344431300E-040, 7.375650828688862700E-040, 1.475130165737772500E-039,
+	2.950260331475545100E-039, 5.900520662951090200E-039, 1.180104132590218000E-038, 2.360208265180436100E-038, 
+	4.720416530360872100E-038, 9.440833060721744200E-038, 1.888166612144348800E-037, 3.776333224288697700E-037, 
+	7.552666448577395400E-037, 1.510533289715479100E-036, 3.021066579430958200E-036, 6.042133158861916300E-036, 
+	1.208426631772383300E-035, 2.416853263544766500E-035, 4.833706527089533100E-035, 9.667413054179066100E-035, 
+	1.933482610835813200E-034, 3.866965221671626400E-034, 7.733930443343252900E-034, 1.546786088668650600E-033, 
+	3.093572177337301200E-033, 6.187144354674602300E-033, 1.237428870934920500E-032, 2.474857741869840900E-032, 
+	4.949715483739681800E-032, 9.899430967479363700E-032, 1.979886193495872700E-031, 3.959772386991745500E-031, 
+	7.919544773983491000E-031, 1.583908954796698200E-030, 3.167817909593396400E-030, 6.335635819186792800E-030, 
+	1.267127163837358600E-029, 2.534254327674717100E-029, 5.068508655349434200E-029, 1.013701731069886800E-028, 
+	2.027403462139773700E-028, 4.054806924279547400E-028, 8.109613848559094700E-028, 1.621922769711818900E-027, 
+	3.243845539423637900E-027, 6.487691078847275800E-027, 1.297538215769455200E-026, 2.595076431538910300E-026, 
+	5.190152863077820600E-026, 1.038030572615564100E-025, 2.076061145231128300E-025, 4.152122290462256500E-025, 
+	8.304244580924513000E-025, 1.660848916184902600E-024, 3.321697832369805200E-024, 6.643395664739610400E-024, 
+	1.328679132947922100E-023, 2.657358265895844200E-023, 5.314716531791688300E-023, 1.062943306358337700E-022, 
+	2.125886612716675300E-022, 4.251773225433350700E-022, 8.503546450866701300E-022, 1.700709290173340300E-021, 
+	3.401418580346680500E-021, 6.802837160693361100E-021, 1.360567432138672200E-020, 2.721134864277344400E-020, 
+	5.442269728554688800E-020, 1.088453945710937800E-019, 2.176907891421875500E-019, 4.353815782843751100E-019, 
+	8.707631565687502200E-019, 1.741526313137500400E-018, 3.483052626275000900E-018, 6.966105252550001700E-018, 
+	1.393221050510000300E-017, 2.786442101020000700E-017, 5.572884202040001400E-017, 1.114576840408000300E-016, 
+	2.229153680816000600E-016, 4.458307361632001100E-016, 8.916614723264002200E-016, 1.783322944652800400E-015, 
+	3.566645889305600900E-015, 7.133291778611201800E-015, 1.426658355722240400E-014, 2.853316711444480700E-014, 
+	5.706633422888961400E-014, 1.141326684577792300E-013, 2.282653369155584600E-013, 4.565306738311169100E-013, 
+	9.130613476622338300E-013, 1.826122695324467700E-012, 3.652245390648935300E-012, 7.304490781297870600E-012, 
+	1.460898156259574100E-011, 2.921796312519148200E-011, 5.843592625038296500E-011, 1.168718525007659300E-010, 
+	2.337437050015318600E-010, 4.674874100030637200E-010, 9.349748200061274400E-010, 1.869949640012254900E-009, 
+	3.739899280024509800E-009, 7.479798560049019500E-009, 1.495959712009803900E-008, 2.991919424019607800E-008, 
+	5.983838848039215600E-008, 1.196767769607843100E-007, 2.393535539215686200E-007, 4.787071078431372500E-007, 
+	9.574142156862745000E-007, 1.914828431372549000E-006, 3.829656862745098000E-006, 7.659313725490196000E-006, 
+	1.531862745098039200E-005, 3.063725490196078400E-005, 6.127450980392156800E-005, 1.225490196078431400E-004, 
+	2.450980392156862700E-004, 4.901960784313725400E-004, 9.803921568627450800E-004, 1.960784313725490200E-003, 
+	3.921568627450980300E-003, 7.843137254901960700E-003, 1.568627450980392100E-002, 3.137254901960784300E-002, 
+	6.274509803921568500E-002, 1.254901960784313700E-001, 2.509803921568627400E-001, 5.019607843137254800E-001, 
+	1.003921568627451000E+000, 2.007843137254901900E+000, 4.015686274509803900E+000, 8.031372549019607700E+000, 
+	1.606274509803921500E+001, 3.212549019607843100E+001, 6.425098039215686200E+001, 1.285019607843137200E+002, 
+	2.570039215686274500E+002, 5.140078431372548900E+002, 1.028015686274509800E+003, 2.056031372549019600E+003, 
+	4.112062745098039200E+003, 8.224125490196078300E+003, 1.644825098039215700E+004, 3.289650196078431300E+004, 
+	6.579300392156862700E+004, 1.315860078431372500E+005, 2.631720156862745100E+005, 5.263440313725490100E+005, 
+	1.052688062745098000E+006, 2.105376125490196000E+006, 4.210752250980392100E+006, 8.421504501960784200E+006, 
+	1.684300900392156800E+007, 3.368601800784313700E+007, 6.737203601568627400E+007, 1.347440720313725500E+008, 
+	2.694881440627450900E+008, 5.389762881254901900E+008, 1.077952576250980400E+009, 2.155905152501960800E+009, 
+	4.311810305003921500E+009, 8.623620610007843000E+009, 1.724724122001568600E+010, 3.449448244003137200E+010, 
+	6.898896488006274400E+010, 1.379779297601254900E+011, 2.759558595202509800E+011, 5.519117190405019500E+011, 
+	1.103823438081003900E+012, 2.207646876162007800E+012, 4.415293752324015600E+012, 8.830587504648031200E+012, 
+	1.766117500929606200E+013, 3.532235001859212500E+013, 7.064470003718425000E+013, 1.412894000743685000E+014, 
+	2.825788001487370000E+014, 5.651576002974740000E+014, 1.130315200594948000E+015, 2.260630401189896000E+015, 
+	4.521260802379792000E+015, 9.042521604759584000E+015, 1.808504320951916800E+016, 3.617008641903833600E+016, 
+	7.234017283807667200E+016, 1.446803456761533400E+017, 2.893606913523066900E+017, 5.787213827046133800E+017, 
+	1.157442765409226800E+018, 2.314885530818453500E+018, 4.629771061636907000E+018, 9.259542123273814000E+018, 
+	1.851908424654762800E+019, 3.703816849309525600E+019, 7.407633698619051200E+019, 1.481526739723810200E+020, 
+	2.963053479447620500E+020, 5.926106958895241000E+020, 1.185221391779048200E+021, 2.370442783558096400E+021, 
+	4.740885567116192800E+021, 9.481771134232385600E+021, 1.896354226846477100E+022, 3.792708453692954200E+022, 
+	7.585416907385908400E+022, 1.517083381477181700E+023, 3.034166762954363400E+023, 6.068333525908726800E+023, 
+	1.213666705181745400E+024, 2.427333410363490700E+024, 4.854666820726981400E+024, 9.709333641453962800E+024, 
+	1.941866728290792600E+025, 3.883733456581585100E+025, 7.767466913163170200E+025, 1.553493382632634000E+026, 
+	3.106986765265268100E+026, 6.213973530530536200E+026, 1.242794706106107200E+027, 2.485589412212214500E+027, 
+	4.971178824424429000E+027, 9.942357648848857900E+027, 1.988471529769771600E+028, 3.976943059539543200E+028, 
+	7.953886119079086300E+028, 1.590777223815817300E+029, 3.181554447631634500E+029, 6.363108895263269100E+029, 
+	1.272621779052653800E+030, 2.545243558105307600E+030, 5.090487116210615300E+030, 1.018097423242123100E+031, 
+	2.036194846484246100E+031, 4.072389692968492200E+031, 8.144779385936984400E+031, 1.628955877187396900E+032, 
+	3.257911754374793800E+032, 6.515823508749587500E+032, 1.303164701749917500E+033, 2.606329403499835000E+033, 
+	5.212658806999670000E+033, 1.042531761399934000E+034, 2.085063522799868000E+034, 4.170127045599736000E+034, 
+	8.340254091199472000E+034, 1.668050818239894400E+035, 3.336101636479788800E+035, 6.672203272959577600E+035 
+};
+
+// You can use this to double check the exponent table and assert that 
+// the precomputation is correct.
+#ifdef DBGFLAG_ASSERT
+#pragma warning(push)
+#pragma warning( disable : 4189 ) // disable unused local variable warning
+static void CheckExponentTable()
+{
+	for( int i = 0; i < 256; i++ )
+	{
+		float testAgainst = pow( 2.0f, i - 128 ) / 255.0f;
+		float diff = testAgainst - power2_n[i] ;
+		float relativeDiff = diff / testAgainst;
+		Assert( testAgainst == 0 ? 
+				power2_n[i] < 1.16E-041 :
+				power2_n[i] == testAgainst );
+	}
+}
+#pragma warning(pop)
+#endif
+
+void BuildGammaTable( float gamma, float texGamma, float brightness, int overbright )
+{
+	int		i, inf;
+	float	g1, g3;
+
+	// Con_Printf("BuildGammaTable %.1f %.1f %.1f\n", g, v_lightgamma.GetFloat(), v_texgamma.GetFloat() );
+
+	float g = gamma;
+	if (g > 3.0) 
+	{
+		g = 3.0;
+	}
+
+	g = 1.0 / g;
+	g1 = texGamma * g; 
+
+	if (brightness <= 0.0) 
+	{
+		g3 = 0.125;
+	}
+	else if (brightness > 1.0) 
+	{
+		g3 = 0.05;
+	}
+	else 
+	{
+		g3 = 0.125 - (brightness * brightness) * 0.075;
+	}
+
+	for (i=0 ; i<256 ; i++)
+	{
+		inf = ( int )( 255 * pow ( i/255.f, g1 ) ); 
+		if (inf < 0)
+			inf = 0;
+		if (inf > 255)
+			inf = 255;
+		texgammatable[i] = inf;
+	}
+
+	for (i=0 ; i<1024 ; i++)
+	{
+		float f;
+
+		f = i / 1023.0;
+
+		// scale up
+		if (brightness > 1.0)
+			f = f * brightness;
+
+		// shift up
+		if (f <= g3)
+			f = (f / g3) * 0.125;
+		else 
+			f = 0.125 + ((f - g3) / (1.0 - g3)) * 0.875;
+
+		// convert linear space to desired gamma space
+		inf = ( int )( 255 * pow ( f, g ) ); 
+
+		if (inf < 0)
+			inf = 0;
+		if (inf > 255)
+			inf = 255;
+		lineartoscreen[i] = inf;
+	}
+
+	/*
+	for (i=0 ; i<1024 ; i++)
+	{
+		// convert from screen gamma space to linear space
+		lineargammatable[i] = 1023 * pow ( i/1023.0, v_gamma.GetFloat() );
+		// convert from linear gamma space to screen space
+		screengammatable[i] = 1023 * pow ( i/1023.0, 1.0 / v_gamma.GetFloat() );
+	}
+	*/
+
+	for (i=0 ; i<256 ; i++)
+	{
+		// convert from nonlinear texture space (0..255) to linear space (0..1)
+		texturetolinear[i] =  pow( i / 255.f, texGamma );
+
+		// convert from linear space (0..1) to nonlinear (sRGB) space (0..1)
+		g_Mathlib_LinearToGamma[i] =  LinearToGammaFullRange( i / 255.f );
+
+		// convert from sRGB gamma space (0..1) to linear space (0..1)
+		g_Mathlib_GammaToLinear[i] =  GammaToLinearFullRange( i / 255.f );
+	}
+
+	for (i=0 ; i<1024 ; i++)
+	{
+		// convert from linear space (0..1) to nonlinear texture space (0..255)
+		lineartotexture[i] = ( int )pow( i / 1023.0, 1.0 / texGamma ) * 255;
+	}
+
+#if 0
+	for (i=0 ; i<256 ; i++)
+	{
+		float f;
+
+		// convert from nonlinear lightmap space (0..255) to linear space (0..4)
+		// f =  (i / 255.0) * sqrt( 4 );
+		f =  i * (2.0 / 255.0);
+		f = f * f;
+
+		texlighttolinear[i] = f;
+	}
+#endif
+
+	{
+		float f;
+		float overbrightFactor = 1.0f;
+
+		// Can't do overbright without texcombine
+		// UNDONE: Add GAMMA ramp to rectify this
+		if ( overbright == 2 )
+		{
+			overbrightFactor = 0.5;
+		}
+		else if ( overbright == 4 )
+		{
+			overbrightFactor = 0.25;
+		}
+
+		for (i=0 ; i<4096 ; i++)
+		{
+			// convert from linear 0..4 (x1024) to screen corrected vertex space (0..1?)
+			f = pow ( i/1024.0, 1.0 / gamma );
+
+			lineartovertex[i] = f * overbrightFactor;
+			if (lineartovertex[i] > 1)
+				lineartovertex[i] = 1;
+
+			int nLightmap = RoundFloatToInt( f * 255 * overbrightFactor );
+			nLightmap = clamp( nLightmap, 0, 255 );
+			lineartolightmap[i] = (unsigned char)nLightmap;
+		}
+	}
+}
+
+float GammaToLinearFullRange( float gamma )
+{
+	return pow( gamma, 2.2f );
+}
+
+float LinearToGammaFullRange( float linear )
+{
+	return pow( linear, 1.0f / 2.2f );
+}
+
+float GammaToLinear( float gamma )
+{
+	Assert( s_bMathlibInitialized );
+	if ( gamma < 0.0f )
+	{
+		return 0.0f;
+	}
+
+	if ( gamma >= 0.95f )
+	{
+		// Use GammaToLinearFullRange maybe if you trip this.
+// X360TEMP
+//		Assert( gamma <= 1.0f );
+		return 1.0f;
+	}
+
+	int index = RoundFloatToInt( gamma * 255.0f );
+	Assert( index >= 0 && index < 256 );
+	return g_Mathlib_GammaToLinear[index];
+}
+
+float LinearToGamma( float linear )
+{
+	Assert( s_bMathlibInitialized );
+	if ( linear < 0.0f )
+	{
+		return 0.0f;
+	}
+	if ( linear > 1.0f )
+	{
+		// Use LinearToGammaFullRange maybe if you trip this.
+		Assert( 0 );
+		return 1.0f;
+	}
+
+	int index = RoundFloatToInt( linear * 255.0f );
+	Assert( index >= 0 && index < 256 );
+	return g_Mathlib_LinearToGamma[index];
+}
+
+//-----------------------------------------------------------------------------
+// Helper functions to convert between sRGB and 360 gamma space
+//-----------------------------------------------------------------------------
+float SrgbGammaToLinear( float flSrgbGammaValue )
+{
+	float x = clamp( flSrgbGammaValue, 0.0f, 1.0f );
+	return ( x <= 0.04045f ) ? ( x / 12.92f ) : ( pow( ( x + 0.055f ) / 1.055f, 2.4f ) );
+}
+
+float SrgbLinearToGamma( float flLinearValue )
+{
+	float x = clamp( flLinearValue, 0.0f, 1.0f );
+	return ( x <= 0.0031308f ) ? ( x * 12.92f ) : ( 1.055f * pow( x, ( 1.0f / 2.4f ) ) ) - 0.055f;
+}
+
+float X360GammaToLinear( float fl360GammaValue )
+{
+	float flLinearValue;
+
+	fl360GammaValue = clamp( fl360GammaValue, 0.0f, 1.0f );
+	if ( fl360GammaValue < ( 96.0f / 255.0f ) )
+	{
+		if ( fl360GammaValue < ( 64.0f / 255.0f ) )
+		{
+			flLinearValue = fl360GammaValue * 255.0f;
+		}
+		else
+		{
+			flLinearValue = fl360GammaValue * ( 255.0f * 2.0f ) - 64.0f;
+			flLinearValue += floor( flLinearValue * ( 1.0f / 512.0f ) );
+		}
+	}
+	else
+	{
+		if( fl360GammaValue < ( 192.0f / 255.0f ) )
+		{
+			flLinearValue = fl360GammaValue * ( 255.0f * 4.0f ) - 256.0f;
+			flLinearValue += floor( flLinearValue * ( 1.0f / 256.0f ) );
+		}
+		else
+		{
+			flLinearValue = fl360GammaValue * ( 255.0f * 8.0f ) - 1024.0f;
+			flLinearValue += floor( flLinearValue * ( 1.0f / 128.0f ) );
+		}
+	}
+
+	flLinearValue *= 1.0f / 1023.0f;
+
+	flLinearValue = clamp( flLinearValue, 0.0f, 1.0f );
+	return flLinearValue;
+}
+
+float X360LinearToGamma( float flLinearValue )
+{
+	float fl360GammaValue;
+
+	flLinearValue = clamp( flLinearValue, 0.0f, 1.0f );
+	if ( flLinearValue < ( 128.0f / 1023.0f ) )
+	{
+		if ( flLinearValue < ( 64.0f / 1023.0f ) )
+		{
+			fl360GammaValue = flLinearValue * ( 1023.0f * ( 1.0f / 255.0f ) );
+		}
+		else
+		{
+			fl360GammaValue = flLinearValue * ( ( 1023.0f / 2.0f ) * ( 1.0f / 255.0f ) ) + ( 32.0f / 255.0f );
+		}
+	}
+	else
+	{
+		if ( flLinearValue < ( 512.0f / 1023.0f ) )
+		{
+			fl360GammaValue = flLinearValue * ( ( 1023.0f / 4.0f ) * ( 1.0f / 255.0f ) ) + ( 64.0f / 255.0f );
+		}
+		else
+		{
+			fl360GammaValue = flLinearValue * ( ( 1023.0f /8.0f ) * ( 1.0f / 255.0f ) ) + ( 128.0f /255.0f ); // 1.0 -> 1.0034313725490196078431372549016
+			if ( fl360GammaValue > 1.0f )
+			{
+				fl360GammaValue = 1.0f;
+			}
+		}
+	}
+
+	fl360GammaValue = clamp( fl360GammaValue, 0.0f, 1.0f );
+	return fl360GammaValue;
+}
+
+float SrgbGammaTo360Gamma( float flSrgbGammaValue )
+{
+	float flLinearValue = SrgbGammaToLinear( flSrgbGammaValue );
+	float fl360GammaValue = X360LinearToGamma( flLinearValue );
+	return fl360GammaValue;
+}
+
+// convert texture to linear 0..1 value
+float TextureToLinear( int c )
+{
+	Assert( s_bMathlibInitialized );
+	if (c < 0)
+		return 0;
+	if (c > 255)
+		return 1.0;
+
+	return texturetolinear[c];
+}
+
+// convert texture to linear 0..1 value
+int LinearToTexture( float f )
+{
+	Assert( s_bMathlibInitialized );
+	int i;
+	i = ( int )( f * 1023 );	// assume 0..1 range
+	if (i < 0)
+		i = 0;
+	if (i > 1023)
+		i = 1023;
+
+	return lineartotexture[i];
+}
+
+
+// converts 0..1 linear value to screen gamma (0..255)
+int LinearToScreenGamma( float f )
+{
+	Assert( s_bMathlibInitialized );
+	int i;
+	i = ( int )( f * 1023 );	// assume 0..1 range
+	if (i < 0)
+		i = 0;
+	if (i > 1023)
+		i = 1023;
+
+	return lineartoscreen[i];
+}
+
+void ColorRGBExp32ToVector( const ColorRGBExp32& in, Vector& out )
+{
+	Assert( s_bMathlibInitialized );
+	// FIXME: Why is there a factor of 255 built into this?
+	out.x = 255.0f * TexLightToLinear( in.r, in.exponent );
+	out.y = 255.0f * TexLightToLinear( in.g, in.exponent );
+	out.z = 255.0f * TexLightToLinear( in.b, in.exponent );
+}
+
+#if 0
+// assumes that the desired mantissa range is 128..255
+static int VectorToColorRGBExp32_CalcExponent( float in )
+{
+	int power = 0;
+	
+	if( in != 0.0f )
+	{
+		while( in > 255.0f )
+		{
+			power += 1;
+			in *= 0.5f;
+		}
+		
+		while( in < 128.0f )
+		{
+			power -= 1;
+			in *= 2.0f;
+		}
+	}
+
+	return power;
+}
+
+void VectorToColorRGBExp32( const Vector& vin, ColorRGBExp32 &c )
+{
+	Vector v = vin;
+	Assert( s_bMathlibInitialized );
+	Assert( v.x >= 0.0f && v.y >= 0.0f && v.z >= 0.0f );
+	int i;		
+	float max = v[0];				
+	for( i = 1; i < 3; i++ )
+	{
+		// Get the maximum value.
+		if( v[i] > max )
+		{
+			max = v[i];
+		}
+	}
+				
+	// figure out the exponent for this luxel.
+	int exponent = VectorToColorRGBExp32_CalcExponent( max );
+				
+	// make the exponent fits into a signed byte.
+	if( exponent < -128 )
+	{
+		exponent = -128;
+	}
+	else if( exponent > 127 )
+	{
+		exponent = 127;
+	}
+				
+	// undone: optimize with a table
+	float scalar = pow( 2.0f, -exponent );
+	// convert to mantissa x 2^exponent format
+	for( i = 0; i < 3; i++ )
+	{
+		v[i] *= scalar;
+		// clamp
+		if( v[i] > 255.0f )
+		{
+			v[i] = 255.0f;
+		}
+	}
+	c.r = ( unsigned char )v[0];
+	c.g = ( unsigned char )v[1];
+	c.b = ( unsigned char )v[2];
+	c.exponent = ( signed char )exponent;
+}
+
+#else
+
+// given a floating point number  f, return an exponent e such that
+// for f' = f * 2^e,  f is on [128..255].
+// Uses IEEE 754 representation to directly extract this information
+// from the float.
+inline static int VectorToColorRGBExp32_CalcExponent( const float *pin )
+{
+	// The thing we will take advantage of here is that the exponent component
+	// is stored in the float itself, and because we want to map to 128..255, we
+	// want an "ideal" exponent of 2^7. So, we compute the difference between the
+	// input exponent and 7 to work out the normalizing exponent. Thus if you pass in 
+	// 32 (represented in IEEE 754 as 2^5), this function will return 2
+	// (because 32 * 2^2 = 128)
+	if (*pin == 0.0f)
+		return 0;
+
+	unsigned int fbits = *reinterpret_cast<const unsigned int *>(pin);
+	
+	// the exponent component is bits 23..30, and biased by +127
+	const unsigned int biasedSeven = 7 + 127;
+
+	signed int expComponent = ( fbits & 0x7F800000 ) >> 23;
+	expComponent -= biasedSeven; // now the difference from seven (positive if was less than, etc)
+	return expComponent;
+}
+
+
+
+/// Slightly faster version of the function to turn a float-vector color into 
+/// a compressed-exponent notation 32bit color. However, still not SIMD optimized.
+/// PS3 developer: note there is a movement of a float onto an int here, which is
+/// bad on the base registers -- consider doing this as Altivec code, or better yet
+/// moving it onto the cell.
+/// \warning: Assumes an IEEE 754 single-precision float representation! Those of you
+/// porting to an 8080 are out of luck.
+void VectorToColorRGBExp32( const Vector& vin, ColorRGBExp32 &c )
+{
+	Assert( s_bMathlibInitialized );
+	Assert( vin.x >= 0.0f && vin.y >= 0.0f && vin.z >= 0.0f );
+
+	// work out which of the channels is the largest ( we will use that to map the exponent )
+	// this is a sluggish branch-based decision tree -- most architectures will offer a [max]
+	// assembly opcode to do this faster.
+	const float *pMax;
+	if (vin.x > vin.y)
+	{
+		if (vin.x > vin.z)
+		{
+			pMax = &vin.x;
+		}
+		else
+		{
+			pMax = &vin.z;
+		}
+	}
+	else
+	{
+		if (vin.y > vin.z)
+		{
+			pMax = &vin.y;
+		}
+		else
+		{
+			pMax = &vin.z;
+		}
+	}
+
+	// now work out the exponent for this luxel. 
+	signed int exponent = VectorToColorRGBExp32_CalcExponent( pMax );
+
+	// make sure the exponent fits into a signed byte.
+	// (in single precision format this is assured because it was a signed byte to begin with)
+	Assert(exponent > -128 && exponent <= 127);
+
+	// promote the exponent back onto a scalar that we'll use to normalize all the numbers
+	float scalar;
+	{
+		unsigned int fbits = (127 - exponent) << 23;
+		scalar = *reinterpret_cast<float *>(&fbits);
+	}
+
+	// we should never need to clamp:
+	Assert(vin.x * scalar <= 255.0f && 
+		   vin.y * scalar <= 255.0f && 
+		   vin.z * scalar <= 255.0f);
+
+	// This awful construction is necessary to prevent VC2005 from using the 
+	// fldcw/fnstcw control words around every float-to-unsigned-char operation.
+	{
+		int red = ( int )(vin.x * scalar);
+		int green = ( int )(vin.y * scalar);
+		int blue = ( int )(vin.z * scalar);
+
+		c.r = red;
+		c.g = green;
+		c.b = blue;
+	}
+	/*
+	c.r = ( unsigned char )(vin.x * scalar);
+	c.g = ( unsigned char )(vin.y * scalar);
+	c.b = ( unsigned char )(vin.z * scalar);
+	*/
+
+	c.exponent = ( signed char )exponent;
+}
+
+#endif

mathlib/datagen.pl

@@ -0,0 +1,74 @@
+#! perl
+use Text::Wrap;
+use Math::Trig;
+
+# generate output data for noise generators
+
+srand(31456);
+
+print <<END
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: static data for noise() primitives.
+//
+// \$Workfile:     \$
+// \$NoKeywords: \$
+//=============================================================================//
+//
+//    **** DO NOT EDIT THIS FILE. GENERATED BY DATAGEN.PL ****
+//
+
+END
+;
+
+@perm_a=0..255;
+
+&fisher_yates_shuffle(\@perm_a);
+
+$Text::Wrap::Columns=78;
+$Text::Wrap::break=",";
+$Text::Wrap::separator=",\n";
+
+print "static int perm_a[]={\n",wrap('    ','   ',join(",",@perm_a)),"\n};\n\n";
+&fisher_yates_shuffle(\@perm_a);
+print "static int perm_b[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+&fisher_yates_shuffle(\@perm_a);
+print "static int perm_c[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+&fisher_yates_shuffle(\@perm_a);
+print "static int perm_d[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+
+for ($i=0;$i<256;$i++)
+  {
+	$float_perm=(1.0/255.0)*$perm_a[$i];
+	$perm_a[$i] = sprintf("%f",$float_perm);
+  }
+&fisher_yates_shuffle(\@perm_a);
+print "static float impulse_xcoords[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+&fisher_yates_shuffle(\@perm_a);
+print "static float impulse_ycoords[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+&fisher_yates_shuffle(\@perm_a);
+print "static float impulse_zcoords[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+
+
+# now, generate 256 random gradient vectors
+for($i=0; $i < 256; $i++)
+  {
+	$z=rand(2)-1;
+	$phi=rand(2.0*3.141592654);
+	$theta=asin($z);
+	$perm_a[$i]=sprintf("%f, %f, %f ", cos($theta)*cos($phi), cos($theat)*sin($phi), $z );
+  }
+print "static float s_randomGradients[]={\n",wrap('    ','    ',join(",",@perm_a)),"\n};\n\n";
+
+
+# fisher_yates_shuffle( \@array ) : generate a random permutation
+# of @array in place
+sub fisher_yates_shuffle {
+    my $array = shift;
+    my $i;
+    for ($i = @$array; --$i; ) {
+        my $j = int rand ($i+1);
+        next if $i == $j;
+        @$array[$i,$j] = @$array[$j,$i];
+    }
+}

mathlib/disjoint_set_forest.cpp

@@ -0,0 +1,3 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+
+int g_nDisjointSetForestDummySymbol = 0; // Fix for linker warning: no public symbols found; archive member will be inaccessible

mathlib/dopvectors.h

@@ -0,0 +1,35 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: static vector table for 32-plane kdops
+//
+// $Workfile:     $
+// $NoKeywords: $
+//=============================================================================//
+//
+//    **** DO NOT EDIT THIS FILE. GENERATED BY genvectors.PL ****
+//
+
+const fltx4 g_KDop32XDirs[] =
+{
+	{ 1.000000, 0.000000, 0.000000, -0.577977,  },
+	{ 0.576708, -0.579777, -0.573256, -0.804198,  },
+	{ -0.802749, 0.806847, 0.592951, -0.005415,  },
+	{ 0.000459, 0.444936, -0.195083, 0.439082,  },
+};
+
+const fltx4 g_KDop32YDirs[] =
+{
+	{ 0.000000, 1.000000, 0.000000, -0.577534,  },
+	{ -0.578924, -0.575789, 0.579000, 0.000771,  },
+	{ 0.596314, 0.590760, -0.001819, -0.806919,  },
+	{ -0.597894, 0.873484, -0.443078, 0.202338,  },
+};
+
+const fltx4 g_KDop32ZDirs[] =
+{
+	{ 0.000000, 0.000000, 1.000000, 0.576538,  },
+	{ 0.576416, -0.576477, 0.579773, 0.594360,  },
+	{ 0.001587, -0.000671, 0.805237, -0.590637,  },
+	{ 0.801575, 0.197632, -0.875000, -0.875366,  },
+};
+

mathlib/dynamictree.cpp

@@ -0,0 +1,641 @@
+//===================== Copyright (c) Valve Corporation. All Rights Reserved. ======================
+#include "dynamictree.h" 
+
+
+//--------------------------------------------------------------------------------------------------
+// Local utilities
+//--------------------------------------------------------------------------------------------------
+static inline Vector Clamp( const Vector &v, const Vector &min, const Vector &max )
+{
+	Vector out;
+	out.x = fpmax( min.x, fpmin( v.x, max.x ) );
+	out.y = fpmax( min.y, fpmin( v.y, max.y ) );
+	out.z = fpmax( min.z, fpmin( v.z, max.z ) );
+
+	return out;
+}
+
+
+//-------------------------------------------------------------------------------------------------
+static inline Vector ClosestPointOnAABB( const Vector &p, const Vector &e, const Vector &q )
+{
+	// Offset vector from center of box to point q
+	Vector dp = q - p;
+
+	// Clamp offset vector to bounds extent
+	dp = Clamp( dp, -e, e );
+
+	// Return closest point
+	return p + dp;
+}
+
+//--------------------------------------------------------------------------------------------------
+static inline AABB_t Merge( const AABB_t& bounds1, const AABB_t& bounds2 )
+{
+	AABB_t out;
+	out.m_vMinBounds = VectorMin( bounds1.m_vMinBounds, bounds2.m_vMinBounds );
+	out.m_vMaxBounds = VectorMax( bounds1.m_vMaxBounds, bounds2.m_vMaxBounds );
+
+	return out;
+}
+
+
+//-------------------------------------------------------------------------------------------------
+static inline bool Overlap( const AABB_t& bounds1, const AABB_t& bounds2 )
+{
+	// No intersection if separated along one axis
+	if ( bounds1.m_vMaxBounds.x < bounds2.m_vMinBounds.x || bounds1.m_vMinBounds.x > bounds2.m_vMaxBounds.x ) return false;
+	if ( bounds1.m_vMaxBounds.y < bounds2.m_vMinBounds.y || bounds1.m_vMinBounds.y > bounds2.m_vMaxBounds.y ) return false;
+	if ( bounds1.m_vMaxBounds.z < bounds2.m_vMinBounds.z || bounds1.m_vMinBounds.z > bounds2.m_vMaxBounds.z ) return false;
+
+	// Overlapping on all axis means bounds are intersecting
+	return true;
+}
+
+
+//-------------------------------------------------------------------------------------------------
+static inline bool Overlap( const AABB_t& bounds, const Vector& vCenter, float flRadius )
+{
+	Vector vBoundsCenter = 0.5f * ( bounds.m_vMaxBounds + bounds.m_vMinBounds );
+	Vector vBoundsExtent = 0.5f * ( bounds.m_vMaxBounds - bounds.m_vMinBounds );
+
+	Vector vClosestPoint = ClosestPointOnAABB( vBoundsCenter, vBoundsExtent, vCenter );
+	Vector vOffset = vClosestPoint - vCenter;
+
+	return DotProduct( vOffset, vOffset ) <= flRadius * flRadius;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+// Dynamic tree
+//--------------------------------------------------------------------------------------------------
+CDynamicTree::CDynamicTree()
+{
+	m_nRoot = NULL_NODE;
+	m_nProxyCount = 0;
+	m_NodePool.Reserve( 32 );
+}
+
+
+//--------------------------------------------------------------------------------------------------
+int CDynamicTree::ProxyCount() const
+{
+	return m_nProxyCount;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+int32 CDynamicTree::CreateProxy( const AABB_t& bounds, void* pUserData )
+{
+	m_nProxyCount++;
+	
+	// Allocate a new node and insert into the tree
+	int32 nProxyId = m_NodePool.Alloc();
+	m_NodePool[ nProxyId ].m_Bounds = bounds;
+	m_NodePool[ nProxyId ].m_nParent = NULL_NODE;
+	m_NodePool[ nProxyId ].m_nChild1 = NULL_NODE;
+	m_NodePool[ nProxyId ].m_nChild2 = NULL_NODE;
+	m_NodePool[ nProxyId ].m_nHeight = 0;
+	m_NodePool[ nProxyId ].m_pUserData = pUserData;
+
+	InsertLeaf( nProxyId );
+
+	return nProxyId;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void* CDynamicTree::DestroyProxy( int32 nProxyId )
+{
+	AssertDbg( m_NodePool[ nProxyId ].IsLeaf() );
+
+	// Grab user data from the node to return it
+	void* pUserData = m_NodePool[ nProxyId ].m_pUserData;
+
+	// Remove node from tree and free it
+	RemoveLeaf( nProxyId );
+	
+	m_NodePool.Free( nProxyId );
+	m_nProxyCount--;
+
+	return pUserData;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::MoveProxy( int32 nProxyId, const AABB_t& bounds )
+{
+	AssertDbg ( m_NodePool[ nProxyId ].IsLeaf() );
+
+	RemoveLeaf( nProxyId );
+
+	// Save new bounds
+	m_NodePool[ nProxyId ].m_Bounds = bounds;
+
+	InsertLeaf( nProxyId );
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::InsertLeaf( int32 nLeaf )
+{
+	if ( m_nRoot == NULL_NODE )
+	{
+		m_nRoot = nLeaf;
+		m_NodePool[ nLeaf ].m_nParent = NULL_NODE;
+
+		return;
+	}
+
+	// Find the best sibling
+	int32 nNode = m_nRoot;
+	while ( !m_NodePool[ nNode ].IsLeaf() )
+	{
+		float flArea = m_NodePool[ nNode ].m_Bounds.GetSurfaceArea();
+
+		AABB_t combined = Merge( m_NodePool[ nNode ].m_Bounds, m_NodePool[ nLeaf ].m_Bounds );
+		float flCombinedArea = combined.GetSurfaceArea();
+
+		// Cost of creating a new parent for this node and the new leaf 
+		float flCost = 2.0f * flCombinedArea;
+
+		// Minimum cost of pushing the leaf further down the tree (we must inflate the parent if this leaf is not contained)
+		float flInheritanceCost = 2.0f * ( flCombinedArea - flArea );
+
+		// Cost of descending into first child
+		int32 nChild1 = m_NodePool[ nNode ].m_nChild1;
+		AABB_t combined1 = Merge( m_NodePool[ nChild1 ].m_Bounds, m_NodePool[ nLeaf ].m_Bounds );
+		float flCost1 = combined1.GetSurfaceArea() + flInheritanceCost;
+
+		if ( !m_NodePool[ nChild1 ].IsLeaf() )
+		{
+			flCost1 -= m_NodePool[ nChild1 ].m_Bounds.GetSurfaceArea();
+		}
+
+		// Cost of descending into second child
+		int32 nChild2 = m_NodePool[ nNode ].m_nChild2;
+		AABB_t combined2 = Merge( m_NodePool[ nChild2 ].m_Bounds, m_NodePool[ nLeaf ].m_Bounds );
+		float flCost2 = combined2.GetSurfaceArea() + flInheritanceCost;
+
+		if ( !m_NodePool[ nChild2 ].IsLeaf() )
+		{
+			flCost2 -= m_NodePool[ nChild2 ].m_Bounds.GetSurfaceArea();	
+		}
+
+		// Break if creating a parent results in minimal cost
+		if ( flCost < flCost1 && flCost < flCost2 )
+		{
+			break;
+		}
+
+		// Descend according to the minimum cost
+		nNode = flCost1 < flCost2 ? nChild1 : nChild2;
+	}
+
+	// Create and insert new parent
+	int32 nNewParent = m_NodePool.Alloc();
+	m_NodePool[ nNewParent ].m_Bounds = Merge( m_NodePool[ nNode ].m_Bounds, m_NodePool[ nLeaf ].m_Bounds );
+	m_NodePool[ nNewParent ].m_nParent = m_NodePool[ nNode ].m_nParent;
+	m_NodePool[ nNewParent ].m_nChild1 = nNode;
+	m_NodePool[ nNewParent ].m_nChild2 = nLeaf;
+	m_NodePool[ nNewParent ].m_nHeight = m_NodePool[ nNode ].m_nHeight + 1;
+	m_NodePool[ nNewParent ].m_pUserData = NULL; 
+
+	int32 nOldParent = m_NodePool[ nNode ].m_nParent;
+	if ( nOldParent != NULL_NODE )
+	{
+		// We are not inserting at the root
+		if ( m_NodePool[ nOldParent ].m_nChild1 == nNode )
+		{
+			m_NodePool[ nOldParent ].m_nChild1 = nNewParent;
+		}
+		else
+		{
+			m_NodePool[ nOldParent ].m_nChild2 = nNewParent;
+		}
+	}
+	else
+	{
+		// Inserting at the root 
+		m_nRoot = nNewParent;
+	}
+
+	m_NodePool[ nNode ].m_nParent = nNewParent;
+	m_NodePool[ nLeaf ].m_nParent = nNewParent;
+
+	// Walk back up the tree and fix heights and AABBs
+	AdjustAncestors( m_NodePool[ nLeaf ].m_nParent );	
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::RemoveLeaf( int32 nLeaf )
+{
+	AssertDbg( m_NodePool[ nLeaf ].IsLeaf() );
+
+	if ( nLeaf == m_nRoot )
+	{
+		m_nRoot = NULL_NODE;
+		return;
+	}
+
+	int32 nParent = m_NodePool[ nLeaf ].m_nParent;
+
+	int32 nSibling = NULL_NODE;
+	if ( m_NodePool[ nParent ].m_nChild1 == nLeaf )
+	{
+		nSibling = m_NodePool[ nParent ].m_nChild2;
+	}
+	else
+	{
+		nSibling = m_NodePool[ nParent ].m_nChild1;
+	}
+
+	int nGrandParent = m_NodePool[ nParent ].m_nParent;
+	if ( nGrandParent != NULL_NODE )
+	{
+		// Destroy parent and connect sibling to grandparent
+		m_NodePool[ nSibling ].m_nParent = nGrandParent;
+
+		if ( m_NodePool[ nGrandParent ].m_nChild1 == nParent )
+		{
+			m_NodePool[ nGrandParent ].m_nChild1 = nSibling;
+		}
+		else
+		{
+			m_NodePool[ nGrandParent ].m_nChild2 = nSibling;
+		}
+
+		// Walk back up the tree and fix heights and AABBs
+		AdjustAncestors( nGrandParent );	
+	}
+	else
+	{
+		m_NodePool[ nSibling ].m_nParent = NULL_NODE;
+		m_nRoot = nSibling;
+	}
+
+	m_NodePool.Free( nParent );
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::AdjustAncestors( int32 nNode )
+{
+	while ( nNode != NULL_NODE )
+	{
+		nNode = Balance( nNode );
+
+		int32 nChild1 = m_NodePool[ nNode ].m_nChild1;
+		AssertDbg( nChild1 != NULL_NODE );
+		int32 nChild2 = m_NodePool[ nNode ].m_nChild2;
+		AssertDbg( nChild2 != NULL_NODE );
+
+		m_NodePool[ nNode ].m_nHeight = 1 + MAX( m_NodePool[ nChild1 ].m_nHeight, m_NodePool[ nChild2 ].m_nHeight );
+		m_NodePool[ nNode ].m_Bounds = Merge( m_NodePool[ nChild1 ].m_Bounds, m_NodePool[ nChild2 ].m_Bounds );
+
+		nNode = m_NodePool[ nNode ].m_nParent;
+	}
+}
+
+	
+//--------------------------------------------------------------------------------------------------
+int32 CDynamicTree::Balance( int32 nNode )
+{
+	int32 nIndexA = nNode;
+	Node_t& A = m_NodePool[ nIndexA ];
+
+	if ( A.IsLeaf() || A.m_nHeight < 2 )
+	{
+		return nNode;
+	}
+
+	int32 nIndexB = A.m_nChild1;
+	int32 nIndexC = A.m_nChild2;
+	Node_t& B = m_NodePool[ nIndexB ];
+	Node_t& C = m_NodePool[ nIndexC ];
+
+	int nBalance = C.m_nHeight - B.m_nHeight;
+
+	// Rotate C up (left rotation)
+	if ( nBalance > 1 )
+	{
+		int32 nIndexF = C.m_nChild1;
+		int32 nIndexG = C.m_nChild2;
+		Node_t& F = m_NodePool[ nIndexF ];
+		Node_t& G = m_NodePool[ nIndexG ];
+
+		// Swap A and C
+		C.m_nChild1 = nIndexA;
+		C.m_nParent = A.m_nParent;
+		A.m_nParent = nIndexC;
+
+		// A's old parent should point to C
+		if ( C.m_nParent != NULL_NODE )
+		{
+			if ( m_NodePool[ C.m_nParent ].m_nChild1 == nIndexA )
+			{
+				m_NodePool[ C.m_nParent ].m_nChild1 = nIndexC;
+			}
+			else
+			{
+				AssertDbg( m_NodePool[ C.m_nParent ].m_nChild2 == nIndexA );
+				m_NodePool[ C.m_nParent ].m_nChild2 = nIndexC;
+			}
+		}
+		else
+		{
+			m_nRoot = nIndexC;
+		}
+
+		// Rotate
+		if ( F.m_nHeight > G.m_nHeight )
+		{
+			G.m_nParent = nIndexA;
+			C.m_nChild2 = nIndexF;
+			A.m_nChild2 = nIndexG;
+			A.m_Bounds = Merge( B.m_Bounds, G.m_Bounds );
+			C.m_Bounds = Merge( A.m_Bounds, F.m_Bounds );
+			A.m_nHeight = 1 + MAX( B.m_nHeight, G.m_nHeight );
+			C.m_nHeight = 1 + MAX( A.m_nHeight, F.m_nHeight );
+		}
+		else
+		{
+			F.m_nParent = nIndexA;
+			C.m_nChild2 = nIndexG;
+			A.m_nChild2 = nIndexF;
+			A.m_Bounds = Merge( B.m_Bounds, F.m_Bounds );
+			C.m_Bounds = Merge( A.m_Bounds, G.m_Bounds );
+			A.m_nHeight = 1 + MAX( B.m_nHeight, F.m_nHeight );
+			C.m_nHeight = 1 + MAX( A.m_nHeight, G.m_nHeight );
+		}
+
+		return nIndexC;
+	}
+
+	// Rotate B up (right rotation)
+	if ( nBalance < -1 )
+	{
+		int32 nIndexD = B.m_nChild1;
+		int32 nIndexE = B.m_nChild2;
+		Node_t& D = m_NodePool[ nIndexD ];
+		Node_t& E = m_NodePool[ nIndexE ];
+
+		// Swap A and B
+		B.m_nChild1 = nIndexA;
+		B.m_nParent = A.m_nParent;
+		A.m_nParent = nIndexB;
+
+		// A's old parent should point to B
+		if ( B.m_nParent != NULL_NODE )
+		{
+			if ( m_NodePool[ B.m_nParent ].m_nChild1 == nIndexA )
+			{
+				m_NodePool[ B.m_nParent ].m_nChild1 = nIndexB;
+			}
+			else
+			{
+				AssertDbg( m_NodePool[ B.m_nParent ].m_nChild2 == nIndexA );
+				m_NodePool[ B.m_nParent ].m_nChild2 = nIndexB;
+			}
+		}
+		else
+		{
+			m_nRoot = nIndexB;
+		}
+
+		// Rotate
+		if ( D.m_nHeight > E.m_nHeight )
+		{
+			E.m_nParent = nIndexA;
+			A.m_nChild1 = nIndexE;
+			B.m_nChild2 = nIndexD;
+			A.m_Bounds = Merge( C.m_Bounds, E.m_Bounds );
+			B.m_Bounds = Merge( A.m_Bounds, D.m_Bounds );
+			A.m_nHeight = 1 + MAX( C.m_nHeight, E.m_nHeight );
+			B.m_nHeight = 1 + MAX( A.m_nHeight, D.m_nHeight );
+		}
+		else
+		{
+			D.m_nParent = nIndexA;
+			A.m_nChild1 = nIndexD;
+			B.m_nChild2 = nIndexE;
+			A.m_Bounds = Merge( C.m_Bounds, D.m_Bounds );
+			B.m_Bounds = Merge( A.m_Bounds, E.m_Bounds );
+			A.m_nHeight = 1 + MAX( C.m_nHeight, D.m_nHeight );
+			B.m_nHeight = 1 + MAX( A.m_nHeight, E.m_nHeight );
+		}
+
+		return nIndexB;
+	}
+
+	return nIndexA;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::Query( CProxyVector& proxies, const AABB_t& bounds ) const
+{
+	if ( m_nRoot == NULL_NODE )
+	{
+		return;
+	}
+
+	int nCount = 0;
+	int32 stack[ STACK_DEPTH ];
+	stack[ nCount++ ] = m_nRoot;
+
+	while ( nCount > 0 )
+	{
+		int32 nNode = stack[ --nCount ];
+		const Node_t& node = m_NodePool[ nNode ];
+
+		if ( Overlap( node.m_Bounds, bounds ) )
+		{
+			if ( !node.IsLeaf() )
+			{
+				AssertDbg( nCount + 2 <= STACK_DEPTH );
+				stack[ nCount++ ] = node.m_nChild2;
+				stack[ nCount++ ] = node.m_nChild1;
+			}
+			else
+			{
+				proxies.AddToTail( nNode );
+			}
+		}
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::Query( CProxyVector& proxies, const Vector& vCenter, float flRadius ) const
+{
+	if ( m_nRoot == NULL_NODE )
+	{
+		return;
+	}
+
+	int nCount = 0;
+	int32 stack[ STACK_DEPTH ];
+	stack[ nCount++ ] = m_nRoot;
+
+	while ( nCount > 0 )
+	{
+		int32 nNode = stack[ --nCount ];
+		const Node_t& node = m_NodePool[ nNode ];
+
+		if ( Overlap( node.m_Bounds, vCenter, flRadius ) )
+		{
+			if ( !node.IsLeaf() )
+			{
+				AssertDbg( nCount + 2 <= STACK_DEPTH );
+				stack[ nCount++ ] = node.m_nChild2;
+				stack[ nCount++ ] = node.m_nChild1;
+			}
+			else
+			{
+				proxies.AddToTail( nNode );
+			}
+		}
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+float CDynamicTree::ClosestProxies( CProxyVector& proxies, const Vector &vQuery ) const
+{
+	if ( m_nRoot == NULL_NODE )
+	{
+		return FLT_MAX;
+	}
+
+    int nCount = 0;
+	int32 stack[ STACK_DEPTH ];
+	stack[ nCount++ ] = m_nRoot;
+
+    float bestDistance = FLT_MAX;
+    
+	while ( nCount > 0 )
+	{
+		int32 nNode = stack[ --nCount ];
+		const Node_t& node = m_NodePool[ nNode ];
+
+        float dist = node.m_Bounds.GetMinDistToPoint( vQuery );
+		if ( dist <= bestDistance )
+		{
+			if ( !node.IsLeaf() )
+			{
+				AssertDbg( nCount + 2 <= STACK_DEPTH );
+				stack[ nCount++ ] = node.m_nChild2;
+				stack[ nCount++ ] = node.m_nChild1;
+			}
+			else
+			{
+                bestDistance = dist;
+				proxies.AddToTail( nNode );
+			}
+		}
+	}
+
+    // We now have a collection of indices that -- at the time they
+    // were added -- pointed to the closest proxies. However, as
+    // 'bestDistance' is updated during processing, this may no longer
+    // be true. So we do one last scan of all the "best" proxies to
+    // find the true closest ones
+    CProxyVector closestProxies;
+    const uint32 numCandidates = proxies.Count();
+    for (uint32 ii=0; ii<numCandidates; ++ii)
+        if ( m_NodePool[ proxies[ii] ].m_Bounds.GetMinDistToPoint( vQuery ) <= bestDistance )
+            closestProxies.AddToTail( proxies[ii] );
+    
+    proxies = closestProxies;
+    return bestDistance;
+}
+
+
+
+//--------------------------------------------------------------------------------------------------
+// Node pool
+//--------------------------------------------------------------------------------------------------
+CDynamicTree::CNodePool::CNodePool()
+{
+	m_nCapacity = 0;
+	m_pObjects = NULL;
+	m_nNext = - 1;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+CDynamicTree::CNodePool::~CNodePool()
+{
+	delete m_pObjects;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::CNodePool::Clear()
+{
+	delete m_pObjects;
+
+	m_nCapacity = 0;
+	m_pObjects = NULL;
+	m_nNext = - 1;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::CNodePool::Reserve( int nCapacity )
+{
+	if ( nCapacity > m_nCapacity )
+	{
+		Node_t* pObjects = m_pObjects;
+		m_pObjects = new Node_t[ nCapacity ];
+		V_memcpy( m_pObjects, pObjects, m_nCapacity * sizeof( Node_t ) );
+		delete pObjects;
+		pObjects = NULL;
+
+		for ( int32 i = m_nCapacity; i < nCapacity - 1; ++i )
+		{
+			int32* nNext = (int32*)( m_pObjects + i );
+			*nNext = i + 1;
+		}
+		int32* nNext = (int32*)( m_pObjects + nCapacity - 1 );
+		*nNext = -1;
+
+		m_nNext = m_nCapacity;
+		m_nCapacity = nCapacity;
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+int32 CDynamicTree::CNodePool::Alloc()
+{
+	// Grow the pool if the free list is empty
+	if ( m_nNext < 0 )
+	{
+		Reserve( MAX( 2, 2 * m_nCapacity ) );
+	}
+
+	// Peel of a node from the free list
+	int32 id = m_nNext;
+	m_nNext = *(int32*)( m_pObjects + id );
+
+#if _DEBUG
+	// Do reuse old objects accidentally
+	V_memset( m_pObjects + id, 0xcd, sizeof( Node_t ) );
+#endif
+
+	return id;
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CDynamicTree::CNodePool::Free( int32 id )
+{
+	// Return node to the pool
+	AssertDbg( 0 <= id && id < m_nCapacity );
+
+	*(int32*)( m_pObjects + id ) = m_nNext;
+	m_nNext = id;	
+}

mathlib/eigen.cpp

@@ -0,0 +1,169 @@
+#include <basetypes.h>
+#include <float.h>
+#include "tier1/utlvector.h"
+#include "eigen.h"
+#include "mathlib/aabb.h"
+
+static matrix3x4_t Transpose(const matrix3x4_t &a)
+{
+	return matrix3x4_t(a[0][0],a[1][0],a[2][0],0,
+		a[0][1],a[1][1],a[2][1],0,
+		a[0][2],a[1][2],a[2][2],0 );
+}
+
+
+
+Quaternion Diagonalizer( const matrix3x4_t &A, Vector &d )
+{
+	// A must be a symmetric matrix.
+	// returns quaternion q such that its corresponding matrix Q
+	// can be used to Diagonalize A
+	// Note, this routine has been adapted to valve's matrix conventions
+	// which is C-style row-major matrix using common mathtext  (openglish) column conventions for
+	// representing transforms (rotation and position):   v' = Mv
+	// Valve's quaternion conventions are the same that everybody uses novodex, d3d, ogl.
+	// Diagonal matrix D = Transpose(Q) * A * Q;  and  A = QT*D*Q
+	// The columns of Q are the eigenvectors D's diagonal is the eigenvalues
+	// As per 'column' convention if Q = q.getmatrix(); then Q*v = q*v*conj(q)
+	int maxsteps = 24;  // certainly wont need that many.
+	int i;
+	Quaternion q( 0, 0, 0, 1 );
+	for ( i = 0; i < maxsteps; i++ )
+	{
+		matrix3x4_t Q  ; //   Q*v == q*v*conj(q)
+		QuaternionMatrix( q, Q );
+		matrix3x4_t D  = Transpose( Q ) * A * Q;  // A = Q*D*Q^T
+		Vector offdiag( D[1][2], D[0][2], D[0][1] ); // elements not on the diagonal
+		d = Vector( D[0][0], D[1][1], D[2][2] );
+		Vector om( fabsf( offdiag.x ), fabsf( offdiag.y ), fabsf( offdiag.z ) ); // mag of each offdiag elem
+		int k = ( om.x > om.y && om.x > om.z ) ? 0 : ( om.y > om.z ) ? 1 : 2; // index of largest element of offdiag
+		int k1 = ( k + 1 ) % 3;
+		int k2 = ( k + 2 ) % 3;
+		if ( offdiag[k] == 0.0f ) break;  // diagonal already
+		float thet = ( D[k2][k2] - D[k1][k1] ) / ( 2.0f * offdiag[k] );
+		float sgn = ( thet > 0.0f ) ? 1.0f : -1.0f;
+		thet    *= sgn; // make it positive
+		float t = sgn / ( thet + ( ( thet < 1.E6f ) ? sqrtf( thet * thet + 1.0f ) : thet ) ) ; // sign(T)/(|T|+sqrt(T^2+1))
+		float c = 1.0f / sqrtf( t * t + 1.0f ); //  c= 1/(t^2+1) , t=s/c
+		if ( c == 1.0f ) break;  // no room for improvement - reached machine precision.
+		Quaternion jr( 0, 0, 0, 0 ); // jacobi rotation for this iteration.
+		jr[k] = sgn * sqrtf( ( 1.0f - c ) / 2.0f );  // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
+		jr[k] *= -1.0f; // ??since our quat-to-matrix convention was for v*M instead of M*v
+		jr.w  = sqrtf( 1.0f - jr[k] * jr[k] );
+		if ( jr.w == 1.0f ) break; // reached limits of floating point precision
+		QuaternionMult( q, jr, q ); //q =  q*jr;
+		QuaternionNormalize( q );
+	}
+	return q;
+}
+
+
+//-----------------------------------------------------------------------------
+// Computes the mean point of a set of points, used by ComputeCovariantMatrix
+//-----------------------------------------------------------------------------
+extern Vector ComputeMeanPoint( const Vector *pPointList, int nPointCount )
+{
+	Vector vMean( 0.0f, 0.0f, 0.0f );
+
+	for ( int ii = 0; ii < nPointCount; ++ii )
+	{
+		vMean += pPointList[ii];
+	}
+
+	vMean /= static_cast< float >( nPointCount );
+
+	return vMean;
+}
+
+
+//-----------------------------------------------------------------------------
+// Computes a covariance matrix for a set of points which measures spatial
+// dispersion of the points against the mean of the points,
+// the covariance matrix is symmetric and suitable for use in Diagonalizer()
+//-----------------------------------------------------------------------------
+void ComputeCovarianceMatrix( matrix3x4_t &covarianceMatrix, const Vector *pPointList, int nPointCount )
+{
+	SetIdentityMatrix( covarianceMatrix );
+
+	if ( nPointCount <= 0 )
+		return;
+
+	const Vector vMean = ComputeMeanPoint( pPointList, nPointCount );
+
+	CUtlVector< Vector > skewedPointList;
+	skewedPointList.CopyArray( pPointList, nPointCount );
+
+	for ( int ii = 0; ii < nPointCount; ++ii )
+	{
+		skewedPointList[ ii ] -= vMean;
+	}
+
+	const float flPointCount = static_cast< float >( nPointCount );
+
+	for ( int ii = 0; ii < 3; ++ii )
+	{
+		for ( int jj = 0; jj < 3; ++jj )
+		{
+			float flCovariance = 0.0f;
+
+			for ( int kk = 0; kk < nPointCount; ++kk )
+			{
+				flCovariance += skewedPointList[kk][ii] * skewedPointList[kk][jj];
+			}
+
+			covarianceMatrix[ii][jj] = flCovariance / flPointCount;
+		}
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Computes the center and scale using qEigenVectors as the orientation to
+// transform a unit cube at the origin to contain the specified point list,
+// calls ComputeCovarianceMatrix(), Diagonalizer()
+//-----------------------------------------------------------------------------
+void ComputeExtents( Vector &vCenter, Vector &vScale, const Quaternion &qEigen, const Vector *pPointList, int nPointCount )
+{
+	if ( nPointCount <= 0 )
+		return;
+
+	AABB_t bbox;
+
+	// Compute bounding box in inverse eigen space
+	const Quaternion qEigenInverse = QuaternionInvert( qEigen );
+	const matrix3x4_t mEigenInverse = QuaternionMatrix( qEigenInverse );	// VectorRotate with a quaternion does this each call
+
+	Vector vTmp;
+	VectorRotate( pPointList[0], mEigenInverse, vTmp );
+
+	bbox.SetToPoint( vTmp );
+
+	for ( int ii = 1; ii < nPointCount; ++ii )
+	{
+		VectorRotate( pPointList[ii], mEigenInverse, vTmp );
+		bbox |= vTmp;
+	}
+
+	VectorRotate( bbox.GetCenter(), qEigen, vCenter );	// Transform center back to eigen space
+	vScale = bbox.GetSize();
+}
+
+
+//-----------------------------------------------------------------------------
+//
+//-----------------------------------------------------------------------------
+extern void ComputeBoundingBoxMatrix( matrix3x4_t &boundingBoxMatrix, const Vector *pPointList, int nPointCount )
+{
+	matrix3x4_t covarianceMatrix;
+	ComputeCovarianceMatrix( covarianceMatrix, pPointList, nPointCount );
+
+	Vector vEigenValues;
+	const Quaternion qEigenVectors = Diagonalizer( covarianceMatrix, vEigenValues );
+
+	Vector vCenter( 0.0f, 0.0f, 0.0f );
+	Vector vScale( 1.0f, 1.0f, 1.0f );
+
+	ComputeExtents( vCenter, vScale, qEigenVectors, pPointList, nPointCount );
+
+	QuaternionMatrix( qEigenVectors, vCenter, vScale, boundingBoxMatrix );
+}

mathlib/expressioncalculator.cpp

@@ -0,0 +1,809 @@
+//========= Copyright c 1996-2008, Valve Corporation, All rights reserved. ============//
+
+#include "tier0/platform.h"
+#include "tier0/dbg.h"
+
+#include "mathlib/mathlib.h"
+#include "mathlib/noise.h"
+#include "mathlib/vector.h"
+
+#include "mathlib/expressioncalculator.h"
+
+#include <ctype.h>
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+
+
+//-----------------------------------------------------------------------------
+// Parsing helper methods
+//-----------------------------------------------------------------------------
+bool ParseLiteral( const char *&expr, float &value )
+{
+	const char *startExpr = expr;
+	value = ( float )strtod( startExpr, const_cast< char** >( &expr ) );
+	return ( startExpr != expr );
+}
+
+bool ParseString( const char *&expr, const char *str )
+{
+	const char *startExpr = expr;
+	while ( ( *expr == ' ' ) || ( *expr == '\t' ) )
+		expr++; // skip whitespace
+
+	expr = StringAfterPrefix( expr, str );
+	if ( expr )
+		return true;
+
+	expr = startExpr;
+	return false;
+}
+
+bool ParseStringList( const char *&expr, const char **pOps, int &nOp )
+{
+	while ( nOp-- )
+	{
+		if ( ParseString( expr, pOps[ nOp ] ) )
+			return true;
+	}
+	return false;
+}
+
+bool ParseStringList( const char *&expr, const CUtlVector< CUtlString > &strings, int &nOp )
+{
+	while ( nOp-- )
+	{
+		if ( ParseString( expr, strings[ nOp ] ) )
+			return true;
+	}
+	return false;
+}
+
+int FindString( const CUtlVector< CUtlString > &strings, const char *str )
+{
+	uint sn = strings.Count();
+	for ( uint si = 0; si < sn; ++si )
+	{
+		if ( !Q_strcmp( str, strings[ si ] ) )
+			return si;
+	}
+	return -1;
+}
+
+class ParseState_t
+{
+public:
+	ParseState_t( const CUtlStack<float> &stack, const char *expr )
+		: m_stacksize( stack.Count() ), m_startingExpr( expr ) {}
+	void Reset( CUtlStack<float> &stack, const char *&expr )
+	{
+		Assert( m_stacksize <= stack.Count() );
+		stack.PopMultiple( stack.Count() - m_stacksize );
+		expr = m_startingExpr;
+	}
+
+private:
+	int m_stacksize;
+	const char* m_startingExpr;
+};
+
+
+
+
+void CExpressionCalculator::SetVariable( int nVariableIndex, float value )
+{
+	m_varValues[ nVariableIndex ] = value;
+}
+
+void CExpressionCalculator::SetVariable( const char *var, float value )
+{
+	int vi = FindString( m_varNames, var );
+	if ( vi >= 0 )
+	{
+		m_varValues[ vi ] = value;
+	}
+	else
+	{
+		m_varNames.AddToTail( var );
+		m_varValues.AddToTail( value );
+	}
+}
+
+void CExpressionCalculator::ModifyVariable( const char *var, float value )
+{
+	int vi = FindString( m_varNames, var );
+	if ( vi >= 0 )
+	{
+		m_varValues[ vi ] += value;
+	}
+	else
+	{
+		SetVariable( var, value );
+	}
+}
+
+int CExpressionCalculator::FindVariableIndex( const char *var )
+{
+	return FindString( m_varNames, var );
+}
+
+bool CExpressionCalculator::Evaluate( float &value )
+{
+	m_bIsBuildingArgumentList = false;
+	m_stack.PopMultiple( m_stack.Count() );
+	const char *pExpr = m_expr.Get();
+	bool success = ParseExpr( pExpr );
+	if ( success && m_stack.Count() == 1 )
+	{
+		value = m_stack.Top();
+		return true;
+	}
+
+	value = 0.0f;
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Builds a list of variable names from the expression
+//-----------------------------------------------------------------------------
+bool CExpressionCalculator::BuildVariableListFromExpression( )
+{
+	m_bIsBuildingArgumentList = true;
+	m_stack.PopMultiple( m_stack.Count() );
+	const char *pExpr = m_expr.Get();
+	bool bSuccess = ParseExpr( pExpr );
+	m_bIsBuildingArgumentList = false;
+	if ( !bSuccess || m_stack.Count() != 1 )
+	{
+		m_varNames.RemoveAll();
+		return false;
+	}
+	return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Iterate over variables
+//-----------------------------------------------------------------------------
+int CExpressionCalculator::VariableCount()
+{
+	return m_varNames.Count();
+}
+
+const char *CExpressionCalculator::VariableName( int nIndex )
+{
+	return m_varNames[nIndex];
+}
+
+
+
+bool CExpressionCalculator::ParseExpr( const char *&expr )
+{
+	return ( expr != NULL ) && ParseConditional( expr );
+}
+
+bool CExpressionCalculator::ParseConditional( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseOr( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseString( expr, "?" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ":" ) &&
+		ParseExpr( expr ) )
+	{
+		float f3 = m_stack.Top();
+		m_stack.Pop();
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		m_stack.Push( f1 != 0.0f ? f2 : f3 );
+		return true; // and matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // equality (or lower) matched
+}
+
+bool CExpressionCalculator::ParseOr( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseAnd( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseString( expr, "||" ) &&
+		ParseOr( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		m_stack.Push( ( f1 != 0.0f ) || ( f2 != 0.0f ) ? 1 : 0 );
+		return true; // and matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // equality (or lower) matched
+}
+
+bool CExpressionCalculator::ParseAnd( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseEquality( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseString( expr, "&&" ) &&
+		ParseAnd( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		m_stack.Push( ( f1 != 0.0f ) && ( f2 != 0.0f ) ? 1 : 0 );
+		return true; // and matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // equality (or lower) matched
+}
+
+bool CExpressionCalculator::ParseEquality( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseLessGreater( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	const char *pOps[] = { "==", "!=" };
+	int nOp = 2;
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseStringList( expr, pOps, nOp ) &&
+		ParseEquality( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nOp )
+		{
+		case 0: // ==
+			m_stack.Push( f1 == f2 ? 1 : 0 );
+			break;
+		case 1: // !=
+			m_stack.Push( f1 != f2 ? 1 : 0 );
+			break;
+		}
+		return true; // equality matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // lessgreater (or lower) matched
+}
+
+bool CExpressionCalculator::ParseLessGreater( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseAddSub( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	const char *pOps[] = { "<", ">", "<=", ">=" };
+	int nOp = 4;
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseStringList( expr, pOps, nOp ) &&
+		ParseLessGreater( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nOp )
+		{
+		case 0: // <
+			m_stack.Push( f1 < f2 ? 1 : 0 );
+			break;
+		case 1: // >
+			m_stack.Push( f1 > f2 ? 1 : 0 );
+			break;
+		case 2: // <=
+			m_stack.Push( f1 <= f2 ? 1 : 0 );
+			break;
+		case 3: // >=
+			m_stack.Push( f1 >= f2 ? 1 : 0 );
+			break;
+		}
+		return true; // inequality matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // addsub (or lower) matched
+}
+
+bool CExpressionCalculator::ParseAddSub( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseDivMul( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	const char *pOps[] = { "+", "-" };
+	int nOp = 2;
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseStringList( expr, pOps, nOp ) &&
+		ParseAddSub( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nOp )
+		{
+		case 0: // +
+			m_stack.Push( f1 + f2 );
+			break;
+		case 1: // -
+			m_stack.Push( f1 - f2 );
+			break;
+		}
+		return true; // addsub matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // divmul (or lower) matched
+}
+
+bool CExpressionCalculator::ParseDivMul( const char *&expr )
+{
+	ParseState_t ps0( m_stack, expr );
+	if ( !ParseUnary( expr ) )
+	{
+		ps0.Reset( m_stack, expr );
+		return false; // nothing matched
+	}
+
+	const char *pOps[] = { "*", "/", "%" };
+	int nOp = 3;
+
+	ParseState_t ps1( m_stack, expr );
+	if ( ParseStringList( expr, pOps, nOp ) &&
+		ParseDivMul( expr ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nOp )
+		{
+		case 0: // *
+			m_stack.Push( f1 * f2 );
+			break;
+		case 1: // /
+			m_stack.Push( f1 / f2 );
+			break;
+		case 2: // %
+			m_stack.Push( fmod( f1, f2 ) );
+			break;
+		}
+		return true; // divmul matched
+	}
+	ps1.Reset( m_stack, expr );
+	return true; // unary (or lower) matched
+}
+
+bool CExpressionCalculator::ParseUnary( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	const char *pOps[] = { "+", "-", "!" };
+	int nOp = 3;
+
+	if ( ParseStringList( expr, pOps, nOp ) &&
+		ParseUnary( expr ) )
+	{
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nOp )
+		{
+		case 0: // +
+			m_stack.Push( f1 );
+			break;
+		case 1: // -
+			m_stack.Push( -f1 );
+			break;
+		case 2: // !
+			m_stack.Push( f1 == 0 ? 1 : 0 );
+			break;
+		}
+		return true;
+	}
+
+	ps.Reset( m_stack, expr );
+	if ( ParsePrimary( expr ) )
+		return true;
+
+	ps.Reset( m_stack, expr );
+	return false;
+}
+
+bool CExpressionCalculator::ParsePrimary( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	float value = 0.0f;
+	if ( ParseLiteral( expr, value ) )
+	{
+		m_stack.Push( value );
+		return true;
+	}
+
+	ps.Reset( m_stack, expr );
+	int nVar = m_varNames.Count();
+	if ( ParseStringList( expr, m_varNames, nVar) )
+	{
+		m_stack.Push( m_varValues[ nVar ] );
+		return true;
+	}
+
+	ps.Reset( m_stack, expr );
+	if ( ParseString( expr, "(" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ")" ) )
+	{
+		return true;
+	}
+
+	ps.Reset( m_stack, expr );
+	if ( Parse1ArgFunc( expr ) ||
+		Parse2ArgFunc( expr ) ||
+		Parse3ArgFunc( expr ) ||
+//		Parse4ArgFunc( expr ) ||
+		Parse5ArgFunc( expr ) )
+	{
+		return true;
+	}
+
+	// If we're parsing it to discover names of variable names, add them here
+	if ( !m_bIsBuildingArgumentList )
+		return false;
+
+	// Variables can't start with a number
+	if ( V_isdigit( *expr ) )
+		return false;
+
+	const char *pStart = expr;
+	while ( V_isalnum( *expr ) || *expr == '_' )
+	{
+		++expr;
+	}
+
+	size_t nLen = (size_t)expr - (size_t)pStart;
+	char *pVariableName = (char*)stackalloc( nLen+1 );
+	memcpy( pVariableName, pStart, nLen );
+	pVariableName[nLen] = 0;
+
+	SetVariable( pVariableName, 0.0f );
+	m_stack.Push( 0.0f );
+	return true;
+}
+
+/*
+dtor(d) : converts degrees to radians
+rtod(r) : converts radians to degrees
+
+abs(a)     : absolute value
+floor(a)   : rounds down to the nearest integer
+ceiling(a) : rounds up to the nearest integer
+round(a)   : rounds to the nearest integer
+sgn(a)     : if a < 0 returns -1 else 1
+sqr(a)     : returns a * a
+sqrt(a)    : returns sqrt(a)
+
+sin(a)     : sin(a), a is in degrees
+asin(a)    : asin(a) returns degrees
+cos(a)     : cos(a), a is in degrees
+acos(a)    : acos(a) returns degrees
+tan(a)     : tan(a), a is in degrees
+
+exp(a)   : returns the exponential function of a
+log(a)   : returns the natural logaritm of a
+*/
+bool CExpressionCalculator::Parse1ArgFunc( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	const char *pFuncs[] = 
+	{
+		"abs", "sqr", "sqrt", "sin", "asin", "cos", "acos", "tan",
+		"exp", "log", "dtor", "rtod", "floor", "ceiling", "round", "sign"
+	};
+	int nFunc = 16;
+
+	if ( ParseStringList( expr, pFuncs, nFunc ) &&
+		ParseString( expr, "(" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ")" ) )
+	{
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nFunc )
+		{
+		case 0: // abs
+			m_stack.Push( fabs( f1 ) );
+			break;
+		case 1: // sqr
+			m_stack.Push( f1 * f1 );
+			break;
+		case 2: // sqrt
+			m_stack.Push( sqrt( f1 ) );
+			break;
+		case 3: // sin
+			m_stack.Push( sin( f1 ) );
+			break;
+		case 4: // asin
+			m_stack.Push( asin( f1 ) );
+			break;
+		case 5: // cos
+			m_stack.Push( cos( f1 ) );
+			break;
+		case 6: // acos
+			m_stack.Push( acos( f1 ) );
+			break;
+		case 7: // tan
+			m_stack.Push( tan( f1 ) );
+			break;
+		case 8: // exp
+			m_stack.Push( exp( f1 ) );
+			break;
+		case 9: // log
+			m_stack.Push( log( f1 ) );
+			break;
+		case 10: // dtor
+			m_stack.Push( DEG2RAD( f1 ) );
+			break;
+		case 11: // rtod
+			m_stack.Push( RAD2DEG( f1 ) );
+			break;
+		case 12: // floor
+			m_stack.Push( floor( f1 ) );
+			break;
+		case 13: // ceiling
+			m_stack.Push( ceil( f1 ) );
+			break;
+		case 14: // round
+			m_stack.Push( floor( f1 + 0.5f ) );
+			break;
+		case 15: // sign
+			m_stack.Push( f1 >= 0.0f ? 1.0f : -1.0f );
+			break;
+		}
+		return true;
+	}
+	return false;
+}
+
+/*
+min(a,b)     : if a<b returns a else b
+max(a,b)     : if a>b returns a else b
+atan2(a,b) : atan2(a/b) returns degrees
+pow(a,b) : function returns a raised to the power of b
+*/
+bool CExpressionCalculator::Parse2ArgFunc( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	const char *pFuncs[] = { "min", "max", "atan2", "pow" };
+	int nFunc = 4;
+
+	if ( ParseStringList( expr, pFuncs, nFunc ) &&
+		ParseString( expr, "(" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ")" ) )
+	{
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nFunc )
+		{
+		case 0: // min
+			m_stack.Push( MIN( f1, f2 ) );
+			break;
+		case 1: // max
+			m_stack.Push( MAX( f1, f2 ) );
+			break;
+		case 2: // atan2
+			m_stack.Push( atan2( f1, f2 ) );
+			break;
+		case 3: // pow
+			m_stack.Push( pow( f1, f2 ) );
+			break;
+		}
+		return true;
+	}
+	return false;
+}
+
+/*
+inrange(x,a,b) : if x is between a and b, returns 1 else returns 0
+clamp(x,a,b)   : see bound() above
+
+ramp(value,a,b)        : returns 0 -> 1 as value goes from a to b
+lerp(factor,a,b)       : returns a -> b as value goes from 0 to 1
+
+cramp(value,a,b)        : clamp(ramp(value,a,b),0,1)
+clerp(factor,a,b)       : clamp(lerp(factor,a,b),a,b)
+
+elerp(x,a,b)         : ramp( 3*x*x - 2*x*x*x, a, b)
+//elerp(factor,a,b)    : lerp(lerp(sind(clerp(factor,-90,90)),0.5,1.0),a,b)
+
+noise(a,b,c) : { solid noise pattern (improved perlin noise) indexed with three numbers }
+*/
+
+float ramp( float x, float a, float b )
+{
+	return ( x - a ) / ( b - a );
+}
+
+float lerp( float x, float a, float b )
+{
+	return a + x * ( b - a );
+}
+
+float smoothstep( float x )
+{
+	return 3*x*x - 2*x*x*x;
+}
+
+bool CExpressionCalculator::Parse3ArgFunc( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	const char *pFuncs[] = { "inrange", "clamp", "ramp", "lerp", "cramp", "clerp", "elerp", "noise" };
+	int nFunc = 8;
+
+	if ( ParseStringList( expr, pFuncs, nFunc ) &&
+		ParseString( expr, "(" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ")" ) )
+	{
+		float f3 = m_stack.Top();
+		m_stack.Pop();
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nFunc )
+		{
+		case 0: // inrange
+			m_stack.Push( ( f1 >= f2 ) && ( f1 <= f3 ) ? 1.0f : 0.0f );
+			break;
+		case 1: // clamp
+			m_stack.Push( clamp( f1, f2, f3 ) );
+			break;
+		case 2: // ramp
+			m_stack.Push( ramp( f1, f2, f3 ) );
+			break;
+		case 3: // lerp
+			m_stack.Push( lerp( f1, f2, f3 ) );
+			break;
+		case 4: // cramp
+			m_stack.Push( clamp( ramp( f1, f2, f3 ), 0, 1 ) );
+			break;
+		case 5: // clerp
+			m_stack.Push( clamp( lerp( f1, f2, f3 ), f2, f3 ) );
+			break;
+		case 6: // elerp
+			m_stack.Push( lerp( smoothstep( f1 ), f2, f3 ) );
+			break;
+		case 7: // noise
+			m_stack.Push( ImprovedPerlinNoise( Vector( f1, f2, f3 ) ) );
+			break;
+		}
+		return true;
+	}
+	return false;
+}
+
+//bool CExpressionCalculator::Parse4ArgFunc( const char *&expr );
+
+/*
+rescale (X,Xa,Xb,Ya,Yb) : lerp(ramp(X,Xa,Xb),Ya,Yb)
+crescale(X,Xa,Xb,Ya,Yb) : clamp(rescale(X,Xa,Xb,Ya,Yb),Ya,Yb)
+*/
+float rescale( float x, float a, float b, float c, float d )
+{
+	return lerp( ramp( x, a, b ), c, d );
+}
+
+bool CExpressionCalculator::Parse5ArgFunc( const char *&expr )
+{
+	ParseState_t ps( m_stack, expr );
+
+	const char *pFuncs[] = { "rescale", "crescale" };
+	int nFunc = 2;
+
+	if ( ParseStringList( expr, pFuncs, nFunc ) &&
+		ParseString( expr, "(" ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, "," ) &&
+		ParseExpr( expr ) &&
+		ParseString( expr, ")" ) )
+	{
+		float f5 = m_stack.Top();
+		m_stack.Pop();
+		float f4 = m_stack.Top();
+		m_stack.Pop();
+		float f3 = m_stack.Top();
+		m_stack.Pop();
+		float f2 = m_stack.Top();
+		m_stack.Pop();
+		float f1 = m_stack.Top();
+		m_stack.Pop();
+		switch ( nFunc )
+		{
+		case 0: // rescale
+			m_stack.Push( rescale( f1, f2, f3, f4, f5 ) );
+			break;
+		case 1: // crescale
+			m_stack.Push( clamp( rescale( f1, f2, f3, f4, f5 ), f4, f5 ) );
+			break;
+		}
+		return true;
+	}
+	return false;
+}
+
+CExpressionCalculator::CExpressionCalculator( const CExpressionCalculator& x )
+{
+	*this = x;
+}
+
+CExpressionCalculator& CExpressionCalculator::operator=( const CExpressionCalculator& x )
+{
+	m_expr = x.m_expr;
+	m_varNames = x.m_varNames;
+	m_varValues = x.m_varValues;
+	m_stack.CopyFrom( x.m_stack );
+	m_bIsBuildingArgumentList = x.m_bIsBuildingArgumentList;
+	return *this;
+}
+
+float EvaluateExpression( char const *pExpr, float flValueToReturnIfFailure )
+{
+	CExpressionCalculator myEvaluator( pExpr );
+	float flResult;
+	bool bSuccess = myEvaluator.Evaluate( flResult );
+	return ( bSuccess ) ? flResult : flValueToReturnIfFailure;
+}
+

mathlib/feagglomerator.cpp

@@ -0,0 +1,348 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+#include "feagglomerator.h"
+#include "bitvec.h"
+
+CFeAgglomerator::CFeAgglomerator( uint nReserveNodes )
+{
+	m_Clusters.EnsureCapacity( nReserveNodes * 2 );
+	m_Clusters.SetCount( nReserveNodes );
+	m_Clusters.FillWithValue( NULL ); // client needs to set all the nodes
+/*
+	for ( uint i = 0; i < nReserveNodes; ++i )
+	{
+		m_Clusters[ i ] = new CCluster;
+	}
+*/
+}
+
+
+CFeAgglomerator::~CFeAgglomerator()
+{
+	m_Clusters.PurgeAndDeleteElements();
+}
+
+
+
+CFeAgglomerator::CCluster::CCluster( int nIndex, int nChildLeafs ) :m_nParent( -1 ), m_nIndex( nIndex ), m_nChildLeafs( nChildLeafs )
+{
+	m_nChild[ 0 ] = -1;
+	m_nChild[ 1 ] = -1;
+}
+
+
+bool CFeAgglomerator::CCluster::HasLinks()const
+{
+	return m_Links.Count() > 0;
+}
+
+float CFeAgglomerator::CCluster::GetBestCost()const
+{
+	if ( HasLinks() )
+	{
+		return m_Links.ElementAtHead().m_flCost;
+	}
+	else
+	{
+		return FLT_MAX;
+	}
+}
+
+void CFeAgglomerator::CCluster::RemoveLink( CCluster *pOtherCluster )
+{
+	for ( int i = 0; i < m_Links.Count(); ++i )
+	{
+		if ( m_Links.Element( i ).m_pOtherCluster == pOtherCluster )
+		{
+			m_Links.RemoveAt( i );
+			return;
+		}
+	}
+	Assert( !"Not found" );
+}
+
+const CFeAgglomerator::CLink *CFeAgglomerator::CCluster::FindLink( CCluster *pOtherCluster )
+{
+	for ( int i = 0; i < m_Links.Count(); ++i )
+	{
+		const CLink &link = m_Links.Element( i );
+		if ( link.m_pOtherCluster == pOtherCluster )
+		{
+			return &link;
+		}
+	}
+	return NULL;
+}
+
+//
+// The cost of a node of the tree is the probability of its collision with another bounding box, times number of points to test.
+// The probably of collision is the probability of their minkowski sum containing the origin, which is proportional to the volume of the minkowski sum.
+// It boils down to guessing the size of the other bounding box. Having no better heuristc, we can just say it'll probably be a box of average size 12x12x12 or something
+// 
+float CFeAgglomerator::CCluster::ComputeCost( const Vector &vSize, int nChildLeafs )
+{
+	return ( vSize.x + 12 ) * ( vSize.y + 12 ) * ( vSize.z + 12 ) * nChildLeafs;
+}
+
+void CFeAgglomerator::CCluster::AddLink( CCluster *pOtherCluster )
+{
+#ifdef _DEBUG
+	for ( int i = 0; i < m_Links.Count(); i++ )
+	{
+		Assert( m_Links.Element( i ).m_pOtherCluster != pOtherCluster );
+	}
+#endif
+
+	CLink link;
+	link.m_pOtherCluster = pOtherCluster;
+	//
+	// Note: GetSurfaceArea() is not a valid heuristic for cost here. E.g. 2 horizontal points will have 0 cost, which is clearly wrong
+	// (m_Aabb + pOtherCluster->m_Aabb ).GetSurfaceArea()
+	//
+	link.m_flCost = ComputeCost( ( m_Aabb + pOtherCluster->m_Aabb ).GetSize(), m_nChildLeafs + pOtherCluster->m_nChildLeafs );
+	m_Links.Insert( link );
+}
+
+
+
+void CFeAgglomerator::AddLink( CCluster* pCluster0, CCluster *pCluster1, ClustersPriorityQueue_t &queue )
+{
+	float flBestDist0 = pCluster0->GetBestCost();
+	float flBestDist1 = pCluster1->GetBestCost();
+
+	pCluster0->AddLink( pCluster1 );
+	pCluster1->AddLink( pCluster0 );
+
+	float flNewBestDist0 = pCluster0->GetBestCost();
+	float flNewBestDist1 = pCluster1->GetBestCost();
+	
+	if ( flNewBestDist0 != flBestDist0 )
+	{
+		queue.RevaluateElement( pCluster0->m_nPriorityIndex );
+	}
+
+	if ( flNewBestDist1 != flBestDist1 )
+	{
+		queue.RevaluateElement( pCluster1->m_nPriorityIndex );
+	}
+}
+
+// register a link between the nodes. 
+// Call this to register all links between all nodes before building agglomerated clusters
+void CFeAgglomerator::LinkNodes( int nNode0, int nNode1 )
+{
+	if ( nNode0 == nNode1 )
+		return;
+	CCluster* pCluster0 = m_Clusters[ nNode0 ];
+	CCluster *pCluster1 = m_Clusters[ nNode1 ];
+	if ( pCluster0->FindLink( pCluster1 ) )
+	{
+		AssertDbg( pCluster1->FindLink( pCluster0 ) );
+	}
+	else
+	{
+		// link not duplicated, create new link
+		pCluster0->AddLink( pCluster1 );
+		pCluster1->AddLink( pCluster0 );
+	}
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
+// Build agglomerated clusters; cannot call this function twice, because it will try to agglomerate clusters at all levels (nodes to root) the 2nd time
+// 
+void CFeAgglomerator::Build( bool bSingleRoot )
+{
+	ClustersPriorityQueue_t queue;
+	for ( int i = 0; i < m_Clusters.Count(); ++i )
+	{
+		queue.Insert( m_Clusters[ i ] );
+	}
+	Process( queue );
+	// create links of all-with-all
+	// this will be ultra-slow if we have degenerate case (like 1000 small disconnected pieces of cloth)
+	if ( bSingleRoot && queue.Count() > 1 )
+	{
+		for ( int i = queue.Count(); i-- > 1; )
+		{
+			for ( int j = i; j-- > 0; )
+			{
+				queue.Element( i )->AddLink( queue.Element( j ) );
+				queue.Element( j )->AddLink( queue.Element( i ) );
+			}			
+		}
+		// the old queue order is destroyed; create a new queue (we could re-heapify the old queue
+		ClustersPriorityQueue_t queue2;
+		for ( int i = queue.Count(); i-- > 0; )
+		{
+			queue2.Insert( queue.Element( i ) );
+		}
+
+		Process( queue2 );
+		Assert( queue2.Count() == 1 );
+	}
+}
+
+
+void CFeAgglomerator::Validate( ClustersPriorityQueue_t *pQueue )
+{
+#ifdef _DEBUG
+	if ( pQueue )
+	{
+		Assert( pQueue->IsHeapified() );
+		CVarBitVec used( m_Clusters.Count() );
+		for ( int i = 0; i < pQueue->Count(); ++i )
+		{
+			int nCluster = pQueue->Element( i )->m_nIndex;
+			Assert( !used.IsBitSet( nCluster ) );
+			used.Set( nCluster );
+		}
+		for ( int i = 0; i < m_Clusters.Count(); ++i )
+		{
+			CCluster *pCluster = m_Clusters[ i ];
+			if ( !used.IsBitSet( i ) )
+			{
+				Assert( pCluster->m_Links.Count() == 0 );
+			}
+		}
+	}
+
+	for ( int nIndex = 0; nIndex < m_Clusters.Count(); ++nIndex )
+	{
+		CCluster *pThisCluster = m_Clusters[ nIndex ];
+		Assert( pThisCluster->m_nIndex == nIndex );
+		if ( pThisCluster->m_nChild[ 0 ] < 0 )
+		{
+			Assert( pThisCluster->m_nChild[ 1 ] < 0 && pThisCluster->m_nChildLeafs == 1 );
+		}
+		else
+		{
+			Assert( m_Clusters[ pThisCluster->m_nChild[ 0 ] ]->m_nChildLeafs + m_Clusters[ pThisCluster->m_nChild[ 1 ] ]->m_nChildLeafs == pThisCluster->m_nChildLeafs );
+		}
+
+		ClusterLinkQueue_t &links = pThisCluster->m_Links;
+		Assert( links.IsHeapified() );
+		CVarBitVec used( m_Clusters.Count() );
+		for ( int i = 0; i < links.Count(); ++i )
+		{
+			CLink *pThisLink = &links.Element( i );
+			CCluster *pOtherCluster = pThisLink->m_pOtherCluster;
+			const CLink *pOtherLink = pOtherCluster->FindLink( pThisCluster );
+			Assert( pOtherLink );
+			Assert( pOtherLink->m_pOtherCluster == pThisCluster && pThisLink->m_flCost == pOtherLink->m_flCost ); // can't have a link with self & the cost of link should be the same from both sides
+			Assert( !used.IsBitSet( pOtherCluster->m_nIndex ) );// can't have 2 links to the same cluster
+			used.Set( pOtherCluster->m_nIndex ); 
+		}
+	}
+#endif
+}
+
+
+void CFeAgglomerator::Process( ClustersPriorityQueue_t &queue )
+{
+	while ( queue.Count() > 0 && queue.ElementAtHead()->HasLinks() )
+	{
+		Validate( &queue );
+
+		// remove the clusters we're merging from priority queue
+		CCluster *pChild[ 2 ];
+		pChild[ 0 ] = queue.ElementAtHead();
+		pChild[ 1 ] = queue.ElementAtHead()->m_Links.ElementAtHead().m_pOtherCluster;
+
+		// remove the children from the queue
+		Assert( pChild[ 0 ]->m_nPriorityIndex == 0 );
+		queue.RemoveAtHead(); // removing pChild[0]
+		queue.RemoveAt( pChild[ 1 ]->m_nPriorityIndex );
+
+		pChild[ 0 ]->m_nPriorityIndex = -1;
+		pChild[ 1 ]->m_nPriorityIndex = -1;
+
+		// make the new cluster, link and compute its distances to nearest clusters
+		int nParentIndex = m_Clusters.AddToTail();
+		CCluster *pParent = new CCluster( nParentIndex, pChild[ 0 ]->m_nChildLeafs + pChild[ 1 ]->m_nChildLeafs );
+		m_Clusters[ nParentIndex ] = pParent ;
+		pParent->m_Aabb = pChild[ 0 ]->m_Aabb + pChild[ 1 ]->m_Aabb;
+		pParent->m_nChild[ 0 ] = pChild[ 0 ]->m_nIndex;
+		pParent->m_nChild[ 1 ] = pChild[ 1 ]->m_nIndex;
+
+
+		pChild[ 0 ]->m_nParent = nParentIndex;
+		pChild[ 1 ]->m_nParent = nParentIndex;
+
+
+		CUtlVectorFixedGrowable< CCluster*, 8 >	reAdd;
+		CVarBitVec skipAddLink( m_Clusters.Count() );
+		// remove all links to the children, replace them with links to the parent
+		{
+			ClusterLinkQueue_t &links = pChild[ 0 ]->m_Links;
+			for ( int i = 0; i < links.Count(); ++i )
+			{
+				CCluster *pOther = links.Element( i ).m_pOtherCluster;
+				Assert( pOther != pChild[ 0 ] );
+				if ( pOther == pChild[ 1 ] )
+					continue; // just skip the connection to the other child
+				Assert( pOther->m_nPriorityIndex >= 0 );
+				// we see this cluster for the first time
+				float flOldDistance = pOther->GetBestCost();
+				pOther->RemoveLink( pChild[ 0 ] );
+				pOther->AddLink( pParent );
+				pParent->AddLink( pOther );
+				skipAddLink.Set( pOther->m_nIndex );
+				float flNewDistance = pOther->GetBestCost();
+				if ( flOldDistance != flNewDistance )
+				{
+					reAdd.AddToTail( pOther );
+					queue.RemoveAt( pOther->m_nPriorityIndex );
+					pOther->m_nPriorityIndex = -1;
+				}
+			}
+			links.Purge(); // the links don't matter any more, we can free the memory
+		}
+		{
+			ClusterLinkQueue_t &links = pChild[ 1 ]->m_Links;
+			for ( int i = 0; i < links.Count(); ++i )
+			{
+				CCluster *pOther = links.Element( i ).m_pOtherCluster;
+				Assert( pOther != pChild[ 1 ] );
+				if ( pOther == pChild[ 0 ] )
+					continue; // just skip the connection to the other child
+				if ( pOther->m_nPriorityIndex >= 0 )
+				{
+					// we see this cluster for the first time
+					float flOldDistance = pOther->GetBestCost();
+					pOther->RemoveLink( pChild[ 1 ] );
+					// if we saw this pOther already, and didn't remove it from the queue, then we marked it as added
+					if ( !skipAddLink.IsBitSet( pOther->m_nIndex ) )
+					{
+						pOther->AddLink( pParent );
+						pParent->AddLink( pOther );
+					}
+					float flNewDistance = pOther->GetBestCost();
+					if ( flOldDistance != flNewDistance )
+					{
+						queue.RevaluateElement( pOther->m_nPriorityIndex ); // no need to remove, this is the first and last edit of this other element
+					}
+				}
+				else
+				{
+					// we've seen this cluster before within this loop, just remove the link; we already added the new link to the new parent
+					pOther->RemoveLink( pChild[ 1 ] );
+				}
+			}
+			links.Purge(); // the links don't matter any more, we can free the memory
+		}
+
+		for ( int nCluster = 0; nCluster < reAdd.Count(); ++nCluster )
+		{
+			queue.Insert( reAdd[nCluster] );
+		}
+		queue.Insert( pParent );
+		
+		Validate( &queue );
+	}
+
+	for ( int i = 0; i < queue.Count(); ++i )
+	{
+		Assert( !queue.Element( i )->HasLinks() );
+	}
+}
+
+

mathlib/femodeldesc.cpp

@@ -0,0 +1,351 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+#include "mathlib/femodeldesc.h"
+#include "mathlib/femodel.h"
+#include "tier1/heapsort.h"
+#include "tier1/fmtstr.h"
+
+template < typename T >
+inline CLockedResource< T > CloneArrayWithMarkers( CResourceStream *pStream, const T *pArray, uint nCount, const char *pName )
+{
+	// create the marker for the start of the array
+	CFmtStr beginMsg( "Begin of %s, %d byte aligned, here: <", pName, VALIGNOF( T ) );
+	// align the marker so that it ends aligned, right before the array data
+	int nBeginMsgLength = beginMsg.Length( );
+	int nPreAlign = ( -int( pStream->GetTotalSize() + nBeginMsgLength ) ) & ( VALIGNOF( T ) - 1 );
+	V_memset( pStream->AllocateBytes( nPreAlign ), '-', nPreAlign );
+	void *pPrefixData = pStream->AllocateBytes( nBeginMsgLength );
+	Assert( !( ( uintp( pPrefixData ) + nBeginMsgLength ) & ( VALIGNOF( T ) - 1 ) ) );
+	V_memcpy( pPrefixData, beginMsg.Get(), nBeginMsgLength );
+	// write out the array
+	CLockedResource< T > result = CloneArray( pStream, pArray, nCount );
+	// add the end marker
+	pStream->WriteString( CFmtStr( "> End of %s, %d bytes total.", pName, nCount * sizeof( T ) ) );
+	return result;
+}
+
+#if 0//def _DEBUG
+#define CloneArray( STREAM, ARRAY, COUNT ) CloneArrayWithMarkers( (STREAM), (ARRAY), (COUNT), #ARRAY ); 
+#endif
+
+
+
+CLockedResource< PhysFeModelDesc_t > Clone( CFeModel *pFeModel, CResourceStream *pStream )
+{
+	CLockedResource< PhysFeModelDesc_t > pFx = pStream->Allocate< PhysFeModelDesc_t >( );
+	uint nDynamicNodes = pFeModel->m_nNodeCount - pFeModel->m_nStaticNodes;
+
+	pFx->m_flLocalForce = pFeModel->m_flLocalForce;
+	pFx->m_flLocalRotation   = pFeModel->m_flLocalRotation;
+
+	pFx->m_nStaticNodeFlags  = pFeModel->m_nStaticNodeFlags;
+	pFx->m_nDynamicNodeFlags = pFeModel->m_nDynamicNodeFlags;
+	pFx->m_nNodeCount      = pFeModel->m_nNodeCount;
+	pFx->m_nStaticNodes    = pFeModel->m_nStaticNodes;
+	pFx->m_nRotLockStaticNodes = pFeModel->m_nRotLockStaticNodes;
+	pFx->m_nSimdTriCount1  = pFeModel->m_nSimdTriCount[ 1 ];
+	pFx->m_nSimdTriCount2  = pFeModel->m_nSimdTriCount[ 2 ];
+	pFx->m_nSimdQuadCount1 = pFeModel->m_nSimdQuadCount[ 1 ];
+	pFx->m_nSimdQuadCount2 = pFeModel->m_nSimdQuadCount[ 2 ];
+	pFx->m_nQuadCount1     = pFeModel->m_nQuadCount[ 1 ];
+	pFx->m_nQuadCount2     = pFeModel->m_nQuadCount[ 2 ];
+	pFx->m_nFitMatrixCount1 = pFeModel->m_nFitMatrixCount[ 1 ];
+	pFx->m_nFitMatrixCount2 = pFeModel->m_nFitMatrixCount[ 2 ];
+	pFx->m_nSimdFitMatrixCount1 = pFeModel->m_nSimdFitMatrixCount[ 1 ];
+	pFx->m_nSimdFitMatrixCount2 = pFeModel->m_nSimdFitMatrixCount[ 2 ];
+	pFx->m_nRopeCount = pFeModel->m_nRopeCount;
+	pFx->m_nTreeDepth      = pFeModel->m_nTreeDepth;
+	pFx->m_flDefaultSurfaceStretch = pFeModel->m_flDefaultSurfaceStretch;
+	pFx->m_flDefaultThreadStretch  = pFeModel->m_flDefaultThreadStretch;
+
+	pFx->m_flDefaultGravityScale = pFeModel->m_flDefaultGravityScale;
+	pFx->m_flDefaultVelAirDrag = pFeModel->m_flDefaultVelAirDrag;
+	pFx->m_flDefaultExpAirDrag = pFeModel->m_flDefaultExpAirDrag;
+	pFx->m_flDefaultVelQuadAirDrag = pFeModel->m_flDefaultVelQuadAirDrag;
+	pFx->m_flDefaultExpQuadAirDrag = pFeModel->m_flDefaultExpQuadAirDrag;
+	pFx->m_flDefaultVelRodAirDrag = pFeModel->m_flDefaultVelRodAirDrag;
+	pFx->m_flDefaultExpRodAirDrag = pFeModel->m_flDefaultExpRodAirDrag;
+	pFx->m_flQuadVelocitySmoothRate = pFeModel->m_flQuadVelocitySmoothRate;
+	pFx->m_flRodVelocitySmoothRate = pFeModel->m_flRodVelocitySmoothRate;
+	pFx->m_flAddWorldCollisionRadius = pFeModel->m_flAddWorldCollisionRadius;
+	pFx->m_nQuadVelocitySmoothIterations = pFeModel->m_nQuadVelocitySmoothIterations;
+	pFx->m_nRodVelocitySmoothIterations = pFeModel->m_nRodVelocitySmoothIterations;
+	pFx->m_flDefaultVolumetricSolveAmount = pFeModel->m_flDefaultVolumetricSolveAmount;
+	pFx->m_flWindage = pFeModel->m_flWindage;
+	pFx->m_flWindDrag = pFeModel->m_flWindDrag;
+
+	pFx->m_SimdQuads     = CloneArray( pStream, pFeModel->m_pSimdQuads, pFeModel->m_nSimdQuadCount[ 0 ] );
+	pFx->m_SimdTris      = CloneArray( pStream, pFeModel->m_pSimdTris, pFeModel->m_nSimdTriCount[ 0 ] );
+	pFx->m_SimdRods      = CloneArray( pStream, pFeModel->m_pSimdRods, pFeModel->m_nSimdRodCount );
+	pFx->m_SimdNodeBases = CloneArray( pStream, pFeModel->m_pSimdNodeBases, pFeModel->m_nSimdNodeBaseCount );
+	pFx->m_SimdFitMatrices = CloneArray( pStream, pFeModel->m_pSimdFitMatrices, pFeModel->m_nSimdFitMatrixCount[ 0 ] );
+	pFx->m_FitMatrices = CloneArray( pStream, pFeModel->m_pFitMatrices, pFeModel->m_nFitMatrixCount[ 0 ] );
+	pFx->m_Quads = CloneArray( pStream, pFeModel->m_pQuads, pFeModel->m_nQuadCount[ 0 ] );
+	pFx->m_CtrlOffsets   = CloneArray( pStream, pFeModel->m_pCtrlOffsets, pFeModel->m_nCtrlOffsets );
+	pFx->m_CtrlOsOffsets = CloneArray( pStream, pFeModel->m_pCtrlOsOffsets, pFeModel->m_nCtrlOsOffsets );
+	pFx->m_Rods          = CloneArray( pStream, pFeModel->m_pRods, pFeModel->m_nRodCount );
+	pFx->m_AxialEdges	 = CloneArray( pStream, pFeModel->m_pAxialEdges, pFeModel->m_nAxialEdgeCount );
+	pFx->m_Ropes         = CloneArray( pStream, pFeModel->m_pRopes, pFeModel->m_nRopeIndexCount );
+	pFx->m_NodeBases     = CloneArray( pStream, pFeModel->m_pNodeBases, pFeModel->m_nNodeBaseCount );
+	pFx->m_SpringIntegrator      = CloneArray( pStream, pFeModel->m_pSpringIntegrator, pFeModel->m_nSpringIntegratorCount );
+	pFx->m_SimdSpringIntegrator  = CloneArray( pStream, pFeModel->m_pSimdSpringIntegrator, pFeModel->m_nSimdSpringIntegratorCount );
+	pFx->m_InitPose		 = CloneArray( pStream, pFeModel->m_pInitPose, pFeModel->m_nCtrlCount );
+	pFx->m_FollowNodes   = CloneArray( pStream, pFeModel->m_pFollowNodes, pFeModel->m_nFollowNodeCount ); 
+	pFx->m_CollisionSpheres = CloneArray( pStream, pFeModel->m_pCollisionSpheres, pFeModel->m_nCollisionSpheres[ 0 ] );
+	pFx->m_CollisionPlanes = CloneArray( pStream, pFeModel->m_pCollisionPlanes, pFeModel->m_nCollisionPlanes );
+	pFx->m_NodeCollisionRadii = CloneArray( pStream, pFeModel->m_pNodeCollisionRadii, nDynamicNodes );
+	pFx->m_LocalRotation = CloneArray( pStream, pFeModel->m_pLocalRotation, nDynamicNodes );
+	pFx->m_LocalForce = CloneArray( pStream, pFeModel->m_pLocalForce, nDynamicNodes );
+	pFx->m_FitWeights = CloneArray( pStream, pFeModel->m_pFitWeights, pFeModel->m_nFitWeightCount );
+	pFx->m_nCollisionSphereInclusiveCount = pFeModel->m_nCollisionSpheres[ 1 ];
+	pFx->m_WorldCollisionParams = CloneArray( pStream, pFeModel->m_pWorldCollisionParams, pFeModel->m_nWorldCollisionParamCount );
+	pFx->m_TaperedCapsuleStretches = CloneArray( pStream, pFeModel->m_pTaperedCapsuleStretches, pFeModel->m_nTaperedCapsuleStretchCount );
+	pFx->m_TaperedCapsuleRigids = CloneArray( pStream, pFeModel->m_pTaperedCapsuleRigids, pFeModel->m_nTaperedCapsuleRigidCount );
+	pFx->m_SphereRigids = CloneArray( pStream, pFeModel->m_pSphereRigids, pFeModel->m_nSphereRigidCount );
+	pFx->m_TreeChildren = CloneArray( pStream, pFeModel->m_pTreeChildren, nDynamicNodes - 1 );
+	pFx->m_TreeParents = CloneArray( pStream, pFeModel->m_pTreeParents, nDynamicNodes + nDynamicNodes - 1 );
+	pFx->m_TreeCollisionMasks = CloneArray( pStream, pFeModel->m_pTreeCollisionMasks, nDynamicNodes + nDynamicNodes - 1 );
+	pFx->m_WorldCollisionNodes = CloneArray( pStream, pFeModel->m_pWorldCollisionNodes, pFeModel->m_nWorldCollisionNodeCount );
+	pFx->m_FreeNodes = CloneArray( pStream, pFeModel->m_pFreeNodes, pFeModel->m_nFreeNodeCount );
+	pFx->m_ReverseOffsets = CloneArray( pStream, pFeModel->m_pReverseOffsets, pFeModel->m_nReverseOffsetCount );
+
+	if ( pFeModel->m_pLegacyStretchForce )
+	{
+		pFx->m_LegacyStretchForce = CloneArray( pStream, pFeModel->m_pLegacyStretchForce, pFeModel->m_nNodeCount );
+	}
+	if ( pFeModel->m_pNodeIntegrator )
+	{
+		pFx->m_NodeIntegrator = CloneArray( pStream, pFeModel->m_pNodeIntegrator, pFeModel->m_nNodeCount );
+	}
+	pFx->m_NodeInvMasses = CloneArray( pStream, pFeModel->m_pNodeInvMasses, pFeModel->m_nNodeCount );
+
+	if ( pFeModel->m_pCtrlHash )
+	{
+		pFx->m_CtrlHash = CloneArray( pStream, pFeModel->m_pCtrlHash, pFeModel->m_nCtrlCount );
+	}
+	if ( pFeModel->m_pCtrlName )
+	{
+		pFx->m_CtrlName = pStream->Allocate< CResourceString >( pFeModel->m_nCtrlCount );
+		for ( uint i = 0; i < pFeModel->m_nCtrlCount; ++i )
+		{
+			pFx->m_CtrlName[ i ] = pStream->WriteString( pFeModel->m_pCtrlName[ i ] );
+		}
+	}
+
+	return pFx;
+}
+
+
+
+
+void Clone( const PhysFeModelDesc_t *pFeDesc, intp nOffsetBytes, char **pCtrlNames, CFeModel *pFeModel )
+{
+	pFeModel->m_nDynamicNodeFlags = pFeDesc->m_nDynamicNodeFlags;
+	pFeModel->m_nStaticNodeFlags = pFeDesc->m_nStaticNodeFlags;
+	pFeModel->m_flLocalForce = pFeDesc->m_flLocalForce;
+	pFeModel->m_flLocalRotation = pFeDesc->m_flLocalRotation;
+
+	pFeModel->m_nAxialEdgeCount = pFeDesc->m_AxialEdges.Count();
+	pFeModel->m_nCtrlCount = pFeDesc->m_CtrlHash.Count();
+	pFeModel->m_nNodeCount = pFeDesc->m_nNodeCount;
+	pFeModel->m_nStaticNodes = pFeDesc->m_nStaticNodes;
+	pFeModel->m_nRotLockStaticNodes = pFeDesc->m_nRotLockStaticNodes;
+	AssertDbg( pFeModel->m_nRotLockStaticNodes <= pFeModel->m_nStaticNodes );
+	pFeModel->m_nTreeDepth = pFeDesc->m_nTreeDepth;
+	// no scalar data
+	pFeModel->m_nQuadCount[ 0 ] = 0;
+	pFeModel->m_nQuadCount[ 1 ] = 0;
+	pFeModel->m_nQuadCount[ 2 ] = 0;
+	pFeModel->m_nTriCount[ 0 ] = 0;
+	pFeModel->m_nTriCount[ 1 ] = 0;
+	pFeModel->m_nTriCount[ 2 ] = 0;
+	pFeModel->m_nSimdQuadCount[ 0 ] = pFeDesc->m_SimdQuads.Count();
+	pFeModel->m_nSimdQuadCount[ 1 ] = pFeDesc->m_nSimdQuadCount1;
+	pFeModel->m_nSimdQuadCount[ 2 ] = pFeDesc->m_nSimdQuadCount2;
+	pFeModel->m_nSimdTriCount[ 0 ] = pFeDesc->m_SimdTris.Count();
+	pFeModel->m_nSimdTriCount[ 1 ] = pFeDesc->m_nSimdTriCount1;
+	pFeModel->m_nSimdTriCount[ 2 ] = pFeDesc->m_nSimdTriCount2;
+	pFeModel->m_nQuadCount[ 0 ] = pFeDesc->m_Quads.Count();
+	pFeModel->m_nQuadCount[ 1 ] = pFeDesc->m_nQuadCount1;
+	pFeModel->m_nQuadCount[ 2 ] = pFeDesc->m_nQuadCount2;
+	pFeModel->m_nFitMatrixCount[ 0 ] = pFeDesc->m_FitMatrices.Count();
+	pFeModel->m_nFitMatrixCount[ 1 ] = pFeDesc->m_nFitMatrixCount1;
+	pFeModel->m_nFitMatrixCount[ 2 ] = pFeDesc->m_nFitMatrixCount2;
+	pFeModel->m_nSimdFitMatrixCount[ 0 ] = pFeDesc->m_SimdFitMatrices.Count();
+	pFeModel->m_nSimdFitMatrixCount[ 1 ] = pFeDesc->m_nSimdFitMatrixCount1;
+	pFeModel->m_nSimdFitMatrixCount[ 2 ] = pFeDesc->m_nSimdFitMatrixCount2;
+	pFeModel->m_flDefaultSurfaceStretch = pFeDesc->m_flDefaultSurfaceStretch;
+	pFeModel->m_flDefaultThreadStretch = pFeDesc->m_flDefaultThreadStretch;
+	pFeModel->m_flDefaultGravityScale = pFeDesc->m_flDefaultGravityScale;
+	pFeModel->m_flDefaultVelAirDrag = pFeDesc->m_flDefaultVelAirDrag;
+	pFeModel->m_flDefaultExpAirDrag = pFeDesc->m_flDefaultExpAirDrag;
+	pFeModel->m_flDefaultVelQuadAirDrag = pFeDesc->m_flDefaultVelQuadAirDrag;
+	pFeModel->m_flDefaultExpQuadAirDrag = pFeDesc->m_flDefaultExpQuadAirDrag;
+	pFeModel->m_flDefaultVelRodAirDrag = pFeDesc->m_flDefaultVelRodAirDrag;
+	pFeModel->m_flDefaultExpRodAirDrag = pFeDesc->m_flDefaultExpRodAirDrag;
+	pFeModel->m_flQuadVelocitySmoothRate = pFeDesc->m_flQuadVelocitySmoothRate;
+	pFeModel->m_flRodVelocitySmoothRate = pFeDesc->m_flRodVelocitySmoothRate;
+	pFeModel->m_nQuadVelocitySmoothIterations = pFeDesc->m_nQuadVelocitySmoothIterations;
+	pFeModel->m_nRodVelocitySmoothIterations = pFeDesc->m_nRodVelocitySmoothIterations;
+	pFeModel->m_flAddWorldCollisionRadius = pFeDesc->m_flAddWorldCollisionRadius;
+	pFeModel->m_flDefaultVolumetricSolveAmount = pFeDesc->m_flDefaultVolumetricSolveAmount;
+	pFeModel->m_nFitWeightCount = pFeDesc->m_FitWeights.Count();
+	pFeModel->m_nReverseOffsetCount = pFeDesc->m_ReverseOffsets.Count();
+	pFeModel->m_flWindage = pFeDesc->m_flWindage;
+	pFeModel->m_flWindDrag = pFeDesc->m_flWindDrag;
+
+	pFeModel->m_nRodCount = pFeDesc->m_Rods.Count();
+	pFeModel->m_nSimdRodCount = pFeDesc->m_SimdRods.Count();
+	pFeModel->m_nFollowNodeCount = pFeDesc->m_FollowNodes.Count();
+	pFeModel->m_nCtrlOffsets = pFeDesc->m_CtrlOffsets.Count();
+	pFeModel->m_nCtrlOsOffsets = pFeDesc->m_CtrlOsOffsets.Count();
+	pFeModel->m_nSpringIntegratorCount = pFeDesc->m_SpringIntegrator.Count();
+	pFeModel->m_nSimdSpringIntegratorCount = pFeDesc->m_SimdSpringIntegrator.Count();
+	pFeModel->m_nWorldCollisionParamCount = pFeDesc->m_WorldCollisionParams.Count();
+	pFeModel->m_nWorldCollisionNodeCount = pFeDesc->m_WorldCollisionNodes.Count();
+	pFeModel->m_nFreeNodeCount = pFeDesc->m_FreeNodes.Count();
+	pFeModel->m_nTaperedCapsuleStretchCount = pFeDesc->m_TaperedCapsuleStretches.Count();
+	pFeModel->m_nTaperedCapsuleRigidCount = pFeDesc->m_TaperedCapsuleRigids.Count();
+	pFeModel->m_nSphereRigidCount = pFeDesc->m_SphereRigids.Count();
+	pFeModel->m_pSimdQuads = ConstCastOffsetPointer( pFeDesc->m_SimdQuads.Base(), nOffsetBytes );
+	pFeModel->m_pQuads = ConstCastOffsetPointer( pFeDesc->m_Quads.Base(), nOffsetBytes );
+	pFeModel->m_pSimdTris = ConstCastOffsetPointer( pFeDesc->m_SimdTris.Base(), nOffsetBytes );
+	pFeModel->m_pTris = NULL;
+	pFeModel->m_pRods = ConstCastOffsetPointer( pFeDesc->m_Rods.Base(), nOffsetBytes );;
+	pFeModel->m_pSimdRods = ConstCastOffsetPointer( pFeDesc->m_SimdRods.Base(), nOffsetBytes );
+	pFeModel->m_pAxialEdges = ConstCastOffsetPointer( pFeDesc->m_AxialEdges.Base(), nOffsetBytes );
+	pFeModel->m_pNodeToCtrl = NULL;
+	pFeModel->m_pCtrlToNode = NULL;
+	pFeModel->m_pCtrlHash = ConstCastOffsetPointer( pFeDesc->m_CtrlHash.Base(), nOffsetBytes );
+	pFeModel->m_pRopes = ConstCastOffsetPointer( pFeDesc->m_Ropes.Base(), nOffsetBytes );
+	pFeModel->m_pNodeBases = ConstCastOffsetPointer( pFeDesc->m_NodeBases.Base(), nOffsetBytes );
+	pFeModel->m_pSimdNodeBases = ConstCastOffsetPointer( pFeDesc->m_SimdNodeBases.Base(), nOffsetBytes );
+	pFeModel->m_pNodeIntegrator = ConstCastOffsetPointer( pFeDesc->m_NodeIntegrator.Base(), nOffsetBytes );
+	pFeModel->m_pSpringIntegrator = ConstCastOffsetPointer( pFeDesc->m_SpringIntegrator.Base(), nOffsetBytes );
+	pFeModel->m_pSimdSpringIntegrator = ConstCastOffsetPointer( pFeDesc->m_SimdSpringIntegrator.Base(), nOffsetBytes );
+	pFeModel->m_pCtrlOffsets = ConstCastOffsetPointer( pFeDesc->m_CtrlOffsets.Base(), nOffsetBytes );
+	pFeModel->m_pCtrlOsOffsets = ConstCastOffsetPointer( pFeDesc->m_CtrlOsOffsets.Base(), nOffsetBytes );
+	pFeModel->m_pFollowNodes = ConstCastOffsetPointer( pFeDesc->m_FollowNodes.Base(), nOffsetBytes );
+	pFeModel->m_pNodeCollisionRadii = ConstCastOffsetPointer( pFeDesc->m_NodeCollisionRadii.Base(), nOffsetBytes );
+	pFeModel->m_pLocalRotation = ConstCastOffsetPointer( pFeDesc->m_LocalRotation.Base(), nOffsetBytes );
+	pFeModel->m_pLocalForce = ConstCastOffsetPointer( pFeDesc->m_LocalForce.Base(), nOffsetBytes );
+	pFeModel->m_pCollisionSpheres = ConstCastOffsetPointer( pFeDesc->m_CollisionSpheres.Base(), nOffsetBytes );
+	pFeModel->m_pCollisionPlanes = ConstCastOffsetPointer( pFeDesc->m_CollisionPlanes.Base(), nOffsetBytes );
+	pFeModel->m_pWorldCollisionNodes = ConstCastOffsetPointer( pFeDesc->m_WorldCollisionNodes.Base(), nOffsetBytes );
+	pFeModel->m_pWorldCollisionParams = ConstCastOffsetPointer( pFeDesc->m_WorldCollisionParams.Base(), nOffsetBytes );
+	pFeModel->m_pLegacyStretchForce = ConstCastOffsetPointer( pFeDesc->m_LegacyStretchForce.Base(), nOffsetBytes );
+	pFeModel->m_pTaperedCapsuleStretches = ConstCastOffsetPointer( pFeDesc->m_TaperedCapsuleStretches.Base(), nOffsetBytes );
+	pFeModel->m_pTaperedCapsuleRigids = ConstCastOffsetPointer( pFeDesc->m_TaperedCapsuleRigids.Base(), nOffsetBytes );
+	pFeModel->m_pSphereRigids = ConstCastOffsetPointer( pFeDesc->m_SphereRigids.Base(), nOffsetBytes );
+	pFeModel->m_pFreeNodes = ConstCastOffsetPointer( pFeDesc->m_FreeNodes.Base(), nOffsetBytes );
+	pFeModel->m_pFitMatrices = ConstCastOffsetPointer( pFeDesc->m_FitMatrices.Base(), nOffsetBytes );
+	pFeModel->m_pSimdFitMatrices = ConstCastOffsetPointer( pFeDesc->m_SimdFitMatrices.Base(), nOffsetBytes );
+	pFeModel->m_pFitWeights = ConstCastOffsetPointer( pFeDesc->m_FitWeights.Base(), nOffsetBytes );
+	pFeModel->m_pReverseOffsets = ConstCastOffsetPointer( pFeDesc->m_ReverseOffsets.Base(), nOffsetBytes );
+	AssertDbg( pFeModel->m_pWorldCollisionParams ? pFeModel->m_pWorldCollisionParams[ pFeModel->m_nWorldCollisionParamCount - 1 ].nListEnd == pFeModel->m_nWorldCollisionNodeCount : !pFeModel->m_pWorldCollisionNodes && !pFeModel->m_nWorldCollisionParamCount && !pFeModel->m_nWorldCollisionNodeCount );
+	pFeModel->m_nRopeCount = pFeDesc->m_nRopeCount;
+	pFeModel->m_nRopeIndexCount = pFeDesc->m_Ropes.Count();
+	pFeModel->m_nNodeBaseCount = pFeDesc->m_NodeBases.Count();
+	pFeModel->m_nSimdNodeBaseCount = pFeDesc->m_SimdNodeBases.Count();
+	pFeModel->m_nCollisionSpheres[ 0 ] = pFeDesc->m_CollisionSpheres.Count();
+	pFeModel->m_nCollisionSpheres[ 1 ] = pFeDesc->m_nCollisionSphereInclusiveCount;
+	pFeModel->m_nCollisionPlanes = pFeDesc->m_CollisionPlanes.Count();
+	Assert( pFeDesc->m_TreeChildren.Count() == 0 || pFeDesc->m_TreeChildren.Count() == ( int )pFeDesc->GetDynamicNodeCount() - 1 );
+	pFeModel->m_pTreeChildren = ConstCastOffsetPointer( pFeDesc->m_TreeChildren.Base(), nOffsetBytes );
+	Assert( pFeDesc->m_TreeParents.Count() == 0 || pFeDesc->m_TreeParents.Count() == 2 * ( int )pFeDesc->GetDynamicNodeCount() - 1 );
+	pFeModel->m_pTreeParents = ConstCastOffsetPointer( pFeDesc->m_TreeParents.Base(), nOffsetBytes );
+	Assert( pFeDesc->m_TreeParents.Count() == pFeDesc->m_TreeCollisionMasks.Count() );
+	pFeModel->m_pTreeCollisionMasks = ConstCastOffsetPointer( pFeDesc->m_TreeCollisionMasks.Base(), nOffsetBytes );
+	Assert( pFeDesc->m_NodeInvMasses.Count() == 0 || pFeDesc->m_NodeInvMasses.Count() == pFeDesc->m_nNodeCount );
+	pFeModel->m_pNodeInvMasses = pFeDesc->m_NodeInvMasses.Count() ? ConstCastOffsetPointer( pFeDesc->m_NodeInvMasses.Base(), nOffsetBytes ) : NULL;
+	if ( pFeDesc->m_CtrlName.IsEmpty() || !pCtrlNames )
+	{
+		pFeModel->m_pCtrlName = NULL;
+	}
+	else
+	{
+		pFeModel->m_pCtrlName = const_cast< const char ** >( pCtrlNames ); //ConstCastOffsetPointer( pFeDesc->m_CtrlName.Base( ), nOffsetBytes );
+		for ( int i = 0; i < pFeModel->m_nCtrlCount; ++i )
+		{
+			const CResourceString &name = pFeDesc->m_CtrlName[ i ];
+			if ( name.IsNull() )
+			{
+				pFeModel->m_pCtrlName[ i ] = "";
+			}
+			else
+			{
+				pFeModel->m_pCtrlName[ i ] = ConstCastOffsetPointer( name.GetPtr(), nOffsetBytes );
+			}
+		}
+	}
+	AssertDbg( pFeDesc->m_InitPose.Count() == pFeModel->m_nCtrlCount );
+	pFeModel->m_pInitPose = ConstCastOffsetPointer( pFeDesc->m_InitPose.Base(), nOffsetBytes );
+}
+
+void SetIdentityPerm( CUtlVector< uint > &perm, uint nCount )
+{
+	perm.SetCount( nCount );
+	for ( uint i = 0; i < nCount; ++i )
+		perm[ i ] = i;
+}
+
+
+struct CtrlHashFunctor_t
+{
+	const CFeModel *m_pFeModel;
+	CtrlHashFunctor_t( const CFeModel *pFeModel ) : m_pFeModel( pFeModel ){}
+	bool operator( )( int nLeft, int nRight ) const
+	{
+		return m_pFeModel->m_pCtrlHash[ nLeft ] < m_pFeModel->m_pCtrlHash[ nRight ];
+	}
+};
+
+
+CFeModelReplaceContext::CFeModelReplaceContext( const CFeModel *pOld, const CFeModel *pNew )
+{
+	m_pOld = pOld;
+	m_pNew = pNew;
+	m_OldToNewNode.SetCount( pOld->m_nNodeCount ); m_OldToNewNode.FillWithValue( -1 );
+	m_OldToNewCtrl.SetCount( pOld->m_nCtrlCount ); m_OldToNewCtrl.FillWithValue( -1 );
+	m_NewToOldNode.SetCount( pNew->m_nNodeCount ); m_NewToOldNode.FillWithValue( -1 );
+	m_NewToOldCtrl.SetCount( pNew->m_nCtrlCount ); m_NewToOldCtrl.FillWithValue( -1 );
+
+	CUtlVector< uint > oldCtrlIndex, newCtrlIndex;
+	SetIdentityPerm( oldCtrlIndex, pOld->m_nCtrlCount );
+	SetIdentityPerm( newCtrlIndex, pNew->m_nCtrlCount );
+	HeapSort( oldCtrlIndex, CtrlHashFunctor_t( pOld ) );
+	HeapSort( newCtrlIndex, CtrlHashFunctor_t( pNew ) );
+
+	for ( uint nOld = 0, nNew = 0; nOld < pOld->m_nCtrlCount && nNew < pNew->m_nCtrlCount; )
+	{
+		uint nOldCtrl = oldCtrlIndex[ nOld ], nNewCtrl = newCtrlIndex[ nNew ];
+		uint nOldCtrlHash = pOld->m_pCtrlHash[ nOldCtrl ], nNewCtrlHash = pNew->m_pCtrlHash[ nNewCtrl ];
+		if ( nOldCtrlHash == nNewCtrlHash )
+		{
+			// we found a match!
+			m_OldToNewCtrl[ nOldCtrl ] = nNewCtrl;
+			m_NewToOldCtrl[ nNewCtrl ] = nOldCtrl;
+			uint nOldNode = pOld->CtrlToNode( nOldCtrl );
+			uint nNewNode = pNew->CtrlToNode( nNewCtrl );
+			if ( nOldNode < pOld->m_nNodeCount && nNewNode < pNew->m_nNodeCount )
+			{
+				// there's a match in nodes
+				m_NewToOldNode[ nNewNode ] = nOldNode;
+				m_OldToNewNode[ nOldNode ] = nNewNode;
+			}
+			nOld++;
+			nNew++;
+		}
+		else if ( nOldCtrlHash < nNewCtrlHash )
+		{
+			AssertDbg( m_OldToNewCtrl[ nOldCtrl ] == -1 );
+			nOld++;
+		}
+		else
+		{
+			AssertDbg( m_NewToOldCtrl[ nNewCtrl ] == -1 );
+			nNew++;
+		}
+	}
+}
+
+

mathlib/genvectors.pl

@@ -0,0 +1,85 @@
+#! perl
+use Math::Trig;
+
+# generate a table of vectors to use for the k dop basis. We will add
+# the 3 basic vectors and then add more vectors until we get to the
+# target of 16 directions.
+
+srand(31456);
+
+print <<END
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: static vector table for 32-plane kdops
+//
+// \$Workfile:     \$
+// \$NoKeywords: \$
+//=============================================================================//
+//
+//    **** DO NOT EDIT THIS FILE. GENERATED BY genvectors.PL ****
+//
+
+END
+;
+
+my $nNumVectors = 3;
+
+for( $i = 0; $i < 3; $i++ )
+{
+  push @XC, &ZeroOne( $i == 0 );
+  push @YC, &ZeroOne( $i == 1 );
+  push @ZC, &ZeroOne( $i == 2 );
+}
+
+# now, generate a bunch of random vectors and keep whichever is farthest away from all vectors chosen thus far
+
+while( $#XC < 15 )
+  {
+	my $mindot = 2.0;
+	for( $t = 0; $t < 1000*100; $t++ )
+	  {
+		my $closest_comp_dot = 0;
+		$z=rand(2)-1;
+		$phi=rand(2.0*3.141592654);
+		$theta=asin($z);
+		$x = cos($theta)*cos($phi);
+		$y = cos($theta)*sin($phi);
+		for( $c = 0; $c <= $#XC; $c++ )
+		  {
+			my $dot = abs( $x * $XC[$c] + $y * $YC[$c] + $z * $ZC[$c] );
+			$closest_comp_dot = $dot if ( $closest_comp_dot < $dot );
+		  }
+		if ( $closest_comp_dot < $mindot )
+		  {
+			$mindot = $closest_comp_dot;
+			$bestx = $x;
+			$besty = $y;
+			$bestz = $z;
+		  }
+	  }
+	#print "dot = $mindot ($bestx, $besty, $bestz)\n";
+	push @XC, $bestx;
+	push @YC, $besty;
+	push @ZC, $bestz;
+  }
+
+# output
+foreach $_ ( ( 'X', 'Y', 'Z' ) )
+{
+  print "const fltx4 g_KDop32$_"."Dirs[] =\n{\n";
+  for( $i = 0; $i <= $#XC; $i++ )
+	{
+	  print "\t{ " if ( ( $i & 3 ) == 0 );
+	  $vname= $_."C";
+	  printf "%f, ",$$vname[$i];
+	  print " },\n" if ( ( $i & 3 ) == 3 );
+	}
+  print "};\n\n";
+}
+
+sub ZeroOne
+  {
+	my $n = pop(@_);
+	return 0 unless( $n );
+	return 1;
+  }

mathlib/halton.cpp

@@ -0,0 +1,55 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+//=====================================================================================//
+
+#include <halton.h>
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+HaltonSequenceGenerator_t::HaltonSequenceGenerator_t(int b)
+{
+	base=b;
+	fbase=(float) b;
+	seed=1;
+
+}
+
+float HaltonSequenceGenerator_t::GetElement(int elem)
+{
+	int tmpseed=seed;
+	float ret=0.0;
+	float base_inv=1.0/fbase;
+	while(tmpseed)
+	{
+		int dig=tmpseed % base;
+		ret+=((float) dig)*base_inv;
+		base_inv/=fbase;
+		tmpseed/=base;
+	}
+	return ret;
+}
+
+
+int InsideOut( int nTotal, int nCounter )
+{
+    int b = 0;
+	for ( int m = nTotal, k = 1; k < nTotal; k <<= 1 )
+	{
+		if ( nCounter << 1 >= m )
+		{
+			b += k;
+			nCounter -= ( m + 1 ) >> 1;
+			m >>= 1;
+		}
+		else 
+		{
+			m = ( m + 1 ) >> 1;
+		}
+	}
+	Assert( ( b >= 0 ) && ( b < nTotal ) );
+	return b;
+}

mathlib/imagequant.cpp

@@ -0,0 +1,100 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//=============================================================================//
+
+#include <quantize.h>
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+#define N_EXTRAVALUES 1
+#define N_DIMENSIONS (3+N_EXTRAVALUES)
+
+#define PIXEL(x,y,c) Image[4*((x)+((Width*(y))))+c]
+
+static uint8 Weights[]={5,7,4,8};
+static int ExtraValueXForms[3*N_EXTRAVALUES]={
+	76,151,28,
+};
+
+  
+
+#define MAX_QUANTIZE_IMAGE_WIDTH 4096
+
+void ColorQuantize(uint8 const *Image,
+				   int Width,
+				   int Height,
+				   int flags, int ncolors,
+				   uint8 *out_pixels,
+				   uint8 *out_palette,
+				   int firstcolor)
+{
+	int Error[MAX_QUANTIZE_IMAGE_WIDTH+1][3][2];
+	struct Sample *s=AllocSamples(Width*Height,N_DIMENSIONS);
+	int x,y,c;
+	for(y=0;y<Height;y++)
+		for(x=0;x<Width;x++)
+		{
+			for(c=0;c<3;c++)
+				NthSample(s,y*Width+x,N_DIMENSIONS)->Value[c]=PIXEL(x,y,c);
+			// now, let's generate extra values to quantize on
+			for(int i=0;i<N_EXTRAVALUES;i++)
+			{
+				int val1=0;
+				for(c=0;c<3;c++)
+					val1+=PIXEL(x,y,c)*ExtraValueXForms[i*3+c];
+				val1>>=8;
+				NthSample(s,y*Width+x,N_DIMENSIONS)->Value[c]=(uint8)
+					(MIN(255,MAX(0,val1)));
+			}
+		}
+	struct QuantizedValue *q=Quantize(s,Width*Height,N_DIMENSIONS,
+									  ncolors,Weights,firstcolor);
+	delete[] s;
+	memset(out_palette,0x55,768);
+	for(int p=0;p<256;p++)
+	{
+		struct QuantizedValue *v=FindQNode(q,p);
+		if (v)
+			for(int c=0;c<3;c++)
+				out_palette[p*3+c]=v->Mean[c];
+	}
+	memset(Error,0,sizeof(Error));
+	for(y=0;y<Height;y++)
+	{
+		int ErrorUse=y & 1;
+		int ErrorUpdate=ErrorUse^1;
+		for(x=0;x<Width;x++)
+		{
+			uint8 samp[3];
+			for(c=0;c<3;c++)
+			{
+				int tryc=PIXEL(x,y,c);
+				if (! (flags & QUANTFLAGS_NODITHER))
+				{
+					tryc+=Error[x][c][ErrorUse];
+					Error[x][c][ErrorUse]=0;
+				}
+				samp[c]=(uint8) MIN(255,MAX(0,tryc));
+			}
+			struct QuantizedValue *f=FindMatch(samp,3,Weights,q);
+			out_pixels[Width*y+x]=(uint8) (f->value);
+			if (! (flags & QUANTFLAGS_NODITHER))
+				for(int i=0;i<3;i++)
+				{
+					int newerr=samp[i]-f->Mean[i];
+					int orthog_error=(newerr*3)/8;
+					Error[x+1][i][ErrorUse]+=orthog_error;
+					Error[x][i][ErrorUpdate]=orthog_error;
+					Error[x+1][i][ErrorUpdate]=newerr-2*orthog_error;
+				}
+		}
+	}
+	if (q) FreeQuantization(q);
+}
+

mathlib/kdop.cpp

@@ -0,0 +1,42 @@
+//===== Copyright © 1996-2005, Valve Corporation, All rights reserved. ======//
+//
+// Purpose: 
+//
+//===========================================================================//
+
+#include "mathlib/ssemath.h"
+#include "mathlib/ssequaternion.h"
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+// get the kdop vectors for k=32
+#include "dopvectors.h"
+
+
+void KDop32_t::AddPointSet( Vector const *pPoints, int nPnts )
+{
+	for( int i = 0; i < nPnts; i++ )
+	{
+		fltx4 fl4PntX = ReplicateX4( pPoints->x );
+		fltx4 fl4PntY = ReplicateX4( pPoints->y );
+		fltx4 fl4PntZ = ReplicateX4( pPoints->z );
+		
+		for( int c = 0; c < 4; c++ )
+		{
+			fltx4 fl4Dot = AddSIMD( AddSIMD( MulSIMD( fl4PntX, g_KDop32XDirs[c] ), MulSIMD( fl4PntY, g_KDop32YDirs[c] ) ),
+									MulSIMD( fl4PntZ, g_KDop32ZDirs[c] ) );
+			
+			m_Mins[c] = MinSIMD( fl4Dot, m_Mins[c] );
+			m_Maxes[c] = MaxSIMD( fl4Dot, m_Maxes[c] );
+		}
+		pPoints++;
+	}
+}
+
+
+void KDop32_t::CreateFromPointSet( Vector const *pPoints, int nPnts )
+{
+	Init();
+	AddPointSet( pPoints, nPnts );
+}

mathlib/lightdesc.cpp

@@ -0,0 +1,355 @@
+//===== Copyright © 1996-2005, Valve Corporation, All rights reserved. ======//
+//
+// Purpose: 
+//
+//===========================================================================//
+
+#include <ssemath.h>
+#include <lightdesc.h>
+#include "mathlib.h"
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+void LightDesc_t::RecalculateOneOverThetaDotMinusPhiDot()
+{
+	float flSpread = m_ThetaDot - m_PhiDot;
+	if ( flSpread > 1.0e-10f )
+	{
+		// note - this quantity is very sensitive to round off error. the sse
+		// reciprocal approximation won't cut it here.
+		m_OneOverThetaDotMinusPhiDot = 1.0f / flSpread;
+	}
+	else
+	{
+		// hard falloff instead of divide by zero
+		m_OneOverThetaDotMinusPhiDot = 1.0f;
+	}				
+}
+
+void LightDesc_t::RecalculateDerivedValues(void)
+{
+	m_Flags = LIGHTTYPE_OPTIMIZATIONFLAGS_DERIVED_VALUES_CALCED;
+	if ( m_Attenuation0 )
+	{
+		m_Flags|=LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION0;
+	}
+	if ( m_Attenuation1 )
+	{
+		m_Flags|=LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION1;
+	}
+	if ( m_Attenuation2 )
+	{
+		m_Flags|=LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION2;
+	}
+	
+	if ( m_Type == MATERIAL_LIGHT_SPOT )
+	{
+		m_ThetaDot = cos( m_Theta );
+		m_PhiDot = cos( m_Phi );
+		RecalculateOneOverThetaDotMinusPhiDot();
+	}	
+	if ( m_Type == MATERIAL_LIGHT_DIRECTIONAL )
+	{
+		// set position to be real far away in the right direction
+		m_Position = m_Direction;
+		m_Position *= 2.0e6;
+	}
+	
+	m_RangeSquared = m_Range*m_Range;
+}
+
+void LightDesc_t::ComputeLightAtPointsForDirectional(
+	const FourVectors &pos, const FourVectors &normal,
+	FourVectors &color, bool DoHalfLambert ) const
+{
+	FourVectors delta;
+	delta.DuplicateVector(m_Direction);
+//	delta.VectorNormalizeFast();
+	fltx4 strength=delta*normal;
+	if (DoHalfLambert)
+	{
+		strength=AddSIMD(MulSIMD(strength,Four_PointFives),Four_PointFives);
+	}
+	else
+		strength=MaxSIMD(Four_Zeros,delta*normal);
+		
+	color.x=AddSIMD(color.x,MulSIMD(strength,ReplicateX4(m_Color.x)));
+	color.y=AddSIMD(color.y,MulSIMD(strength,ReplicateX4(m_Color.y)));
+	color.z=AddSIMD(color.z,MulSIMD(strength,ReplicateX4(m_Color.z)));
+}
+
+
+float LightDesc_t::DistanceAtWhichBrightnessIsLessThan( float flAmount ) const
+{
+	float bright = m_Color.Length();
+	if ( bright > 0.0 )
+	{
+		flAmount /= m_Color.Length();
+		
+		// calculate terms for quadratic equation
+		float a = flAmount * m_Attenuation2;
+		float b = flAmount * m_Attenuation1;
+		float c = flAmount * m_Attenuation0 - 1;
+		
+		float r0, r1;
+		if ( SolveQuadratic( a, b, c, r0, r1 ) )
+		{
+			float rslt = MAX( 0, MAX( r0, r1 ) );
+#ifdef _DEBUG
+			if ( rslt > 0.0 )
+			{
+				float fltest = 1.0 / ( m_Attenuation0 + rslt * m_Attenuation1 + rslt * rslt * m_Attenuation2 );
+				Assert( fabs( fltest - flAmount ) < 0.1 );
+			}
+#endif
+			return rslt;
+		}
+	}
+	return 0;
+}
+
+void LightDesc_t::ComputeLightAtPoints( const FourVectors &pos, const FourVectors &normal,
+										FourVectors &color, bool DoHalfLambert ) const
+{
+	FourVectors delta;
+	Assert((m_Type==MATERIAL_LIGHT_POINT) || (m_Type==MATERIAL_LIGHT_SPOT) || (m_Type==MATERIAL_LIGHT_DIRECTIONAL));
+	switch (m_Type)
+	{
+		case MATERIAL_LIGHT_POINT:
+		case MATERIAL_LIGHT_SPOT:
+			delta.DuplicateVector(m_Position);
+			delta-=pos;
+			break;
+				
+		case MATERIAL_LIGHT_DIRECTIONAL:
+			ComputeLightAtPointsForDirectional( pos, normal, color, DoHalfLambert );
+			return;
+	}
+
+	fltx4 dist2 = delta*delta;
+
+	dist2=MaxSIMD( Four_Ones, dist2 );
+
+	fltx4 falloff;
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION0 )
+	{
+		falloff = ReplicateX4(m_Attenuation0);
+	}
+	else
+		falloff= Four_Epsilons;
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION1 )
+	{
+		falloff=AddSIMD(falloff,MulSIMD(ReplicateX4(m_Attenuation1),SqrtEstSIMD(dist2)));
+	}
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION2 )
+	{
+		falloff=AddSIMD(falloff,MulSIMD(ReplicateX4(m_Attenuation2),dist2));
+	}
+
+	falloff=ReciprocalEstSIMD(falloff);
+	// Cull out light beyond this radius
+	// now, zero out elements for which dist2 was > range^2. !!speed!! lights should store dist^2 in sse format
+	if (m_Range != 0.f)
+	{
+		fltx4 RangeSquared=ReplicateX4(m_RangeSquared); // !!speed!!
+		falloff=AndSIMD(falloff,CmpLtSIMD(dist2,RangeSquared));
+	}
+
+	delta.VectorNormalizeFast();
+	fltx4 strength=delta*normal;
+	if (DoHalfLambert)
+	{
+		strength=AddSIMD(MulSIMD(strength,Four_PointFives),Four_PointFives);
+	}
+	else
+		strength=MaxSIMD(Four_Zeros,delta*normal);
+		
+	switch(m_Type)
+	{
+		case MATERIAL_LIGHT_POINT:
+			// half-lambert
+			break;
+				
+		case MATERIAL_LIGHT_SPOT:
+		{
+			fltx4 dot2=SubSIMD(Four_Zeros,delta*m_Direction); // dot position with spot light dir for cone falloff
+
+
+			fltx4 cone_falloff_scale=MulSIMD(ReplicateX4(m_OneOverThetaDotMinusPhiDot),
+												 SubSIMD(dot2,ReplicateX4(m_PhiDot)));
+			cone_falloff_scale=MinSIMD(cone_falloff_scale,Four_Ones);
+			
+			if ((m_Falloff!=0.0) && (m_Falloff!=1.0))
+			{
+				// !!speed!! could compute integer exponent needed by powsimd and store in light
+				cone_falloff_scale=PowSIMD(cone_falloff_scale,m_Falloff);
+			}
+			strength=MulSIMD(cone_falloff_scale,strength);
+
+			// now, zero out lighting where dot2<phidot. This will mask out any invalid results
+			// from pow function, etc
+			bi32x4 OutsideMask=CmpGtSIMD(dot2,ReplicateX4(m_PhiDot)); // outside light cone?
+			strength=AndSIMD(OutsideMask,strength);
+		}
+		break;
+			
+
+	}
+	strength=MulSIMD(strength,falloff);
+	color.x=AddSIMD(color.x,MulSIMD(strength,ReplicateX4(m_Color.x)));
+	color.y=AddSIMD(color.y,MulSIMD(strength,ReplicateX4(m_Color.y)));
+	color.z=AddSIMD(color.z,MulSIMD(strength,ReplicateX4(m_Color.z)));
+}
+
+
+
+void LightDesc_t::ComputeNonincidenceLightAtPoints( const FourVectors &pos, FourVectors &color ) const
+{
+	FourVectors delta;
+	Assert((m_Type==MATERIAL_LIGHT_POINT) || (m_Type==MATERIAL_LIGHT_SPOT) || (m_Type==MATERIAL_LIGHT_DIRECTIONAL));
+	switch (m_Type)
+	{
+		case MATERIAL_LIGHT_POINT:
+		case MATERIAL_LIGHT_SPOT:
+			delta.DuplicateVector(m_Position);
+			delta-=pos;
+			break;
+				
+		case MATERIAL_LIGHT_DIRECTIONAL:
+			return;
+	}
+
+	fltx4 dist2 = delta*delta;
+
+	dist2=MaxSIMD( Four_Ones, dist2 );
+
+	fltx4 falloff;
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION0 )
+	{
+		falloff = ReplicateX4(m_Attenuation0);
+	}
+	else
+		falloff= Four_Epsilons;
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION1 )
+	{
+		falloff=AddSIMD(falloff,MulSIMD(ReplicateX4(m_Attenuation1),SqrtEstSIMD(dist2)));
+	}
+
+	if( m_Flags & LIGHTTYPE_OPTIMIZATIONFLAGS_HAS_ATTENUATION2 )
+	{
+		falloff=AddSIMD(falloff,MulSIMD(ReplicateX4(m_Attenuation2),dist2));
+	}
+
+	falloff=ReciprocalEstSIMD(falloff);
+	// Cull out light beyond this radius
+	// now, zero out elements for which dist2 was > range^2. !!speed!! lights should store dist^2 in sse format
+	if (m_Range != 0.f)
+	{
+		fltx4 RangeSquared=ReplicateX4(m_RangeSquared); // !!speed!!
+		falloff=AndSIMD(falloff,CmpLtSIMD(dist2,RangeSquared));
+	}
+
+	delta.VectorNormalizeFast();
+	fltx4 strength = Four_Ones;
+	//fltx4 strength=delta;
+	//fltx4 strength = MaxSIMD(Four_Zeros,delta);
+		
+	switch(m_Type)
+	{
+		case MATERIAL_LIGHT_POINT:
+			// half-lambert
+			break;
+				
+		case MATERIAL_LIGHT_SPOT:
+		{
+			fltx4 dot2=SubSIMD(Four_Zeros,delta*m_Direction); // dot position with spot light dir for cone falloff
+
+
+			fltx4 cone_falloff_scale=MulSIMD(ReplicateX4(m_OneOverThetaDotMinusPhiDot),
+												 SubSIMD(dot2,ReplicateX4(m_PhiDot)));
+			cone_falloff_scale=MinSIMD(cone_falloff_scale,Four_Ones);
+			
+			if ((m_Falloff!=0.0) && (m_Falloff!=1.0))
+			{
+				// !!speed!! could compute integer exponent needed by powsimd and store in light
+				cone_falloff_scale=PowSIMD(cone_falloff_scale,m_Falloff);
+			}
+			strength=MulSIMD(cone_falloff_scale,strength);
+
+			// now, zero out lighting where dot2<phidot. This will mask out any invalid results
+			// from pow function, etc
+			bi32x4 OutsideMask=CmpGtSIMD(dot2,ReplicateX4(m_PhiDot)); // outside light cone?
+			strength=AndSIMD(OutsideMask,strength);
+		}
+		break;
+			
+
+	}
+	strength=MulSIMD(strength,falloff);
+	color.x=AddSIMD(color.x,MulSIMD(strength,ReplicateX4(m_Color.x)));
+	color.y=AddSIMD(color.y,MulSIMD(strength,ReplicateX4(m_Color.y)));
+	color.z=AddSIMD(color.z,MulSIMD(strength,ReplicateX4(m_Color.z)));
+}
+
+
+
+void LightDesc_t::SetupOldStyleAttenuation( float fQuadraticAttn, float fLinearAttn, float fConstantAttn )
+{
+	// old-style manually typed quadrtiac coefficients
+	if ( fQuadraticAttn < EQUAL_EPSILON )
+		fQuadraticAttn = 0;
+	
+	if ( fLinearAttn < EQUAL_EPSILON)
+		fLinearAttn = 0;
+	
+	if ( fConstantAttn < EQUAL_EPSILON)
+		fConstantAttn = 0;
+	
+	if ( ( fConstantAttn < EQUAL_EPSILON ) && 
+		 ( fLinearAttn < EQUAL_EPSILON ) && 
+		 ( fQuadraticAttn < EQUAL_EPSILON ) )
+		fConstantAttn = 1;
+
+	m_Attenuation2=fQuadraticAttn;
+	m_Attenuation1=fLinearAttn;
+	m_Attenuation0=fConstantAttn;
+	float fScaleFactor = fQuadraticAttn * 10000 + fLinearAttn * 100 + fConstantAttn;
+	
+	if ( fScaleFactor > 0 )
+		m_Color *= fScaleFactor;
+}
+
+void LightDesc_t::SetupNewStyleAttenuation( float fFiftyPercentDistance, 
+											float fZeroPercentDistance )
+{
+	// new style storing 50% and 0% distances
+	float d50=fFiftyPercentDistance;
+	float d0=fZeroPercentDistance;
+	if (d0<d50)
+	{
+		// !!warning in lib code???!!!
+		Warning("light has _fifty_percent_distance of %f but no zero_percent_distance\n",d50);
+		d0=2.0*d50;
+	}
+	float a=0,b=1,c=0;
+	if (! SolveInverseQuadraticMonotonic(0,1.0,d50,2.0,d0,256.0,a,b,c))
+	{
+		Warning("can't solve quadratic for light %f %f\n",d50,d0);
+	}
+	float v50=c+d50*(b+d50*a);
+	float scale=2.0/v50;
+	a*=scale;
+	b*=scale;
+	c*=scale;
+	m_Attenuation2=a;
+	m_Attenuation1=b;
+	m_Attenuation0=c;
+}
+

mathlib/mathlib.vpc

@@ -0,0 +1,105 @@
+//-----------------------------------------------------------------------------
+//	MATHLIB.VPC
+//
+//	Project Script
+//-----------------------------------------------------------------------------
+
+$macro SRCDIR		".."
+
+$include "$SRCDIR\vpc_scripts\source_lib_base.vpc"
+
+$Configuration
+{
+	$Compiler
+	{
+		$AdditionalIncludeDirectories	"$BASE;..\public\mathlib"
+		$PreprocessorDefinitions		"$BASE;MATHLIB_LIB"
+	}
+}
+
+$Project "mathlib"
+{
+	$Folder	"Source Files"
+	{
+		$File	"expressioncalculator.cpp"
+		$File	"color_conversion.cpp"
+		$File   "cholesky.cpp"
+		$File	"halton.cpp"
+		$File	"lightdesc.cpp"
+		$File	"mathlib_base.cpp"
+		$File	"powsse.cpp"
+		$File	"sparse_convolution_noise.cpp"
+		$File	"sseconst.cpp"
+		$File	"sse.cpp"
+		$File	"ssenoise.cpp"
+		$File	"anorms.cpp"
+		$File	"bumpvects.cpp"
+		$File	"IceKey.cpp"
+		$File 	"kdop.cpp"
+		$File	"imagequant.cpp"
+		$File   "spherical.cpp"
+		$File	"polyhedron.cpp"
+		$File	"quantize.cpp"
+		$File	"randsse.cpp"
+		$File	"simdvectormatrix.cpp"
+		$File	"vmatrix.cpp"
+		$File	"almostequal.cpp"
+		$File   "simplex.cpp"
+		$File   "eigen.cpp"
+		$File   "box_buoyancy.cpp" [!$OSX32] // doesn't compile in debug under GCC 4.2.X
+		$File   "camera.cpp"
+		$File   "planefit.cpp"
+		$File	"polygon.cpp"
+		$File   "volumeculler.cpp"
+		$File	"transform.cpp"
+		$File	"sphere.cpp"
+		$File	"capsule.cpp"
+	}
+
+	$Folder	"Public Header Files"
+	{
+		$File	"$SRCDIR\public\mathlib\anorms.h"
+		$File	"$SRCDIR\public\mathlib\bumpvects.h"		
+		$File	"$SRCDIR\public\mathlib\beziercurve.h"
+		$File   "$SRCDIR/public/mathlib/camera.h"
+		$File	"$SRCDIR\public\mathlib\compressed_3d_unitvec.h"
+		$File	"$SRCDIR\public\mathlib\compressed_light_cube.h"
+		$File	"$SRCDIR\public\mathlib\compressed_vector.h"
+		$File	"$SRCDIR\public\mathlib\expressioncalculator.h"		
+		$File	"$SRCDIR\public\mathlib\halton.h"
+		$File	"$SRCDIR\public\mathlib\IceKey.H"
+		$File	"$SRCDIR\public\mathlib\lightdesc.h"
+		$File	"$SRCDIR\public\mathlib\math_pfns.h"
+		$File	"$SRCDIR\public\mathlib\mathlib.h"
+		$File	"$SRCDIR\public\mathlib\noise.h"
+		$File	"$SRCDIR\public\mathlib\polyhedron.h"
+		$File	"$SRCDIR\public\mathlib\quantize.h"
+		$File	"$SRCDIR\public\mathlib\simdvectormatrix.h"
+		$File	"$SRCDIR\public\mathlib\spherical_geometry.h"		
+		$File	"$SRCDIR\public\mathlib\ssemath.h"		
+		$File	"$SRCDIR\public\mathlib\ssequaternion.h"		
+		$File	"$SRCDIR\public\mathlib\vector.h"
+		$File	"$SRCDIR\public\mathlib\vector2d.h"
+		$File	"$SRCDIR\public\mathlib\vector4d.h"
+		$File	"$SRCDIR\public\mathlib\vmatrix.h"
+		$File	"$SRCDIR\public\mathlib\vplane.h"
+		$File	"$SRCDIR\public\mathlib\simplex.h"
+		$File	"$SRCDIR\public\mathlib\eigen.h"
+		$File   "$SRCDIR\public\mathlib\box_buoyancy.h"
+		$File   "$SRCDIR\public\mathlib\cholesky.h"
+		$File   "$SRCDIR\public\mathlib\planefit.h"
+		$File   "$SRCDIR\public\mathlib\intvector3d.h"
+		$File   "$SRCDIR\public\mathlib\polygon.h"
+		$File   "$SRCDIR\public\mathlib\quadric.h"
+		$File   "$SRCDIR\public\mathlib\volumeculler.h"
+		$File   "$SRCDIR\public\mathlib\transform.h"
+		$File	"$SRCDIR/public/mathlib/sphere.h"
+		$File	"$SRCDIR/public/mathlib/capsule.h"
+	}
+
+	$Folder	"Header Files"
+	{
+		$File	"noisedata.h"
+		$File	"sse.h"	
+	}
+}

mathlib/mathlib.vpc.vpc_cache

@@ -0,0 +1,13 @@
+"vpc_cache"
+{
+	"CacheVersion"		"1"
+	"win32"
+	{
+		"CRCFile"		"mathlib.vcxproj.vpc_crc"
+		"OutputFiles"
+		{
+			"0"		"mathlib.vcxproj"
+			"1"		"mathlib.vcxproj.filters"
+		}
+	}
+}

mathlib/mathlib_extended.vpc

@@ -0,0 +1,59 @@
+ //-----------------------------------------------------------------------------
+//	MATHLIB_EXTENDED.VPC
+//
+//	Project Script
+//-----------------------------------------------------------------------------
+
+$macro SRCDIR		".."
+$Macro INTERMEDIATESUBDIR "extended"
+
+$include "$SRCDIR\vpc_scripts\source_lib_base.vpc"
+
+$Configuration
+{
+	$Compiler
+	{
+		$AdditionalIncludeDirectories	"$BASE;..\public\mathlib"
+		$PreprocessorDefinitions		"$BASE;MATHLIB_EXTENDED_LIB;SOURCE1"
+	}
+}
+
+$Project "mathlib_extended"
+{
+	$Folder	"Source Files"
+	{
+		$File	"disjoint_set_forest.cpp"
+		$File	"dynamictree.cpp"
+		$File   "eigen.cpp"
+		$File	"simdvectormatrix.cpp"
+		$File	"femodel.cpp"
+		$File	"femodelbuilder.cpp"
+		$File	"feagglomerator.cpp"
+		$File	"svd.cpp"
+		$File	"transform.cpp"
+		$File	"femodeldesc.cpp"
+		$File	"softbody.cpp"
+		$File	"softbodyenvironment.cpp"
+	}
+
+	$Folder	"Public Header Files"
+	{
+		$File	"$SRCDIR/public/mathlib/aabb.h"
+		$File	"$SRCDIR/public/mathlib/transform.h"
+		$File	"$SRCDIR/public/mathlib/disjoint_set_forest.h"
+		$File	"$SRCDIR/public/mathlib/dynamictree.h"
+		$File	"$SRCDIR/public/mathlib/dynamictree.inl"
+		$File	"$SRCDIR/public/mathlib/eigen.h"
+		$File	"$SRCDIR/public/mathlib/simdvectormatrix.h"
+		$File	"$SRCDIR/public/mathlib/femodel.h"
+		$File	"$SRCDIR/public/mathlib/ssequaternion.h"		
+		$File	"$SRCDIR/public/mathlib/femodeldesc.h"
+		$File	"$SRCDIR/public/mathlib/femodel.inl"
+		$File	"$SRCDIR/public/mathlib/femodelbuilder.h"
+		$File	"$SRCDIR/public/mathlib/feagglomerator.h"
+		$File	"$SRCDIR/public/mathlib/svd.h"
+		$File	"$SRCDIR/public/mathlib/softbody.h"
+		$File	"$SRCDIR/public/mathlib/softbody.inl"
+		$File	"$SRCDIR/public/mathlib/softbodyenvironment.h"
+	}
+}

mathlib/mathlib_extended.vpc.vpc_cache

@@ -0,0 +1,13 @@
+"vpc_cache"
+{
+	"CacheVersion"		"1"
+	"win32"
+	{
+		"CRCFile"		"mathlib_extended.vcxproj.vpc_crc"
+		"OutputFiles"
+		{
+			"0"		"mathlib_extended.vcxproj"
+			"1"		"mathlib_extended.vcxproj.filters"
+		}
+	}
+}

mathlib/noisedata.h

@@ -0,0 +1,309 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: static data for noise() primitives.
+//
+// $Workfile:     $
+// $NoKeywords: $
+//=============================================================================//
+//
+//    **** DO NOT EDIT THIS FILE. GENERATED BY DATAGEN.PL ****
+//
+
+static int perm_a[]={
+    66,147,106,213,89,115,239,25,171,175,9,114,141,226,118,128,41,208,4,56,
+   180,248,43,82,246,219,94,245,133,131,222,103,160,130,168,145,238,38,23,6,
+   236,67,99,2,70,232,80,209,1,3,68,65,102,210,13,73,55,252,187,170,22,36,
+   52,181,117,163,46,79,166,224,148,75,113,95,156,185,220,164,51,142,161,35,
+   206,251,45,136,197,190,132,32,218,127,63,27,137,93,242,20,189,108,183,
+   122,139,191,249,253,87,98,69,0,144,64,24,214,97,116,158,42,107,15,53,212,
+   83,111,152,240,74,237,62,77,205,149,26,151,178,204,91,176,234,49,154,203,
+   33,221,125,134,165,124,86,39,37,60,150,157,179,109,110,44,159,153,5,100,
+   10,207,40,186,96,215,143,162,230,184,101,54,174,247,76,59,241,223,192,84,
+   104,78,169,146,138,30,48,85,233,19,29,92,126,17,199,250,31,81,188,225,28,
+   112,88,11,182,173,211,129,194,172,14,120,200,167,135,12,177,227,229,155,
+   201,61,105,195,193,244,235,58,8,196,123,254,16,18,50,121,71,243,90,57,
+   202,119,255,47,7,198,228,21,217,216,231,140,72,34
+};
+
+static int perm_b[]={
+    123,108,201,64,40,75,24,221,137,110,191,142,9,69,230,83,7,247,51,54,115,
+    133,180,248,109,116,62,99,251,55,89,253,65,106,228,167,131,132,58,143,
+    97,102,163,202,149,234,12,117,174,94,121,74,32,113,20,60,159,182,204,29,
+    244,118,3,178,255,38,6,114,36,93,30,134,213,90,245,209,88,232,162,125,
+    84,166,70,136,208,231,27,71,157,80,76,0,170,225,203,176,33,161,196,128,
+    252,236,246,2,138,1,250,197,77,243,218,242,19,164,68,212,14,237,144,63,
+    46,103,177,188,85,223,8,160,222,4,216,219,35,15,44,23,126,127,100,226,
+    235,37,168,101,49,22,11,73,61,135,111,183,72,96,185,239,82,18,50,155,
+    186,153,17,233,146,156,107,5,254,10,192,198,148,207,104,13,124,48,95,
+    129,120,206,199,81,249,91,150,210,119,240,122,194,92,34,28,205,175,227,
+    179,220,140,152,79,26,195,47,66,173,169,241,53,184,187,145,112,238,214,
+    147,98,171,229,200,151,25,67,78,189,217,130,224,57,172,59,41,43,16,105,
+    158,165,21,45,56,141,139,215,190,86,42,52,39,87,181,31,154,193,211
+};
+
+static int perm_c[]={
+    97,65,96,25,122,26,219,85,148,251,102,0,140,130,136,213,138,60,236,52,
+    178,131,115,183,144,78,147,168,39,45,169,70,57,146,67,142,252,216,28,54,
+    86,222,194,200,48,5,205,125,214,56,181,255,196,155,37,218,153,208,66,
+    242,73,248,206,61,62,246,177,2,197,107,162,152,89,41,6,160,94,8,201,38,
+    235,228,165,93,111,239,74,231,121,47,166,221,157,64,77,244,29,105,150,
+    123,190,191,225,118,133,42,10,84,185,159,124,132,240,180,44,1,9,19,99,
+    254,12,207,186,71,234,184,11,20,16,193,139,175,98,59,113,27,170,230,91,
+    187,46,156,249,108,195,171,114,14,188,82,192,233,24,32,241,87,164,90,43,
+    163,245,92,40,215,55,226,15,3,112,158,250,172,22,227,137,35,128,145,247,
+    161,119,80,217,189,81,7,63,202,120,223,83,179,4,106,199,229,95,53,50,33,
+    182,72,143,23,243,75,18,173,141,167,198,204,58,174,237,17,129,238,127,
+    31,101,176,36,30,110,209,34,203,135,232,68,149,49,134,126,212,79,76,117,
+    104,210,211,224,253,100,220,109,116,88,13,151,154,69,21,51,103
+};
+
+static int perm_d[]={
+    94,234,145,235,151,166,187,238,4,5,128,115,87,107,229,175,190,108,218,
+    32,17,220,97,90,122,121,71,109,64,227,225,75,81,19,27,162,3,89,139,69,
+    92,26,48,215,116,191,114,2,104,157,66,39,1,127,96,124,30,0,82,233,219,
+    42,131,173,35,201,182,144,14,98,148,244,160,159,179,91,31,68,119,154,
+    205,113,149,167,44,60,18,228,251,245,43,10,80,15,129,67,181,174,6,45,
+    194,237,213,52,99,232,211,212,164,217,57,153,156,102,134,20,249,132,55,
+    204,65,33,231,85,61,37,163,193,189,170,226,63,168,236,165,224,242,195,
+    41,200,40,70,112,100,36,172,130,74,137,252,243,135,230,161,207,16,146,
+    198,118,150,24,29,250,188,25,209,103,23,105,47,7,46,133,83,184,50,79,
+    110,120,53,253,206,214,9,240,101,147,152,183,254,59,126,216,197,171,51,
+    208,248,202,58,176,28,72,177,185,141,12,11,56,222,86,178,155,223,88,111,
+    73,142,210,138,239,221,199,192,84,93,241,125,76,77,255,95,8,78,247,186,
+    123,196,13,140,180,143,54,106,136,34,62,169,38,117,22,21,49,203,158,246
+};
+
+static float impulse_xcoords[]={
+    0.788235,0.541176,0.972549,0.082353,0.352941,0.811765,0.286275,0.752941,
+    0.203922,0.705882,0.537255,0.886275,0.580392,0.137255,0.800000,0.533333,
+    0.117647,0.447059,0.129412,0.925490,0.086275,0.478431,0.666667,0.568627,
+    0.678431,0.313725,0.321569,0.349020,0.988235,0.419608,0.898039,0.219608,
+    0.243137,0.623529,0.501961,0.772549,0.952941,0.517647,0.949020,0.701961,
+    0.454902,0.505882,0.564706,0.960784,0.207843,0.007843,0.831373,0.184314,
+    0.576471,0.462745,0.572549,0.247059,0.262745,0.694118,0.615686,0.121569,
+    0.384314,0.749020,0.145098,0.717647,0.415686,0.607843,0.105882,0.101961,
+    0.200000,0.807843,0.521569,0.780392,0.466667,0.552941,0.996078,0.627451,
+    0.992157,0.529412,0.407843,0.011765,0.709804,0.458824,0.058824,0.819608,
+    0.176471,0.317647,0.392157,0.223529,0.156863,0.490196,0.325490,0.074510,
+    0.239216,0.164706,0.890196,0.603922,0.921569,0.839216,0.854902,0.098039,
+    0.686275,0.843137,0.152941,0.372549,0.062745,0.474510,0.486275,0.227451,
+    0.400000,0.298039,0.309804,0.274510,0.054902,0.815686,0.647059,0.635294,
+    0.662745,0.976471,0.094118,0.509804,0.650980,0.211765,0.180392,0.003922,
+    0.827451,0.278431,0.023529,0.525490,0.450980,0.725490,0.690196,0.941176,
+    0.639216,0.560784,0.196078,0.364706,0.043137,0.494118,0.796078,0.113725,
+    0.760784,0.729412,0.258824,0.290196,0.584314,0.674510,0.823529,0.905882,
+    0.917647,0.070588,0.862745,0.345098,0.913725,0.937255,0.031373,0.215686,
+    0.768627,0.333333,0.411765,0.423529,0.945098,0.721569,0.039216,0.792157,
+    0.956863,0.266667,0.254902,0.047059,0.294118,0.658824,0.250980,1.000000,
+    0.984314,0.756863,0.027451,0.305882,0.835294,0.513725,0.360784,0.776471,
+    0.611765,0.192157,0.866667,0.858824,0.592157,0.803922,0.141176,0.435294,
+    0.588235,0.619608,0.341176,0.109804,0.356863,0.270588,0.737255,0.847059,
+    0.050980,0.764706,0.019608,0.870588,0.933333,0.784314,0.549020,0.337255,
+    0.631373,0.929412,0.231373,0.427451,0.078431,0.498039,0.968627,0.654902,
+    0.125490,0.698039,0.015686,0.878431,0.713725,0.368627,0.431373,0.874510,
+    0.403922,0.556863,0.443137,0.964706,0.909804,0.301961,0.035294,0.850980,
+    0.882353,0.741176,0.380392,0.133333,0.470588,0.643137,0.282353,0.396078,
+    0.980392,0.168627,0.149020,0.235294,0.670588,0.596078,0.733333,0.160784,
+    0.376471,0.682353,0.545098,0.482353,0.745098,0.894118,0.188235,0.329412,
+    0.439216,0.901961,0.000000,0.600000,0.388235,0.172549,0.090196,0.066667
+};
+
+static float impulse_ycoords[]={
+    0.827451,0.337255,0.941176,0.886275,0.878431,0.239216,0.400000,0.164706,
+    0.490196,0.411765,0.964706,0.349020,0.803922,0.317647,0.647059,0.431373,
+    0.933333,0.156863,0.094118,0.219608,0.039216,0.521569,0.498039,0.705882,
+    0.717647,0.047059,0.631373,0.517647,0.984314,0.847059,0.482353,0.439216,
+    0.250980,0.862745,0.690196,0.913725,0.270588,0.070588,0.027451,0.694118,
+    0.811765,0.000000,0.494118,0.823529,0.800000,0.600000,0.003922,0.443137,
+    0.639216,0.376471,0.031373,0.035294,0.552941,0.215686,0.305882,0.133333,
+    0.564706,0.176471,0.211765,0.874510,0.360784,0.654902,0.223529,0.807843,
+    0.372549,0.137255,0.321569,0.015686,0.007843,0.262745,0.125490,0.078431,
+    0.396078,0.976471,0.929412,1.000000,0.937255,0.509804,0.188235,0.850980,
+    0.831373,0.392157,0.741176,0.541176,0.592157,0.286275,0.345098,0.572549,
+    0.537255,0.725490,0.839216,0.184314,0.772549,0.149020,0.505882,0.423529,
+    0.780392,0.011765,0.890196,0.086275,0.427451,0.023529,0.788235,0.050980,
+    0.760784,0.603922,0.066667,0.643137,0.623529,0.960784,0.172549,0.333333,
+    0.082353,0.290196,0.992157,0.709804,0.894118,0.596078,0.243137,0.752941,
+    0.486275,0.670588,0.949020,0.784314,0.145098,0.560784,0.513725,0.180392,
+    0.580392,0.996078,0.380392,0.556863,0.407843,0.945098,0.117647,0.058824,
+    0.678431,0.129412,0.192157,0.105882,0.968627,0.545098,0.462745,0.227451,
+    0.019608,0.866667,0.674510,0.207843,0.627451,0.819608,0.921569,0.356863,
+    0.447059,0.533333,0.435294,0.341176,0.054902,0.529412,0.235294,0.764706,
+    0.615686,0.043137,0.745098,0.266667,0.501961,0.619608,0.776471,0.450980,
+    0.309804,0.325490,0.200000,0.635294,0.247059,0.698039,0.721569,0.168627,
+    0.854902,0.141176,0.611765,0.525490,0.415686,0.298039,0.254902,0.858824,
+    0.568627,0.329412,0.062745,0.843137,0.588235,0.733333,0.607843,0.478431,
+    0.576471,0.662745,0.470588,0.666667,0.980392,0.113725,0.898039,0.203922,
+    0.294118,0.152941,0.098039,0.909804,0.796078,0.768627,0.713725,0.196078,
+    0.368627,0.419608,0.352941,0.090196,0.749020,0.121569,0.882353,0.278431,
+    0.388235,0.917647,0.701961,0.729412,0.835294,0.258824,0.301961,0.101961,
+    0.792157,0.474510,0.686275,0.658824,0.364706,0.682353,0.458824,0.815686,
+    0.282353,0.160784,0.870588,0.988235,0.756863,0.549020,0.274510,0.384314,
+    0.650980,0.737255,0.901961,0.956863,0.972549,0.584314,0.925490,0.403922,
+    0.074510,0.454902,0.952941,0.109804,0.313725,0.905882,0.231373,0.466667
+};
+
+static float impulse_zcoords[]={
+    0.082353,0.643137,0.415686,0.929412,0.568627,0.509804,0.537255,0.815686,
+    0.698039,0.941176,0.776471,0.752941,0.737255,0.525490,0.498039,0.423529,
+    0.792157,0.125490,0.619608,0.164706,0.368627,0.870588,0.137255,0.372549,
+    0.466667,0.486275,0.501961,0.513725,0.709804,0.576471,0.203922,0.258824,
+    0.152941,0.556863,0.223529,0.047059,0.235294,0.474510,0.764706,0.552941,
+    0.847059,0.145098,0.176471,0.937255,0.654902,0.894118,0.729412,0.054902,
+    0.666667,0.749020,0.262745,0.560784,0.431373,0.286275,0.352941,0.239216,
+    0.156863,0.839216,0.427451,0.949020,0.384314,0.227451,0.180392,0.074510,
+    0.172549,0.356863,0.066667,0.517647,0.447059,0.184314,0.062745,0.670588,
+    0.603922,0.219608,0.270588,0.976471,0.505882,0.627451,0.819608,0.854902,
+    0.843137,0.019608,0.713725,0.035294,0.925490,0.349020,0.866667,0.701961,
+    0.909804,0.811765,0.717647,0.141176,0.917647,0.023529,0.098039,0.803922,
+    0.733333,0.658824,0.827451,0.133333,0.858824,0.800000,0.635294,1.000000,
+    0.078431,0.450980,0.835294,0.321569,0.360784,0.529412,0.725490,0.572549,
+    0.639216,0.341176,0.533333,0.094118,0.149020,0.545098,0.101961,0.901961,
+    0.278431,0.694118,0.521569,0.490196,0.454902,0.329412,0.274510,0.027451,
+    0.745098,0.933333,0.443137,0.168627,0.192157,0.988235,0.070588,0.972549,
+    0.768627,0.400000,0.470588,0.207843,0.215686,0.388235,0.439216,0.780392,
+    0.482353,0.121569,0.964706,0.086275,0.890196,0.337255,0.109804,0.305882,
+    0.113725,0.435294,0.721569,0.772549,0.807843,0.741176,0.254902,0.596078,
+    0.494118,0.317647,0.419608,0.000000,0.188235,0.031373,0.376471,0.380392,
+    0.611765,0.945098,0.411765,0.313725,0.874510,0.588235,0.678431,0.160784,
+    0.007843,0.090196,0.850980,0.788235,0.705882,0.266667,0.309804,0.541176,
+    0.231373,0.129412,0.294118,0.243137,0.913725,0.996078,0.117647,0.478431,
+    0.290196,0.549020,0.682353,0.784314,0.396078,0.831373,0.984314,0.584314,
+    0.039216,0.250980,0.600000,0.392157,0.298039,0.050980,0.364706,0.105882,
+    0.623529,0.886275,0.980392,0.325490,0.247059,0.690196,0.674510,0.960784,
+    0.647059,0.211765,0.882353,0.686275,0.823529,0.058824,0.956863,0.043137,
+    0.345098,0.301961,0.592157,0.862745,0.607843,0.458824,0.282353,0.003922,
+    0.580392,0.760784,0.564706,0.011765,0.968627,0.905882,0.756863,0.952941,
+    0.662745,0.015686,0.898039,0.196078,0.333333,0.992157,0.650980,0.407843,
+    0.796078,0.615686,0.878431,0.921569,0.631373,0.200000,0.403922,0.462745
+};
+
+static float s_randomGradients[]={
+    -0.460087, -0.887463, -0.058594 ,-0.458151, 0.861646, -0.430176 ,
+    -0.930437, 0.316048, -0.195496 ,-0.883558, -0.393287, -0.276550 ,
+    0.171025, -0.983455, -0.329712 ,-0.033573, -0.941867, -0.994995 ,
+    -0.476492, 0.014764, 0.879150 ,0.834786, -0.454571, 0.348755 ,-0.585801,
+     -0.782531, -0.338745 ,0.973990, -0.023774, 0.225403 ,-0.989659,
+     -0.011313, -0.143005 ,0.507109, -0.838016, -0.369141 ,-0.609995,
+     -0.766277, 0.314087 ,0.429987, 0.599850, -0.843323 ,0.089587,
+     -0.904071, -0.977783 ,-0.306997, -0.901432, 0.705078 ,0.031606,
+     0.994782, -0.950806 ,0.797663, -0.161508, -0.588806 ,0.811569,
+     -0.505360, 0.339783 ,0.936130, -0.114223, 0.334778 ,0.217280,
+     -0.970264, 0.440674 ,0.600976, -0.712375, -0.516418 ,0.197935,
+     0.979260, 0.213501 ,0.002956, 0.999995, -0.268127 ,-0.912763, 0.084651,
+     -0.401062 ,-0.193271, -0.945607, -0.804382 ,0.662480, 0.640156,
+     -0.506348 ,0.363459, -0.884439, 0.627197 ,-0.433415, 0.685363,
+     0.803589 ,-0.721652, 0.416952, -0.607971 ,0.647676, 0.296700,
+     0.734863 ,0.723040, -0.444294, 0.590454 ,-0.716318, -0.420435,
+     -0.613770 ,-0.039076, -0.996459, 0.885437 ,0.175225, -0.969092,
+     0.703918 ,0.116952, -0.991832, -0.399048 ,-0.504674, -0.013997,
+     0.863281 ,-0.436364, -0.817916, 0.651733 ,0.098030, -0.995090,
+     0.137573 ,0.637157, -0.766031, -0.132263 ,-0.594718, 0.583153,
+     -0.681213 ,-0.625632, 0.419913, -0.724426 ,-0.607341, -0.394521,
+     0.750427 ,-0.312161, 0.698925, 0.899719 ,0.101228, -0.927363,
+     -0.962708 ,-0.934241, 0.041214, -0.354553 ,-0.826005, -0.284775,
+     -0.507446 ,-0.363751, -0.929287, -0.173584 ,-0.141266, 0.983869,
+     -0.613525 ,-0.436139, -0.074329, 0.899292 ,-0.875355, -0.480839,
+     0.057556 ,0.250714, 0.071270, 0.967896 ,0.182131, 0.811467, 0.950195 ,
+    -0.687696, -0.668570, -0.380554 ,0.785175, -0.540171, -0.359863 ,
+    0.399774, 0.848526, 0.655151 ,-0.412243, -0.004602, 0.911072 ,-0.132187,
+     -0.990485, 0.278198 ,0.212421, 0.764179, 0.944214 ,-0.694878, 0.234042,
+     -0.699402 ,0.404273, 0.904644, -0.316406 ,0.358393, 0.087135,
+     0.933044 ,-0.473398, 0.820774, -0.559692 ,0.044667, -0.997938,
+     0.718201 ,0.603896, -0.046386, 0.796570 ,-0.968822, 0.180966,
+     0.172058 ,-0.458206, 0.886932, -0.126221 ,-0.656709, -0.410319,
+     0.693848 ,0.999495, -0.018023, 0.026184 ,-0.486069, -0.740178,
+     -0.690979 ,0.942399, -0.333819, 0.022461 ,-0.294545, 0.867619,
+     0.805664 ,0.886791, -0.416081, -0.221252 ,-0.797187, 0.587661,
+     -0.171021 ,-0.617708, -0.762817, -0.295654 ,0.449351, -0.853660,
+     -0.505615 ,0.065153, -0.995535, 0.723572 ,0.996518, 0.000000,
+     0.083374 ,0.263346, 0.088663, -0.964417 ,-0.221316, -0.970864,
+     0.383423 ,-0.512560, 0.718804, 0.675598 ,0.588859, 0.406293,
+     -0.764648 ,-0.803841, -0.592769, -0.061646 ,0.860199, 0.492898,
+     -0.150330 ,-0.351871, 0.858024, 0.728455 ,0.515724, -0.815149,
+     0.455322 ,-0.122322, -0.960484, 0.898254 ,-0.529020, 0.844443,
+     -0.156799 ,0.530671, -0.725304, 0.637024 ,-0.748915, -0.248928,
+     -0.634094 ,-0.188099, 0.584087, 0.972778 ,0.974165, 0.222094,
+     -0.041992 ,0.595326, -0.701663, -0.549438 ,-0.060279, -0.998047,
+     -0.262451 ,-0.191682, -0.782292, -0.951477 ,0.528851, -0.596315,
+     0.752319 ,0.612134, 0.639567, -0.604919 ,0.882803, 0.200541, 0.433594 ,
+    -0.936278, -0.039490, 0.349304 ,0.940848, -0.121649, 0.318604 ,
+    -0.115022, 0.048685, -0.993347 ,-0.324162, -0.935726, -0.394226 ,
+    -0.937457, -0.294685, 0.193909 ,0.894463, -0.437237, 0.104065 ,
+    -0.861852, -0.165102, -0.486206 ,-0.980480, -0.139899, 0.139526 ,
+    -0.024496, 0.960750, -0.996094 ,-0.699760, 0.714256, -0.018860 ,
+    0.538575, -0.792107, 0.470581 ,0.309926, -0.943720, 0.349182 ,0.525671,
+     -0.772280, 0.561523 ,-0.793079, 0.268745, 0.567505 ,0.697504,
+     -0.421131, 0.639221 ,-0.737871, 0.672553, -0.076660 ,-0.390769,
+     -0.894942, -0.482666 ,-0.593469, 0.191892, 0.796448 ,0.439379,
+     -0.896646, 0.123108 ,0.337698, -0.703709, -0.879822 ,-0.654687,
+     0.749517, 0.148071 ,-0.482070, -0.700569, 0.737305 ,0.626971, 0.761948,
+     -0.250610 ,0.616585, 0.015339, -0.787231 ,-0.175877, -0.982000,
+     0.364624 ,0.891483, -0.324585, -0.334167 ,0.858029, 0.438272,
+     -0.297913 ,0.949369, 0.258757, 0.184448 ,0.105948, -0.901183,
+     0.969666 ,-0.261581, 0.943276, -0.615845 ,-0.682063, -0.528339,
+     -0.595520 ,-0.810856, 0.514103, -0.326050 ,-0.163757, 0.986118,
+     0.165527 ,-0.595927, -0.221907, 0.791504 ,-0.160374, -0.977354,
+     0.652405 ,-0.428837, 0.641628, -0.829102 ,-0.634149, -0.486378,
+     -0.687927 ,-0.093271, -0.995222, -0.295654 ,0.988659, -0.150144,
+     -0.003357 ,0.730821, -0.497396, -0.538818 ,-0.781913, -0.621260,
+     -0.065674 ,-0.655884, -0.753313, -0.073486 ,0.845542, -0.409094,
+     0.375977 ,-0.630041, -0.514925, -0.678101 ,0.205571, 0.978634,
+     -0.019531 ,0.582841, 0.763684, -0.430054 ,0.685084, -0.728464,
+     0.000000 ,-0.241437, -0.958430, -0.532898 ,0.741884, 0.020899,
+     -0.670349 ,0.740273, -0.318412, 0.624634 ,-0.738068, -0.539041,
+     0.481812 ,-0.965798, -0.034508, -0.257141 ,0.495184, 0.805372,
+     0.549683 ,-0.572524, 0.809558, -0.221008 ,-0.537181, 0.834652,
+     0.220825 ,-0.899741, 0.097826, -0.427368 ,-0.370148, 0.494066,
+     0.904846 ,0.711387, 0.577688, 0.490356 ,0.183324, -0.722791,
+     -0.964172 ,0.552815, -0.807753, -0.347351 ,-0.096050, 0.994565,
+     -0.386047 ,-0.884907, 0.369536, 0.305115 ,-0.832976, -0.551898,
+     0.047363 ,0.338883, 0.641922, 0.897034 ,0.805354, 0.506187, 0.357727 ,
+    -0.040128, 0.998805, -0.570923 ,0.466918, -0.602455, 0.811035 ,0.139166,
+     -0.983697, 0.633362 ,-0.253765, -0.340498, -0.962891 ,-0.448806,
+     0.843929, 0.547791 ,-0.859087, -0.434649, -0.300110 ,0.287570,
+     0.957661, 0.047729 ,0.379100, 0.795023, 0.780640 ,0.154245, -0.987903,
+     -0.103088 ,-0.538067, 0.794791, -0.462524 ,-0.466455, -0.180966,
+     0.880371 ,-0.175736, -0.983766, 0.202576 ,-0.891655, 0.192080,
+     -0.417725 ,-0.688716, -0.619004, 0.480652 ,0.120790, -0.987844,
+     -0.629456 ,-0.075080, 0.983385, 0.910461 ,0.147032, -0.960431,
+     -0.849304 ,0.732309, 0.671559, 0.152283 ,0.804657, 0.273913,
+     -0.547729 ,0.391462, -0.913976, 0.263184 ,-0.567300, 0.783128,
+     0.409607 ,0.214917, 0.167182, -0.975952 ,0.367428, -0.789995,
+     -0.800537 ,-0.320112, 0.912727, -0.621399 ,0.659247, -0.647346,
+     -0.501892 ,0.222842, -0.696452, -0.950562 ,-0.697513, -0.576278,
+     0.521118 ,0.602260, -0.756081, 0.391418 ,-0.116043, 0.992942,
+     0.206665 ,0.220693, -0.968855, -0.453552 ,0.737991, 0.670137,
+     0.106812 ,0.198419, -0.696590, 0.960999 ,-0.391866, -0.883543,
+     0.547668 ,0.082067, -0.996213, 0.330200 ,-0.806059, 0.491897,
+     -0.377991 ,-0.992265, 0.120698, 0.029236 ,0.406622, -0.867524,
+     0.575928 ,0.789945, 0.608406, 0.096191 ,-0.531904, -0.004218,
+     -0.846802 ,0.558298, -0.089427, 0.828125 ,-0.783155, 0.363828,
+     -0.541382 ,0.981706, -0.183228, 0.052673 ,-0.388642, 0.920618,
+     -0.096497 ,-0.506403, -0.044662, -0.862000 ,-0.512421, -0.852059,
+     -0.204163 ,0.559542, 0.339777, 0.803772 ,0.527502, -0.846389,
+     0.137573 ,-0.184315, -0.952725, 0.794983 ,0.125024, -0.977110,
+     -0.809082 ,-0.643507, 0.678632, 0.482056 ,-0.277474, 0.954056,
+     0.377380 ,-0.622333, -0.717603, 0.448914 ,0.366846, -0.110794,
+     -0.929382 ,0.120402, 0.992596, 0.131653 ,-0.982921, 0.103550,
+     -0.152954 ,-0.058333, -0.997913, -0.428894 ,0.132631, 0.979299,
+     0.755432 ,0.326398, 0.937806, 0.340637 ,0.211720, 0.976659, 0.168640 ,
+    0.957557, -0.019174, -0.287659 ,-0.016554, 0.999650, 0.780090 ,
+    -0.271222, 0.827292, -0.875732 ,0.850790, -0.448069, 0.307129 ,0.115949,
+     0.600003, -0.989441 ,0.285877, -0.940896, -0.536255 ,-0.321317,
+     -0.278336, -0.942383 ,-0.422133, 0.754447, 0.765747 ,0.669674,
+     -0.741852, -0.051514 ,0.213604, -0.949888, 0.730103 ,0.619681,
+     -0.751798, -0.341797 ,-0.223762, 0.438616, -0.968506 ,-0.302925,
+     -0.945732, 0.361877 ,0.121093, -0.977151, -0.821838 ,0.127125,
+     0.758710, -0.980774 ,0.691682, 0.695626, 0.270203 ,0.241114, 0.967463,
+     -0.303040 ,-0.829705, 0.422869, 0.402100 ,-0.484170, -0.741723,
+     0.692017 ,-0.431259, -0.777492, -0.727844 ,0.835756, -0.211986,
+     0.518311 ,0.297724, 0.932993, 0.561829 ,0.633475, -0.764920,
+     -0.181091 ,-0.833849, -0.453546, -0.353027 ,-0.369433, 0.839581,
+     -0.733154 ,0.555847, 0.392934, -0.796631 ,-0.856065, 0.028375,
+     0.516296 ,0.067161, 0.997565, 0.269409 ,-0.962279, -0.051749,
+     0.267456 ,-0.738893, 0.080065, -0.671204 ,-0.764325, 0.462240,
+     0.507019 ,0.148758, 0.751545, 0.974243 ,-0.153430, -0.318230,
+     0.986816 ,-0.439372, 0.776405, 0.716919 
+};
+

mathlib/planefit.cpp

@@ -0,0 +1,327 @@
+//============ Copyright (c) Valve Corporation, All rights reserved. ============
+//
+// Code to compute the equation of a plane with a least-squares residual fit.
+//
+//===============================================================================
+
+#include "vplane.h"
+#include "mathlib.h"
+#include <algorithm>
+using namespace std;
+
+//////////////////////////////////////////////////////////////////////////
+// Forward Declarations
+//////////////////////////////////////////////////////////////////////////
+
+static const float DETERMINANT_EPSILON = 1e-6f;
+
+template< int PRIMARY_AXIS >
+bool ComputeLeastSquaresPlaneFit( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane );
+
+//////////////////////////////////////////////////////////////////////////
+// Public Implementation
+//////////////////////////////////////////////////////////////////////////
+
+bool ComputeLeastSquaresPlaneFitX( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane )
+{
+	return ComputeLeastSquaresPlaneFit<0>( pPoints, nNumPoints, pFitPlane );
+}
+
+bool ComputeLeastSquaresPlaneFitY( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane )
+{
+	return ComputeLeastSquaresPlaneFit<1>( pPoints, nNumPoints, pFitPlane );
+}
+
+bool ComputeLeastSquaresPlaneFitZ( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane )
+{
+	return ComputeLeastSquaresPlaneFit<2>( pPoints, nNumPoints, pFitPlane );
+}
+
+float ComputeSquaredError( const Vector *pPoints, int nNumPoints, const VPlane *pFitPlane )
+{
+	float flSqrError = 0.0f;
+	float flError = 0.0f;
+	for ( int i = 0; i < nNumPoints; ++ i )
+	{
+		float flDist = pFitPlane->DistTo( pPoints[i] );
+		flError += flDist;
+		flSqrError += flDist * flDist;
+	}
+
+	return flSqrError;
+}
+
+//////////////////////////////////////////////////////////////////////////
+// Private Implementation
+//////////////////////////////////////////////////////////////////////////
+
+// Because this is not a least-squares orthogonal distance fit, an axis must be specified along which residuals are computed.
+// A traditional least-squares linear regression computes residuals along the y-axis and fits to a function of x, meaning that vertical lines cannot be properly fit.
+// Similarly, this algorithm cannot properly fit planes which lie along a plane parallel to the primary axis
+// 
+// PRIMARY_AXIS 
+// X = 0
+// Y = 1
+// Z = 2
+template< int PRIMARY_AXIS >
+bool ComputeLeastSquaresPlaneFit( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane )
+{
+	memset( pFitPlane, 0, sizeof( VPlane ) );
+
+	if ( nNumPoints < 3 )
+	{
+		// We must have at least 3 points to fit a plane
+		return false;
+	}
+
+	Vector vCentroid( 0, 0, 0 ); // averages: x-bar, y-bar, z-bar
+	Vector vSquaredSums( 0, 0, 0 ); // x => x*x, y => y*y, z => z*z
+	Vector vCrossSums( 0, 0, 0 ); // x => y*z, y => x*z, z >= x*y
+	float flNumPoints = ( float )nNumPoints;
+	
+	for ( int i = 0; i < nNumPoints; ++ i )
+	{
+		vCentroid += pPoints[i];
+		vSquaredSums += pPoints[i] * pPoints[i];
+		vCrossSums.x += pPoints[i].y * pPoints[i].z;
+		vCrossSums.y += pPoints[i].x * pPoints[i].z;
+		vCrossSums.z += pPoints[i].x * pPoints[i].y;
+	}
+
+	vCentroid /= ( float ) nNumPoints;
+	
+	if ( PRIMARY_AXIS == 0 )
+	{
+		// swap X and Z
+		swap( vCentroid.x, vCentroid.z );
+		swap( vSquaredSums.x, vSquaredSums.z );
+		swap( vCrossSums.x, vCrossSums.z );
+	}
+	else if ( PRIMARY_AXIS == 1 )
+	{
+		// Swap Y and Z
+		swap( vCentroid.y, vCentroid.z );
+		swap( vSquaredSums.y, vSquaredSums.z );
+		swap( vCrossSums.y, vCrossSums.z );
+	}
+
+	// Solve system of equations:
+	// (example assumes primary axis is Z)
+	//
+	// A * ( sum( xi * xi ) - n * vCentroid.x^2 ) + B * ( sum( xi * yi ) - n * vCentroid.x * vCentroid.y ) - sum( xi * zi ) + n * vCentroid.x * vCentroid.z = 0
+	// A * ( sum( xi * yi ) - n * vCentroid.x * vCentroid.y ) + B * ( sum( yi * yi ) - n * vCentroid.y ^ 2 ) - sum( yi * zi ) + n * vCentroid.y * vCentroid.z = 0
+	// C = vCentroid.z - A * vCentroid.x - B * vCentroid.y
+	//
+	// where z = Ax + By + C
+	//
+	// Transform to:
+	// [ m11 m12 ] [ A ]  =  [ c1 ]
+	// [ m21 m22 ] [ B ]  =  [ c2 ]
+	//
+	// M * x = C
+	// Take the inverse of M, post-multiply by C:
+	// x = M_inverse * C
+
+	float flM11 = vSquaredSums.x - flNumPoints * vCentroid.x * vCentroid.x;
+	float flM12 = vCrossSums.z - flNumPoints * vCentroid.x * vCentroid.y;
+	float flC1 = vCrossSums.y - flNumPoints * vCentroid.x * vCentroid.z;
+	float flM21 = vCrossSums.z - flNumPoints * vCentroid.x * vCentroid.y;
+	float flM22 = vSquaredSums.y - flNumPoints * vCentroid.y * vCentroid.y;
+	float flC2 = vCrossSums.x - flNumPoints * vCentroid.y * vCentroid.z;
+
+	float flDeterminant = flM11 * flM22 - flM12 * flM21;
+	if ( fabsf( flDeterminant ) > DETERMINANT_EPSILON )
+	{
+		float flInvDeterminant = 1.0f / flDeterminant;
+		float flA = flInvDeterminant * ( flM22 * flC1 - flM12 * flC2 );
+		float flB = flInvDeterminant * ( -flM21 * flC1 + flM11 * flC2 );
+		float flC = vCentroid.z - flA * vCentroid.x - flB * vCentroid.y;
+
+		pFitPlane->m_Normal = Vector( -flA, -flB, 1.0f );
+		float flScale = pFitPlane->m_Normal.NormalizeInPlace();
+		pFitPlane->m_Dist = flC * 1.0f / flScale;
+
+		if ( PRIMARY_AXIS == 0 )
+		{
+			// swap X and Z
+			swap( pFitPlane->m_Normal.x, pFitPlane->m_Normal.z );
+		}
+		else if ( PRIMARY_AXIS == 1 )
+		{
+			// Swap Y and Z
+			swap( pFitPlane->m_Normal.y, pFitPlane->m_Normal.z );
+		}
+
+		return true;
+	}
+
+	// Bad determinant
+	return false;
+}
+
+
+struct Complex_t
+{
+	float r;
+	float i;
+
+	Complex_t() { }
+	Complex_t( float flR, float flI ) : r( flR ), i( flI ) { }
+
+	static Complex_t FromPolar( float flRadius, float flTheta )
+	{
+		return Complex_t( flRadius * cosf( flTheta ), flRadius * sinf( flTheta ) );
+	}
+
+	static Complex_t SquareRoot( float flValue )
+	{
+		if ( flValue < 0.0f )
+		{
+			return Complex_t( 0.0f, sqrtf( -flValue ) );
+		}
+		else
+		{
+			return Complex_t( sqrtf( flValue ), 0.0f );
+		}
+	}
+
+	Complex_t operator+( const Complex_t &rhs ) const
+	{
+		return Complex_t( r + rhs.r, i + rhs.i );
+	}
+
+	Complex_t operator-( const Complex_t &rhs ) const
+	{
+		return Complex_t( r - rhs.r, i - rhs.i );
+	}
+
+	Complex_t operator*( const Complex_t &rhs ) const
+	{
+		return Complex_t( r * rhs.r - i * rhs.i, r * rhs.i + i * rhs.r );
+	}
+
+	Complex_t operator*( float rhs ) const
+	{
+		return Complex_t( r * rhs, i * rhs );
+	}
+
+	Complex_t operator/( float rhs ) const
+	{
+		return Complex_t( r / rhs, i / rhs );
+	}
+
+	Complex_t CubeRoot() const
+	{
+		float flRadius = sqrtf( r * r + i * i );
+		float flTheta = atan2f( i, r );
+		//if ( flTheta < 0.0f ) flTheta += 2.0f * 3.14159f;
+
+		// Demoivre's theorem for principal root
+		return FromPolar( powf( flRadius, 1.0f / 3.0f ), flTheta / 3.0f );
+	}
+};
+
+// [kutta]
+// This code is a work-in-progress; need to write code to robustly find an eigenvector given its eigenvalue.
+#if USE_ORTHOGONAL_LEAST_SQUARES
+
+template< int PRIMARY_AXIS = 0 >
+bool TryFindEigenvector( float flEigenvalue, const Vector *pMatrix, Vector *pEigenvector )
+{
+	const float flCoefficientEpsilon = 1e-3;
+
+	const int nOtherRow1 = ( PRIMARY_AXIS + 1 ) % 3;
+	const int nOtherRow2 = ( PRIMARY_AXIS + 2 ) % 3;
+
+	bool bUseRow1 = fabsf( pMatrix[0][nOtherRow1] / flEigenvalue ) > flCoefficientEpsilon && fabsf( pMatrix[0][nOtherRow2] / flEigenvalue ) > flCoefficientEpsilon );
+	bool bUseRow2 = fabsf( pMatrix[1][nOtherRow1] / flEigenvalue ) > flCoefficientEpsilon && fabsf( pMatrix[1][nOtherRow2] / flEigenvalue ) > flCoefficientEpsilon );
+	bool bUseRow3 = fabsf( pMatrix[2][nOtherRow1] / flEigenvalue ) > flCoefficientEpsilon && fabsf( pMatrix[2][nOtherRow2] / flEigenvalue ) > flCoefficientEpsilon );
+
+	// ...
+}
+
+bool ComputeLeastSquaresOrthogonalPlaneFit( const Vector *pPoints, int nNumPoints, VPlane *pFitPlane )
+{
+	memset( pFitPlane, 0, sizeof( VPlane ) );
+
+	if ( nNumPoints < 3 )
+	{
+		// We must have at least 3 points to fit a plane
+		return false;
+	}
+
+	Vector vCentroid( 0, 0, 0 ); // averages: x-bar, y-bar, z-bar
+	Vector vSquaredSums( 0, 0, 0 ); // x => x*x, y => y*y, z => z*z
+	Vector vCrossSums( 0, 0, 0 ); // x => y*z, y => x*z, z >= x*y
+	float flNumPoints = ( float )nNumPoints;
+
+	for ( int i = 0; i < nNumPoints; ++ i )
+	{
+		vCentroid += pPoints[i];
+		vSquaredSums += pPoints[i] * pPoints[i];
+		vCrossSums.x += pPoints[i].y * pPoints[i].z;
+		vCrossSums.y += pPoints[i].x * pPoints[i].z;
+		vCrossSums.z += pPoints[i].x * pPoints[i].y;
+	}
+
+	vCentroid /= ( float ) nNumPoints;
+
+	// Re-center the squared and cross sums
+	vSquaredSums.x -= flNumPoints * vCentroid.x * vCentroid.x;
+	vSquaredSums.y -= flNumPoints * vCentroid.y * vCentroid.y;
+	vSquaredSums.z -= flNumPoints * vCentroid.z * vCentroid.z;
+	vCrossSums.x -= flNumPoints * vCentroid.y * vCentroid.z;
+	vCrossSums.y -= flNumPoints * vCentroid.x * vCentroid.z;
+	vCrossSums.z -= flNumPoints * vCentroid.x * vCentroid.y;
+
+	// Best fit normal occurs at the minimum of the Rayleigh quotient:
+	//
+	// n' * M * n
+	// ----------
+	//   n' *  n
+	//
+	// Where M is the covariance matrix.
+	// M is computed from ( A' * A ) where A is a 3xN matrix of x/y/z residuals for each point in the data set.
+
+
+	// Solve for eigenvalues & eigenvectors of 3x3 real symmetric covariance matrix.
+	// The resulting characteristic polynormial equation is a cubic of the form:
+	// x^3 + Ax^2 + Bx + C = 0
+	//
+	// All roots of the equation and eigenvalues are positive real values; the lowest one corresponds to the eigenvalue which is the normal to the best fit plane.
+
+	float flA = -( vSquaredSums.x + vSquaredSums.y + vSquaredSums.z );
+	float flB = vSquaredSums.x * vSquaredSums.z + vSquaredSums.x * vSquaredSums.y + vSquaredSums.y * vSquaredSums.z - vCrossSums.x * vCrossSums.x - vCrossSums.y * vCrossSums.y - vCrossSums.z * vCrossSums.z;
+	float flC = -( vSquaredSums.x * vSquaredSums.y * vSquaredSums.z + 2.0f * vCrossSums.x * vCrossSums.y * vCrossSums.z ) + ( vSquaredSums.x * vCrossSums.x * vCrossSums.x + vSquaredSums.y * vCrossSums.y * vCrossSums.y + vSquaredSums.z * vCrossSums.z * vCrossSums.z );
+
+	// Using formula for roots of cubic polynomial, see http://en.wikipedia.org/wiki/Cubic_function
+	float flM = 2.0f * flA * flA * flA - 9.0f * flA * flB + 27.0f * flC;
+	float flK = flA * flA - 3.0f * flB;
+	float flN = flM * flM - 4.0f * flK * flK * flK;
+
+	Complex_t flSolutions[3];
+	const Complex_t omega1( -0.5f, 0.5f * sqrtf( 3.0f ) );
+	const Complex_t omega2( -0.5f, -0.5f * sqrtf( 3.0f ) );
+	Complex_t complexA = Complex_t( flA, 0.0f );
+	Complex_t complexM = Complex_t( flM, 0.0f );
+	Complex_t intermediateA = ( ( complexM + Complex_t::SquareRoot( flN ) ) / 2.0f );
+	Complex_t intermediateB = ( ( complexM - Complex_t::SquareRoot( flN ) ) / 2.0f );
+	Complex_t cubeA = intermediateA.CubeRoot(); 
+	Complex_t cubeB = intermediateB.CubeRoot();
+	Complex_t tempA = cubeA * cubeA * cubeA;
+	Complex_t tempB = cubeB * cubeB * cubeB;
+
+	flSolutions[0] = ( complexA + cubeA + cubeB ) * -1.0f / 3.0f;
+	flSolutions[1] = ( complexA + ( omega2 * cubeA ) + ( omega1 * cubeB ) ) * -1.0f / 3.0f;
+	flSolutions[2] = ( complexA + ( omega1 * cubeA ) + ( omega2 * cubeB ) ) * -1.0f / 3.0f;
+
+	float flMinEigenvalue = MIN( flSolutions[0].r, flSolutions[1].r );
+	flMinEigenvalue = MIN( flMinEigenvalue, flSolutions[2].r );
+
+	// Subtract eigenvalue from the diagonal of the matrix to get a 3x3, real-symmetric, non-invertible matrix.
+	// Pick 2 non-zero rows from this matrix and construct a system of 2 equations.
+	
+	return true;
+}
+
+#endif // USE_ORTHOGONAL_LEAST_SQUARES

mathlib/polygon.cpp

@@ -0,0 +1,665 @@
+//============ Copyright (c) Valve Corporation, All rights reserved. ============
+//
+// Utility functions for polygon simplification / convex decomposition.
+//
+//===============================================================================
+
+#include "polygon.h"
+#include "quadric.h"
+
+// Assumes that points are ordered clockwise when looking at the face of the polygon
+void SimplifyPolygon( CUtlVector< Vector > *pPoints, const Vector &vNormal, float flMaximumDeviation )
+{
+	if ( pPoints->Count() < 3 )
+	{
+		return;
+	}
+
+	int nVertices = pPoints->Count();
+	Vector vPrevious = pPoints->Element( nVertices - 1 );
+	Vector vCurrent = pPoints->Element( 0 );
+	Vector vNext;
+	
+	// Walk around the polygon removing redundant vertices
+	for ( int i = 0; i < nVertices; ++ i )
+	{
+		vNext = pPoints->Element( ( i + 1 ) % nVertices );
+
+		// Is the current vertex redundant?
+
+		Vector vCurrentToNext = vNext - vCurrent;
+		Vector vCurrentToPrevious = vPrevious - vCurrent;
+		// Compute the area of of the error triangle added (if negative) or removed (if positive) with the removal of this vertex
+		float flAreaDeviation = 0.5f * vCurrentToPrevious.Cross( vCurrentToNext ).Dot( vNormal );
+
+		// Only consider a point for removal if it would decrease the polygon area (be conservative)
+		if ( flAreaDeviation >= 0.0f && flAreaDeviation < flMaximumDeviation )
+		{
+			// redundant
+			pPoints->Remove( i );
+			-- nVertices;
+			-- i;
+		}
+		else
+		{
+			vPrevious = vCurrent;
+		}
+
+		vCurrent = vNext;
+	}
+}
+
+static void ComputeQuadric( const CUtlVector< Vector > &points, int nVertex, const Vector &vNormal, CQuadricError *pError )
+{
+	int nVertices = points.Count();
+	Vector vPrevious = points[( nVertex + nVertices - 1 ) % nVertices];
+	Vector vCurrent = points[nVertex];
+	Vector vNext = points[( nVertex + 1 ) % nVertices];
+	Vector vAbove = vCurrent + vNormal;
+
+	pError->InitFromPlane( vNormal, -DotProduct( vNormal, vCurrent ), 1.0f );
+	
+	CQuadricError line1, line2;
+	line1.InitFromTriangle( vPrevious, vCurrent, vAbove, 0.0f );
+	*pError += line1;
+	line2.InitFromTriangle( vNext, vCurrent, vAbove, 0.0f );
+	*pError += line2;
+}
+
+void SimplifyPolygonQEM( CUtlVector< Vector > *pPoints, const Vector &vNormal, float flMaximumSquaredError, bool bUseOptimalPointPlacement )
+{
+	if ( pPoints->Count() < 3 )
+	{
+		return;
+	}
+	
+	CUtlVector< CQuadricError > quadrics;
+	quadrics.EnsureCapacity( pPoints->Count() );
+	
+	int nVertices = pPoints->Count();
+	Vector vPrevious = pPoints->Element( nVertices - 1 );
+	Vector vCurrent = pPoints->Element( 0 );
+	Vector vNext;
+
+	// Compute quadric error functions for each vertex
+	for ( int i = 0; i < nVertices; ++ i )
+	{
+		vNext = pPoints->Element( ( i + 1 ) % nVertices );
+
+		Vector vAbove = vCurrent + vNormal;
+		quadrics.AddToTail();
+		quadrics[i].InitFromPlane( vNormal, -DotProduct( vNormal, vCurrent ), 1.0f );
+		CQuadricError line1, line2;
+		line1.InitFromTriangle( vPrevious, vCurrent, vAbove, 0.0f );
+		quadrics[i] += line1;
+		line2.InitFromTriangle( vNext, vCurrent, vAbove, 0.0f );
+		quadrics[i] += line2;
+
+		vPrevious = vCurrent;
+		vCurrent = vNext;
+	}
+
+	// @TODO: use a sorted heap instead of pseudo-bubble-sort
+	// We like quadrilaterals...don't try to go simpler than that
+	while ( pPoints->Count() > 4 )
+	{
+		float flLowestError = flMaximumSquaredError;
+		Vector vBestCollapse;
+		int nCollapseIndex = -1;
+		for ( int i = 0; i < nVertices; ++ i )
+		{
+			int nNextIndex = ( i + 1 ) % nVertices;
+			CQuadricError sum = quadrics[i] + quadrics[nNextIndex];
+
+			if ( bUseOptimalPointPlacement )
+			{
+				// Solve for optimal point and collapse to it
+				Vector vOptimalPoint = sum.SolveForMinimumError();
+				float flError = sum.ComputeError( vOptimalPoint );
+				if ( flError < flLowestError )
+				{
+					flLowestError = flError;
+					vBestCollapse = vOptimalPoint;
+					nCollapseIndex = i;
+				}
+			}
+			else
+			{
+				// Only collapse to endpoints
+				Vector vA = pPoints->Element( i );
+				float flErrorA = sum.ComputeError( vA );
+				if ( flErrorA < flLowestError )
+				{
+					flLowestError = flErrorA;
+					vBestCollapse = vA;
+					nCollapseIndex = i;
+				}
+				Vector vB = pPoints->Element( nNextIndex );
+				float flErrorB = sum.ComputeError( vB );
+				if ( flErrorB < flLowestError )
+				{
+					flLowestError = flErrorB;
+					vBestCollapse = vB;
+					nCollapseIndex = i;
+				}
+			}
+		}
+
+		if ( nCollapseIndex != -1 )
+		{
+			pPoints->Element( nCollapseIndex ) = vBestCollapse;
+			int nNextIndex = ( nCollapseIndex + 1 ) % nVertices;
+			
+			pPoints->Remove( nNextIndex );
+			quadrics.Remove( nNextIndex );
+			-- nVertices;
+			
+			if ( nNextIndex < nCollapseIndex ) 
+				-- nCollapseIndex;
+
+			int nPrevIndex = ( nCollapseIndex + nVertices - 1 ) % nVertices;
+			nNextIndex = ( nCollapseIndex + 1 ) % nVertices;
+			
+			ComputeQuadric( *pPoints, nPrevIndex, vNormal, &quadrics[nPrevIndex] );
+			ComputeQuadric( *pPoints, nCollapseIndex, vNormal, &quadrics[nCollapseIndex] );
+			ComputeQuadric( *pPoints, nNextIndex, vNormal, &quadrics[nNextIndex] );
+		}
+		else
+		{
+			// we're done
+			break;
+		}
+	}
+}
+
+bool IsConcave( const Vector &v0, const Vector &v1, const Vector &v2, const Vector &vNormal )
+{
+	Vector vRay1 = v2 - v1;
+	Vector vRay2 = v0 - v1;
+	float flSign = vRay2.Cross( vRay1 ).Dot( vNormal );
+	return ( flSign < 0 );
+}
+
+bool IsConcave( const Vector *pPolygonPoints, int nPointCount, int nVertex, const Vector &vNormal )
+{
+	Assert( nPointCount >= 3 );
+	int nPrevVertex = ( nVertex + nPointCount - 1 ) % nPointCount;
+	int nNextVertex = ( nVertex + 1 ) % nPointCount;
+
+	return IsConcave( pPolygonPoints[nPrevVertex], pPolygonPoints[nVertex], pPolygonPoints[nNextVertex], vNormal );
+}
+
+bool IsConcave( const Vector *pPolygonPoints, int nPointCount, const Vector &vNormal )
+{
+	Assert( nPointCount >= 3 );
+
+	Vector vPrevVertex = pPolygonPoints[nPointCount - 2];
+	Vector vVertex = pPolygonPoints[nPointCount - 1];
+	Vector vNextVertex;
+	for ( int i = 0; i < nPointCount; ++ i )
+	{
+		vNextVertex = pPolygonPoints[i];
+		if ( IsConcave( vPrevVertex, vVertex, vNextVertex, vNormal ) )
+		{
+			return true;
+		}
+		vPrevVertex = vVertex;
+		vVertex = vNextVertex;
+	}
+
+	return false;
+}
+
+static bool IsPointInPolygon( const CUtlVector< Vector > &polygonPoints, const SubPolygon_t &convexRegion, const Vector &vNormal, const Vector &vPoint )
+{
+	int nIndex = convexRegion.m_Indices[convexRegion.m_Indices.Count() - 1];
+	Vector v0 = SubPolygon_t::GetPoint( polygonPoints, nIndex );
+
+	for ( int i = 0; i < convexRegion.m_Indices.Count(); ++ i )
+	{
+		nIndex = convexRegion.m_Indices[i];
+		Vector v1 = SubPolygon_t::GetPoint( polygonPoints, nIndex );
+		
+		Vector vRay = v1 - v0;
+		Vector vRight = vRay.Cross( vNormal );
+		if ( vRight.Dot( vPoint - v0 ) < -POINT_IN_POLYGON_EPSILON )
+		{
+			return false;
+		}
+
+		v0 = v1;
+	}
+
+	return true;
+}
+
+static bool PointsInsideConvexArea( const CUtlVector< Vector > &polygonPoints, const SubPolygon_t &originalPolygon, const Vector &vNormal, int nFirstIndex, int nLastIndex, const SubPolygon_t &convexRegion )
+{
+	nFirstIndex %= originalPolygon.m_Indices.Count();
+	nLastIndex %= originalPolygon.m_Indices.Count();
+	if ( nFirstIndex < 0 ) nFirstIndex += originalPolygon.m_Indices.Count();
+	if ( nLastIndex < 0 ) nLastIndex += originalPolygon.m_Indices.Count();
+
+	for ( int i = nFirstIndex; i != nLastIndex; i = ( i + 1 ) % originalPolygon.m_Indices.Count() )
+	{
+		int nVertex = originalPolygon.GetVertexIndex( i );
+		Vector vPoint = SubPolygon_t::GetPoint( polygonPoints, nVertex );
+		if ( IsPointInPolygon( polygonPoints, convexRegion, vNormal, vPoint ) )
+		{
+			bool bIsDoubleVertex = false;
+			// Allow points on the boundary of the convex region if they are coincident with one of the points of the region itself
+			for ( int j = 0; j < convexRegion.m_Indices.Count(); ++ j )
+			{
+				Vector vConvexRegionTestPoint = SubPolygon_t::GetPoint( polygonPoints, convexRegion.GetVertexIndex( j ) );
+				if ( VectorsAreEqual( vConvexRegionTestPoint, vPoint, POINT_IN_POLYGON_EPSILON ) )
+				{
+					bIsDoubleVertex = true;
+					break;
+				}
+			}
+			if ( !bIsDoubleVertex )
+			{
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+
+static const float LINE_INTERSECT_EPSILON = 1e-3f;
+
+// superceded by CalcLineToLineIntersectionSegment
+
+// bool LineSegmentsIntersect( const Vector &vNormal, const Vector &v1a, const Vector &v1b, const Vector &v2a, const Vector &v2b, float flEpsilon, float *pTimeOfIntersection1, float *pTimeOfIntersection2 )
+// {
+// 	Vector vDir1 = v1b - v1a;
+// 	Vector vDir2 = v2b - v2a;
+// 	Vector v1Perpendicular = vDir1.Cross( vNormal );
+// 	Vector vStartDiff = v1a - v2a;
+// 	float flNumerator = vStartDiff.Dot( v1Perpendicular );
+// 	float flDenominator = vDir2.Dot( v1Perpendicular );
+// 	// @TODO: we should probably use a different epsilon since the denominator is in different units, but this works well so far
+// 	if ( fabsf( flDenominator ) < flEpsilon )
+// 	{
+// 		return false;
+// 	}
+// 	float t2 = flNumerator / flDenominator;
+// 	if ( t2 >= -flEpsilon && t2 <= ( 1.0f + flEpsilon ) )
+// 	{
+// 		Vector vClosestPoint2 = t2 * vDir2 + v2a;
+// 		float flLength1 = vDir1.NormalizeInPlace();
+// 		// Can't be 0 otherwise flDenominator would have been 0
+// 		float t1 = ( vClosestPoint2 - v1a ).Dot( vDir1 ) / flLength1;
+// 
+// 		if ( t1 >= -flEpsilon && t1 <= ( 1.0f + flEpsilon ) )
+// 		{
+// 			*pTimeOfIntersection1 = t1;
+// 			*pTimeOfIntersection2 = t2;
+// 			return true;
+// 		}
+// 	}
+// 	return false;
+// }
+
+//-----------------------------------------------------------------------------
+// Note: this is not a general purpose intersection test;
+// it makes some assumptions that the winding of the polygon is 
+// counter-clockwise (because it's expected to be a hole) and that,
+// if the line segment vA-vB intersects a line segment in the hole 
+// (denoted by vHoleA-vHoleB, in counter-clockwise ordering),
+// then the line segment vA-vHoleB must not intersect any other part of 
+// the hole polygon.
+//-----------------------------------------------------------------------------
+static bool LineSegmentIntersectsPolygon( const CUtlVector< Vector > &polygonPoints, const Vector &vNormal, const Vector &vA, const Vector &vB, const SubPolygon_t &hole, float *pLowestTimeOfIntersection, Vector *pA, Vector *pB, int *pHoleVertexIndex )
+{
+	*pLowestTimeOfIntersection = 2.0f; // a valid time is between 0 and 1, anything outside the range is considered invalid
+	float flTimeOfIntersection;
+	Vector vPrev = SubPolygon_t::GetPoint( polygonPoints, hole.m_Indices[hole.m_Indices.Count() - 1] );
+	bool bIntersect = false;
+	Vector vInside = ( vB - vA ).Cross( vNormal );
+	
+	for ( int i = 0; i < hole.m_Indices.Count(); ++ i )
+	{
+		float flOtherTimeOfIntersection;
+		Vector vCurrent = SubPolygon_t::GetPoint( polygonPoints, hole.m_Indices[i] );
+		
+		// @TODO: make sure the replacement is ok before deleting
+		//if ( LineSegmentsIntersect( vNormal, vA, vB, vPrev, vCurrent, &flTimeOfIntersection, &flOtherTimeOfIntersection ) )
+
+		CalcLineToLineIntersectionSegment( vA, vB, vPrev, vCurrent, NULL, NULL, &flTimeOfIntersection, &flOtherTimeOfIntersection );
+		if ( flTimeOfIntersection >= -LINE_INTERSECT_EPSILON && flTimeOfIntersection <= 1.0f + LINE_INTERSECT_EPSILON && flOtherTimeOfIntersection >= -LINE_INTERSECT_EPSILON && flTimeOfIntersection <= 1.0f + LINE_INTERSECT_EPSILON )
+		{
+			// If the line segment intersection occurs right at the beginning of the hole line segment, ignore it because we'll catch it as an intersection at the end of 
+			// another hole line segment.
+			// This is required because we want to guarantee that a line segment from vA to vCurrent does not intersect the polygon at any point.
+			if ( flTimeOfIntersection < *pLowestTimeOfIntersection && flOtherTimeOfIntersection > LINE_INTERSECT_EPSILON )
+			{
+				*pLowestTimeOfIntersection = flTimeOfIntersection;
+				*pA = vPrev;
+				*pB = vCurrent;
+
+				float flPrevInsideDistance = ( vPrev - vA ).Dot( vInside );
+				float flCurrentInsideDistance = ( vCurrent - vA ).Dot( vInside );
+				if ( flCurrentInsideDistance > flPrevInsideDistance )
+				{
+					*pHoleVertexIndex = i;					
+				}
+				else
+				{
+					*pHoleVertexIndex = ( i + hole.m_Indices.Count() - 1 ) % hole.m_Indices.Count();					
+				}
+				
+				bIntersect = true;
+			}
+		}
+		vPrev = vCurrent;
+	}
+	return bIntersect;
+}
+
+void FindLineSegmentIntersectingDiagonal( const CUtlVector< Vector > &polygonPoints, const Vector &vNormal, const CUtlVector< SubPolygon_t > &holes, const Vector &vA, const Vector &vB, Vector *pHoleSegmentA, Vector *pHoleSegmentB, int *pHoleIndex, int *pHoleVertexIndex )
+{
+	float flLowestTimeOfIntersection = 2.0f;
+	int nHoleVertexIndex;
+	*pHoleIndex = -1;
+
+	// Test for holes
+	for ( int i = 0; i < holes.Count(); ++ i )
+	{
+		float flTimeOfIntersection;
+		Vector vTempA, vTempB;
+		if ( LineSegmentIntersectsPolygon( polygonPoints, vNormal, vA, vB, holes[i], &flTimeOfIntersection, &vTempA, &vTempB, &nHoleVertexIndex ) )
+		{
+			if ( flTimeOfIntersection < flLowestTimeOfIntersection )
+			{
+				flLowestTimeOfIntersection = flTimeOfIntersection;
+				*pHoleSegmentA = vTempA;
+				*pHoleSegmentB = vTempB;
+				*pHoleIndex = i;
+				*pHoleVertexIndex = nHoleVertexIndex;
+			}
+		}
+	}
+}
+
+void DecomposePolygon_Step( const CUtlVector< Vector > &polygonPoints, const Vector &vNormal, CUtlVector< SubPolygon_t > *pHoles, SubPolygon_t *pNewPartition, SubPolygon_t *pOriginalPolygon, int *pFirstIndex )
+{
+	Assert( *pFirstIndex >= 0 && *pFirstIndex < pOriginalPolygon->m_Indices.Count() );
+	
+	// Always start decomposition on a notch
+	Vector vPrev = SubPolygon_t::GetPoint( polygonPoints, pOriginalPolygon->GetVertexIndex( *pFirstIndex - 1 ) );
+	Vector vCurrent = SubPolygon_t::GetPoint( polygonPoints, pOriginalPolygon->GetVertexIndex( *pFirstIndex ) );
+	Vector vNext = SubPolygon_t::GetPoint( polygonPoints, pOriginalPolygon->GetVertexIndex( *pFirstIndex + 1 ) );
+	for ( int i = 0; i < pOriginalPolygon->m_Indices.Count(); ++ i )
+	{
+		if ( IsConcave( vPrev, vCurrent, vNext, vNormal) )
+		{
+			*pFirstIndex = ( *pFirstIndex + i ) % pOriginalPolygon->m_Indices.Count();
+			break;
+		}
+		else
+		{
+			vPrev = vCurrent;
+			vCurrent = vNext;
+			vNext = SubPolygon_t::GetPoint( polygonPoints, pOriginalPolygon->GetVertexIndex( *pFirstIndex + i + 2 ) );
+		}
+	}
+	
+	// On termination of the loop, pOriginalPolygon->m_Indices[*pFirstIndex] is the vertex index (in polygonPoints) from
+	// which to begin decomposition
+
+
+	// Attempt decomposition (first clockwise, then counter-clockwise if that fails)
+	for ( int i = 0; i < 2; ++ i )
+	{
+		bool bClockwise = ( i == 0 );
+
+		pNewPartition->m_Indices.RemoveAll();
+
+		// Grab the first 2 vertices from the remaining polygon and add to the new potential convex partition
+		int nFirstVertex = pOriginalPolygon->GetVertexIndex( *pFirstIndex );
+		int nSecondVertex = pOriginalPolygon->GetVertexIndex( *pFirstIndex + ( bClockwise ? 1 : -1 ) );
+		if ( bClockwise )
+		{
+			pNewPartition->m_Indices.AddToTail( nFirstVertex );
+			pNewPartition->m_Indices.AddToTail( nSecondVertex );
+		}
+		else
+		{
+			pNewPartition->m_Indices.AddToTail( nSecondVertex );
+			pNewPartition->m_Indices.AddToTail( nFirstVertex );
+		}		
+
+		Vector vFirst = SubPolygon_t::GetPoint( polygonPoints, nFirstVertex );
+		Vector vSecond = SubPolygon_t::GetPoint( polygonPoints, nSecondVertex );
+		Vector vPrevPrev = vFirst;
+		vPrev = vSecond;
+
+		int nNextIndex = *pFirstIndex + ( bClockwise ? 2 : -2 );
+		int nNextVertex = pOriginalPolygon->GetVertexIndex( nNextIndex );
+
+		// At the start of each iteration, *pFirstIndex refers to the index of the first vertex from the original polygon that is in the partition
+		// and nNextIndex refers to 1 past the index of the last vertex from the original polygon that is in the partition.
+		// If clockwise, you can find the new convex partition by iterating indices in original polygon from [*pFirstIndex, nNextIndex-1], 
+		// if counter-clockwise, iterate from [nNextIndex+1, *pFirstIndex].
+		while ( pNewPartition->m_Indices.Count() < pOriginalPolygon->m_Indices.Count() ) 
+		{
+			vCurrent = SubPolygon_t::GetPoint( polygonPoints, nNextVertex );
+
+			bool bConcave;
+			if ( bClockwise )
+			{
+				bConcave = IsConcave( vPrevPrev, vPrev, vCurrent, vNormal ) || IsConcave( vPrev, vCurrent, vFirst, vNormal ) || IsConcave( vCurrent, vFirst, vSecond, vNormal );
+			}
+			else
+			{
+				bConcave = IsConcave( vCurrent, vPrev, vPrevPrev, vNormal ) || IsConcave( vFirst, vCurrent, vPrev, vNormal ) || IsConcave( vSecond, vFirst, vCurrent, vNormal );
+			}
+
+			if ( bConcave )
+			{
+				// Shape is no longer convex with the addition of vCurrent
+				break;
+			}
+			else
+			{
+				if ( bClockwise )
+				{
+					pNewPartition->m_Indices.AddToTail( nNextVertex );
+					++ nNextIndex;
+				}
+				else
+				{
+					pNewPartition->m_Indices.AddToHead( nNextVertex );				
+					-- nNextIndex;
+				}
+
+				nNextVertex = pOriginalPolygon->GetVertexIndex( nNextIndex );
+				vPrevPrev = vPrev;
+				vPrev = vCurrent;
+			}
+		} 
+
+		int nFirstIndexInRemainingPolygon = bClockwise ? nNextIndex : *pFirstIndex + 1;
+		int nLastIndexInRemainingPolygon = bClockwise ? *pFirstIndex : nNextIndex + 1;
+		
+		// Test to see if any points in the remaining polygon are within the bounds of the convex polygon we're about to peel off.
+		while ( pNewPartition->m_Indices.Count() >= 3 && PointsInsideConvexArea( polygonPoints, *pOriginalPolygon, vNormal, nFirstIndexInRemainingPolygon, nLastIndexInRemainingPolygon, *pNewPartition ) )
+		{
+			if ( bClockwise )
+			{
+				pNewPartition->m_Indices.RemoveMultipleFromTail( 1 );
+				-- nNextIndex;
+			}
+			else
+			{
+				pNewPartition->m_Indices.RemoveMultipleFromHead( 1 );
+				++ nNextIndex;
+			}
+		}
+		
+		// Found a convex chunk of the original concave polygon, now check for
+		// holes or concavities which intersect this convex chunk.
+		if ( pNewPartition->m_Indices.Count() >= 3 )
+		{
+			Vector vA = SubPolygon_t::GetPoint( polygonPoints, pNewPartition->m_Indices[pNewPartition->m_Indices.Count() - 1] );
+			Vector vB = SubPolygon_t::GetPoint( polygonPoints, pNewPartition->m_Indices[0] );
+			Vector vHoleSegmentA, vHoleSegmentB;
+			int nHoleIndex = -1;
+			int nLastHoleIndex;
+			int nHoleVertexIndex;
+
+			// See if the diagonal which closes this convex region intersects any holes
+			FindLineSegmentIntersectingDiagonal( polygonPoints, vNormal, *pHoles, vA, vB, &vHoleSegmentA, &vHoleSegmentB, &nHoleIndex, &nHoleVertexIndex );
+			
+			// If there was an intersection, keep refining the diagonal until it no longer changes
+			if ( nHoleIndex != -1 )
+			{
+				do 
+				{
+					nLastHoleIndex = nHoleIndex;
+					vB = SubPolygon_t::GetPoint( polygonPoints, pHoles->Element( nHoleIndex ).GetVertexIndex( nHoleVertexIndex ) );
+					FindLineSegmentIntersectingDiagonal( polygonPoints, vNormal, *pHoles, vA, vB, &vHoleSegmentA, &vHoleSegmentB, &nHoleIndex, &nHoleVertexIndex );
+
+					Assert( nHoleIndex != -1 );
+				} while ( nHoleIndex != nLastHoleIndex );
+			}
+			
+			// If there was no intersection, check to see if this convex region completely encloses any holes			
+			if ( nHoleIndex == -1 )
+			{
+				Vector vHolePoint;
+				int nHoleInRegionIndex = -1;
+				for ( int i = 0; i < pHoles->Count(); ++ i )
+				{
+					vHolePoint = SubPolygon_t::GetPoint( polygonPoints, pHoles->Element( i ).m_Indices[0] );
+					if ( IsPointInPolygon( polygonPoints, *pNewPartition, vNormal, vHolePoint ) )
+					{
+						// hole is within the region
+						nHoleInRegionIndex = i;
+						break;
+					}
+				}
+
+				if ( nHoleInRegionIndex != -1 )
+				{
+					// If any holes are enclosed, "fix" the diagonal to connect to an arbitrary vertex on one of the enclosed holes
+					vB = vHolePoint;
+
+					// Now test to see if this new diagonal intersects any holes
+					FindLineSegmentIntersectingDiagonal( polygonPoints, vNormal, *pHoles, vA, vB, &vHoleSegmentA, &vHoleSegmentB, &nHoleIndex, &nHoleVertexIndex );
+
+					// If there was an intersection, keep refining the diagonal until it no longer changes
+					if ( nHoleIndex != -1 )
+					{
+						do 
+						{
+							nLastHoleIndex = nHoleIndex;
+							vB = SubPolygon_t::GetPoint( polygonPoints, pHoles->Element( nHoleIndex ).GetVertexIndex( nHoleVertexIndex ) );
+							FindLineSegmentIntersectingDiagonal( polygonPoints, vNormal, *pHoles, vA, vB, &vHoleSegmentA, &vHoleSegmentB, &nHoleIndex, &nHoleVertexIndex );
+							Assert( nHoleIndex != -1 );
+						} while ( nHoleIndex != nLastHoleIndex );
+					}
+				}
+			}
+
+			// At this point, we should either have the original convex region which contains no holes and intersects with nothing,
+			// or a refined diagonal which connects to a hole without intersecting any other holes or convex region points
+			if ( nHoleIndex != -1 )
+			{
+				// We have a refined diagonal; absorb the hole into the original polygon.
+				// Reject this partition but add the hole's vertices to the original polygon.
+
+				int nInsertAfterIndex = ( bClockwise ? nNextIndex + pOriginalPolygon->m_Indices.Count() - 1 : *pFirstIndex ) % pOriginalPolygon->m_Indices.Count();
+				const SubPolygon_t *pHolePolygon = &pHoles->Element( nHoleIndex );
+				int nConnectBackToVertex = pOriginalPolygon->m_Indices[nInsertAfterIndex];
+				for ( int i = 0; i < pHolePolygon->m_Indices.Count(); ++ i )
+				{
+					pOriginalPolygon->m_Indices.InsertAfter( nInsertAfterIndex, pHolePolygon->m_Indices[( i + nHoleVertexIndex ) % pHolePolygon->m_Indices.Count()] );
+					++ nInsertAfterIndex;
+				}
+				pOriginalPolygon->m_Indices.InsertAfter( nInsertAfterIndex, pHolePolygon->m_Indices[nHoleVertexIndex] );
+				++ nInsertAfterIndex;
+				pOriginalPolygon->m_Indices.InsertAfter( nInsertAfterIndex, nConnectBackToVertex );
+				
+				pNewPartition->m_Indices.RemoveAll();
+				pHoles->Remove( nHoleIndex );
+			}
+			else
+			{
+				// We have the original, valid diagonal.
+				// Remove the corresponding indices from the original polygon to peel off the new convex region
+				int nIndexToRemove = ( ( bClockwise ? *pFirstIndex : nNextIndex + 1 ) + 1 ) % pOriginalPolygon->m_Indices.Count();
+				if ( nIndexToRemove < 0 ) nIndexToRemove += pOriginalPolygon->m_Indices.Count();
+
+				for ( int i = 1; i < pNewPartition->m_Indices.Count() - 1; ++ i )
+				{
+					nIndexToRemove = nIndexToRemove % pOriginalPolygon->m_Indices.Count();
+					Assert( pOriginalPolygon->m_Indices[nIndexToRemove] == pNewPartition->m_Indices[i] );
+					pOriginalPolygon->m_Indices.Remove( nIndexToRemove );
+				}
+
+				*pFirstIndex = nIndexToRemove % pOriginalPolygon->m_Indices.Count();
+			}
+
+			// Done!
+			return;
+		}
+	}
+
+	// Couldn't find a match either clockwise or counter-clockwise
+	*pFirstIndex = ( *pFirstIndex + 1 ) % pOriginalPolygon->m_Indices.Count();
+}
+
+void DecomposePolygon( const CUtlVector< Vector > &polygonPoints, const Vector &vNormal, SubPolygon_t *pOriginalPolygon, CUtlVector< SubPolygon_t > *pHoles, CUtlVector< SubPolygon_t > *pPartitions )
+{
+	int nFirstIndex = 0; // The Nth vertex in the remaining polygon
+
+	SubPolygon_t *pNewPartition = NULL;
+	while ( pOriginalPolygon->m_Indices.Count() >= 3 )
+	{
+		if ( !pNewPartition )
+		{
+			pNewPartition = &pPartitions->Element( pPartitions->AddToTail() );
+		}
+		DecomposePolygon_Step( polygonPoints, vNormal, pHoles, pNewPartition, pOriginalPolygon, &nFirstIndex );
+		if ( pNewPartition->m_Indices.Count() >= 3 )
+		{
+			pNewPartition = NULL;
+		}
+		else
+		{
+			pNewPartition->m_Indices.RemoveAll();
+		}
+	}
+}
+
+bool IsPointInPolygonPrism( const Vector *pPolygonPoints, int nPointCount, const Vector &vPoint, float flThreshold, float *pHeight )
+{
+	Assert( nPointCount >= 3 );
+
+	Vector vNormal = ( pPolygonPoints[0] - pPolygonPoints[1] ).Cross( pPolygonPoints[2] - pPolygonPoints[1] );
+	Vector vPrev = pPolygonPoints[nPointCount - 1];
+	for ( int nVertexIndex = 0; nVertexIndex < nPointCount; ++ nVertexIndex )
+	{
+		Vector vCurrent = pPolygonPoints[nVertexIndex];
+		Vector vAbove = vPrev + vNormal;
+		Vector vOutwardPlaneNormal = ( vPrev - vCurrent ).Cross( vAbove - vCurrent );
+
+		if ( ( vPoint - vCurrent ).Dot( vOutwardPlaneNormal ) > flThreshold )
+		{
+			// Outside the prism
+			return false;
+		}
+		vPrev = vCurrent;
+	}
+
+	if ( pHeight != NULL )
+	{
+		vNormal.NormalizeInPlace();
+		*pHeight = ( vPoint - pPolygonPoints[0] ).Dot( vNormal );
+	}
+
+	return true;
+}

mathlib/powsse.cpp

@@ -0,0 +1,100 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+//=====================================================================================//
+
+#include "mathlib/ssemath.h"
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+fltx4 Pow_FixedPoint_Exponent_SIMD( const fltx4 & x, int exponent)
+{
+	fltx4 rslt=Four_Ones;									// x^0=1.0
+	int xp=abs(exponent);
+	if (xp & 3)												// fraction present?
+	{
+		fltx4 sq_rt=SqrtEstSIMD(x);
+		if (xp & 1)											// .25?
+			rslt=SqrtEstSIMD(sq_rt);						// x^.25
+		if (xp & 2)
+			rslt=MulSIMD(rslt,sq_rt);
+	}
+	xp>>=2;													// strip fraction
+	fltx4 curpower=x;										// curpower iterates through  x,x^2,x^4,x^8,x^16...
+
+	while(1)
+	{
+		if (xp & 1)
+			rslt=MulSIMD(rslt,curpower);
+		xp>>=1;
+		if (xp)
+			curpower=MulSIMD(curpower,curpower);
+		else
+			break;
+	}
+	if (exponent<0)
+		return ReciprocalEstSaturateSIMD(rslt);				// pow(x,-b)=1/pow(x,b)
+	else
+		return rslt;
+}
+
+
+
+
+#ifndef _PS3 // these aren't fast (or correct) on the PS3
+/*
+ * (c) Ian Stephenson
+ *
+ * ian@dctsystems.co.uk
+ *
+ * Fast pow() reference implementation
+ */
+
+
+static float shift23=(1<<23);
+static float OOshift23=1.0/(1<<23);
+
+float FastLog2(float i)
+{
+	float LogBodge=0.346607f;
+	float x;
+	float y;
+	x=*(int *)&i;
+	x*= OOshift23; //1/pow(2,23);
+	x=x-127;
+
+	y=x-floorf(x);
+	y=(y-y*y)*LogBodge;
+	return x+y;
+}
+float FastPow2(float i)
+{
+	float PowBodge=0.33971f;
+	float x;
+	float y=i-floorf(i);
+	y=(y-y*y)*PowBodge;
+
+	x=i+127-y;
+	x*= shift23; //pow(2,23);
+	*(int*)&x=(int)x;
+	return x;
+}
+float FastPow(float a, float b)
+{
+	if (a <= OOshift23)
+	{
+		return 0.0f;
+	}
+	return FastPow2(b*FastLog2(a));
+}
+float FastPow10( float i )
+{
+	return FastPow2( i * 3.321928f );
+}
+#else
+#pragma message("TODO: revisit fast logs on all PPC hardware")
+#endif
+

mathlib/quantize.cpp

@@ -0,0 +1,684 @@
+//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//=============================================================================//
+
+#ifndef STDIO_H
+#include <stdio.h>
+#endif
+
+#ifndef STRING_H
+#include <string.h>
+#endif
+
+#ifndef QUANTIZE_H
+#include <quantize.h>
+#endif
+
+#include <stdlib.h>
+
+
+#include <math.h>
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+static int current_ndims;
+static struct QuantizedValue *current_root;
+static int current_ssize;
+
+static uint8 *current_weights;
+
+double SquaredError;
+
+#define SPLIT_THEN_SORT 1
+
+#define SQ(x) ((x)*(x))
+
+static struct QuantizedValue *AllocQValue(void)
+{
+	struct QuantizedValue *ret=new QuantizedValue;
+	ret->Samples=0;
+	ret->Children[0]=ret->Children[1]=0;
+	ret->NSamples=0;
+  
+	ret->ErrorMeasure=new double[current_ndims];
+	ret->Mean=new uint8[current_ndims];
+	ret->Mins=new uint8[current_ndims];
+	ret->Maxs=new uint8[current_ndims];
+	ret->Sums=new int [current_ndims];
+	memset(ret->Sums,0,sizeof(int)*current_ndims);
+	ret->NQuant=0;
+	ret->sortdim=-1;
+	return ret;
+}
+
+void FreeQuantization(struct QuantizedValue *t)
+{
+	if (t)
+	{
+		delete[] t->ErrorMeasure;
+		delete[] t->Mean;
+		delete[] t->Mins;
+		delete[] t->Maxs;
+		FreeQuantization(t->Children[0]);
+		FreeQuantization(t->Children[1]);
+		delete[] t->Sums;
+		delete[] t;
+	}
+}
+
+static int QNumSort(void const *a, void const *b)
+{
+	int32 as=((struct Sample *) a)->QNum;
+	int32 bs=((struct Sample *) b)->QNum;
+	if (as==bs) return 0;
+	return (as>bs)?1:-1;
+}
+
+#if SPLIT_THEN_SORT
+#else
+static int current_sort_dim;
+
+static int samplesort(void const *a, void const *b)
+{
+	uint8 as=((struct Sample *) a)->Value[current_sort_dim];
+	uint8 bs=((struct Sample *) b)->Value[current_sort_dim];
+	if (as==bs) return 0;
+	return (as>bs)?1:-1;
+}
+#endif
+
+static int sortlong(void const *a, void const *b)
+{
+	// treat the entire vector of values as a long integer for duplicate removal.
+	return memcmp(((struct Sample *) a)->Value,
+				  ((struct Sample *) b)->Value,current_ndims);
+}
+
+
+  
+#define NEXTSAMPLE(s) ( (struct Sample *) (((uint8 *) s)+current_ssize))
+#define SAMPLE(s,i) NthSample(s,i,current_ndims)
+
+static void SetNDims(int n)
+{
+	current_ssize=sizeof(struct Sample)+(n-1);
+	current_ndims=n;
+}
+
+int CompressSamples(struct Sample *s, int nsamples, int ndims)
+{
+	SetNDims(ndims);
+	qsort(s,nsamples,current_ssize,sortlong);
+	// now, they are all sorted by treating all dimensions as a large number.
+	// we may now remove duplicates.
+	struct Sample *src=s;
+	struct Sample *dst=s;
+	struct Sample *lastdst=dst;
+	dst=NEXTSAMPLE(dst);		// copy first sample to get the ball rolling
+	src=NEXTSAMPLE(src);
+	int noutput=1;
+	while(--nsamples)		// while some remain
+	{
+		if (memcmp(src->Value,lastdst->Value,current_ndims))
+		{
+			// yikes, a difference has been found!
+			memcpy(dst,src,current_ssize);
+			lastdst=dst;
+			dst=NEXTSAMPLE(dst);
+			noutput++;
+		}
+		else
+			lastdst->Count++;
+		src=NEXTSAMPLE(src);
+	}
+	return noutput;
+}
+
+void PrintSamples(struct Sample const *s, int nsamples, int ndims)
+{
+	SetNDims(ndims);
+	int cnt=0;
+	while(nsamples--)
+	{
+		printf("sample #%d, count=%d, values=\n { ",cnt++,s->Count);
+		for(int d=0;d<ndims;d++)
+			printf("%02x,",s->Value[d]);
+		printf("}\n");
+		s=NEXTSAMPLE(s);
+	}
+}
+
+void PrintQTree(struct QuantizedValue const *p,int idlevel)
+{
+	int i;
+
+	if (p)
+	{
+		for(i=0;i<idlevel;i++)
+			printf(" ");
+		printf("node=%p NSamples=%d value=%d Mean={",p,p->NSamples,p->value);
+		for(i=0;i<current_ndims;i++)
+			printf("%x,",p->Mean[i]);
+		printf("}\n");
+		for(i=0;i<idlevel;i++)
+			printf(" ");
+		printf("Errors={");
+		for(i=0;i<current_ndims;i++)
+			printf("%f,",p->ErrorMeasure[i]);
+		printf("}\n");
+		for(i=0;i<idlevel;i++)
+			printf(" ");
+		printf("Mins={");
+		for(i=0;i<current_ndims;i++)
+			printf("%d,",p->Mins[i]);
+		printf("} Maxs={");
+		for(i=0;i<current_ndims;i++)
+			printf("%d,",p->Maxs[i]);
+		printf("}\n");
+		PrintQTree(p->Children[0],idlevel+2);
+		PrintQTree(p->Children[1],idlevel+2);
+	}
+}
+
+static void UpdateStats(struct QuantizedValue *v)
+{
+	// first, find mean
+	int32 Means[MAXDIMS];
+	double Errors[MAXDIMS];
+	double WorstError[MAXDIMS];
+	int i,j;
+  
+	memset(Means,0,sizeof(Means));
+	int N=0;
+	for(i=0;i<v->NSamples;i++)
+	{
+		struct Sample *s=SAMPLE(v->Samples,i);
+		N+=s->Count;
+		for(j=0;j<current_ndims;j++)
+		{
+			uint8 v=s->Value[j];
+			Means[j]+=v*s->Count;
+		}
+	}
+	for(j=0;j<current_ndims;j++)
+	{
+		if (N) v->Mean[j]=(uint8) (Means[j]/N);
+		Errors[j]=WorstError[j]=0.;
+	}
+	for(i=0;i<v->NSamples;i++)
+	{
+		struct Sample *s=SAMPLE(v->Samples,i);
+		double c=s->Count;
+		for(j=0;j<current_ndims;j++)
+		{
+			double diff=SQ(s->Value[j]-v->Mean[j]);
+			Errors[j]+=c*diff; // charles uses abs not sq()
+			if (diff>WorstError[j])
+				WorstError[j]=diff;
+		}
+	}
+	v->TotalError=0.;
+	double ErrorScale=1.; // /sqrt((double) (N));
+	for(j=0;j<current_ndims;j++)
+	{
+		v->ErrorMeasure[j]=(ErrorScale*Errors[j]*current_weights[j]);
+		v->TotalError+=v->ErrorMeasure[j];
+#if SPLIT_THEN_SORT
+		v->ErrorMeasure[j]*=WorstError[j];
+#endif
+	}
+	v->TotSamples=N;
+}
+
+static int ErrorDim;
+static double ErrorVal;
+static struct QuantizedValue *ErrorNode;
+
+static void UpdateWorst(struct QuantizedValue *q)
+{
+	if (q->Children[0])
+	{
+		// not a leaf node
+		UpdateWorst(q->Children[0]);
+		UpdateWorst(q->Children[1]);
+	}
+	else
+	{
+		if (q->TotalError>ErrorVal)
+		{
+			ErrorVal=q->TotalError;
+			ErrorNode=q;
+			ErrorDim=0;
+			for(int d=0;d<current_ndims;d++)
+				if (q->ErrorMeasure[d]>q->ErrorMeasure[ErrorDim])
+					ErrorDim=d;
+		}
+	}
+}
+
+static int FindWorst(void)
+{
+	ErrorVal=-1.;
+	UpdateWorst(current_root);
+	return (ErrorVal>0);
+}
+
+
+
+static void SubdivideNode(struct QuantizedValue *n, int whichdim)
+{
+	int NAdded=0;
+	int i;
+
+#if SPLIT_THEN_SORT
+	// we will try the "split then sort" method. This works by finding the
+	// means for all samples above and below the mean along the given axis.
+	// samples are then split into two groups, with the selection based upon
+	// which of the n-dimensional means the sample is closest to.
+	double LocalMean[MAXDIMS][2];
+	int totsamps[2];
+	for(i=0;i<current_ndims;i++)
+		LocalMean[i][0]=LocalMean[i][1]=0.;
+	totsamps[0]=totsamps[1]=0;
+	uint8 minv=255;
+	uint8 maxv=0;
+	struct Sample *minS=0,*maxS=0;
+	for(i=0;i<n->NSamples;i++)
+	{
+		uint8 v;
+		int whichside=1;
+		struct Sample *sl;
+		sl=SAMPLE(n->Samples,i);
+		v=sl->Value[whichdim];
+		if (v<minv) { minv=v; minS=sl; }
+		if (v>maxv) { maxv=v; maxS=sl; }
+		if (v<n->Mean[whichdim])
+			whichside=0;
+		totsamps[whichside]+=sl->Count;
+		for(int d=0;d<current_ndims;d++)
+			LocalMean[d][whichside]+=
+				sl->Count*sl->Value[d];
+	}
+
+	if (totsamps[0] && totsamps[1])
+		for(i=0;i<current_ndims;i++)
+		{
+			LocalMean[i][0]/=totsamps[0];
+			LocalMean[i][1]/=totsamps[1];
+		}
+	else
+	{
+		// it is possible that the clustering failed to split the samples.
+		// this can happen with a heavily biased sample (i.e. all black
+		// with a few stars). If this happens, we will cluster around the
+		// extrema instead. LocalMean[i][0] will be the point with the lowest
+		// value on the dimension and LocalMean[i][1] the one with the lowest
+		// value.
+		for(int i=0;i<current_ndims;i++)
+		{
+			LocalMean[i][0]=minS->Value[i];
+			LocalMean[i][1]=maxS->Value[i];
+		}
+	}
+
+	// now, we have 2 n-dimensional means. We will label each sample
+	// for which one it is nearer to by using the QNum field.
+	for(i=0;i<n->NSamples;i++)
+	{
+		double dist[2];
+		dist[0]=dist[1]=0.;
+		struct Sample *s=SAMPLE(n->Samples,i);
+		for(int d=0;d<current_ndims;d++)
+			for(int w=0;w<2;w++)
+				dist[w]+=current_weights[d]*SQ(LocalMean[d][w]-s->Value[d]);
+		s->QNum=(dist[0]<dist[1]);
+    }
+
+
+	// hey ho! we have now labelled each one with a candidate bin. Let's
+	// sort the array by moving the 0-labelled ones to the head of the array.
+	n->sortdim=-1;
+	qsort(n->Samples,n->NSamples,current_ssize,QNumSort);
+	for(i=0;i<n->NSamples;i++,NAdded++)
+		if (SAMPLE(n->Samples,i)->QNum)
+			break;
+  
+#else
+	if (whichdim != n->sortdim)
+	{
+		current_sort_dim=whichdim;
+		qsort(n->Samples,n->NSamples,current_ssize,samplesort);
+		n->sortdim=whichdim;
+	}
+	// now, the samples are sorted along the proper dimension.  we need
+	// to find the place to cut in order to split the node.  this is
+	// complicated by the fact that each sample entry can represent many
+	// samples. What we will do is start at the beginning of the array,
+	// adding samples to the first node, until either the number added
+	// is >=TotSamples/2, or there is only one left.
+	int TotAdded=0;
+	for(;;)
+	{
+		if (NAdded==n->NSamples-1)
+			break;
+		if (TotAdded>=n->TotSamples/2)
+			break;
+		TotAdded+=SAMPLE(n->Samples,NAdded)->Count;
+		NAdded++;
+	}
+#endif
+	struct QuantizedValue *a=AllocQValue();
+	a->sortdim=n->sortdim;
+	a->Samples=n->Samples;
+	a->NSamples=NAdded;
+	n->Children[0]=a;
+	UpdateStats(a);
+	a=AllocQValue();
+	a->Samples=SAMPLE(n->Samples,NAdded);
+	a->NSamples=n->NSamples-NAdded;
+	a->sortdim=n->sortdim;
+	n->Children[1]=a;
+	UpdateStats(a);
+}
+
+static int colorid=0;
+
+static void Label(struct QuantizedValue *q, int updatecolor)
+{
+	// fill in max/min values for tree, etc.
+	if (q)
+	{
+		Label(q->Children[0],updatecolor);
+		Label(q->Children[1],updatecolor);
+		if (! q->Children[0])	// leaf node?
+		{
+			if (updatecolor)
+			{
+				q->value=colorid++;
+				for(int j=0;j<q->NSamples;j++)
+				{
+					SAMPLE(q->Samples,j)->QNum=q->value;
+					SAMPLE(q->Samples,j)->qptr=q;
+				}
+			}
+			for(int i=0;i<current_ndims;i++)
+			{
+				q->Mins[i]=q->Mean[i];
+				q->Maxs[i]=q->Mean[i];
+			}
+		}
+		else
+			for(int i=0;i<current_ndims;i++)
+			{
+				q->Mins[i]=MIN(q->Children[0]->Mins[i],q->Children[1]->Mins[i]);
+				q->Maxs[i]=MAX(q->Children[0]->Maxs[i],q->Children[1]->Maxs[i]);
+			}
+	}
+}    
+
+struct QuantizedValue *FindQNode(struct QuantizedValue const *q, int32 code)
+{
+	if (! (q->Children[0]))
+		if (code==q->value) return (struct QuantizedValue *) q;
+		else return 0;
+	else
+	{
+		struct QuantizedValue *found=FindQNode(q->Children[0],code);
+		if (! found) found=FindQNode(q->Children[1],code);
+		return found;
+	}
+}
+
+
+void CheckInRange(struct QuantizedValue *q, uint8 *max, uint8 *min)
+{
+	if (q)
+	{
+		if (q->Children[0])
+		{
+			// non-leaf node
+			CheckInRange(q->Children[0],q->Maxs, q->Mins);
+			CheckInRange(q->Children[1],q->Maxs, q->Mins);
+			CheckInRange(q->Children[0],max, min);
+			CheckInRange(q->Children[1],max, min);
+		}
+		for (int i=0;i<current_ndims;i++)
+		{
+			if (q->Maxs[i]>max[i]) printf("error1\n");
+			if (q->Mins[i]<min[i]) printf("error2\n");
+		}
+	}
+}
+
+struct QuantizedValue *Quantize(struct Sample *s, int nsamples, int ndims,
+								int nvalues, uint8 *weights, int firstvalue)
+{
+	SetNDims(ndims);
+	current_weights=weights;
+	current_root=AllocQValue();
+	current_root->Samples=s;
+	current_root->NSamples=nsamples;
+	UpdateStats(current_root);
+	while(--nvalues)
+	{
+		if (! FindWorst())
+			break;                          // if <n unique ones, stop now
+		SubdivideNode(ErrorNode,ErrorDim);
+	}
+	colorid=firstvalue;
+	Label(current_root,1);
+	return current_root;
+}
+
+double MinimumError(struct QuantizedValue const *q, uint8 const *sample,
+					int ndims, uint8 const *weights)
+{
+	double err=0;
+	for(int i=0;i<ndims;i++)
+	{
+		int val1;
+		int val2=sample[i];
+		if ((q->Mins[i]<=val2) && (q->Maxs[i]>=val2)) val1=val2;
+		else
+		{
+			val1=(val2<=q->Mins[i])?q->Mins[i]:q->Maxs[i];
+		}
+		err+=weights[i]*SQ(val1-val2);
+	}
+	return err;
+}
+
+double MaximumError(struct QuantizedValue const *q, uint8 const *sample,
+					int ndims, uint8 const *weights)
+{
+	double err=0;
+	for(int i=0;i<ndims;i++)
+	{
+		int val2=sample[i];
+		int val1=(abs(val2-q->Mins[i])>abs(val2-q->Maxs[i]))?
+			q->Mins[i]:
+			q->Maxs[i];
+		err+=weights[i]*SQ(val2-val1);
+	}
+	return err;
+}
+
+				     
+
+// heap (priority queue) routines used for nearest-neghbor searches
+struct FHeap {
+	int heap_n;
+	double *heap[MAXQUANT];
+};
+
+void InitHeap(struct FHeap *h)
+{
+  h->heap_n=0;
+}
+
+
+void UpHeap(int k, struct FHeap *h)
+{
+  double *tmpk=h->heap[k];
+  double tmpkn=*tmpk;
+  while((k>1) && (tmpkn <= *(h->heap[k/2])))
+    {
+      h->heap[k]=h->heap[k/2];
+      k/=2;
+    }
+  h->heap[k]=tmpk;
+}
+
+void HeapInsert(struct FHeap *h,double *elem)
+{
+  h->heap_n++;
+  h->heap[h->heap_n]=elem;
+  UpHeap(h->heap_n,h);
+}
+
+void DownHeap(int k, struct FHeap *h)
+{
+  double *v=h->heap[k];
+  while(k<=h->heap_n/2)
+    {
+      int j=2*k;
+      if (j<h->heap_n)
+	if (*(h->heap[j]) >= *(h->heap[j+1]))
+	  j++;
+      if (*v < *(h->heap[j]))
+	{
+	  h->heap[k]=v;
+	  return;
+	}
+      h->heap[k]=h->heap[j]; k=j;
+    }
+  h->heap[k]=v;
+}
+
+void *RemoveHeapItem(struct FHeap *h)
+{
+  void *ret=0;
+  if (h->heap_n!=0)
+    {
+      ret=h->heap[1];
+      h->heap[1]=h->heap[h->heap_n];
+      h->heap_n--;
+      DownHeap(1,h);
+    }
+  return ret;
+}
+
+// now, nearest neighbor finder. Use a heap to traverse the tree, stopping
+// when there are no nodes with a minimum error < the current error.
+
+struct FHeap TheQueue;
+
+#define PUSHNODE(a) { \
+  (a)->MinError=MinimumError(a,sample,ndims,weights); \
+  if ((a)->MinError < besterror) HeapInsert(&TheQueue,&(a)->MinError); \
+ }
+
+struct QuantizedValue *FindMatch(uint8 const *sample, int ndims,
+								 uint8 *weights, struct QuantizedValue *q)
+{
+	InitHeap(&TheQueue);
+	struct QuantizedValue *bestmatch=0;
+	double besterror=1.0e63;
+	PUSHNODE(q);
+	for(;;)
+	{
+		struct QuantizedValue *test=(struct QuantizedValue *)
+			RemoveHeapItem(&TheQueue);
+		if (! test) break;		// heap empty
+//    printf("got pop node =%p minerror=%f\n",test,test->MinError);
+    
+		if (test->MinError>besterror) break;
+		if (test->Children[0])
+		{
+			// it's a parent node. put the children on the queue
+			struct QuantizedValue *c1=test->Children[0];
+			struct QuantizedValue *c2=test->Children[1];
+			c1->MinError=MinimumError(c1,sample,ndims,weights);
+			if (c1->MinError < besterror)
+				HeapInsert(&TheQueue,&(c1->MinError));
+			c2->MinError=MinimumError(c2,sample,ndims,weights);
+			if (c2->MinError < besterror)
+				HeapInsert(&TheQueue,&(c2->MinError));
+		}
+		else
+		{
+			// it's a leaf node. This must be a new minimum or the MinError
+			// test would have failed.
+			if (test->MinError < besterror)
+			{
+				bestmatch=test;
+				besterror=test->MinError;
+			}
+		}
+	}
+	if (bestmatch)
+	{
+		SquaredError+=besterror;
+		bestmatch->NQuant++;
+		for(int i=0;i<ndims;i++)
+			bestmatch->Sums[i]+=sample[i];
+	}
+	return bestmatch;
+}
+
+static void RecalcMeans(struct QuantizedValue *q)
+{
+	if (q)
+	{
+		if (q->Children[0])
+		{
+			// not a leaf, invoke recursively.
+			RecalcMeans(q->Children[0]);
+			RecalcMeans(q->Children[0]);
+		}
+		else
+		{
+			// it's a leaf. Set the means
+			if (q->NQuant)
+			{
+				for(int i=0;i<current_ndims;i++)
+				{
+					q->Mean[i]=(uint8) (q->Sums[i]/q->NQuant);
+					q->Sums[i]=0;
+				}
+				q->NQuant=0;
+			}
+		}
+	}
+}
+		      
+void OptimizeQuantizer(struct QuantizedValue *q, int ndims)
+{
+	SetNDims(ndims);
+	RecalcMeans(q);		// reset q values
+	Label(q,0);			// update max/mins
+}
+
+
+static void RecalcStats(struct QuantizedValue *q)
+{
+	if (q)
+	{
+		UpdateStats(q);
+		RecalcStats(q->Children[0]);
+		RecalcStats(q->Children[1]);
+	}
+}
+
+void RecalculateValues(struct QuantizedValue *q, int ndims)
+{
+	SetNDims(ndims);
+	RecalcStats(q);
+	Label(q,0);
+}

mathlib/randsse.cpp

@@ -0,0 +1,113 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: generates 4 randum numbers in the range 0..1 quickly, using SIMD
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#ifndef _PS3
+#include <memory.h>
+#endif
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+#include "mathlib/ssemath.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+// see knuth volume 3 for insight.
+
+class SIMDRandStreamContext
+{
+	fltx4 m_RandY[55];
+
+	fltx4 *m_pRand_J, *m_pRand_K;
+
+
+public:
+	void Seed( uint32 seed )
+	{
+		m_pRand_J=m_RandY+23; m_pRand_K=m_RandY+54;
+		for(int i=0;i<55;i++)
+		{
+			for(int j=0;j<4;j++)
+			{
+				SubFloat( m_RandY[i], j) = (seed>>16)/65536.0;
+				seed=(seed+1)*3141592621u;
+			}
+		}
+	}
+
+	inline fltx4 RandSIMD( void )
+	{
+		// ret= rand[k]+rand[j]
+		fltx4 retval=AddSIMD( *m_pRand_K, *m_pRand_J );
+		
+		// if ( ret>=1.0) ret-=1.0
+		bi32x4 overflow_mask=CmpGeSIMD( retval, Four_Ones );
+		retval=SubSIMD( retval, AndSIMD( Four_Ones, overflow_mask ) );
+		
+		*m_pRand_K = retval;
+		
+		// update pointers w/ wrap-around
+		if ( --m_pRand_J < m_RandY )
+			m_pRand_J=m_RandY+54;
+		if ( --m_pRand_K < m_RandY )
+			m_pRand_K=m_RandY+54;
+		
+		return retval;
+	}
+};
+
+#define MAX_SIMULTANEOUS_RANDOM_STREAMS 32
+
+static SIMDRandStreamContext s_SIMDRandContexts[MAX_SIMULTANEOUS_RANDOM_STREAMS];
+
+static volatile int s_nRandContextsInUse[MAX_SIMULTANEOUS_RANDOM_STREAMS];
+
+void SeedRandSIMD(uint32 seed)
+{
+	for( int i = 0; i<MAX_SIMULTANEOUS_RANDOM_STREAMS; i++)
+		s_SIMDRandContexts[i].Seed( seed+i );
+}
+
+fltx4 RandSIMD( int nContextIndex )
+{
+	return s_SIMDRandContexts[nContextIndex].RandSIMD();
+}
+
+int GetSIMDRandContext( void )
+{
+	for(;;)
+	{
+		for(int i=0; i < NELEMS( s_SIMDRandContexts ); i++)
+		{
+			if ( ! s_nRandContextsInUse[i] )				// available?
+			{
+				// try to take it!
+				if ( ThreadInterlockedAssignIf( &( s_nRandContextsInUse[i]), 1, 0 ) )
+				{
+					ThreadMemoryBarrier();
+					return i;								// done!
+				}
+			}
+		}
+		Assert(0);											// why don't we have enough buffers?
+		ThreadSleep();
+	}
+}
+
+void ReleaseSIMDRandContext( int nContext )
+{
+	ThreadMemoryBarrier();
+	s_nRandContextsInUse[ nContext ] = 0;
+}
+
+
+fltx4 RandSIMD( void )
+{
+	return s_SIMDRandContexts[0].RandSIMD();
+}

mathlib/simdvectormatrix.cpp

@@ -0,0 +1,140 @@
+//====== Copyright © 1996-2006, Valve Corporation, All rights reserved. =======//
+//
+// Purpose: Provide a class (SSE/SIMD only) holding a 2d matrix of class FourVectors,
+// for high speed processing in tools.
+//
+// $NoKeywords: $
+//
+//=============================================================================//
+
+#include "basetypes.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/simdvectormatrix.h"
+#include "mathlib/ssemath.h"
+#include "tier0/dbg.h"
+
+// NOTE: This has to be the last file included!
+#include "tier0/memdbgon.h"
+
+
+void CSIMDVectorMatrix::CreateFromCSOAAttributes( CSOAContainer const *pSrc,
+												  int nAttrIdx0, int nAttrIdx1, int nAttrIdx2 )
+{
+	SetSize( pSrc->NumCols(), pSrc->NumRows() );
+
+	FourVectors *p_write_ptr = m_pData;
+	int n_vectors_per_source_line = pSrc->NumQuadsPerRow();
+	for( int y = 0; y < pSrc->NumRows(); y++ )
+	{
+		fltx4 const * data_in0 = reinterpret_cast<fltx4 const *>( pSrc->ConstRowPtr( nAttrIdx0, y ) );
+		fltx4 const * data_in1 = reinterpret_cast<fltx4 const *>( pSrc->ConstRowPtr( nAttrIdx1, y ) );
+		fltx4 const * data_in2 = reinterpret_cast<fltx4 const *>( pSrc->ConstRowPtr( nAttrIdx2, y ) );
+
+		fltx4 *data_out = reinterpret_cast < fltx4 *> ( p_write_ptr );
+		// copy full input blocks
+		for( int x = 0; x < n_vectors_per_source_line; x++ )
+		{
+			*(data_out++) = (* data_in0++ );
+			*(data_out++) = (* data_in1++ );
+			*(data_out++) = (* data_in2++ );
+		}
+		// advance ptrs to next line
+		p_write_ptr += m_nPaddedWidth;
+	}
+}
+
+void CSIMDVectorMatrix::CreateFromRGBA_FloatImageData( int srcwidth, int srcheight,
+													  float const * srcdata )
+{
+	Assert( srcwidth && srcheight && srcdata );
+	SetSize( srcwidth, srcheight );
+
+	FourVectors * p_write_ptr = m_pData;
+	int n_vectors_per_source_line = ( srcwidth >> 2 );
+	int ntrailing_pixels_per_source_line = ( srcwidth & 3 );
+	for( int y = 0; y < srcheight; y++ )
+	{
+		float const * data_in = srcdata;
+		float * data_out = reinterpret_cast < float *> ( p_write_ptr );
+		// copy full input blocks
+		for( int x = 0; x < n_vectors_per_source_line; x++ )
+		{
+			for( int c = 0; c < 3; c++ )
+			{
+				data_out[0]= data_in[c];					// x0
+				data_out[1]= data_in[4 + c];				// x1
+				data_out[2]= data_in[8 + c];				// x2
+				data_out[3]= data_in[12 + c];				// x3
+				data_out += 4;
+			}
+			data_in += 16;
+		}
+		// now, copy trailing data and pad with copies
+		if ( ntrailing_pixels_per_source_line )
+		{
+			for( int c = 0; c < 3; c++ )
+			{
+				for( int cp = 0; cp < 4; cp++ )
+				{
+					int real_cp = MIN( cp, ntrailing_pixels_per_source_line - 1 );
+					data_out[4 * c + cp]= data_in[c + 4 * real_cp];
+				}
+			}
+		}
+		// advance ptrs to next line
+		p_write_ptr += m_nPaddedWidth;
+		srcdata += 4 * srcwidth;
+	}
+}
+
+void CSIMDVectorMatrix::RaiseToPower( float power )
+{
+	int nv = NVectors();
+	if ( nv )
+	{
+		int fixed_point_exp = ( int ) ( 4.0 * power );
+		FourVectors * src = m_pData;
+		do
+		{
+			src->x = Pow_FixedPoint_Exponent_SIMD( src->x, fixed_point_exp );
+			src->y = Pow_FixedPoint_Exponent_SIMD( src->y, fixed_point_exp );
+			src->z = Pow_FixedPoint_Exponent_SIMD( src->z, fixed_point_exp );
+			src++;
+		} while (-- nv );
+	}
+}
+
+CSIMDVectorMatrix & CSIMDVectorMatrix::operator += ( CSIMDVectorMatrix const & src )
+{
+	Assert( m_nWidth == src.m_nWidth );
+	Assert( m_nHeight == src.m_nHeight );
+	int nv = NVectors();
+	if ( nv )
+	{
+		FourVectors * srcv = src.m_pData;
+		FourVectors * destv = m_pData;
+		do													// !! speed !! inline more iters
+		{
+			* ( destv++ ) += * ( srcv++ );
+		} while (-- nv );
+	}
+	return * this;
+}
+
+CSIMDVectorMatrix & CSIMDVectorMatrix::operator *= ( Vector const & src )
+{
+	int nv = NVectors();
+	if ( nv )
+	{
+		FourVectors scalevalue;
+		scalevalue.DuplicateVector( src );
+		FourVectors * destv = m_pData;
+		do													// !! speed !! inline more iters
+		{
+			destv->VProduct( scalevalue );
+			destv++;
+		} while (-- nv );
+	}
+	return * this;
+}
+

mathlib/simplex.cpp

@@ -0,0 +1,495 @@
+#include <basetypes.h>
+#include <float.h>
+#include "simplex.h"
+
+// a nice tutorial on simplex method: http://math.uww.edu/~mcfarlat/ism.htm
+CSimplex::CSimplex():
+	m_numVariables(0),m_numConstraints(0),m_pTableau(0),m_pInitialTableau(0), m_pSolution(0), m_pBasis(0)
+{
+}
+
+CSimplex::CSimplex(int numVariables, int numConstraints):
+	m_numVariables(0),m_numConstraints(0),m_pTableau(0),m_pInitialTableau(0), m_pSolution(0), m_pBasis(0)
+{
+	Init(numVariables, numConstraints);
+}
+
+void CSimplex::Init(int numVariables, int numConstraints)
+{
+	Destruct();
+	m_numVariables = numVariables; m_numConstraints = numConstraints;
+	m_pTableau = new float[(NumRows()+1) * NumColumns()];
+	m_pInitialTableau = new float[(NumRows()+1) * NumColumns()];
+	m_pSolution = m_pTableau + NumRows() * NumColumns();
+	// allocating basis and non-basis indices in one call
+	m_pBasis = new int[m_numConstraints + m_numVariables];
+	m_pNonBasis = m_pBasis + m_numConstraints;
+	m_state = kUnknown;
+}
+
+
+void CSimplex::PrintTableau()const
+{
+	Msg("problem.Init(%d,%d);\nfloat test[%d]={", m_numVariables, m_numConstraints, (m_numVariables+1)*(m_numConstraints+1));
+	for(int i = 0; i < NumRows(); ++i)
+	{
+		for(int j = 0;j < NumColumns(); ++j)
+		{
+			Msg(" %g,",Tableau(i,j));
+		}
+		Msg("\n");
+	}
+	Msg("}");
+}
+
+
+void CSimplex::InitTableau(const float *pTableau)
+{
+	const float *p = pTableau;
+	for(int nRow = 0; nRow  <= m_numConstraints; ++nRow)
+	{
+		for(int nColumn = 0; nColumn < m_numVariables; ++nColumn)
+		{
+			Tableau(nRow, nColumn) = *(p++);
+		}
+		Tableau(nRow, NumColumns()-1) = *(p++);
+	}
+}
+
+CSimplex::~CSimplex()
+{
+	Destruct();
+}
+
+void CSimplex::Destruct()
+{
+	delete[]m_pInitialTableau;
+	m_pInitialTableau = NULL;
+	delete[]m_pTableau;
+	m_pTableau = NULL;
+	delete[]m_pBasis;
+	m_pBasis = NULL;
+}
+
+
+CSimplex::StateEnum CSimplex::Solve(float flThreshold, int maxStallIterations)
+{
+	m_state = kUnknown;
+	PrepareTableau();
+	if(SolvePhase1(flThreshold, maxStallIterations) == kUnknown)
+		SolvePhase2(flThreshold, maxStallIterations);
+	GatherSolution();
+	return m_state;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// bring constraints to b>=0 form for phase-2 full solution
+CSimplex::StateEnum CSimplex::SolvePhase1(float flThreshold, int maxStallIterations)
+{
+	for(int nPotentiallyInfiniteLoop = 0; nPotentiallyInfiniteLoop < maxStallIterations; ++nPotentiallyInfiniteLoop)
+	{
+		if(!IteratePhase1())
+			break;
+	}
+	return m_state;
+}
+
+//////////////////////////////////////////////////////////////////////////
+// Solve the linear problem ; 
+//   \param flThreshold         - this is how much we need to improve objective every step that's not considered lost
+//   \param maxStallIterations  - this is how many "lost" (see flThreshold) steps we may take before we bail
+//
+CSimplex::StateEnum CSimplex::SolvePhase2(float flThreshold, int maxStallIterations)
+{
+	for(int nPotentiallyInfiniteLoop = 0; nPotentiallyInfiniteLoop < maxStallIterations; ++nPotentiallyInfiniteLoop)
+	{
+		if(!IteratePhase2())
+			break;
+	}
+	Validate();
+	return m_state;
+}
+
+// fill out m-pSolution array (primal solution)
+void CSimplex::GatherSolution()
+{
+	// Notes:
+	// PRIMAL SOLUTION is indicated by the rightmost column of the tableau;
+	// there are at most m_numConstraint basic variables that participate in the solution.
+	// The original problem PRIMAL unknowns are numbered 0..m_numVariables; the rest (m_numVariables+1..m_numVariables+m_numConstraints) are the PRIMAL SLACK variables
+	// DUAL SOLUTION is in the row [m_numConstraints], and it's basic variables are indicated by m_pNonBasic array and are reversed:
+	// first the DUAL SLACK variables are numbered 0..m_numVariables; the rest (m_numVariables+1..m_numVariables+m_numConstraints) are the DUAL variables
+
+	memset(m_pSolution, 0, sizeof(*m_pSolution) * NumColumns()); // initial value of all X's are 0's
+	for(int nRow = 0; nRow < m_numConstraints; ++nRow)
+	{
+		int nBasisVariable = m_pBasis[nRow];
+		m_pSolution[nBasisVariable] = Tableau(nRow, NumColumns()-1);
+	}
+	m_pSolution[m_numVariables+m_numConstraints] = Tableau(m_numConstraints, NumColumns()-1);
+}
+
+
+///////////////////////////////////////////////////////////////////////////
+// Find and pivot a row with negative constraint const (right side)
+// return false - if can't find such constraint or can't pivot
+//
+bool CSimplex::IteratePhase1()
+{
+	int nFixRow = FindLastNegConstrRow();
+	if(nFixRow < 0)
+		return false; // phase 1 complete: no rows to fix
+	int nPivotColumn = ChooseNegativeElementInRow(nFixRow);
+	if(nPivotColumn < 0)
+	{
+		m_state = kInfeasible;
+		return false;
+	}
+
+	int nPivotRow = nFixRow;
+	float flMinimizer = Tableau (nPivotRow, NumColumns()-1)/Tableau(nPivotRow, nPivotColumn); // minimize this
+
+	// UNTESTED! What's the rule to choose pivot in phase1?
+	for(int nCandidatePivotRow = nPivotRow + 1; nCandidatePivotRow < m_numConstraints; ++nCandidatePivotRow)
+	{
+		float flCandidateConst = Tableau (nCandidatePivotRow,NumColumns()-1), flCandidatePivot = Tableau (nCandidatePivotRow, nPivotColumn);
+		if ( flCandidateConst < 0 && flCandidatePivot > 1e-6f )
+		{
+			float flCandidateMinimizer = flCandidateConst / flCandidatePivot;
+			if(flCandidateMinimizer < flMinimizer)
+			{
+				flCandidateMinimizer = flMinimizer;
+				nPivotRow = nCandidatePivotRow;      // UNTESTED! 
+			}
+		}
+	}
+
+	return Pivot(nPivotRow, nPivotColumn);
+}
+
+//////////////////////////////////////////////////////////////////////////
+// Return the index of the last row with negative Constraint Const (b[i] in A.x<=b formulation)
+int CSimplex::FindLastNegConstrRow()
+{
+	int nFixRow = -1;
+	for(int nRow = 0;  nRow < m_numConstraints; ++nRow)
+	{
+		if(Tableau(nRow, NumColumns()-1) < 0)
+		{
+			nFixRow = nRow;
+		}
+	}
+	return nFixRow;
+}
+
+///////////////////////////////////////////////////////////////////////////
+// Choose some (e.g. the most negative) negative number in the row
+int CSimplex::ChooseNegativeElementInRow(int nFixRow)
+{
+	int indexNegElement = -1;
+	float flMinElement = 0;
+	for(int nColumn = 0; nColumn < m_numVariables; ++nColumn)
+	{
+		float flElement = Tableau(nFixRow, nColumn);
+		if(flElement < flMinElement)
+		{
+			indexNegElement = nColumn;
+			flMinElement = flElement;
+		}
+	}
+	return indexNegElement;
+}
+
+bool CSimplex::IteratePhase2()
+{
+	int nPivotColumn = FindPivotColumn();
+	if(nPivotColumn < 0)
+	{
+		m_state = kOptimal;
+		return false;
+	}
+	int nPivotRow = FindPivotRow(nPivotColumn);
+	if(nPivotRow < 0)
+	{
+		m_state = kUnbound;
+		return false;
+	}
+
+	bool ok = Pivot(nPivotRow, nPivotColumn);
+
+	// since we replaced the basis variable, we have to replace its corresponding column
+
+	return ok;
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Self-explanatory, isn't it?
+bool CSimplex::Pivot(int nPivotRow, int nPivotColumn)
+{
+	if(fabs(Tableau(nPivotRow, nPivotColumn)) < 1e-8f)
+	{
+		m_state = kCannotPivot;
+		return false; // Can NOT pivot on zero :( choose another (ie. fancier) pivot rule
+	}
+
+	/// get the 1/Tij, then replace the multiplied element with it
+	float flFactor = 1.0f / Tableau(nPivotRow, nPivotColumn);
+	MultiplyRow(nPivotRow, flFactor);
+
+	for(int i = 0; i <= m_numConstraints; ++i)
+	{
+		if(i != nPivotRow)
+		{
+			float flFactorOther = -Tableau(i,nPivotColumn);
+			AddRowMulFactor(i, nPivotRow, flFactorOther);
+			Tableau(i,nPivotColumn) = flFactorOther * flFactor; // replace the column with original column / -pivot
+		}
+	}
+	Tableau(nPivotRow, nPivotColumn) = flFactor;
+
+	int nEnteringVariable = m_pNonBasis[nPivotColumn];
+	int nExitingVariable = m_pBasis[nPivotRow];
+
+	// remember the index of the entering new basis var
+	m_pBasis[nPivotRow]        = nEnteringVariable;
+	m_pNonBasis[nPivotColumn]  = nExitingVariable;
+
+	Validate();
+	return true;
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// find the column with the most negative number in the last (objective) row
+int CSimplex::FindPivotColumn()
+{
+	int nBest = -1;
+	float flBest = 0;
+	for(int i = 0; i < m_numVariables; ++i)
+	{
+		float flElement = Tableau(m_numConstraints, i);
+		if(flElement > flBest)
+		{
+			flBest = flElement;
+			nBest = i;
+		}
+	}
+	if(nBest < 0)
+	{
+		m_state = kOptimal;
+		return -1;
+	}
+	else
+		return nBest;
+};
+
+
+int CSimplex::FindPivotRow(int nColumn)
+{
+	float flBest = FLT_MAX;
+	int nBest = -1;
+	for(int nRow = 0; nRow < m_numConstraints; ++nRow)
+	{		    
+		float flPivotCandidate = Tableau(nRow, nColumn);
+		if(flPivotCandidate > 1e-6f)
+		{
+			// don't perform any tests unless flTest is finite
+			float flTest = Tableau(nRow, NumColumns()-1) / flPivotCandidate;
+			if(flTest < flBest)
+			{
+				// flBest is either Infinity or is worse; it's worse in any case, so replace it
+				flBest = flTest;
+				nBest = nRow;
+			}
+		}
+	}
+
+	return nBest;
+}
+
+
+void CSimplex::MultiplyRow(int nRow, float flFactor)
+{
+	for(int nColumn = 0; nColumn < NumColumns(); ++nColumn)
+	{
+		Tableau(nRow, nColumn) *= flFactor;
+	}
+}
+
+
+void CSimplex::AddRowMulFactor(int nTargetRow, int nPivotRow, float fFactor)
+{
+	for(int nColumn = 0; nColumn < NumColumns(); ++nColumn)
+	{
+		Tableau(nTargetRow, nColumn) += Tableau(nPivotRow, nColumn) * fFactor;
+	}
+}
+
+
+// set the I matrix in the slack columns of the tableau
+void CSimplex::PrepareTableau()
+{
+/*
+	for(int nRow = 0; nRow < m_numConstraints + 1; ++nRow)
+	{
+		for(int nColumn = 0; nColumn < m_numConstraints; ++nColumn)
+			Tableau(nRow, nColumn + m_numVariables) = 0;
+	}
+*/
+	for(int nonBasis = 0; nonBasis < m_numVariables; ++nonBasis)
+	{
+		m_pNonBasis[nonBasis] = nonBasis;
+	}
+	for(int nConstraint = 0; nConstraint < m_numConstraints; ++nConstraint)
+	{
+		m_pBasis[nConstraint] = m_numVariables + nConstraint;	// slack variables
+		//Tableau(nConstraint, nConstraint + m_numVariables) = 1.0f;
+	} 
+	//m_pSolution[m_numVariables+m_numConstraints] = 
+	Tableau(m_numConstraints, NumColumns()-1) = 0.0f; // starting with "0" objective, and all "0" variables
+	memcpy(m_pInitialTableau,m_pTableau,(NumRows()+1) * NumColumns() * sizeof(float));
+}
+
+
+
+void CSimplex::SetConstraintConst(int nConstraint, float fConst)
+{
+	m_pSolution[m_numVariables + nConstraint] = Tableau(nConstraint, NumColumns()-1) = fConst;
+}
+
+
+void CSimplex::SetConstraintFactor(int nConstraint, int nConstant, float fFactor)
+{
+	Tableau(nConstraint, nConstant) = fFactor;
+}
+
+void CSimplex::SetObjectiveFactor(int nConstant, float fFactor)
+{
+	// the objective factor is negated because for the objective P = cx , we write it as -c x + P -> max
+	Tableau(m_numConstraints, nConstant) = fFactor;
+}
+
+void CSimplex::SetObjectiveFactors(int numFactors, const float *pFactors)
+{
+	Assert(numFactors == m_numVariables);
+	for(int i =0; i < m_numVariables && i < numFactors; ++i)
+		SetObjectiveFactor(i,pFactors[i]);
+}
+
+
+
+float CSimplex::GetSolution(int nVariable)const
+{
+	Assert(nVariable < m_numVariables);
+	return m_pSolution[nVariable];
+}
+
+
+float CSimplex::GetSlack(int nConstraint)const
+{
+	Assert(nConstraint < m_numConstraints);
+	return m_pSolution[m_numVariables + nConstraint];
+}
+
+
+float CSimplex::GetObjective()const
+{
+/*
+	float flResult = 0;
+	for(int i = 0; i < m_numVariables + m_numConstraints;  ++i)
+		flResult -= m_pSolution[i] * Tableau(m_numConstraints,i);
+	return flResult;
+*/
+	return Tableau(m_numConstraints, NumColumns()-1);
+}
+
+
+void CSimplex::Validate()
+{
+#if defined(_DEBUG) && 0
+	GatherSolution();
+	for(int i = 0; i <= m_numConstraints; ++i)
+	{
+		float flRes = 0;
+		for(int j = 0; j < m_numVariables; ++j)
+			flRes += GetInitialTableau(i,j) * m_pSolution[j];
+		if(i == m_numConstraints)
+		{
+			Msg("Objective = %g; basis:",flRes);
+			for (int j = 0; j < m_numVariables; ++j)
+				Msg(" %g", m_pSolution[j]);
+			Msg(" |slacks:");
+			for(int j = 0; j < m_numConstraints; ++j)
+				Msg(" %g", m_pSolution[j+m_numVariables]);
+			Msg("\n");
+		}
+		else
+			Msg("%g\t<= %g\n", flRes, GetInitialTableau(i,NumColumns()-1));
+	}
+#endif
+}
+
+
+
+class CSimplexTestUnit
+{
+public:
+	CSimplexTestUnit()
+	{
+		CSimplex test(3,2);
+		test.SetObjectiveFactor(0, 12);
+		test.SetObjectiveFactor(1, 8);
+		test.SetObjectiveFactor(2, 24);
+		test.SetConstraintFactor(0, 0, 6);
+		test.SetConstraintFactor(0, 1, 2);
+		test.SetConstraintFactor(0, 2, 4);
+		test.SetConstraintConst(0, 200);
+		test.SetConstraintFactor(1, 0, 2);
+		test.SetConstraintFactor(1, 1, 2);
+		test.SetConstraintFactor(1, 2, 12);
+		test.SetConstraintConst(1, 160);
+
+		test.Solve();
+
+		test.Init(2,2);
+		float test2[] = {2,1,3,  3,1,4,  17,5,0};
+		test.InitTableau(test2);
+		test.Solve();
+		// m_pSolution (test.m_pSolution) should be : 30 40 | 0 0 | 4100
+
+		//////////////////////////////////////////////////////////////////////////
+		// unbound-solution problem: x1-x2<=1 && x2-x1<=1, maximize x1+x2; if x1==x2, we can go unbound x1==x2 -> +inf
+		// the dual formulation is infeasible in this case: v2-v1 >= 1 && v1-v2 >= 1, which are self-contradictory
+		test.Init(2,2);
+		float testUnsolvable[] = {-1,1,1, 1,-1,1, 1,1,0};
+		test.InitTableau(testUnsolvable);
+		test.Solve();
+
+
+		//////////////////////////////////////////////////////////////////////////
+		// General Simplex problem: equality constraint
+		test.Init(2, 3);
+		float testGenSimplex[] = {1,1,20, 1,2,30, -1,-2,-30, 2,1,0};
+		test.InitTableau(testGenSimplex);
+		test.Solve();
+
+		test.Init(7,6);
+		float testA[56]={ -1, 1, 0, -0, -0, 0, 1, 13.0048,
+			1, -1, 0, -0, -0, 0, 1, 13.0048,
+			0, -0, -1, 1, -0, 0, 1, 13.0048,
+			0, -0, 1, -1, -0, 0, 1, 13.0048,
+			0, -0, 0, -0, 1, -1, 1, 0.00100005,
+			0, -0, 0, -0, -1, 1, 1, 0.405401,
+			0, 0, 0, 0, 0, 0, 1, 0
+		};
+		test.InitTableau(testA);
+		test.Solve();
+	}
+};
+
+
+// this is for debugging and unit-testing
+//static CSimplexTestUnit s_test;

mathlib/softbodyenvironment.cpp

@@ -0,0 +1,156 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+// This is analogous to RnWorld from source2, but only for softbodies
+// 
+#include "mathlib/softbodyenvironment.h"
+
+//-----------------------------------------------------------------------------------------------------------------------------
+CSoftbodyCollisionFilter::CSoftbodyCollisionFilter()
+{
+	V_memset( m_GroupPairs, 0, sizeof( m_GroupPairs ) );
+
+	// Allow default-default shape collision by default to avoid confusion, 
+	// in test worlds that won't set up their own collision filtering.
+	// INTERSECTION_COLLISION_GROUP_ALWAYS is the default collision group
+	InitGroup( COLLISION_GROUP_ALWAYS, INTERSECTION_PAIR_DEFAULT_COLLISION );
+}
+
+
+//-----------------------------------------------------------------------------------------------------------------------------
+uint16 CSoftbodyCollisionFilter::TestSimulation( const RnCollisionAttr_t &left, const RnCollisionAttr_t &right )const
+{
+	Assert( left.m_nCollisionGroup < MAX_GROUPS && right.m_nCollisionGroup < MAX_GROUPS );
+
+	uint16 nIntersectionFlags = 0;
+
+	if ( left.IsSolidContactEnabled() && right.IsSolidContactEnabled() )
+	{
+		// Return value from a table of PxPairFlags.
+		nIntersectionFlags = m_GroupPairs[ left.m_nCollisionGroup ][ right.m_nCollisionGroup ];
+	}
+
+	if ( ( ( left.m_nInteractsAs & right.m_nInteractsExclude ) | ( left.m_nInteractsExclude & right.m_nInteractsAs ) ) != 0 )
+	{
+		nIntersectionFlags = 0;
+	}
+	else if ( ( ( left.m_nInteractsAs & right.m_nInteractsWith ) | ( left.m_nInteractsWith & right.m_nInteractsAs ) ) != 0 )
+	{
+		nIntersectionFlags |= INTERSECTION_PAIR_TRIGGER;
+	}
+	else if ( left.m_nCollisionGroup == COLLISION_GROUP_CONDITIONALLY_SOLID || right.m_nCollisionGroup == COLLISION_GROUP_CONDITIONALLY_SOLID )
+	{
+		nIntersectionFlags = 0;
+	}
+
+	if ( !left.IsTouchEventEnabled() || !right.IsTouchEventEnabled() )
+	{
+		nIntersectionFlags &= ~INTERSECTION_PAIR_TRIGGER;
+	}
+
+	// no interactions within the same hierarchy
+	// 0xFFFF is a special case because entindex() is -1 for all client-side-only entities
+	if ( left.m_nHierarchyId != 0 && left.m_nHierarchyId != 0xFFFF && left.m_nHierarchyId == right.m_nHierarchyId )
+	{
+		nIntersectionFlags = 0;
+	}
+
+	return nIntersectionFlags;
+}
+
+
+//-----------------------------------------------------------------------------------------------------------------------------
+void CSoftbodyCollisionFilter::InitGroup( int nGroup, CollisionGroupPairFlags defaultFlags )
+{
+	for ( int i = 0; i < MAX_GROUPS; ++i )
+	{
+		m_GroupPairs[ i ][ nGroup ] = m_GroupPairs[ nGroup ][ i ] = defaultFlags;
+	}
+	m_GroupPairs[ COLLISION_GROUP_ALWAYS ][ nGroup ] = m_GroupPairs[ nGroup ][ COLLISION_GROUP_ALWAYS ] = INTERSECTION_PAIR_DEFAULT_COLLISION;
+	m_GroupPairs[ COLLISION_GROUP_TRIGGER ][ nGroup ] = m_GroupPairs[ nGroup ][ COLLISION_GROUP_TRIGGER ] = 0;
+}
+
+
+AABB_t CSoftbodyCollisionSphere::GetBbox()const
+{
+	AABB_t aabb;
+	aabb.m_vMinBounds = m_vCenter - Vector( m_flRadius, m_flRadius, m_flRadius );
+	aabb.m_vMaxBounds = m_vCenter + Vector( m_flRadius, m_flRadius, m_flRadius );
+	return aabb;
+}
+
+AABB_t CSoftbodyCollisionCapsule::GetBbox()const
+{
+	AABB_t aabb;
+	aabb.m_vMinBounds = VectorMin( m_vCenter[ 0 ], m_vCenter[ 1 ] ) - Vector( m_flRadius, m_flRadius, m_flRadius );
+	aabb.m_vMaxBounds = VectorMax( m_vCenter[ 0 ], m_vCenter[ 1 ] ) + Vector( m_flRadius, m_flRadius, m_flRadius );
+	return aabb;
+}
+
+
+void CSoftbodyEnvironment::Add( CSoftbodyCollisionShape * pShape )
+{
+	if ( pShape->GetProxyId() >= 0 )
+		return; // already added
+
+	AABB_t bbox = pShape->GetBbox();
+	// bbox.Expand( 8 ); // when we add the shape for the first time, we might not know if it's going to move at all - so a good heuristic might be to NOT expand the proxy bounds at first
+
+	int32 nProxyId = m_BroadphaseTree.CreateProxy( bbox, pShape );
+	pShape->SetProxyId( nProxyId );
+}
+
+
+void CSoftbodyEnvironment::Update( CSoftbodyCollisionShape * pShape )
+{
+	int32 nProxyId = pShape->GetProxyId();
+	if ( nProxyId < 0 )
+		return; // proxy not added to the broadphase
+
+	// Did the bbox move enough to warrant moving the proxy?
+	AABB_t bbox = pShape->GetBbox();
+	if ( !m_BroadphaseTree.GetBounds( nProxyId).Contains( bbox ) )
+	{
+		bbox.Expand( 8 ); // we moved the proxy... maybe a good heuristic here would be to keep track of movements and expand adaptively, depending on how much the proxy moves
+		m_BroadphaseTree.MoveProxy( nProxyId, bbox );
+	}
+}
+
+
+void CSoftbodyEnvironment::AddOrUpdate( CSoftbodyCollisionShape * pShape )
+{
+	int32 nProxyId = pShape->GetProxyId();
+	// Did the bbox move enough to warrant moving the proxy?
+	AABB_t bbox = pShape->GetBbox();
+	if ( nProxyId < 0 )
+	{
+		int32 nProxyId = m_BroadphaseTree.CreateProxy( bbox, pShape );
+		pShape->SetProxyId( nProxyId );
+	}
+	else if ( !m_BroadphaseTree.GetBounds( nProxyId ).Contains( bbox ) )
+	{
+		bbox.Expand( 8 ); // we moved the proxy... maybe a good heuristic here would be to keep track of movements and expand adaptively, depending on how much the proxy moves
+		m_BroadphaseTree.MoveProxy( nProxyId, bbox );
+	}
+}
+
+void CSoftbodyEnvironment::Remove( CSoftbodyCollisionShape * pShape )
+{
+	int nProxyId = pShape->GetProxyId();
+	if ( nProxyId >= 0 )
+	{
+		m_BroadphaseTree.DestroyProxy( pShape->GetProxyId() );
+		pShape->SetProxyId( -1 );
+	}
+}
+
+void CSoftbodyEnvironment::SetWind( const Vector & vWind )
+{
+	float flStrength = vWind.Length();
+	if ( fabsf( m_vWindDesc.w ) < FLT_EPSILON )
+	{
+		SetNoWind();
+	}
+	else
+	{
+		m_vWindDesc.Init( vWind / flStrength, flStrength );
+	}
+}

mathlib/sparse_convolution_noise.cpp

@@ -0,0 +1,251 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: noise() primitives.
+//
+//=====================================================================================//
+
+#include <math.h>
+#include "basetypes.h"
+#ifndef _PS3
+#include <memory.h>
+#endif
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+#include "mathlib/noise.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+// generate high quality noise based upon "sparse convolution". HIgher quality than perlin noise,
+// and no direcitonal artifacts.
+
+#include "noisedata.h"
+
+#define N_IMPULSES_PER_CELL 5
+#define NORMALIZING_FACTOR 1.0
+
+//(0.5/N_IMPULSES_PER_CELL)
+
+static inline int LatticeCoord(float x)
+{
+	return ((int) floor(x)) & 0xff;
+}
+
+static inline int Hash4D(int ix, int iy, int iz, int idx)
+{
+	int ret=perm_a[ix];
+	ret=perm_b[(ret+iy) & 0xff];
+	ret=perm_c[(ret+iz) & 0xff];
+	ret=perm_d[(ret+idx) & 0xff];
+	return ret;
+}
+
+#define SQ(x) ((x)*(x))
+
+static float CellNoise( int ix, int iy, int iz, float xfrac, float yfrac, float zfrac,
+						float (*pNoiseShapeFunction)(float) )
+{
+	float ret=0;
+	for(int idx=0;idx<N_IMPULSES_PER_CELL;idx++)
+	{
+		int coord_idx=Hash4D( ix, iy, iz, idx );
+		float dsq=SQ(impulse_xcoords[coord_idx]-xfrac)+
+			SQ(impulse_ycoords[coord_idx]-yfrac)+
+			SQ(impulse_zcoords[coord_idx]-zfrac);
+		dsq = sqrt( dsq );
+		if (dsq < 1.0 )
+		{
+			ret += (*pNoiseShapeFunction)( 1-dsq );
+		}
+	}
+	return ret;
+}
+
+
+float SparseConvolutionNoise( Vector const &pnt )
+{
+	return SparseConvolutionNoise( pnt, QuinticInterpolatingPolynomial );
+}
+
+float FractalNoise( Vector const &pnt, int n_octaves)
+{
+	float scale=1.0;
+	float iscale=1.0;
+	float ret=0;
+	float sumscale=0;
+	for(int o=0;o<n_octaves;o++)
+	{
+		Vector p1=pnt;
+		p1 *= scale;
+		ret+=iscale * SparseConvolutionNoise( p1 );
+		sumscale += iscale;
+		scale *= 2.0;
+		iscale *= 0.5;
+	}
+	return ret * ( 1.0/sumscale );
+}
+
+float Turbulence( Vector const &pnt, int n_octaves)
+{
+	float scale=1.0;
+	float iscale=1.0;
+	float ret=0;
+	float sumscale=0;
+	for(int o=0;o<n_octaves;o++)
+	{
+		Vector p1=pnt;
+		p1 *= scale;
+		ret+=iscale * fabs ( 2.0*( SparseConvolutionNoise( p1 )-.5 ) );
+		sumscale += iscale;
+		scale *= 2.0;
+		iscale *= 0.5;
+	}
+	return ret * ( 1.0/sumscale );
+}
+
+#ifdef MEASURE_RANGE
+float fmin1=10000000.0;
+float fmax1=-1000000.0;
+#endif
+
+float SparseConvolutionNoise(Vector const &pnt, float (*pNoiseShapeFunction)(float) )
+{
+	// computer integer lattice point
+	int ix=LatticeCoord(pnt.x);
+	int iy=LatticeCoord(pnt.y);
+	int iz=LatticeCoord(pnt.z);
+
+	// compute offsets within unit cube
+	float xfrac=pnt.x-floor(pnt.x);
+	float yfrac=pnt.y-floor(pnt.y);
+	float zfrac=pnt.z-floor(pnt.z);
+
+	float sum_out=0.;
+
+	for(int ox=-1; ox<=1; ox++)
+		for(int oy=-1; oy<=1; oy++)
+			for(int oz=-1; oz<=1; oz++)
+			{
+				sum_out += CellNoise( ix+ox, iy+oy, iz+oz,
+									  xfrac-ox, yfrac-oy, zfrac-oz,
+									  pNoiseShapeFunction );
+			}
+#ifdef MEASURE_RANGE
+	fmin1=min(sum_out,fmin1);
+	fmax1=max(sum_out,fmax1);
+#endif
+	return RemapValClamped( sum_out, .544487, 9.219176, 0.0, 1.0 );
+}
+
+
+float TileableSparseConvolutionNoise(Vector const &pnt, float (*pNoiseShapeFunction)(float) )
+{
+	// computer integer lattice point
+	int ix=LatticeCoord(pnt.x);
+	int iy=LatticeCoord(pnt.y);
+	int iz=LatticeCoord(pnt.z);
+
+	// compute offsets within unit cube
+	float xfrac=pnt.x-floor(pnt.x);
+	float yfrac=pnt.y-floor(pnt.y);
+	float zfrac=pnt.z-floor(pnt.z);
+
+	float sum_out=0.;
+
+	for(int ox=-1; ox<=1; ox++)
+		for(int oy=-1; oy<=1; oy++)
+			for(int oz=-1; oz<=1; oz++)
+			{
+				sum_out += CellNoise( ix+ox, iy+oy, iz+oz,
+									  xfrac-ox, yfrac-oy, zfrac-oz,
+									  pNoiseShapeFunction );
+			}
+#ifdef MEASURE_RANGE
+	fmin1=min(sum_out,fmin1);
+	fmax1=max(sum_out,fmax1);
+#endif
+	return RemapValClamped( sum_out, .544487, 9.219176, 0.0, 1.0 );
+}
+
+
+
+// Improved Perlin Noise
+// The following code is the c-ification of Ken Perlin's new noise algorithm
+// "JAVA REFERENCE IMPLEMENTATION OF IMPROVED NOISE - COPYRIGHT 2002 KEN PERLIN"
+// as available here: http://mrl.nyu.edu/~perlin/noise/
+
+float NoiseGradient(int hash, float x, float y, float z)
+{
+	int h = hash & 15;                      // CONVERT LO 4 BITS OF HASH CODE
+	float u = h<8 ? x : y;                  // INTO 12 GRADIENT DIRECTIONS.
+	float v = h<4 ? y : (h==12||h==14 ? x : z);
+	return ((h&1) == 0 ? u : -u) + ((h&2) == 0 ? v : -v);
+}
+
+int NoiseHashIndex( int i )
+{
+	static int s_permutation[] = 
+	{
+		151,160,137,91,90,15,
+			131,13,201,95,96,53,194,233,7,225,140,36,103,30,69,142,8,99,37,240,21,10,23,
+			190, 6,148,247,120,234,75,0,26,197,62,94,252,219,203,117,35,11,32,57,177,33,
+			88,237,149,56,87,174,20,125,136,171,168, 68,175,74,165,71,134,139,48,27,166,
+			77,146,158,231,83,111,229,122,60,211,133,230,220,105,92,41,55,46,245,40,244,
+			102,143,54, 65,25,63,161, 1,216,80,73,209,76,132,187,208, 89,18,169,200,196,
+			135,130,116,188,159,86,164,100,109,198,173,186, 3,64,52,217,226,250,124,123,
+			5,202,38,147,118,126,255,82,85,212,207,206,59,227,47,16,58,17,182,189,28,42,
+			223,183,170,213,119,248,152, 2,44,154,163, 70,221,153,101,155,167, 43,172,9,
+			129,22,39,253, 19,98,108,110,79,113,224,232,178,185, 112,104,218,246,97,228,
+			251,34,242,193,238,210,144,12,191,179,162,241, 81,51,145,235,249,14,239,107,
+			49,192,214, 31,181,199,106,157,184, 84,204,176,115,121,50,45,127, 4,150,254,
+			138,236,205,93,222,114,67,29,24,72,243,141,128,195,78,66,215,61,156,180
+	};
+
+	return s_permutation[ i & 0xff ];
+}
+
+float ImprovedPerlinNoise( Vector const &pnt )
+{
+	float fx = floor(pnt.x);
+	float fy = floor(pnt.y);
+	float fz = floor(pnt.z);
+
+	int X = (int)fx & 255;								// FIND UNIT CUBE THAT
+	int Y = (int)fy & 255;								// CONTAINS POINT.
+	int Z = (int)fz & 255;
+
+	float x = pnt.x - fx;								// FIND RELATIVE X,Y,Z
+	float y = pnt.y - fy;								// OF POINT IN CUBE.
+	float z = pnt.z - fz;
+
+	float u = QuinticInterpolatingPolynomial(x);		// COMPUTE FADE CURVES
+	float v = QuinticInterpolatingPolynomial(y);		// FOR EACH OF X,Y,Z.
+	float w = QuinticInterpolatingPolynomial(z);
+
+	int A  = NoiseHashIndex( X ) + Y;					// HASH COORDINATES OF
+	int AA = NoiseHashIndex( A ) + Z;					// THE 8 CUBE CORNERS,
+	int AB = NoiseHashIndex( A + 1 ) + Z;
+	int B  = NoiseHashIndex( X + 1 ) + Y;
+	int BA = NoiseHashIndex( B ) + Z;
+	int BB = NoiseHashIndex( B + 1 ) + Z;
+
+	float g0 = NoiseGradient(NoiseHashIndex(AA  ), x  , y  , z   );
+	float g1 = NoiseGradient(NoiseHashIndex(BA  ), x-1, y  , z   );
+	float g2 = NoiseGradient(NoiseHashIndex(AB  ), x  , y-1, z   );
+	float g3 = NoiseGradient(NoiseHashIndex(BB  ), x-1, y-1, z   );
+	float g4 = NoiseGradient(NoiseHashIndex(AA+1), x  , y  , z-1 );
+	float g5 = NoiseGradient(NoiseHashIndex(BA+1), x-1, y  , z-1 );
+	float g6 = NoiseGradient(NoiseHashIndex(AB+1), x  , y-1, z-1 );
+	float g7 = NoiseGradient(NoiseHashIndex(BB+1), x-1, y-1, z-1 );
+
+	// AND ADD BLENDED RESULTS FROM 8 CORNERS OF CUBE
+	float g01 = Lerp( u, g0, g1 );
+	float g23 = Lerp( u, g2, g3 );
+	float g45 = Lerp( u, g4, g5 );
+	float g67 = Lerp( u, g6, g7 );
+	float g0123 = Lerp( v, g01, g23 );
+	float g4567 = Lerp( v, g45, g67 );
+
+	return Lerp( w, g0123,g4567 );
+}

mathlib/sphere.cpp

@@ -0,0 +1,75 @@
+//========= Copyright © Valve Corporation, All rights reserved. ============//
+#include "sphere.h"
+//#include "body.h"
+//#include "gjk.h"
+//#include "toi.h"
+
+
+//--------------------------------------------------------------------------------------------------
+// Local utilities 
+//--------------------------------------------------------------------------------------------------
+static void CastStationaryHit( CShapeCastResult& out, float c, const Vector &p, const Vector &m, float mm )
+{
+	// return a sphere hit for zero-length ray at point p, with
+	// m = p - m_vCenter
+	// mm = DotProduct( m, m )
+	// c = mm - Sqr( m_flRadius )
+
+	if( c <= 0 )
+	{
+		out.m_flHitTime = 0;
+		out.m_vHitPoint = p;
+		if( mm > FLT_EPSILON )
+		{
+			out.m_vHitNormal = m / sqrtf( mm );
+		}
+		else
+		{
+			out.m_vHitNormal = Vector( 0,0,1 );
+		}
+	}
+	else
+	{
+		// we didn't hit - we're outside and we don't move
+		out.m_flHitTime = FLT_MAX;
+	}
+}
+
+
+//--------------------------------------------------------------------------------------------------
+void CastSphereRay( CShapeCastResult& out, const Vector &m, const Vector& p, const Vector& d, float flRadius )
+{
+	float a = DotProduct( d, d ), mm = DotProduct( m, m ), c = mm - Sqr( flRadius );
+	if( a < FLT_EPSILON * FLT_EPSILON )
+	{
+		// we barely move; just detect if we're in the sphere or not
+		CastStationaryHit( out, c, p, m, mm );
+		return;
+	}
+
+	float b = DotProduct( m, d ); // solve: at^2+2bt+c=0; t = (-b±sqrt(b^2-ac))/a = -b/a ± sqrt((b/a)^2-c/a))
+	float D = Sqr( b ) - a * c;
+	if( D < 0 )
+	{
+		// no intersection at all
+		out.m_flHitTime = FLT_MAX;
+		return;
+	}
+	float sqrtD = sqrtf( D );
+	float t = ( -b - sqrtD ) / a;
+	if( t < 0 )
+	{
+		// this was the first hit in the past - determine if we're still inside the sphere at time t=0
+		// we could do that by checking if float t1 = ( b + sqrtD ) / a; is > 0 or not, but it's easier to:
+		// we barely move; just detect if we're in the sphere or not
+		CastStationaryHit( out, c, p, m, mm );
+	}
+	else
+	{
+		out.m_flHitTime = t;
+		Vector dt = d * t;
+		out.m_vHitPoint = p + dt;
+		out.m_vHitNormal = ( m + dt ) / flRadius; // Should I normalize this here or is this sufficient precision?
+	}
+}
+

mathlib/spherical.cpp

@@ -0,0 +1,126 @@
+//========= Copyright © 1996-2007, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: spherical math routines
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#ifndef _PS3
+#include <memory.h>
+#endif
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+#include "mathlib/spherical_geometry.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+float s_flFactorials[]={
+	1.,
+	1.,
+	2.,
+	6.,
+	24.,
+	120.,
+	720.,
+	5040.,
+	40320.,
+	362880.,
+	3628800.,
+	39916800.,
+	479001600.,
+	6227020800.,
+	87178291200.,
+	1307674368000.,
+	20922789888000.,
+	355687428096000.,
+	6402373705728000.,
+	121645100408832000.,
+	2432902008176640000.,
+	51090942171709440000.,
+	1124000727777607680000.,
+	25852016738884976640000.,
+	620448401733239439360000.,
+	15511210043330985984000000.,
+	403291461126605635584000000.,
+	10888869450418352160768000000.,
+	304888344611713860501504000000.,
+	8841761993739701954543616000000.,
+	265252859812191058636308480000000.,
+	8222838654177922817725562880000000.,
+	263130836933693530167218012160000000.,
+	8683317618811886495518194401280000000.
+};
+
+float AssociatedLegendrePolynomial( int nL, int nM, float flX )
+{
+	// evaluate associated legendre polynomial at flX, using recurrence relation
+	float flPmm = 1.;
+	if ( nM > 0 )
+	{
+		float flSomX2 = sqrt( ( 1 - flX ) * ( 1 + flX ) );
+		float flFact = 1.;
+		for( int i = 0 ; i < nM; i++ )
+		{
+			flPmm *= -flFact * flSomX2;
+			flFact += 2.0;
+		}
+	}
+	if ( nL == nM )
+		return flPmm;
+	float flPmmp1 = flX * ( 2.0 * nM + 1.0 ) * flPmm;
+	if ( nL == nM + 1 ) 
+		return flPmmp1;
+	float flPll = 0.;
+	for( int nLL = nM + 2 ; nLL <= nL; nLL++ )
+	{
+		flPll = ( ( 2.0 * nLL - 1.0 ) * flX * flPmmp1 - ( nLL + nM - 1.0 ) * flPmm ) * ( 1.0 / ( nLL - nM ) );
+		flPmm = flPmmp1;
+		flPmmp1 = flPll;
+	}
+	return flPll;
+}
+
+static float SHNormalizationFactor( int nL, int nM )
+{
+	double flTemp = ( ( 2. * nL + 1.0 ) * s_flFactorials[ nL - nM ] )/ ( 4. * M_PI * s_flFactorials[ nL + nM ] );
+	return sqrt( flTemp );
+}
+
+#define SQRT_2 1.414213562373095 
+
+FORCEINLINE float SphericalHarmonic( int nL, int nM, float flTheta, float flPhi, float flCosTheta )
+{
+	if ( nM == 0 )
+		return SHNormalizationFactor( nL, 0 ) * AssociatedLegendrePolynomial( nL, nM, flCosTheta );
+
+	if ( nM > 0 )
+		return SQRT_2 * SHNormalizationFactor( nL, nM ) * cos ( nM * flPhi ) *
+			AssociatedLegendrePolynomial( nL, nM, flCosTheta );
+
+	return 
+		SQRT_2 * SHNormalizationFactor( nL, -nM ) * sin( -nM * flPhi ) * AssociatedLegendrePolynomial( nL, -nM, flCosTheta );
+
+}
+
+float SphericalHarmonic( int nL, int nM, float flTheta, float flPhi )
+{
+	return SphericalHarmonic( nL, nM, flTheta, flPhi, cos( flTheta ) );
+}
+
+float SphericalHarmonic( int nL, int nM, Vector const &vecDirection )
+{
+	Assert( fabs( VectorLength( vecDirection ) - 1.0 ) < 0.0001 );
+	float flPhi = acos( vecDirection.z );
+	float flTheta = 0;
+	float S = Square( vecDirection.x ) + Square( vecDirection.y );
+	if ( S > 0 )
+	{
+		flTheta = atan2( vecDirection.y, vecDirection.x );
+	}
+	return SphericalHarmonic( nL, nM, flTheta, flPhi, cos( flTheta ) );
+}
+

mathlib/sse.cpp

@@ -0,0 +1,760 @@
+//========= Copyright � 1996-2005, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: SSE Math primitives.
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+#include "sse.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+static const uint32 _sincos_masks[]	  = { (uint32)0x0,  (uint32)~0x0 };
+static const uint32 _sincos_inv_masks[] = { (uint32)~0x0, (uint32)0x0 };
+
+//-----------------------------------------------------------------------------
+// Macros and constants required by some of the SSE assembly:
+//-----------------------------------------------------------------------------
+
+#ifdef _WIN32
+	#define _PS_EXTERN_CONST(Name, Val) \
+		const __declspec(align(16)) float _ps_##Name[4] = { Val, Val, Val, Val }
+
+	#define _PS_EXTERN_CONST_TYPE(Name, Type, Val) \
+		const __declspec(align(16)) Type _ps_##Name[4] = { Val, Val, Val, Val }; \
+
+	#define _EPI32_CONST(Name, Val) \
+		static const __declspec(align(16)) __int32 _epi32_##Name[4] = { Val, Val, Val, Val }
+
+	#define _PS_CONST(Name, Val) \
+		static const __declspec(align(16)) float _ps_##Name[4] = { Val, Val, Val, Val }
+#elif POSIX
+	#define _PS_EXTERN_CONST(Name, Val) \
+		const float _ps_##Name[4] __attribute__((aligned(16))) = { Val, Val, Val, Val }
+
+	#define _PS_EXTERN_CONST_TYPE(Name, Type, Val) \
+		const Type _ps_##Name[4]  __attribute__((aligned(16))) = { Val, Val, Val, Val }; \
+
+	#define _EPI32_CONST(Name, Val) \
+		static const int32 _epi32_##Name[4]  __attribute__((aligned(16))) = { Val, Val, Val, Val }
+
+ 	#define _PS_CONST(Name, Val) \
+ 		static const float _ps_##Name[4]  __attribute__((aligned(16))) = { Val, Val, Val, Val }
+#endif
+
+_PS_EXTERN_CONST(am_0, 0.0f);
+_PS_EXTERN_CONST(am_1, 1.0f);
+_PS_EXTERN_CONST(am_m1, -1.0f);
+_PS_EXTERN_CONST(am_0p5, 0.5f);
+_PS_EXTERN_CONST(am_1p5, 1.5f);
+_PS_EXTERN_CONST(am_pi, (float)M_PI);
+_PS_EXTERN_CONST(am_pi_o_2, (float)(M_PI / 2.0));
+_PS_EXTERN_CONST(am_2_o_pi, (float)(2.0 / M_PI));
+_PS_EXTERN_CONST(am_pi_o_4, (float)(M_PI / 4.0));
+_PS_EXTERN_CONST(am_4_o_pi, (float)(4.0 / M_PI));
+_PS_EXTERN_CONST_TYPE(am_sign_mask, int32, 0x80000000);
+_PS_EXTERN_CONST_TYPE(am_inv_sign_mask, int32, ~0x80000000);
+_PS_EXTERN_CONST_TYPE(am_min_norm_pos,int32, 0x00800000);
+_PS_EXTERN_CONST_TYPE(am_mant_mask, int32, 0x7f800000);
+_PS_EXTERN_CONST_TYPE(am_inv_mant_mask, int32, ~0x7f800000);
+
+_EPI32_CONST(1, 1);
+_EPI32_CONST(2, 2);
+
+_PS_CONST(sincos_p0, 0.15707963267948963959e1f);
+_PS_CONST(sincos_p1, -0.64596409750621907082e0f);
+_PS_CONST(sincos_p2, 0.7969262624561800806e-1f);
+_PS_CONST(sincos_p3, -0.468175413106023168e-2f);
+
+#ifdef PFN_VECTORMA
+void  __cdecl _SSE_VectorMA( const float *start, float scale, const float *direction, float *dest );
+#endif
+
+//-----------------------------------------------------------------------------
+// SSE implementations of optimized routines:
+//-----------------------------------------------------------------------------
+
+
+
+float FASTCALL _SSE_VectorNormalize (Vector& vec)
+{
+	Assert( s_bMathlibInitialized );
+
+	// NOTE: This is necessary to prevent an memory overwrite...
+	// sice vec only has 3 floats, we can't "movaps" directly into it.
+#ifdef _WIN32
+	__declspec(align(16)) float result[4];
+#elif POSIX
+	 float result[4] __attribute__((aligned(16)));
+#endif
+
+	float *v = &vec[0];
+	float *r = &result[0];
+
+	float	radius = 0.f;
+	// Blah, get rid of these comparisons ... in reality, if you have all 3 as zero, it shouldn't 
+	// be much of a performance win, considering you will very likely miss 3 branch predicts in a row.
+	if ( v[0] || v[1] || v[2] )
+	{
+#if defined( _WIN32 ) && !defined( _WIN64 )
+	_asm
+		{
+			mov			eax, v
+			mov			edx, r
+#ifdef ALIGNED_VECTOR
+			movaps		xmm4, [eax]			// r4 = vx, vy, vz, X
+			movaps		xmm1, xmm4			// r1 = r4
+#else
+			movups		xmm4, [eax]			// r4 = vx, vy, vz, X
+			movaps		xmm1, xmm4			// r1 = r4
+#endif
+			mulps		xmm1, xmm4			// r1 = vx * vx, vy * vy, vz * vz, X
+			movhlps		xmm3, xmm1			// r3 = vz * vz, X, X, X
+			movaps		xmm2, xmm1			// r2 = r1
+			shufps		xmm2, xmm2, 1		// r2 = vy * vy, X, X, X
+			addss		xmm1, xmm2			// r1 = (vx * vx) + (vy * vy), X, X, X
+			addss		xmm1, xmm3			// r1 = (vx * vx) + (vy * vy) + (vz * vz), X, X, X
+			sqrtss		xmm1, xmm1			// r1 = sqrt((vx * vx) + (vy * vy) + (vz * vz)), X, X, X
+			movss		radius, xmm1		// radius = sqrt((vx * vx) + (vy * vy) + (vz * vz))
+			rcpss		xmm1, xmm1			// r1 = 1/radius, X, X, X
+			shufps		xmm1, xmm1, 0		// r1 = 1/radius, 1/radius, 1/radius, X
+			mulps		xmm4, xmm1			// r4 = vx * 1/radius, vy * 1/radius, vz * 1/radius, X
+			movaps		[edx], xmm4			// v = vx * 1/radius, vy * 1/radius, vz * 1/radius, X
+		}
+#elif _WIN64
+		// Inline assembly isn't allowed in 64-bit MSVC. Sadness.
+		float recipSqrt = FastRSqrt(  vec.x * vec.x + vec.y * vec.y + vec.z * vec.z );
+		r[ 0 ] = vec.x * recipSqrt;
+		r[ 1 ] = vec.y * recipSqrt;
+		r[ 2 ] = vec.z * recipSqrt;
+
+#elif POSIX
+		__asm__ __volatile__(
+#ifdef ALIGNED_VECTOR
+            "movaps          %2, %%xmm4 \n\t"
+            "movaps          %%xmm4, %%xmm1 \n\t"
+#else
+            "movups          %2, %%xmm4 \n\t"
+            "movaps          %%xmm4, %%xmm1 \n\t"
+#endif
+            "mulps           %%xmm4, %%xmm1 \n\t"
+            "movhlps         %%xmm1, %%xmm3 \n\t"
+            "movaps          %%xmm1, %%xmm2 \n\t"
+            "shufps          $1, %%xmm2, %%xmm2 \n\t"
+            "addss           %%xmm2, %%xmm1 \n\t"
+            "addss           %%xmm3, %%xmm1 \n\t"
+            "sqrtss          %%xmm1, %%xmm1 \n\t"
+            "movss           %%xmm1, %0 \n\t"
+            "rcpss           %%xmm1, %%xmm1 \n\t"
+            "shufps          $0, %%xmm1, %%xmm1 \n\t"
+            "mulps           %%xmm1, %%xmm4 \n\t"
+            "movaps          %%xmm4, %1 \n\t"
+            : "=m" (radius), "=m" (result)
+            : "m" (*v)
+ 		);
+#else
+	#error "Not Implemented"
+#endif
+		vec.x = result[0];
+		vec.y = result[1];
+		vec.z = result[2];
+
+	}
+
+	return radius;
+}
+
+
+#if defined( _WIN32 ) && !defined( _WIN64 )
+void FastSinCos( float x, float* s, float* c )  // any x
+{
+	float t4, t8, t12;
+
+	__asm
+	{
+		movss	xmm0, x
+		movss	t12, xmm0
+		movss	xmm1, _ps_am_inv_sign_mask
+		mov		eax, t12
+		mulss	xmm0, _ps_am_2_o_pi
+		andps	xmm0, xmm1
+		and		eax, 0x80000000
+
+		cvttss2si	edx, xmm0
+		mov		ecx, edx
+		mov		t12, esi
+		mov		esi, edx
+		add		edx, 0x1	
+		shl		ecx, (31 - 1)
+		shl		edx, (31 - 1)
+
+		movss	xmm4, _ps_am_1
+		cvtsi2ss	xmm3, esi
+		mov		t8, eax
+		and		esi, 0x1
+
+		subss	xmm0, xmm3
+		movss	xmm3, _sincos_inv_masks[esi * 4]
+		minss	xmm0, xmm4
+
+		subss	xmm4, xmm0
+
+		movss	xmm6, xmm4
+		andps	xmm4, xmm3
+		and		ecx, 0x80000000
+		movss	xmm2, xmm3
+		andnps	xmm3, xmm0
+		and		edx, 0x80000000
+		movss	xmm7, t8
+		andps	xmm0, xmm2
+		mov		t8, ecx
+		mov		t4, edx
+		orps	xmm4, xmm3
+
+		mov		eax, s     //mov eax, [esp + 4 + 16]
+		mov		edx, c //mov edx, [esp + 4 + 16 + 4]
+
+		andnps	xmm2, xmm6
+		orps	xmm0, xmm2
+
+		movss	xmm2, t8
+		movss	xmm1, xmm0
+		movss	xmm5, xmm4
+		xorps	xmm7, xmm2
+		movss	xmm3, _ps_sincos_p3
+		mulss	xmm0, xmm0
+		mulss	xmm4, xmm4
+		movss	xmm2, xmm0
+		movss	xmm6, xmm4
+		orps	xmm1, xmm7
+		movss	xmm7, _ps_sincos_p2
+		mulss	xmm0, xmm3
+		mulss	xmm4, xmm3
+		movss	xmm3, _ps_sincos_p1
+		addss	xmm0, xmm7
+		addss	xmm4, xmm7
+		movss	xmm7, _ps_sincos_p0
+		mulss	xmm0, xmm2
+		mulss	xmm4, xmm6
+		addss	xmm0, xmm3
+		addss	xmm4, xmm3
+		movss	xmm3, t4
+		mulss	xmm0, xmm2
+		mulss	xmm4, xmm6
+		orps	xmm5, xmm3
+		mov		esi, t12
+		addss	xmm0, xmm7
+		addss	xmm4, xmm7
+		mulss	xmm0, xmm1
+		mulss	xmm4, xmm5
+
+		// use full stores since caller might reload with full loads
+		movss	[eax], xmm0
+		movss	[edx], xmm4
+	}
+}
+#if 0
+//-----------------------------------------------------------------------------
+// SSE2 implementations of optimized routines:
+//-----------------------------------------------------------------------------
+void FastSinCos( float x, float* s, float* c )  // any x
+{
+	__asm
+	{
+		movss	xmm0, x
+		movaps	xmm7, xmm0
+		movss	xmm1, _ps_am_inv_sign_mask
+		movss	xmm2, _ps_am_sign_mask
+		movss	xmm3, _ps_am_2_o_pi
+		andps	xmm0, xmm1
+		andps	xmm7, xmm2
+		mulss	xmm0, xmm3
+
+		pxor	xmm3, xmm3
+		movd	xmm5, _epi32_1
+		movss	xmm4, _ps_am_1
+
+		cvttps2dq	xmm2, xmm0
+		pand	xmm5, xmm2
+		movd	xmm1, _epi32_2
+		pcmpeqd	xmm5, xmm3
+		movd	xmm3, _epi32_1
+		cvtdq2ps	xmm6, xmm2
+		paddd	xmm3, xmm2
+		pand	xmm2, xmm1
+		pand	xmm3, xmm1
+		subss	xmm0, xmm6
+		pslld	xmm2, (31 - 1)
+		minss	xmm0, xmm4
+
+		mov		eax, s     // mov eax, [esp + 4 + 16]
+		mov		edx, c	   // mov edx, [esp + 4 + 16 + 4]
+
+		subss	xmm4, xmm0
+		pslld	xmm3, (31 - 1)
+
+		movaps	xmm6, xmm4
+		xorps	xmm2, xmm7
+		movaps	xmm7, xmm5
+		andps	xmm6, xmm7
+		andnps	xmm7, xmm0
+		andps	xmm0, xmm5
+		andnps	xmm5, xmm4
+		movss	xmm4, _ps_sincos_p3
+		orps	xmm6, xmm7
+		orps	xmm0, xmm5
+		movss	xmm5, _ps_sincos_p2
+
+		movaps	xmm1, xmm0
+		movaps	xmm7, xmm6
+		mulss	xmm0, xmm0
+		mulss	xmm6, xmm6
+		orps	xmm1, xmm2
+		orps	xmm7, xmm3
+		movaps	xmm2, xmm0
+		movaps	xmm3, xmm6
+		mulss	xmm0, xmm4
+		mulss	xmm6, xmm4
+		movss	xmm4, _ps_sincos_p1
+		addss	xmm0, xmm5
+		addss	xmm6, xmm5
+		movss	xmm5, _ps_sincos_p0
+		mulss	xmm0, xmm2
+		mulss	xmm6, xmm3
+		addss	xmm0, xmm4
+		addss	xmm6, xmm4
+		mulss	xmm0, xmm2
+		mulss	xmm6, xmm3
+		addss	xmm0, xmm5
+		addss	xmm6, xmm5
+		mulss	xmm0, xmm1
+		mulss	xmm6, xmm7
+
+		// use full stores since caller might reload with full loads
+		movss	[eax], xmm0
+		movss	[edx], xmm6
+	}
+}
+#endif
+#elif defined( _OSX ) || defined (LINUX) || defined( _WIN64 )
+// [will] - Note: could use optimization.
+void FastSinCos( float x, float* s, float* c )  // any x
+{
+	if( c != NULL )
+	{
+		*c = FastCos(x);
+	}
+	if( s != NULL )
+	{
+		*s = sin(x);
+	}
+}
+#endif
+
+
+#ifdef POSIX
+//#define _PS_CONST(Name, Val) static const ALIGN16 float _ps_##Name[4] ALIGN16_POST = { Val, Val, Val, Val }
+#define _PS_CONST_TYPE(Name, Type, Val) static const ALIGN16 Type _ps_##Name[4] ALIGN16_POST = { Val, Val, Val, Val }
+
+_PS_CONST_TYPE(sign_mask, int, 0x80000000);
+_PS_CONST_TYPE(inv_sign_mask, int, ~0x80000000);
+
+
+#define _PI32_CONST(Name, Val)  static const ALIGN16 int _pi32_##Name[4]  ALIGN16_POST = { Val, Val, Val, Val }
+
+_PI32_CONST(1, 1);
+_PI32_CONST(inv1, ~1);
+_PI32_CONST(2, 2);
+_PI32_CONST(4, 4);
+_PI32_CONST(0x7f, 0x7f);
+_PS_CONST(1  , 1.0f);
+_PS_CONST(0p5, 0.5f);
+
+_PS_CONST(minus_cephes_DP1, -0.78515625);
+_PS_CONST(minus_cephes_DP2, -2.4187564849853515625e-4);
+_PS_CONST(minus_cephes_DP3, -3.77489497744594108e-8);
+_PS_CONST(sincof_p0, -1.9515295891E-4);
+_PS_CONST(sincof_p1,  8.3321608736E-3);
+_PS_CONST(sincof_p2, -1.6666654611E-1);
+_PS_CONST(coscof_p0,  2.443315711809948E-005);
+_PS_CONST(coscof_p1, -1.388731625493765E-003);
+_PS_CONST(coscof_p2,  4.166664568298827E-002);
+_PS_CONST(cephes_FOPI, 1.27323954473516); // 4 / M_PI
+
+typedef union xmm_mm_union {
+	__m128 xmm;
+	__m64 mm[2];
+} xmm_mm_union;
+
+#define COPY_MM_TO_XMM(mm0_, mm1_, xmm_) { xmm_mm_union u; u.mm[0]=mm0_; u.mm[1]=mm1_; xmm_ = u.xmm; }
+
+typedef __m128 v4sf;  // vector of 4 float (sse1)
+typedef __m64 v2si;   // vector of 2 int (mmx)
+
+#endif
+
+
+float FastCos( float x )  
+{
+#if defined( _WIN32 ) && !defined( _WIN64 )
+	float temp;
+	__asm
+	{
+		movss	xmm0, x
+		movss	xmm1, _ps_am_inv_sign_mask
+		andps	xmm0, xmm1
+		addss	xmm0, _ps_am_pi_o_2
+		mulss	xmm0, _ps_am_2_o_pi
+		
+		cvttss2si	ecx, xmm0
+		movss	xmm5, _ps_am_1
+		mov		edx, ecx
+		shl		edx, (31 - 1)
+		cvtsi2ss	xmm1, ecx
+		and		edx, 0x80000000
+		and		ecx, 0x1
+		
+		subss	xmm0, xmm1
+		movss	xmm6, _sincos_masks[ecx * 4]
+		minss	xmm0, xmm5
+		
+		movss	xmm1, _ps_sincos_p3
+		subss	xmm5, xmm0
+		
+		andps	xmm5, xmm6
+		movss	xmm7, _ps_sincos_p2
+		andnps	xmm6, xmm0
+		mov		temp, edx
+		orps	xmm5, xmm6
+		movss	xmm0, xmm5
+		
+		mulss	xmm5, xmm5
+		movss	xmm4, _ps_sincos_p1
+		movss	xmm2, xmm5
+		mulss	xmm5, xmm1
+		movss	xmm1, _ps_sincos_p0
+		addss	xmm5, xmm7
+		mulss	xmm5, xmm2
+		movss	xmm3, temp
+		addss	xmm5, xmm4
+		mulss	xmm5, xmm2
+		orps	xmm0, xmm3
+		addss	xmm5, xmm1
+		mulss	xmm0, xmm5
+		
+		movss   x,    xmm0
+		
+	}
+#elif defined( _WIN64 )
+	return cosf( x );
+#elif POSIX
+	
+	v4sf xmm1, xmm2 = _mm_setzero_ps(), xmm3, y;
+	v2si mm0, mm1, mm2, mm3;
+	/* take the absolute value */
+	v4sf  xx = _mm_load_ss( &x );
+	
+	xx = _mm_and_ps(xx, *(v4sf*)_ps_inv_sign_mask);
+	
+	/* scale by 4/Pi */
+	y = _mm_mul_ps(xx, *(v4sf*)_ps_cephes_FOPI);
+	
+	/* store the integer part of y in mm0:mm1 */
+	xmm2 = _mm_movehl_ps(xmm2, y);
+	mm2 = _mm_cvttps_pi32(y);
+	mm3 = _mm_cvttps_pi32(xmm2);
+	
+	/* j=(j+1) & (~1) (see the cephes sources) */
+	mm2 = _mm_add_pi32(mm2, *(v2si*)_pi32_1);
+	mm3 = _mm_add_pi32(mm3, *(v2si*)_pi32_1);
+	mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_inv1);
+	mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_inv1);
+	
+	y = _mm_cvtpi32x2_ps(mm2, mm3);
+	
+	
+	mm2 = _mm_sub_pi32(mm2, *(v2si*)_pi32_2);
+	mm3 = _mm_sub_pi32(mm3, *(v2si*)_pi32_2);
+	
+	/* get the swap sign flag in mm0:mm1 and the 
+	 polynom selection mask in mm2:mm3 */
+	
+	mm0 = _mm_andnot_si64(mm2, *(v2si*)_pi32_4);
+	mm1 = _mm_andnot_si64(mm3, *(v2si*)_pi32_4);
+	mm0 = _mm_slli_pi32(mm0, 29);
+	mm1 = _mm_slli_pi32(mm1, 29);
+	
+	mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_2);
+	mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_2);
+	
+	mm2 = _mm_cmpeq_pi32(mm2, _mm_setzero_si64());
+	mm3 = _mm_cmpeq_pi32(mm3, _mm_setzero_si64());
+	
+	v4sf sign_bit, poly_mask;
+	COPY_MM_TO_XMM(mm0, mm1, sign_bit);
+	COPY_MM_TO_XMM(mm2, mm3, poly_mask);
+	_mm_empty(); /* good-bye mmx */
+	
+	/* The magic pass: "Extended precision modular arithmetic" 
+	 x = ((x - y * DP1) - y * DP2) - y * DP3; */
+	xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+	xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+	xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+	xmm1 = _mm_mul_ps(y, xmm1);
+	xmm2 = _mm_mul_ps(y, xmm2);
+	xmm3 = _mm_mul_ps(y, xmm3);
+	xx = _mm_add_ps(xx, xmm1);
+	xx = _mm_add_ps(xx, xmm2);
+	xx = _mm_add_ps(xx, xmm3);
+	
+	/* Evaluate the first polynom  (0 <= x <= Pi/4) */
+	y = *(v4sf*)_ps_coscof_p0;
+	v4sf z = _mm_mul_ps(xx,xx);
+	
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+	y = _mm_mul_ps(y, z);
+	y = _mm_mul_ps(y, z);
+	v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+	y = _mm_sub_ps(y, tmp);
+	y = _mm_add_ps(y, *(v4sf*)_ps_1);
+	
+	/* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+	
+	v4sf y2 = *(v4sf*)_ps_sincof_p0;
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_mul_ps(y2, xx);
+	y2 = _mm_add_ps(y2, xx);
+	
+	/* select the correct result from the two polynoms */  
+	xmm3 = poly_mask;
+	y2 = _mm_and_ps(xmm3, y2); //, xmm3);
+	y = _mm_andnot_ps(xmm3, y);
+	y = _mm_add_ps(y,y2);
+	/* update the sign */
+	
+	_mm_store_ss( &x, _mm_xor_ps(y, sign_bit) );
+	
+#else
+#error "Not Implemented"
+#endif
+	return x;
+}
+
+
+// SSE Version of VectorTransform
+void VectorTransformSSE(const float *in1, const matrix3x4_t& in2, float *out1)
+{
+	Assert( s_bMathlibInitialized );
+	Assert( in1 != out1 );
+
+#if defined( _WIN32 ) && !defined( _WIN64 )
+	__asm
+	{
+		mov eax, in1;
+		mov ecx, in2;
+		mov edx, out1;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		addss xmm0, [ecx+12]
+ 		movss [edx], xmm0;
+		add ecx, 16;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		addss xmm0, [ecx+12]
+		movss [edx+4], xmm0;
+		add ecx, 16;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		addss xmm0, [ecx+12]
+		movss [edx+8], xmm0;
+	}
+#else
+	out1[0] = DotProduct(in1, in2[0]) + in2[0][3];
+	out1[1] = DotProduct(in1, in2[1]) + in2[1][3];
+	out1[2] = DotProduct(in1, in2[2]) + in2[2][3];
+#endif
+}
+
+void VectorRotateSSE( const float *in1, const matrix3x4_t& in2, float *out1 )
+{
+	Assert( s_bMathlibInitialized );
+	Assert( in1 != out1 );
+
+#if defined( _WIN32 ) && !defined( _WIN64 )
+	__asm
+	{
+		mov eax, in1;
+		mov ecx, in2;
+		mov edx, out1;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+ 		movss [edx], xmm0;
+		add ecx, 16;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		movss [edx+4], xmm0;
+		add ecx, 16;
+
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		movss [edx+8], xmm0;
+	}
+#else
+	out1[0] = DotProduct( in1, in2[0] );
+	out1[1] = DotProduct( in1, in2[1] );
+	out1[2] = DotProduct( in1, in2[2] );
+#endif
+}
+
+#if defined( _WIN32 ) && !defined( _WIN64 )
+void _declspec(naked) _SSE_VectorMA( const float *start, float scale, const float *direction, float *dest )
+{
+	// FIXME: This don't work!! It will overwrite memory in the write to dest
+	Assert(0);
+
+	Assert( s_bMathlibInitialized );
+	_asm {  // Intel SSE only routine
+		mov	eax, DWORD PTR [esp+0x04]	; *start, s0..s2
+		mov ecx, DWORD PTR [esp+0x0c]	; *direction, d0..d2
+		mov edx, DWORD PTR [esp+0x10]	; *dest
+		movss	xmm2, [esp+0x08]		; x2 = scale, 0, 0, 0
+#ifdef ALIGNED_VECTOR
+		movaps	xmm3, [ecx]				; x3 = dir0,dir1,dir2,X
+		pshufd	xmm2, xmm2, 0			; x2 = scale, scale, scale, scale
+		movaps	xmm1, [eax]				; x1 = start1, start2, start3, X
+		mulps	xmm3, xmm2				; x3 *= x2
+		addps	xmm3, xmm1				; x3 += x1
+		movaps	[edx], xmm3				; *dest = x3
+#else
+		movups	xmm3, [ecx]				; x3 = dir0,dir1,dir2,X
+		pshufd	xmm2, xmm2, 0			; x2 = scale, scale, scale, scale
+		movups	xmm1, [eax]				; x1 = start1, start2, start3, X
+		mulps	xmm3, xmm2				; x3 *= x2
+		addps	xmm3, xmm1				; x3 += x1
+		movups	[edx], xmm3				; *dest = x3
+#endif
+	}
+}
+#endif
+
+#ifdef _WIN32
+#ifdef PFN_VECTORMA
+void _declspec(naked) __cdecl _SSE_VectorMA( const Vector &start, float scale, const Vector &direction, Vector &dest )
+{
+	// FIXME: This don't work!! It will overwrite memory in the write to dest
+	Assert(0);
+
+	Assert( s_bMathlibInitialized );
+	_asm 
+	{  
+		// Intel SSE only routine
+		mov	eax, DWORD PTR [esp+0x04]	; *start, s0..s2
+		mov ecx, DWORD PTR [esp+0x0c]	; *direction, d0..d2
+		mov edx, DWORD PTR [esp+0x10]	; *dest
+		movss	xmm2, [esp+0x08]		; x2 = scale, 0, 0, 0
+#ifdef ALIGNED_VECTOR
+		movaps	xmm3, [ecx]				; x3 = dir0,dir1,dir2,X
+		pshufd	xmm2, xmm2, 0			; x2 = scale, scale, scale, scale
+		movaps	xmm1, [eax]				; x1 = start1, start2, start3, X
+		mulps	xmm3, xmm2				; x3 *= x2
+		addps	xmm3, xmm1				; x3 += x1
+		movaps	[edx], xmm3				; *dest = x3
+#else
+		movups	xmm3, [ecx]				; x3 = dir0,dir1,dir2,X
+		pshufd	xmm2, xmm2, 0			; x2 = scale, scale, scale, scale
+		movups	xmm1, [eax]				; x1 = start1, start2, start3, X
+		mulps	xmm3, xmm2				; x3 *= x2
+		addps	xmm3, xmm1				; x3 += x1
+		movups	[edx], xmm3				; *dest = x3
+#endif
+	}
+}
+float (__cdecl *pfVectorMA)(Vector& v) = _VectorMA;
+#endif
+#endif
+
+
+// SSE DotProduct -- it's a smidgen faster than the asm DotProduct...
+//   Should be validated too!  :)
+//   NJS: (Nov 1 2002) -NOT- faster.  may time a couple cycles faster in a single function like 
+//   this, but when inlined, and instruction scheduled, the C version is faster.  
+//   Verified this via VTune
+/*
+vec_t DotProduct (const vec_t *a, const vec_t *c)
+{
+	vec_t temp;
+
+	__asm
+	{
+		mov eax, a;
+		mov ecx, c;
+		mov edx, DWORD PTR [temp]
+		movss xmm0, [eax];
+		mulss xmm0, [ecx];
+		movss xmm1, [eax+4];
+		mulss xmm1, [ecx+4];
+		movss xmm2, [eax+8];
+		mulss xmm2, [ecx+8];
+		addss xmm0, xmm1;
+		addss xmm0, xmm2;
+		movss [edx], xmm0;
+		fld DWORD PTR [edx];
+		ret
+	}
+}
+*/
+

mathlib/sse.h

@@ -0,0 +1,23 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+//=====================================================================================//
+
+#ifndef _SSE_H
+#define _SSE_H
+
+float _SSE_Sqrt(float x);
+float _SSE_RSqrtAccurate(float a);
+float _SSE_RSqrtFast(float x);
+float FASTCALL _SSE_VectorNormalize(Vector& vec);
+void FASTCALL _SSE_VectorNormalizeFast(Vector& vec);
+float _SSE_InvRSquared(const float* v);
+void _SSE_SinCos(float x, float* s, float* c);
+float _SSE_cos( float x);
+void _SSE2_SinCos(float x, float* s, float* c);
+float _SSE2_cos(float x); 
+void VectorTransformSSE(const float *in1, const matrix3x4_t& in2, float *out1);
+void VectorRotateSSE( const float *in1, const matrix3x4_t& in2, float *out1 );
+
+#endif // _SSE_H

mathlib/ssenoise.cpp

@@ -0,0 +1,235 @@
+//========= Copyright © 1996-2006, Valve Corporation, All rights reserved. ============//
+//
+// Purpose: Fast low quality noise suitable for real time use
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "mathlib/vector.h"
+#include "mathlib/ssemath.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+#include "noisedata.h"
+
+
+#define MAGIC_NUMBER (1<<15)								// gives 8 bits of fraction
+
+static fltx4 Four_MagicNumbers = { MAGIC_NUMBER, MAGIC_NUMBER, MAGIC_NUMBER, MAGIC_NUMBER };
+
+
+static ALIGN16 int32 idx_mask[4]= {0xffff, 0xffff, 0xffff, 0xffff};
+
+#define MASK255 (*((fltx4 *)(& idx_mask )))
+
+// returns 0..1
+static inline float GetLatticePointValue( int idx_x, int idx_y, int idx_z )
+{
+	int ret_idx = perm_a[idx_x & 0xff];
+	ret_idx = perm_b[( idx_y + ret_idx ) & 0xff];
+	ret_idx = perm_c[( idx_z + ret_idx ) & 0xff];
+	return impulse_xcoords[ret_idx];
+
+}
+
+fltx4 NoiseSIMD( const fltx4 & x, const fltx4 & y, const fltx4 & z )
+{
+	// use magic to convert to integer index
+	fltx4 x_idx = AndSIMD( MASK255, AddSIMD( x, Four_MagicNumbers ) );
+	fltx4 y_idx = AndSIMD( MASK255, AddSIMD( y, Four_MagicNumbers ) );
+	fltx4 z_idx = AndSIMD( MASK255, AddSIMD( z, Four_MagicNumbers ) );
+
+	fltx4 lattice000 = Four_Zeros, lattice001 = Four_Zeros, lattice010 = Four_Zeros, lattice011 = Four_Zeros;
+	fltx4 lattice100 = Four_Zeros, lattice101 = Four_Zeros, lattice110 = Four_Zeros, lattice111 = Four_Zeros;
+
+	// FIXME: Converting the input vectors to int indices will cause load-hit-stores (48 bytes)
+	//        Converting the indexed noise values back to vectors will cause more (128 bytes)
+	//        The noise table could store vectors if we chunked it into 2x2x2 blocks.
+	fltx4 xfrac = Four_Zeros, yfrac = Four_Zeros, zfrac = Four_Zeros;
+#define DOPASS(i)															\
+    {	unsigned int xi = SubInt( x_idx, i );								\
+		unsigned int yi = SubInt( y_idx, i );								\
+		unsigned int zi = SubInt( z_idx, i );								\
+		SubFloat( xfrac, i ) = (xi & 0xff)*(1.0/256.0);						\
+		SubFloat( yfrac, i ) = (yi & 0xff)*(1.0/256.0);						\
+		SubFloat( zfrac, i ) = (zi & 0xff)*(1.0/256.0);						\
+		xi>>=8;																\
+		yi>>=8;																\
+		zi>>=8;																\
+																			\
+		SubFloat( lattice000, i ) = GetLatticePointValue( xi,yi,zi );		\
+		SubFloat( lattice001, i ) = GetLatticePointValue( xi,yi,zi+1 );		\
+		SubFloat( lattice010, i ) = GetLatticePointValue( xi,yi+1,zi );		\
+		SubFloat( lattice011, i ) = GetLatticePointValue( xi,yi+1,zi+1 );	\
+		SubFloat( lattice100, i ) = GetLatticePointValue( xi+1,yi,zi );		\
+		SubFloat( lattice101, i ) = GetLatticePointValue( xi+1,yi,zi+1 );	\
+		SubFloat( lattice110, i ) = GetLatticePointValue( xi+1,yi+1,zi );	\
+		SubFloat( lattice111, i ) = GetLatticePointValue( xi+1,yi+1,zi+1 );	\
+    }
+
+	DOPASS( 0 );
+	DOPASS( 1 );
+	DOPASS( 2 );
+	DOPASS( 3 );
+
+	// now, we have 8 lattice values for each of four points as m128s, and interpolant values for
+	// each axis in m128 form in [xyz]frac. Perfom the trilinear interpolation as SIMD ops
+
+	// first, do x interpolation
+	fltx4 l2d00 = AddSIMD( lattice000, MulSIMD( xfrac, SubSIMD( lattice100, lattice000 ) ) );
+	fltx4 l2d01 = AddSIMD( lattice001, MulSIMD( xfrac, SubSIMD( lattice101, lattice001 ) ) );
+	fltx4 l2d10 = AddSIMD( lattice010, MulSIMD( xfrac, SubSIMD( lattice110, lattice010 ) ) );
+	fltx4 l2d11 = AddSIMD( lattice011, MulSIMD( xfrac, SubSIMD( lattice111, lattice011 ) ) );
+
+	// now, do y interpolation
+	fltx4 l1d0 = AddSIMD( l2d00, MulSIMD( yfrac, SubSIMD( l2d10, l2d00 ) ) );
+	fltx4 l1d1 = AddSIMD( l2d01, MulSIMD( yfrac, SubSIMD( l2d11, l2d01 ) ) );
+
+	// final z interpolation
+	fltx4 rslt = AddSIMD( l1d0, MulSIMD( zfrac, SubSIMD( l1d1, l1d0 ) ) );
+
+	// map to 0..1
+	return MulSIMD( Four_Twos, SubSIMD( rslt, Four_PointFives ) );
+
+
+}
+
+static inline void GetVectorLatticePointValue( int idx, fltx4 &x, fltx4 &y, fltx4 &z,
+											   int idx_x, int idx_y, int idx_z )
+{
+	int ret_idx = perm_a[idx_x & 0xff];
+	ret_idx = perm_b[( idx_y + ret_idx ) & 0xff];
+	ret_idx = perm_c[( idx_z + ret_idx ) & 0xff];
+	float const *pData = s_randomGradients + ret_idx * 3;
+	SubFloat( x, idx ) = pData[0];
+	SubFloat( y, idx ) = pData[1];
+	SubFloat( z, idx ) = pData[2];
+
+}
+
+FourVectors DNoiseSIMD( const fltx4 & x, const fltx4 & y, const fltx4 & z )
+{
+	// use magic to convert to integer index
+	fltx4 x_idx = AndSIMD( MASK255, AddSIMD( x, Four_MagicNumbers ) );
+	fltx4 y_idx = AndSIMD( MASK255, AddSIMD( y, Four_MagicNumbers ) );
+	fltx4 z_idx = AndSIMD( MASK255, AddSIMD( z, Four_MagicNumbers ) );
+
+	fltx4 xlattice000 = Four_Zeros, xlattice001 = Four_Zeros, xlattice010 = Four_Zeros, xlattice011 = Four_Zeros;
+	fltx4 xlattice100 = Four_Zeros, xlattice101 = Four_Zeros, xlattice110 = Four_Zeros, xlattice111 = Four_Zeros;
+	fltx4 ylattice000 = Four_Zeros, ylattice001 = Four_Zeros, ylattice010 = Four_Zeros, ylattice011 = Four_Zeros;
+	fltx4 ylattice100 = Four_Zeros, ylattice101 = Four_Zeros, ylattice110 = Four_Zeros, ylattice111 = Four_Zeros;
+	fltx4 zlattice000 = Four_Zeros, zlattice001 = Four_Zeros, zlattice010 = Four_Zeros, zlattice011 = Four_Zeros;
+	fltx4 zlattice100 = Four_Zeros, zlattice101 = Four_Zeros, zlattice110 = Four_Zeros, zlattice111 = Four_Zeros;
+
+	// FIXME: Converting the input vectors to int indices will cause load-hit-stores (48 bytes)
+	//        Converting the indexed noise values back to vectors will cause more (128 bytes)
+	//        The noise table could store vectors if we chunked it into 2x2x2 blocks.
+	fltx4 xfrac = Four_Zeros, yfrac = Four_Zeros, zfrac = Four_Zeros;
+#define DODPASS(i)															\
+    {	unsigned int xi = SubInt( x_idx, i );								\
+		unsigned int yi = SubInt( y_idx, i );								\
+		unsigned int zi = SubInt( z_idx, i );								\
+		SubFloat( xfrac, i ) = (xi & 0xff)*(1.0/256.0);						\
+		SubFloat( yfrac, i ) = (yi & 0xff)*(1.0/256.0);						\
+		SubFloat( zfrac, i ) = (zi & 0xff)*(1.0/256.0);						\
+		xi>>=8;																\
+		yi>>=8;																\
+		zi>>=8;																\
+																			\
+		GetVectorLatticePointValue( i, xlattice000, ylattice000, zlattice000, xi,yi,zi );		\
+		GetVectorLatticePointValue( i, xlattice001, ylattice001, zlattice001, xi,yi,zi+1 );		\
+		GetVectorLatticePointValue( i, xlattice010, ylattice010, zlattice010, xi,yi+1,zi );		\
+		GetVectorLatticePointValue( i, xlattice011, ylattice011, zlattice011, xi,yi+1,zi+1 );	\
+		GetVectorLatticePointValue( i, xlattice100, ylattice100, zlattice100, xi+1,yi,zi );		\
+		GetVectorLatticePointValue( i, xlattice101, ylattice101, zlattice101, xi+1,yi,zi+1 );	\
+		GetVectorLatticePointValue( i, xlattice110, ylattice110, zlattice110, xi+1,yi+1,zi );	\
+		GetVectorLatticePointValue( i, xlattice111, ylattice111, zlattice111, xi+1,yi+1,zi+1 );	\
+    }
+
+	DODPASS( 0 );
+	DODPASS( 1 );
+	DODPASS( 2 );
+	DODPASS( 3 );
+
+	// now, we have 8 lattice values for each of four points as m128s, and interpolant values for
+	// each axis in m128 form in [xyz]frac. Perfom the trilinear interpolation as SIMD ops
+
+	// first, do x interpolation
+	fltx4 xl2d00 = AddSIMD( xlattice000, MulSIMD( xfrac, SubSIMD( xlattice100, xlattice000 ) ) );
+	fltx4 xl2d01 = AddSIMD( xlattice001, MulSIMD( xfrac, SubSIMD( xlattice101, xlattice001 ) ) );
+	fltx4 xl2d10 = AddSIMD( xlattice010, MulSIMD( xfrac, SubSIMD( xlattice110, xlattice010 ) ) );
+	fltx4 xl2d11 = AddSIMD( xlattice011, MulSIMD( xfrac, SubSIMD( xlattice111, xlattice011 ) ) );
+
+	// now, do y interpolation
+	fltx4 xl1d0 = AddSIMD( xl2d00, MulSIMD( yfrac, SubSIMD( xl2d10, xl2d00 ) ) );
+	fltx4 xl1d1 = AddSIMD( xl2d01, MulSIMD( yfrac, SubSIMD( xl2d11, xl2d01 ) ) );
+
+	// final z interpolation
+	FourVectors rslt;
+	rslt.x = AddSIMD( xl1d0, MulSIMD( zfrac, SubSIMD( xl1d1, xl1d0 ) ) );
+
+	fltx4 yl2d00 = AddSIMD( ylattice000, MulSIMD( xfrac, SubSIMD( ylattice100, ylattice000 ) ) );
+	fltx4 yl2d01 = AddSIMD( ylattice001, MulSIMD( xfrac, SubSIMD( ylattice101, ylattice001 ) ) );
+	fltx4 yl2d10 = AddSIMD( ylattice010, MulSIMD( xfrac, SubSIMD( ylattice110, ylattice010 ) ) );
+	fltx4 yl2d11 = AddSIMD( ylattice011, MulSIMD( xfrac, SubSIMD( ylattice111, ylattice011 ) ) );
+
+	// now, do y interpolation
+	fltx4 yl1d0 = AddSIMD( yl2d00, MulSIMD( yfrac, SubSIMD( yl2d10, yl2d00 ) ) );
+	fltx4 yl1d1 = AddSIMD( yl2d01, MulSIMD( yfrac, SubSIMD( yl2d11, yl2d01 ) ) );
+
+	// final z interpolation
+	rslt.y = AddSIMD( yl1d0, MulSIMD( zfrac, SubSIMD( yl1d1, yl1d0 ) ) );
+
+	fltx4 zl2d00 = AddSIMD( zlattice000, MulSIMD( xfrac, SubSIMD( zlattice100, zlattice000 ) ) );
+	fltx4 zl2d01 = AddSIMD( zlattice001, MulSIMD( xfrac, SubSIMD( zlattice101, zlattice001 ) ) );
+	fltx4 zl2d10 = AddSIMD( zlattice010, MulSIMD( xfrac, SubSIMD( zlattice110, zlattice010 ) ) );
+	fltx4 zl2d11 = AddSIMD( zlattice011, MulSIMD( xfrac, SubSIMD( zlattice111, zlattice011 ) ) );
+
+	// now, do y interpolation
+	fltx4 zl1d0 = AddSIMD( zl2d00, MulSIMD( yfrac, SubSIMD( zl2d10, zl2d00 ) ) );
+	fltx4 zl1d1 = AddSIMD( zl2d01, MulSIMD( yfrac, SubSIMD( zl2d11, zl2d01 ) ) );
+
+	// final z interpolation
+	rslt.z = AddSIMD( zl1d0, MulSIMD( zfrac, SubSIMD( zl1d1, zl1d0 ) ) );
+
+	return rslt;
+
+
+}
+
+fltx4 NoiseSIMD( FourVectors const &pos )
+{
+	return NoiseSIMD( pos.x, pos.y, pos.z );
+}
+
+FourVectors DNoiseSIMD( FourVectors const &pos )
+{
+	return DNoiseSIMD( pos.x, pos.y, pos.z );
+}
+
+FourVectors CurlNoiseSIMD( FourVectors const &pos )
+{
+	FourVectors fl4Comp1 = DNoiseSIMD( pos );
+	FourVectors fl4Pos = pos;
+	fl4Pos.x = AddSIMD( fl4Pos.x, ReplicateX4( 43.256 ) );
+	fl4Pos.y = AddSIMD( fl4Pos.y, ReplicateX4( -67.89 ) );
+	fl4Pos.z = AddSIMD( fl4Pos.z, ReplicateX4( 1338.2 ) );
+	FourVectors fl4Comp2 = DNoiseSIMD( fl4Pos );
+	fl4Pos.x = AddSIMD( fl4Pos.x, ReplicateX4( -129.856 ) );
+	fl4Pos.y = AddSIMD( fl4Pos.y, ReplicateX4( -967.23 ) );
+	fl4Pos.z = AddSIMD( fl4Pos.z, ReplicateX4( 2338.98 ) );
+	FourVectors fl4Comp3 = DNoiseSIMD( fl4Pos );
+	
+	// now we have the 3 derivatives of a vector valued field. return the curl of the field.
+	FourVectors fl4Ret;
+	fl4Ret.x = SubSIMD( fl4Comp3.y, fl4Comp2.z );
+	fl4Ret.y = SubSIMD( fl4Comp1.z, fl4Comp3.x );
+	fl4Ret.z = SubSIMD( fl4Comp2.x, fl4Comp1.y );
+	return fl4Ret;
+
+}
+

mathlib/svd.cpp

@@ -0,0 +1,40 @@
+// svd.cpp : Defines the entry point for the console application.
+//
+#include "svd.h"
+
+namespace SVD
+{
+	void Test()
+	{
+		SvdIterator<float> test;
+		for ( int nTest = 0; nTest< 100; ++nTest )
+		{
+			Matrix3<float> a;
+			for ( int i = 0; i < 3; ++i )
+				for ( int j = 0; j < 3; ++j )
+					a.m[ i ][ j ] = i * 3 + j;//float( rand() ) / RAND_MAX - 0.5f;
+
+			test.Init( a );
+			Msg( "%d", nTest );
+			for ( int i = 0; i < 5; ++i )
+			{
+				Matrix3< float > v = test.ComputeV();
+				// B = US = AV
+				Matrix3< float > us = a * v;
+				//float flOrtho = OrthogonalityError( us );
+				Matrix3< float > reconstruction = MulT( us, v );
+				//float flRec = ( reconstruction - a ).FrobeniusNorm();
+				SymMatrix3< float > ata = AtA( a );
+				float flOffDiagError = ata.OffDiagNorm() / ata.DiagNorm();
+
+				// Msg( "\t%g", logf( flRec ) / logf( 10 ) );
+				// Msg( "\t%g", logf( flOrtho ) / logf( 10 ) );
+				Msg( "\t%g", logf( flOffDiagError ) / logf( 10 ) );
+
+				test.Iterate( 1 );
+			}
+			Msg( "\n" );
+		}
+	}
+
+}

mathlib/transform.cpp

@@ -0,0 +1,178 @@
+//==== Copyright (c) 1996-2011, Valve Corporation, All rights reserved. =====//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//===========================================================================//
+
+#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)
+
+#include "mathlib/transform.h"
+#include "mathlib/mathlib.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+ 
+const CTransform g_TransformIdentity( Vector( 0.0f, 0.0f, 0.0f ), Quaternion( 0.0f, 0.0f, 0.0f, 1.0f ) );
+
+void SetIdentityTransform( CTransform &out )
+{
+	out.m_vPosition = vec3_origin;
+	out.m_orientation = quat_identity;
+}
+
+void ConcatTransforms( const CTransform &in1, const CTransform &in2, CTransform &out )
+{
+	// Store in temp to avoid problems if out == in1 or out == in2
+	CTransform result;
+	QuaternionMult( in1.m_orientation, in2.m_orientation, result.m_orientation );
+	QuaternionMultiply( in1.m_orientation, in2.m_vPosition, result.m_vPosition );
+	result.m_vPosition += in1.m_vPosition;
+	out = result;
+}
+
+void VectorIRotate( const Vector &v, const CTransform &t, Vector &out )
+{
+	// FIXME: Make work directly with the transform
+	matrix3x4_t m;
+	TransformMatrix( t, m );
+	VectorIRotate( v, m, out );
+}
+
+void VectorITransform( const Vector &v, const CTransform &t, Vector &out )
+{
+	// FIXME: Make work directly with the transform
+	matrix3x4_t m;
+	TransformMatrix( t, m );
+	VectorITransform( v, m, out );
+}
+
+void TransformSlerp( const CTransform &p, const CTransform &q, float t, CTransform &qt )
+{
+	QuaternionSlerp( p.m_orientation, q.m_orientation, t, qt.m_orientation );
+	VectorLerp( p.m_vPosition, q.m_vPosition, t, qt.m_vPosition );
+}
+
+void TransformLerp( const CTransform &p, const CTransform &q, float t, CTransform &qt )
+{
+	QuaternionBlend( p.m_orientation, q.m_orientation, t, qt.m_orientation );
+	VectorLerp( p.m_vPosition, q.m_vPosition, t, qt.m_vPosition );
+}
+
+void TransformMatrix( const CTransform &in, matrix3x4_t &out )
+{
+	QuaternionMatrix( in.m_orientation, in.m_vPosition, out );
+}
+
+void TransformMatrix( const CTransformUnaligned &in, matrix3x4_t &out )
+{
+	QuaternionMatrix( in.m_orientation, in.m_vPosition, out );
+}
+
+void TransformMatrix( const CTransform &in, const Vector &vScaleIn, matrix3x4_t &out )
+{
+	QuaternionMatrix( in.m_orientation, in.m_vPosition, vScaleIn, out );
+}
+
+void MatrixTransform( const matrix3x4_t &in, CTransformUnaligned &out )
+{
+	MatrixQuaternion( in, out.m_orientation );
+	MatrixGetColumn( in, ORIGIN, out.m_vPosition );
+}
+
+void MatrixTransform( const matrix3x4_t &in, CTransform &out )
+{
+	MatrixQuaternion( in, out.m_orientation );
+	MatrixGetColumn( in, ORIGIN, out.m_vPosition );
+}
+
+void MatrixTransform( const matrix3x4_t &in, CTransform &out, Vector &vScaleOut )
+{
+	matrix3x4_t norm;
+	vScaleOut = MatrixNormalize( in, norm );
+	MatrixTransform( norm, out );
+}
+
+void AngleTransform( const QAngle &angles, const Vector &origin, CTransform &out )
+{
+	AngleQuaternion( angles, out.m_orientation );
+	out.m_vPosition = origin;
+}
+
+void TransformInvert( const CTransform &in, CTransform &out )
+{
+	QuaternionInvert( in.m_orientation, out.m_orientation );
+	QuaternionMultiply( out.m_orientation, in.m_vPosition, out.m_vPosition );
+	out.m_vPosition *= -1.0f;
+}
+
+void AxisAngleTransform( const Vector &vecAxis, float flAngleDegrees, CTransform &out )
+{
+	AxisAngleQuaternion( vecAxis, flAngleDegrees, out.m_orientation );
+	out.m_vPosition = vec3_origin;
+}
+
+void TransformVectorsFLU( const CTransform &in, Vector* pForward, Vector *pLeft, Vector *pUp )
+{
+	QuaternionVectorsFLU( in.m_orientation, pForward, pLeft, pUp );
+}
+
+void TransformVectorsForward( const CTransform &in, Vector* pForward )
+{
+	QuaternionVectorsForward( in.m_orientation, pForward );
+}
+
+bool TransformsAreEqual( const CTransform &src1, const CTransform &src2, float flPosTolerance, float flRotTolerance )
+{
+	if ( !VectorsAreEqual( src1.m_vPosition, src2.m_vPosition, flPosTolerance ) )
+		return false;
+	return QuaternionsAreEqual( src1.m_orientation, src2.m_orientation, flRotTolerance );
+}
+
+// FIXME: optimize this with simd goodness
+void TransformToWorldSpace( int nRootTransformCount, int nTransformCount, const int *pParentIndices, CTransform *pTransforms )
+{
+#ifdef _DEBUG
+	for ( int i = 0; i < nRootTransformCount; ++i )
+	{
+		Assert( pParentIndices[i] < 0 );
+	}
+#endif
+
+	for ( int i = nRootTransformCount; i < nTransformCount; ++i )
+	{	
+		int nParentBone = pParentIndices[i];
+		Assert( nParentBone >= 0 && nParentBone < i );
+		ConcatTransforms( pTransforms[ nParentBone ], pTransforms[ i ], pTransforms[ i ] );
+	}
+}
+
+// FIXME: optimize this with simd goodness
+void TransformToParentSpace( int nRootTransformCount, int nTransformCount, const int *pParentIndices, CTransform *pTransforms )
+{
+#ifdef _DEBUG
+	for ( int i = 0; i < nRootTransformCount; ++i )
+	{
+		Assert( pParentIndices[i] < 0 );
+	}
+#endif
+
+	bool *pComputedParentTransform = (bool*)stackalloc( nTransformCount * sizeof(bool) );
+	memset( pComputedParentTransform, 0, nTransformCount * sizeof(bool) );
+	CTransform *pWorldToParentTransforms = (CTransform*)stackalloc( nTransformCount * sizeof(CTransform) );
+
+	for ( int b = nTransformCount; --b >= nRootTransformCount; )
+	{
+		int nParentBone = pParentIndices[ b ];
+		if ( !pComputedParentTransform[ nParentBone ] )
+		{
+			TransformInvert( pTransforms[ nParentBone ], pWorldToParentTransforms[ nParentBone ] );
+			pComputedParentTransform[ nParentBone ] = true;
+		}
+		ConcatTransforms( pWorldToParentTransforms[ nParentBone ], pTransforms[ b ], pTransforms[ b ] );
+	}
+}
+
+#endif // !_STATIC_LINKED || _SHARED_LIB
+

mathlib/volumeculler.cpp

@@ -0,0 +1,274 @@
+//========= Copyright © 1996-2012, Valve Corporation, All rights reserved. ============//
+//
+//
+//=====================================================================================//
+
+#include <math.h>
+#include <float.h>	// needed for flt_epsilon
+#include "basetypes.h"
+#include "tier0/dbg.h"
+#include "mathlib/vector4d.h"
+#include "mathlib/vector.h"
+#include "mathlib/volumeculler.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+// Returns true if the AABB is completely within the frustum.
+// Basic scalar approach derived from "Real Time Rendering" 2nd edition section 13.13.3.
+// TODO: Replace this a function similar to CFrustum::CheckBoxInline().
+static inline bool AABBInsideFrustum( const fltx4 *pPlanes, FLTX4 vCenter4, FLTX4 vDiagonal4 )
+{
+	fltx4 mp0 = Dot4SIMD( vCenter4, pPlanes[0] );
+	fltx4 mp1 = Dot4SIMD( vCenter4, pPlanes[1] );
+	fltx4 mp2 = Dot4SIMD( vCenter4, pPlanes[2] );
+	fltx4 mp3 = Dot4SIMD( vCenter4, pPlanes[3] );
+	fltx4 mp4 = Dot4SIMD( vCenter4, pPlanes[4] );
+	fltx4 mp5 = Dot4SIMD( vCenter4, pPlanes[5] );
+
+	fltx4 np0 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[0] ) );
+	fltx4 np1 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[1] ) );
+	fltx4 np2 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[2] ) );
+	fltx4 np3 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[3] ) );
+	fltx4 np4 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[4] ) );
+	fltx4 np5 = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[5] ) );
+
+	fltx4 s0 = SubSIMD( mp0, np0 );
+	fltx4 s1 = SubSIMD( mp1, np1 );     
+	fltx4 s2 = SubSIMD( mp2, np2 );     
+	fltx4 s3 = SubSIMD( mp3, np3 );     
+	fltx4 s4 = SubSIMD( mp4, np4 );     
+	fltx4 s5 = SubSIMD( mp5, np5 );     
+
+	fltx4 minS = MinSIMD( MinSIMD( MinSIMD( MinSIMD( MinSIMD( s0, s1 ), s2 ), s3 ), s4 ), s5 );
+
+	if ( IsAnyNegative( minS ) )
+	{
+		return false;
+	}
+
+	// completely inside
+	return true;
+}
+
+// Returns true if the AABB either touches or is completely within a convex volume defined by X planes.
+// Same basic approach as above.
+// TODO: Replace this a function similar to CFrustum::CheckBoxInline().
+static inline bool AABBTouchesOrInsideVolume( const fltx4 *pPlanes, uint nNumPlanes, FLTX4 vCenter4, FLTX4 vDiagonal4 )
+{
+	fltx4 minA = Four_Ones;
+	for ( uint i = 0; i < nNumPlanes; ++i )
+	{
+		fltx4 np = Dot3SIMD( vDiagonal4, AbsSIMD( pPlanes[i] ) );
+		fltx4 mp = Dot4SIMD( vCenter4, pPlanes[i] );
+		fltx4 a = AddSIMD( np, mp );     
+		minA = MinSIMD( minA, a );
+	}
+	if ( IsAnyNegative( minA ) )
+	{
+		return false;
+	}
+	return true;
+}
+
+bool AABBTouches( const fourplanes_t *planes, const fltx4 &fl4Center, const fltx4 &fl4Extents )
+{
+	fltx4 centerx = SplatXSIMD(fl4Center);
+	fltx4 centery = SplatYSIMD(fl4Center);
+	fltx4 centerz = SplatZSIMD(fl4Center);
+	fltx4 extx = SplatXSIMD(fl4Extents);
+	fltx4 exty = SplatYSIMD(fl4Extents);
+	fltx4 extz = SplatZSIMD(fl4Extents);
+
+	// compute the dot product of the normal and the farthest corner
+	for ( int i = 0; i < 2; i++ )
+	{
+		fltx4 xTotalBack = AddSIMD( MulSIMD( planes[i].nX, centerx ), MulSIMD(planes[i].nXAbs, extx ) );
+		fltx4 yTotalBack = AddSIMD( MulSIMD( planes[i].nY, centery ), MulSIMD(planes[i].nYAbs, exty ) );
+		fltx4 zTotalBack = AddSIMD( MulSIMD( planes[i].nZ, centerz ), MulSIMD(planes[i].nZAbs, extz ) );
+		fltx4 dotBack = AddSIMD( xTotalBack, AddSIMD(yTotalBack, zTotalBack) );
+		// if plane of the farthest corner is behind the plane, then the box is completely outside this plane
+		if  ( IsVector4LessThan( dotBack, planes[i].dist ) )
+			return false;
+	}
+	return true;
+}
+
+bool CVolumeCuller::CheckBox( const VectorAligned &mins, const VectorAligned &maxs ) const
+{
+	m_Stats.m_nTotalAABB++;
+	
+	if ( m_bCullSmallObjects )
+	{
+		VectorAligned diag( maxs - mins );
+		// Not really box volume - hacked so one function is useful on zero thickness boxes too.
+		float flVol = ( diag.x * diag.x ) + ( diag.y * diag.y ) + ( diag.z * diag.z );
+		if ( flVol < m_flSmallObjectCullVolumeThreshold )
+			return false;
+	}
+
+	fltx4 vMins4 = LoadAlignedSIMD( &mins.x );
+	fltx4 vMaxs4 = LoadAlignedSIMD( &maxs.x );
+
+	// Converts from 3D interval to center/diagonal form.
+	fltx4 vCenter4 = MulSIMD( AddSIMD( vMaxs4, vMins4 ), Four_PointFives );
+	fltx4 vDiagonal4 = SubSIMD( vMaxs4, vCenter4 );  
+
+	// Ensure vCenter.w is 1.0f.
+	vCenter4 = SetWSIMD( vCenter4, Four_Ones );
+
+	if ( m_bHasBaseFrustum )
+	{
+		if ( !AABBTouches( m_baseplanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_bHasExclusionFrustum )
+	{
+		if ( AABBInsideFrustum( m_ExclusionFrustumPlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_nNumInclusionVolumePlanes )
+	{
+		if ( !AABBTouchesOrInsideVolume( m_InclusionVolumePlanes, m_nNumInclusionVolumePlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}
+	
+	m_Stats.m_nTotalAABBPassed++;
+
+	return true;
+}
+
+bool CVolumeCuller::CheckBox( const Vector &mins, const Vector &maxs ) const
+{
+	m_Stats.m_nTotalAABB++;
+
+	if ( m_bCullSmallObjects )
+	{
+		Vector diag( maxs - mins );
+		// Not really box volume - hacked so one function is useful on zero thickness boxes too.
+		float flVol = ( diag.x * diag.x ) + ( diag.y * diag.y ) + ( diag.z * diag.z );
+		if ( flVol < m_flSmallObjectCullVolumeThreshold )
+			return false;
+	}
+
+	fltx4 vMins4 = LoadUnalignedSIMD( &mins.x );
+	fltx4 vMaxs4 = LoadUnalignedSIMD( &maxs.x );
+
+	// Converts from 3D interval to center/diagonal form.
+	fltx4 vCenter4 = MulSIMD( AddSIMD( vMaxs4, vMins4 ), Four_PointFives );
+	fltx4 vDiagonal4 = SubSIMD( vMaxs4, vCenter4 );  
+
+	// Ensure vCenter.w is 1.0f.
+	vCenter4 = SetWSIMD( vCenter4, Four_Ones );
+
+	if ( m_bHasBaseFrustum )
+	{
+		if ( !AABBTouches( m_baseplanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_bHasExclusionFrustum )
+	{
+		if ( AABBInsideFrustum( m_ExclusionFrustumPlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_nNumInclusionVolumePlanes )
+	{
+		if ( !AABBTouchesOrInsideVolume( m_InclusionVolumePlanes, m_nNumInclusionVolumePlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}
+
+	m_Stats.m_nTotalAABBPassed++;
+
+	return true;
+}
+
+bool CVolumeCuller::CheckBoxCenterHalfDiagonal( const VectorAligned &center, const VectorAligned &halfDiagonal ) const
+{
+	m_Stats.m_nTotalCenterHalfDiagonal++;
+
+	fltx4 vCenter4 = LoadAlignedSIMD( &center.x );
+	fltx4 vDiagonal4 = LoadAlignedSIMD( &halfDiagonal.x );
+		
+	// Ensure vCenter.w is 1.0f.
+	vCenter4 = SetWSIMD( vCenter4, Four_Ones );
+
+	if ( m_bHasBaseFrustum )
+	{
+		if ( !AABBTouches( m_baseplanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_bHasExclusionFrustum )
+	{
+		if ( AABBInsideFrustum( m_ExclusionFrustumPlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}   
+
+	if ( m_nNumInclusionVolumePlanes )
+	{
+		if ( !AABBTouchesOrInsideVolume( m_InclusionVolumePlanes, m_nNumInclusionVolumePlanes, vCenter4, vDiagonal4 ) )
+			return false;
+	}
+
+	m_Stats.m_nTotalCenterHalfDiagonalPassed++;
+
+	return true;
+}
+
+void CVolumeCuller::SetExclusionFrustumPlanes( const VPlane *pPlanes )
+{
+	COMPILE_TIME_ASSERT( sizeof( VPlane ) == sizeof( fltx4 ) );
+
+	if ( !pPlanes )
+	{
+		m_bHasExclusionFrustum = false;
+	}
+	else
+	{
+		for ( int i = 0; i < cNumExclusionFrustumPlanes; ++i )
+		{
+			// Convert VPlane to plane equation form.
+			reinterpret_cast< Vector4D & >( m_ExclusionFrustumPlanes[i] ).Init( pPlanes[i].m_Normal.x, pPlanes[i].m_Normal.y, pPlanes[i].m_Normal.z, -pPlanes[i].m_Dist );
+		}
+		m_bHasExclusionFrustum = true;
+	}
+}
+
+void CVolumeCuller::SetBaseFrustumPlanes( const VPlane *pPlanes )
+{
+	COMPILE_TIME_ASSERT( sizeof( VPlane ) == sizeof( fltx4 ) );
+
+	if ( !pPlanes )
+	{
+		m_bHasBaseFrustum = false;
+	}
+	else
+	{
+		m_baseplanes[0].Set4Planes( pPlanes );
+		m_baseplanes[1].Set2Planes( pPlanes + 4 );
+		m_bHasBaseFrustum = true;
+	}
+}
+
+void CVolumeCuller::GetBaseFrustumPlanes( VPlane *pBasePlanes ) const
+{
+	m_baseplanes[0].Get4Planes( pBasePlanes );
+	m_baseplanes[1].Get2Planes( pBasePlanes + 4 );
+}
+
+void CVolumeCuller::SetInclusionVolumePlanes( const VPlane *pPlanes, uint nNumPlanes )
+{
+	Assert( nNumPlanes <= cMaxInclusionVolumePlanes );
+	nNumPlanes = MIN( nNumPlanes, cMaxInclusionVolumePlanes );
+	m_nNumInclusionVolumePlanes = nNumPlanes;
+
+	for ( uint i = 0; i < nNumPlanes; ++i )
+	{
+		// Convert VPlane to plane equation form.
+		reinterpret_cast< Vector4D & >( m_InclusionVolumePlanes[i] ).Init( pPlanes[i].m_Normal.x, pPlanes[i].m_Normal.y, pPlanes[i].m_Normal.z, -pPlanes[i].m_Dist );
+	}
+}

mathlib/vsi.nul

@@ -0,0 +1 @@
+IMPORTANT: Do not remove the custom build step for this file