path: root/vphysics/trace.cpp

//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose: 
//
// $NoKeywords: $
//
//=============================================================================//
#include "cbase.h"
#include "cmodel.h"
#include "physics_trace.h"
#include "ivp_surman_polygon.hxx"
#include "ivp_compact_ledge.hxx"
#include "ivp_compact_ledge_solver.hxx"
#include "ivp_compact_surface.hxx"
#include "tier0/vprof.h"
#include "mathlib/ssemath.h"
#include "tier0/tslist.h"
// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"

// this skips the sphere tree stuff for tracing
#define DEBUG_TEST_ALL_LEDGES 0
// this skips the optimization that shrinks the ray as each intersection is encountered
#define DEBUG_KEEP_FULL_RAY 0
// this skips the optimization that looks up the first vert in a cubemap
#define USE_COLLIDE_MAP 1

// objects with small numbers of verts build a cache of pre-transformed verts
#define USE_VERT_CACHE 1
#define USE_RLE_SPANS 1

// UNDONE: This is a boost on PC, but doesn't work yet on x360 - investigate
#define SIMD_MATRIX	0

// turn this on to get asserts in the low-level collision solver
#define CHECK_TOI_CALCS 0

#define BRUTE_FORCE_VERT_COUNT 128

// NOTE: This is in inches (HL units)
#define TEST_EPSILON	(g_PhysicsUnits.collisionSweepIncrementalEpsilon)

struct simplexvert_t
{
	Vector	position;
	unsigned short testIndex : 15;
	unsigned short sweepIndex : 1;
	unsigned short obstacleIndex;
};

struct simplex_t
{
	simplexvert_t	verts[4];
	int				vertCount;

	inline bool PointSimplex( const simplexvert_t &newPoint, Vector *pOut );
	inline bool EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut );
	inline bool TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut );

	bool SolveGJKSet( const simplexvert_t &newPoint, Vector *pOut );
	bool SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut );
	bool SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut );
	bool SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut );

	Vector ClipRayToTetrahedronBase( const Vector &dir );
	Vector ClipRayToTetrahedron( const Vector &dir );
	float ClipRayToTriangle( const Vector &dir, float epsilon );
};

class CTraceCone : public ITraceObject
{
public:
	CTraceCone( const truncatedcone_t &cone, const Vector &translation )
	{
		m_cone = cone;
		m_cone.origin += translation;
		float cosTheta;
		SinCos( DEG2RAD(m_cone.theta), &m_sinTheta, &cosTheta );
		m_radius = m_cone.h * m_sinTheta / cosTheta;
		m_centerBase = m_cone.origin + m_cone.h * m_cone.normal;
	}

	virtual int SupportMap( const Vector &dir, Vector *pOut ) const
	{
		Vector unitDir = dir;
		VectorNormalize(unitDir);

		float dot = DotProduct( unitDir, m_cone.normal );

		// anti-cone is -normal, angle = 90 - theta
		// If the normal is in the anti-cone, then return the apex

		// not in anti-cone, support map is on the surface of the disc
		if ( dot > -m_sinTheta )
		{
			unitDir -= m_cone.normal * dot;
			float len = VectorNormalize( unitDir );
			if ( len > 1e-4f )
			{
				*pOut = m_centerBase + (unitDir * m_radius);
				return 0;
			}
			*pOut = m_centerBase;
			return 0;


		}
		// outside the cone's angle, support map is on the surface of the cone
		*pOut = m_cone.origin;
		return 0;
	}

	// BUGBUG: Doesn't work!
	virtual Vector GetVertByIndex( int index ) const { return  m_cone.origin; }
	virtual float Radius( void ) const { return m_cone.h + m_radius; }

	truncatedcone_t	m_cone;
	float			m_radius;
	float			m_sinTheta;
	Vector			m_centerBase;
};

 
// really this is indexing a vertex, but the iteration code needs a triangle + edge index.
// edge is always 0-2 so return it in the bottom 2 bits
static unsigned short GetPackedIndex( const IVP_Compact_Ledge *pLedge, const IVP_U_Float_Point &dir )
{
	const IVP_Compact_Poly_Point *RESTRICT pPoints = pLedge->get_point_array();
	const IVP_Compact_Triangle *RESTRICT pTri = pLedge->get_first_triangle();
	const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( 0 );
	int best = pEdge->get_start_point_index();
	float bestDot = pPoints[best].dot_product( &dir );
	int triCount = pLedge->get_n_triangles();
	const IVP_Compact_Triangle *RESTRICT pBestTri = pTri;
	// this loop will early out, but keep it from being infinite
	int i;
	// hillclimbing search to find the best support vert
	for ( i = 0; i < triCount; i++ )
	{
		// get the index to the end vert of this edge (start vert on next edge)
		pEdge = pEdge->get_prev();
		int stopVert = pEdge->get_start_point_index();

		// loop through the verts that can be reached along edges from this vert
		// stop if you get back to the one you're starting on.
		int vert = stopVert;
		do
		{
			float dot = pPoints[vert].dot_product( &dir );
			if ( dot > bestDot )
			{
				bestDot = dot;
				best = vert;
				pBestTri = pEdge->get_triangle();
				break;
			}
			// tri opposite next edge, same starting vert as next edge
			pEdge = pEdge->get_opposite()->get_prev();
			vert = pEdge->get_start_point_index();
		} while ( vert != stopVert );

		// if you exhausted the possibilities for this vert, it must be the best vert
		if ( vert != best )
			break;
	}

	int triIndex = pBestTri - pLedge->get_first_triangle();
	int edgeIndex = 0;
	// just do a search for the edge containing this vert instead of storing it along the way
	for ( i = 0; i < 3; i++ )
	{
		if ( pBestTri->get_edge(i)->get_start_point_index() == best )
		{
			edgeIndex = i;
			break;
		}
	}

	return (unsigned short) ( (triIndex<<2) + edgeIndex );
}


void InitLeafmap( IVP_Compact_Ledge *pLedge, leafmap_t *pLeafmapOut )
{
	pLeafmapOut->pLeaf = pLedge;
	pLeafmapOut->vertCount = 0;
	pLeafmapOut->flags = 0;
	pLeafmapOut->spanCount = 0;
	if ( pLedge && pLedge->is_terminal() )
	{
		// for small numbers of verts it's much faster to simply do dot products with all verts
		// since the best case for hillclimbing is to touch the start vert plus all neighbors (avg_valence+1 dots)
		// in t
		int triCount = pLedge->get_n_triangles();
		// this is a guess that anything with more than brute_force * 4 tris will have at least brute_force verts
		if ( triCount <= BRUTE_FORCE_VERT_COUNT*4 )
		{
			Assert(triCount>0);
			int minV = MAX_CONVEX_VERTS;
			int maxV = 0;
			for ( int i = 0; i < triCount; i++ )
			{
				const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i;
				for ( int j = 0; j < 3; j++ )
				{
					const IVP_Compact_Edge *pEdge = pTri->get_edge( j );
					int v = pEdge->get_start_point_index();
					if ( v < minV )
					{
						minV = v;
					}
					if ( v > maxV )
					{
						maxV = v;
					}
				}
			}
			int vertCount = (maxV-minV) + 1;
			// max possible verts is < 48, so this is just here for some real failure
			// or vert sharing with a large collection of convexes.  In that case the 
			// number could be high, but this approach to implementing support is invalid
			// because the vert range is polluted
			if ( vertCount < BRUTE_FORCE_VERT_COUNT )
			{
				char hasVert[BRUTE_FORCE_VERT_COUNT];
				memset(hasVert, 0, sizeof(hasVert[0])*vertCount);
				for ( int i = 0; i < triCount; i++ )
				{
					const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i;
					for ( int j = 0; j < 3; j++ )
					{
						// mark each vert in the list
						const IVP_Compact_Edge *pEdge = pTri->get_edge( j );
						int v = pEdge->get_start_point_index();
						hasVert[v-minV] = true;
					}
				}
				// now find the vertex spans and encode them
				byte spans[BRUTE_FORCE_VERT_COUNT];
				int spanIndex = 0;
				char has = hasVert[0];
				Assert(has);
				byte count = 1;
				for ( int i = 1; i < vertCount && spanIndex < BRUTE_FORCE_VERT_COUNT; i++ )
				{
					// each change of state is a new span
					if ( has != hasVert[i] )
					{
						spans[spanIndex] = count;
						has = hasVert[i];
						count = 0;
						spanIndex++;
					}
					count++;
					Assert(count < 255);
				}

				// rle spans only supported with vertex caching
#if USE_VERT_CACHE && USE_RLE_SPANS
				if ( spanIndex < BRUTE_FORCE_VERT_COUNT )
#else
				if ( spanIndex < 1 )
#endif
				{
					spans[spanIndex] = count;
					spanIndex++;
					pLeafmapOut->SetRLESpans( minV, spanIndex, spans );
				}
			}
		}
	}

	if ( !pLeafmapOut->HasSpans() )
	{
		// otherwise make a 8-way directional map to pick the best start vert for hillclimbing
		pLeafmapOut->SetHasCubemap();
		for ( int i = 0; i < 8; i++ )
		{
			IVP_U_Float_Point tmp;
			tmp.k[0] = ( i & 1 ) ? -1 : 1;
			tmp.k[1] = ( i & 2 ) ? -1 : 1;
			tmp.k[2] = ( i & 4 ) ? -1 : 1;
			pLeafmapOut->startVert[i] = GetPackedIndex( pLedge, tmp );
		}
	}
}


void GetStartVert( const leafmap_t *pLeafmap, const IVP_U_Float_Point &localDirection, int &triIndex, int &edgeIndex )
{
	if ( !pLeafmap || !pLeafmap->HasCubemap() )
		return;

	// map dir to index
	int cacheIndex = (localDirection.k[0] < 0 ? 1 : 0) + (localDirection.k[1] < 0 ? 2 : 0) + (localDirection.k[2] < 0 ? 4 : 0 );
	triIndex = pLeafmap->startVert[cacheIndex] >> 2;
	edgeIndex = pLeafmap->startVert[cacheIndex] & 0x3;
}

CTSPool<CVisitHash> g_VisitHashPool;

CVisitHash *AllocVisitHash()
{
	return g_VisitHashPool.GetObject();
}

void FreeVisitHash(CVisitHash *pFree)
{
	if ( pFree )
	{
		g_VisitHashPool.PutObject(pFree);
	}
}

//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an IVP object
//-----------------------------------------------------------------------------
class CTraceIVP : public ITraceObject
{
public:
	CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles );
	~CTraceIVP()
	{
		if ( m_pVisitHash )
			FreeVisitHash(m_pVisitHash);
	}
	virtual int SupportMap( const Vector &dir, Vector *pOut ) const;
	virtual Vector GetVertByIndex( int index ) const;

	// UNDONE: Do general ITraceObject center/offset computation and move the ray to account
	// for this delta like we do in TraceSweepIVP()
	// Then we can shrink the radius of objects with mass centers NOT at the origin
	virtual float Radius( void ) const 
	{ 
		return m_radius;
	}

	inline float TransformLengthToLocal( float length )
	{
		return ConvertDistanceToIVP( length );
	}
	// UNDONE: Optimize this by storing 3 matrices? (one for each transform that includes rot/scale for HL/IVP)?
	// UNDONE: Not necessary if we remove the coordinate conversion
	inline void TransformDirectionToLocal( const Vector &dir, IVP_U_Float_Point &local ) const
	{
		IVP_U_Float_Point tmp;
		ConvertDirectionToIVP( dir, tmp );
		m_matrix.vimult3( &tmp, &local );
	}

	inline void RotateRelativePositionToLocal( const Vector &delta, IVP_U_Float_Point &local ) const
	{
		IVP_U_Float_Point tmp;
		ConvertPositionToIVP( delta, tmp );
		m_matrix.vimult3( &tmp, &local );
	}

	inline void TransformPositionToLocal( const Vector &pos, IVP_U_Float_Point &local ) const
	{
		IVP_U_Float_Point tmp;
		ConvertPositionToIVP( pos, tmp );
		m_matrix.vimult4( &tmp, &local );
	}

	inline void TransformPositionFromLocal( const IVP_U_Float_Point &local, Vector &out ) const
	{
		VectorTransform( *(Vector *)&local, *((const matrix3x4_t *)&m_ivpLocalToHLWorld), out );
	}

#if USE_VERT_CACHE
	inline Vector CachedVertByIndex(int index) const
	{
		int subIndex = index & 3;
		return m_vertCache[index>>2].Vec(subIndex);
	}
#endif

	bool IsValid( void ) { return m_pLedge != NULL; }

	void AllocateVisitHash()
	{
		if ( !m_pVisitHash )
			m_pVisitHash = AllocVisitHash();
	}

	void SetLedge( const IVP_Compact_Ledge *pLedge )
	{
		m_pLedge = pLedge;
		m_pLeafmap = NULL;
		if ( !pLedge )
			return;

#if USE_VERT_CACHE
		m_cacheCount = 0;
#endif
		if ( m_pCollideMap )
		{
			for ( int i = 0; i < m_pCollideMap->leafCount; i++ )
			{
				if ( m_pCollideMap->leafmap[i].pLeaf == pLedge )
				{
					m_pLeafmap = &m_pCollideMap->leafmap[i];
					if ( !BuildLeafmapCache( &m_pCollideMap->leafmap[i] ) )
					{
						AllocateVisitHash();
					}
					return;
				}
			}
		}
		AllocateVisitHash();
	}

	bool SetSingleConvex( void )
	{
		const IVP_Compact_Ledgetree_Node *node = m_pSurface->get_compact_ledge_tree_root();
		if ( node->is_terminal() == IVP_TRUE )
		{
			SetLedge( node->get_compact_ledge() );
			return true;
		}
		SetLedge( NULL );
		return false;
	}
	bool BuildLeafmapCache(const leafmap_t * RESTRICT pLeafmap);
	bool BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap );
	inline int SupportMapCached( const Vector &dir, Vector *pOut ) const;
	const collidemap_t			*m_pCollideMap;
	const IVP_Compact_Surface	*m_pSurface;

private:
	const leafmap_t				*m_pLeafmap;
	const IVP_Compact_Ledge		*m_pLedge;
	CVisitHash					*m_pVisitHash;
#if SIMD_MATRIX
	FourVectors					m_ivpLocalToHLWorld;
#else
	matrix3x4_t					m_ivpLocalToHLWorld;
#endif
	IVP_U_Matrix				m_matrix;
	// transform that includes scale from IVP to HL coords, do not VectorITransform or VectorRotate with this
	float						m_radius;
	int							m_nPointTest;
	int							m_nStartPoint;
	bool						m_bHasTranslation;
#if USE_VERT_CACHE
	int							m_cacheCount;	// number of FourVectors used
	FourVectors					m_vertCache[BRUTE_FORCE_VERT_COUNT/4];
#endif
};

// GCC 4.2.1 can't handle loading a static const into a m128 register :(
#ifdef WIN32
static const 
#endif
fltx4 g_IVPToHLDir = { 1.0f, -1.0f, 1.0f, 1.0f };

//static const fltx4 g_IVPToHLPosition = { IVP2HL(1.0f), -IVP2HL(1.0f), IVP2HL(1.0f), IVP2HL(1.0f) };

#if defined(_X360)

FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a )
{
	fltx4 t = __vpermwi( a, VPERMWI_CONST( 0, 2, 1, 3 ) );
	// negate Y
	return MulSIMD( t, g_IVPToHLDir );
}
#else
FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a )
{
	// swap Z & Y
	fltx4 t = _mm_shuffle_ps( a, a, MM_SHUFFLE_REV( 0, 2, 1, 3 ) );
	// negate Y
	return MulSIMD( t, g_IVPToHLDir );
}
#endif

CTraceIVP::CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles )
{
#if USE_COLLIDE_MAP
	m_pCollideMap = pCollide->GetCollideMap();
#else
	m_pCollideMap = NULL;
#endif
	m_pSurface = pCollide->GetCompactSurface();
	m_pLedge = NULL;
	m_pVisitHash = NULL;

	m_bHasTranslation = (origin==vec3_origin) ? false : true;
	// UNDONE: Move this offset calculation into the tracing routines
	// I didn't do this now because it seems to require changes to most of the
	// transform routines - and this would cause bugs.
	float centerOffset = VectorLength( m_pSurface->mass_center.k );
#if SIMD_MATRIX
	VectorAligned forward, right, up;
	IVP_U_Float_Point ivpForward, ivpLeft, ivpUp;

	AngleVectors( angles, &forward, &right, &up );

	Vector left = -right;
	Vector down = -up;

	ConvertDirectionToIVP( forward, ivpForward );
	ConvertDirectionToIVP( left, ivpLeft );
	ConvertDirectionToIVP( down, ivpUp );

	m_matrix.set_col( IVP_INDEX_X, &ivpForward );
	m_matrix.set_col( IVP_INDEX_Z, &ivpLeft );
	m_matrix.set_col( IVP_INDEX_Y, &ivpUp );
	ConvertPositionToIVP( origin, m_matrix.vv );

	forward.w = HL2IVP(origin.x);
	// This vector is supposed to be left, so we'll negate it later, but we don't want to 
	// negate the position, so add another minus to cancel out
	right.w = -HL2IVP(origin.y);
	up.w = HL2IVP(origin.z);
	fltx4 rx = ConvertDirectionToIVP(LoadAlignedSIMD(forward.Base()));
	fltx4 ry = ConvertDirectionToIVP(SubSIMD( Four_Zeros, LoadAlignedSIMD(right.Base())) );
	fltx4 rz = ConvertDirectionToIVP(LoadAlignedSIMD(up.Base()) );

	fltx4 scaleHL = ReplicateX4(IVP2HL(1.0f));
	m_ivpLocalToHLWorld.x = MulSIMD( scaleHL, rx );
	m_ivpLocalToHLWorld.y = MulSIMD( scaleHL, ry );
	m_ivpLocalToHLWorld.z = MulSIMD( scaleHL, rz );
#else
	ConvertRotationToIVP( angles, m_matrix );
	ConvertPositionToIVP( origin, m_matrix.vv );
	float scale = IVP2HL(1.0f);
	float negScale = IVP2HL(-1.0f);
	// copy the existing IVP local->world matrix (swap Y & Z)
	m_ivpLocalToHLWorld.m_flMatVal[0][0] = m_matrix.get_elem(IVP_INDEX_X,0) * scale; 
	m_ivpLocalToHLWorld.m_flMatVal[0][1] = m_matrix.get_elem(IVP_INDEX_X,1) * scale; 
	m_ivpLocalToHLWorld.m_flMatVal[0][2] = m_matrix.get_elem(IVP_INDEX_X,2) * scale; 

	m_ivpLocalToHLWorld.m_flMatVal[1][0] = m_matrix.get_elem(IVP_INDEX_Z,0) * scale; 
	m_ivpLocalToHLWorld.m_flMatVal[1][1] = m_matrix.get_elem(IVP_INDEX_Z,1) * scale; 
	m_ivpLocalToHLWorld.m_flMatVal[1][2] = m_matrix.get_elem(IVP_INDEX_Z,2) * scale; 

	m_ivpLocalToHLWorld.m_flMatVal[2][0] = m_matrix.get_elem(IVP_INDEX_Y,0) * negScale; 
	m_ivpLocalToHLWorld.m_flMatVal[2][1] = m_matrix.get_elem(IVP_INDEX_Y,1) * negScale; 
	m_ivpLocalToHLWorld.m_flMatVal[2][2] = m_matrix.get_elem(IVP_INDEX_Y,2) * negScale; 

	m_ivpLocalToHLWorld.m_flMatVal[0][3] = m_matrix.vv.k[0] * scale;
	m_ivpLocalToHLWorld.m_flMatVal[1][3] = m_matrix.vv.k[2] * scale;
	m_ivpLocalToHLWorld.m_flMatVal[2][3] = m_matrix.vv.k[1] * negScale;

#endif

	m_radius = ConvertDistanceToHL( m_pSurface->upper_limit_radius + centerOffset );
}

bool CTraceIVP::BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap )
{
	// iterate the rle spans of verts and output them to a buffer in post-transform space
	int startPoint = pLeafmap->startVert[0];
	int pointCount = pLeafmap->vertCount;
	m_cacheCount = (pointCount + 3)>>2;
	const byte *RESTRICT pSpans = pLeafmap->GetSpans();
	int countThisSpan = pSpans[0];
	int spanIndex = 1;
	int baseVert = 0;
	const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint];
	for ( int i = 0; i < m_cacheCount-1; i++ )
	{
		if ( countThisSpan < 4 )
		{
			// unrolled for perf
			// we need a batch of four verts, but they aren't in a single span
			int v0, v1, v2, v3;
			if ( !countThisSpan )
			{
				baseVert += pSpans[spanIndex];
				countThisSpan = pSpans[spanIndex+1];
				spanIndex += 2;
			}
			v0 = baseVert++;
			countThisSpan--;
			if ( !countThisSpan )
			{
				baseVert += pSpans[spanIndex];
				countThisSpan = pSpans[spanIndex+1];
				spanIndex += 2;
			}
			v1 = baseVert++;
			countThisSpan--;
			if ( !countThisSpan )
			{
				baseVert += pSpans[spanIndex];
				countThisSpan = pSpans[spanIndex+1];
				spanIndex += 2;
			}
			v2 = baseVert++;
			countThisSpan--;
			if ( !countThisSpan )
			{
				baseVert += pSpans[spanIndex];
				countThisSpan = pSpans[spanIndex+1];
				spanIndex += 2;
			}
			v3 = baseVert++;
			countThisSpan--;
			m_vertCache[i].LoadAndSwizzleAligned( pVerts[v0].Base(), pVerts[v1].Base(), pVerts[v2].Base(), pVerts[v3].Base() );
		}
		else
		{
			// we have four verts in this span, just grab the next four
			m_vertCache[i].LoadAndSwizzleAligned( pVerts[baseVert+0].Base(), pVerts[baseVert+1].Base(), pVerts[baseVert+2].Base(), pVerts[baseVert+3].Base() );
			baseVert += 4;
			countThisSpan -= 4;
		}
	}
	// the last iteration needs multiple spans and clamping to the last vert
	int v[4];
	for ( int i = 0; i < 4; i++ )
	{
		if ( spanIndex < pLeafmap->spanCount && !countThisSpan )
		{
			baseVert += pSpans[spanIndex];
			countThisSpan = pSpans[spanIndex+1];
			spanIndex += 2;
		}
		if ( spanIndex < pLeafmap->spanCount )
		{
			v[i] = baseVert;
			baseVert++;
			countThisSpan--;
		}
		else
		{
			v[i] = baseVert;
			if ( countThisSpan > 1 )
			{
				countThisSpan--;
				baseVert++;
			}
		}
	}
	m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[v[0]].Base(), pVerts[v[1]].Base(), pVerts[v[2]].Base(), pVerts[v[3]].Base() );
	FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) );

	return true;
}

bool CTraceIVP::BuildLeafmapCache( const leafmap_t * RESTRICT pLeafmap )
{
#if !USE_VERT_CACHE
	return false;
#else
	if ( !pLeafmap || !pLeafmap->HasSpans() || m_bHasTranslation )
		return false;
	if ( pLeafmap->HasRLESpans() )
	{
		return BuildLeafmapCacheRLE(pLeafmap);
	}

	// single vertex span, just xform + copy

	// iterate the span of verts and output them to a buffer in post-transform space
	// just iterate the range if one is specified
	int startPoint = pLeafmap->startVert[0];
	int pointCount = pLeafmap->vertCount;
	m_cacheCount = (pointCount + 3)>>2;
	Assert(m_cacheCount>=0 && m_cacheCount<= (BRUTE_FORCE_VERT_COUNT/4));

	const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint];
	for ( int i = 0; i < m_cacheCount-1; i++ )
	{
		m_vertCache[i].LoadAndSwizzleAligned( pVerts[0].Base(), pVerts[1].Base(), pVerts[2].Base(), pVerts[3].Base() );
		pVerts += 4;
	}

	int remIndex = (pointCount-1) & 3;
	int x0 = 0;
	int x1 = min(1,remIndex);
	int x2 = min(2,remIndex);
	int x3 = min(3,remIndex);
	m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[x0].Base(), pVerts[x1].Base(), pVerts[x2].Base(), pVerts[x3].Base() );
	FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) );
	return true;
#endif
}

static const fltx4 g_IndexBase = {0,1,2,3};
int CTraceIVP::SupportMapCached( const Vector &dir, Vector *pOut ) const
{
	VPROF("SupportMapCached");
#if USE_VERT_CACHE
	FourVectors fourDir;
#if defined(_X360)
	fltx4 vec = LoadUnaligned3SIMD( dir.Base() );
	fourDir.x = SplatXSIMD(vec);
	fourDir.y = SplatYSIMD(vec);
	fourDir.z = SplatZSIMD(vec);
#else
	fourDir.DuplicateVector(dir);
#endif

	fltx4 index = g_IndexBase;
	fltx4 maxIndex = g_IndexBase;
	fltx4 maxDot = fourDir * m_vertCache[0];
	for ( int i = 1; i < m_cacheCount; i++ )
	{
		index = AddSIMD(index, Four_Fours);
		fltx4 dot = fourDir * m_vertCache[i];
		fltx4 cmpMask = CmpGtSIMD(dot,maxDot);
		maxIndex = MaskedAssign( cmpMask, index, maxIndex );
		maxDot = MaxSIMD(dot, maxDot);
	}

	// find highest of 4
	fltx4 rot = RotateLeft2(maxDot);
	fltx4 rotIndex = RotateLeft2(maxIndex);
	fltx4 cmpMask = CmpGtSIMD(rot,maxDot);
	maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex);
	maxDot = MaxSIMD(rot,maxDot);
	rotIndex = RotateLeft(maxIndex);
	rot = RotateLeft(maxDot);
	cmpMask = CmpGtSIMD(rot,maxDot);
	maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex);
	// not needed unless we need the actual max dot at the end
	//	maxDot = MaxSIMD(rot,maxDot);

	int bestIndex = SubFloatConvertToInt(maxIndex,0);
	*pOut = CachedVertByIndex(bestIndex);

	return bestIndex;
#else
	Assert(0);
#endif
}

int CTraceIVP::SupportMap( const Vector &dir, Vector *pOut ) const
{
#if USE_VERT_CACHE
	if ( m_cacheCount )
		return SupportMapCached( dir, pOut );
#endif

	if ( m_pLeafmap && m_pLeafmap->HasSingleVertexSpan() )
	{
		VPROF("SupportMap_Leaf");
		const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array();
		IVP_U_Float_Point mapdir;
		TransformDirectionToLocal( dir, mapdir );
		// just iterate the range if one is specified
		int startPoint = m_pLeafmap->startVert[0];
		int pointCount = m_pLeafmap->vertCount;
		float bestDot = pPoints[startPoint].dot_product(&mapdir);
		int best = startPoint;
		for ( int i = 1; i < pointCount; i++ )
		{
			float dot = pPoints[startPoint+i].dot_product(&mapdir);
			if ( dot > bestDot )
			{
				bestDot = dot;
				best = startPoint+i;
			}
		}

		TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space
		return best;
	}
	else
	{
		VPROF("SupportMap_Walk");
		const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array();
		IVP_U_Float_Point mapdir;
		TransformDirectionToLocal( dir, mapdir );
		int triCount = m_pLedge->get_n_triangles();
		Assert( m_pVisitHash );
		m_pVisitHash->NewVisit();

		float dot;
		int triIndex = 0, edgeIndex = 0;
		GetStartVert( m_pLeafmap, mapdir, triIndex, edgeIndex );
		const IVP_Compact_Triangle *RESTRICT pTri = m_pLedge->get_first_triangle() + triIndex;
		const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( edgeIndex );
		int best = pEdge->get_start_point_index();
		float bestDot = pPoints[best].dot_product( &mapdir );
		m_pVisitHash->VisitVert(best);

		// This should never happen.  MAX_CONVEX_VERTS is very large (millions), none of our
		// models have anywhere near this many verts in a convex piece
		Assert(triCount*3<MAX_CONVEX_VERTS);
		// this loop will early out, but keep it from being infinite
		for ( int i = 0; i < triCount; i++ )
		{
			// get the index to the end vert of this edge (start vert on next edge)
			pEdge = pEdge->get_prev();
			int stopVert = pEdge->get_start_point_index();

			// loop through the verts that can be reached along edges from this vert
			// stop if you get back to the one you're starting on.
			int vert = stopVert;
			do
			{
				if ( !m_pVisitHash->WasVisited(vert) )
				{
					// this lets us skip doing dot products on this vert
					m_pVisitHash->VisitVert(vert);
					dot = pPoints[vert].dot_product( &mapdir );
					if ( dot > bestDot )
					{
						bestDot = dot;
						best = vert;
						break;
					}
				}
				// tri opposite next edge, same starting vert as next edge
				pEdge = pEdge->get_opposite()->get_prev();
				vert = pEdge->get_start_point_index();
			} while ( vert != stopVert );

			// if you exhausted the possibilities for this vert, it must be the best vert
			if ( vert != best )
				break;
		}

		// code to do the brute force method with no hill-climbing
#if 0
		for ( i = 0; i < triCount; i++ )
		{
			pTri = m_pLedge->get_first_triangle() + i;
			for ( int j = 0; j < 3; j++ )
			{
				pEdge = pTri->get_edge( j );
				int test = pEdge->get_start_point_index();
				dot = pPoints[test].dot_product( &mapdir );
				if ( dot > bestDot )
				{
					Assert(0);		// shouldn't hit this unless the hill-climb is broken
					bestDot = dot;
					best = test;
				}
			}
		}
#endif
		TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space

		return best;
	}
}

Vector CTraceIVP::GetVertByIndex( int index ) const
{
#if USE_VERT_CACHE
	if ( m_cacheCount )
	{
		return CachedVertByIndex(index);
	}
#endif
	const IVP_Compact_Poly_Point *pPoints = m_pLedge->get_point_array();

	Vector out;
	TransformPositionFromLocal( pPoints[index], out );
	return out;
}

//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an AABB
//-----------------------------------------------------------------------------
class CTraceAABB : public ITraceObject
{
public:
	CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint );
	virtual int SupportMap( const Vector &dir, Vector *pOut ) const;
	virtual Vector GetVertByIndex( int index ) const;
	virtual float Radius( void ) const { return m_radius; }

private:
	float	m_x[2];
	float	m_y[2];
	float	m_z[2];
	float	m_radius;
	bool	m_empty;
};


CTraceAABB::CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint )
{
	if ( isPoint )
	{
		m_x[0] = m_x[1] = 0;
		m_y[0] = m_y[1] = 0;
		m_z[0] = m_z[1] = 0;
		m_radius = 0;
		m_empty = true;
	}
	else
	{
		m_x[0] = hlmaxs[0];
		m_x[1] = hlmins[0];
		m_y[0] = hlmaxs[1];
		m_y[1] = hlmins[1];
		m_z[0] = hlmaxs[2];
		m_z[1] = hlmins[2];
		m_radius = hlmaxs.Length();
		m_empty = false;
	}
}


int CTraceAABB::SupportMap( const Vector &dir, Vector *pOut ) const
{
	Vector out;

	if ( m_empty )
	{
		pOut->Init();
		return 0;
	}
	// index is formed by the 3-bit bitfield SzSySx (negative is 1, positive is 0)
	int x = ((*((unsigned int *)&dir.x)) & 0x80000000UL) >> 31;
	int y = ((*((unsigned int *)&dir.y)) & 0x80000000UL) >> 31;
	int z = ((*((unsigned int *)&dir.z)) & 0x80000000UL) >> 31;
	pOut->x = m_x[x];
	pOut->y = m_y[y];
	pOut->z = m_z[z];
	return (z<<2) | (y<<1) | x;
}

Vector CTraceAABB::GetVertByIndex( int index ) const
{
	Vector out;
	out.x = m_x[(index&1)];
	out.y = m_y[(index&2)>>1];
	out.z = m_z[(index&4)>>2];

	return out;
}


//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an IVP object
//-----------------------------------------------------------------------------
class CTraceRay
{
public:
	CTraceRay( const Vector &hlstart, const Vector &hlend );
	CTraceRay( const Ray_t &ray );
	CTraceRay( const Ray_t &ray, const Vector &offset );
	void Init( const Vector &hlstart, const Vector &delta );
	int SupportMap( const Vector &dir, Vector *pOut ) const;
	Vector GetVertByIndex( int index ) const { return ( index ) ? m_end : m_start; }
	float Radius( void ) const { return m_length * 0.5f; }

	void Reset( float fraction );

	Vector	m_start;
	Vector	m_end;
	Vector	m_delta;
	Vector	m_dir;

	float	m_length;
	float	m_baseLength;
	float	m_ooBaseLength;
	float	m_bestDist;
};
CTraceRay::CTraceRay( const Vector &hlstart, const Vector &hlend )
{
	Init(hlstart, hlend-hlstart);
}

void CTraceRay::Init( const Vector &hlstart, const Vector &delta )
{
	m_start = hlstart;
	m_end = hlstart + delta;
	m_delta = delta;
	m_dir = delta;
	float len = DotProduct(delta, delta);
	// don't use fast/sse sqrt here we need the precision
	m_length = sqrt(len);
	m_ooBaseLength = 0.0f;
	if ( m_length > 0 )
	{
		m_ooBaseLength = 1.0f / m_length;
		m_dir *= m_ooBaseLength;
	}
	m_baseLength = m_length;
	m_bestDist = 0.f;
}

CTraceRay::CTraceRay( const Ray_t &ray )
{
	Init( ray.m_Start, ray.m_Delta );
}

CTraceRay::CTraceRay( const Ray_t &ray, const Vector &offset )
{
	Vector start;
	VectorAdd( ray.m_Start, offset, start );
	Init( start, ray.m_Delta );
}

void CTraceRay::Reset( float fraction )
{
	// recompute from base values for max precision
	m_length = m_baseLength * fraction;
	m_end = m_start + fraction * m_delta;
	m_bestDist = 0.f;
}

int CTraceRay::SupportMap( const Vector &dir, Vector *pOut ) const
{
	if ( DotProduct( dir, m_delta ) > 0 )
	{
		*pOut = m_end;
		return 1;
	}
	*pOut = m_start;
	return 0;
}

static char			*map_nullname = "**empty**";
static csurface_t	nullsurface = { map_nullname, 0 };

static void CM_ClearTrace( trace_t *trace )
{
	memset( trace, 0, sizeof(*trace));
	trace->fraction = 1.f;
	trace->fractionleftsolid = 0;
	trace->surface = nullsurface;
}

class CDefConvexInfo : public IConvexInfo
{
public:
	IConvexInfo *GetPtr() { return this; }

	virtual unsigned int GetContents( int convexGameData ) { return CONTENTS_SOLID; }
};

class CTraceSolver
{
public:
	CTraceSolver( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, ITraceObject *obstacle, const Vector &axis )
	{
		m_pTotalTrace = ptr;
		m_sweepObject = sweepobject;
		m_sweepObjectRadius = m_sweepObject->Radius();
		m_obstacle = obstacle;
		m_ray = ray;
		m_traceLength = 0;
		m_totalTraceLength = max( ray->m_baseLength, 1e-8f );
		m_pointClosestToIntersection = axis;
		m_epsilon = g_PhysicsUnits.collisionSweepEpsilon;
	}

	bool SweepSingleConvex( void );
	float SolveMeshIntersection( simplex_t &simplex );
	float SolveMeshIntersection2D( simplex_t &simplex );
	virtual void DoSweep( void )
	{
		SweepSingleConvex();
		*m_pTotalTrace = m_trace;
	}


	void SetEpsilon( float epsilon )
	{
		m_epsilon = epsilon;
	}

protected:
	trace_t				m_trace;

	Vector				m_pointClosestToIntersection;
	ITraceObject		*m_sweepObject;
	ITraceObject		*m_obstacle;
	CTraceRay			*m_ray;
	trace_t				*m_pTotalTrace;
	float				m_traceLength;
	float				m_totalTraceLength;
	float				m_sweepObjectRadius;
	float				m_epsilon;
private:
	CTraceSolver( const CTraceSolver & );
};

class CTraceSolverSweptObject : public CTraceSolver
{
public:
	CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo );

	void InitOSRay( void );
	void SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node );
	inline bool SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius );
	virtual void DoSweep( void );
	inline void SweepAgainstNode( const IVP_Compact_Ledgetree_Node *node );

	CTraceIVP			*m_obstacleIVP;
	IConvexInfo			*m_pConvexInfo;
	unsigned int		m_contentsMask;
	CDefConvexInfo		m_fakeConvexInfo;


	IVP_U_Float_Point	m_rayCenterOS;
	IVP_U_Float_Point	m_rayStartOS;
	IVP_U_Float_Point	m_rayDirOS;
	IVP_U_Float_Point	m_rayDeltaOS;
	float				m_rayLengthOS;

private:
	CTraceSolverSweptObject( const CTraceSolverSweptObject & ); // no implementation, quells compiler warning
};

CTraceSolverSweptObject::CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo )
: CTraceSolver( ptr, sweepobject, ray, obstacle, axis )
{
	m_obstacleIVP = obstacle;
	m_contentsMask = contentsMask;
	m_pConvexInfo = (pConvexInfo != NULL) ? pConvexInfo : m_fakeConvexInfo.GetPtr();
}


bool CTraceSolverSweptObject::SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius )
{
	// disable this to help find bugs
#if DEBUG_TEST_ALL_LEDGES
	return true;
#endif
	// the ray is actually a line-swept-sphere with sweep object's radius
	IVP_U_Float_Point delta_vec; // quick check for ends of ray
	delta_vec.subtract( sphereCenter, &m_rayCenterOS );
	radius += m_sweepObjectRadius;
	// Is the sphere close enough to the ray at the center?
	float qsphere_rad = radius * radius;

	// If this is a 0 length ray, then the conservative test is 100% accurate
	if ( m_rayLengthOS > 0 )
	{
		// Calculate the perpendicular distance to the sphere 
		// The perpendicular forms a right triangle with the vector between the ray/sphere centers
		// and the ray direction vector.  Calculate the projection of the hypoteneuse along the perpendicular
		IVP_U_Float_Point h;
		h.inline_calc_cross_product(&m_rayDirOS, &delta_vec);

		if( h.quad_length() < qsphere_rad )
			return true;
	}
	else
	{
		float quad_center_dist = delta_vec.quad_length();

		if ( quad_center_dist < qsphere_rad )
		{
			return true;
		}

		// Could a ray in any direction away from the ray center intersect this sphere?
		float qrad_sum = m_rayLengthOS * 0.5f + radius;
		qrad_sum *= qrad_sum;
		if ( quad_center_dist >= qrad_sum )
		{
			return false;
		}
	}

	return false;
}

inline void CTraceSolverSweptObject::SweepAgainstNode(const IVP_Compact_Ledgetree_Node *node)
{
	const IVP_Compact_Ledge *ledge = node->get_compact_ledge();
	unsigned int ledgeContents = m_pConvexInfo->GetContents( ledge->get_client_data() );
	if (m_contentsMask & ledgeContents)
	{
		m_obstacleIVP->SetLedge( ledge );
		if ( SweepSingleConvex() )
		{
			if ( m_traceLength < m_totalTraceLength )
			{
				m_pTotalTrace->plane.normal = m_trace.plane.normal;
				m_pTotalTrace->startsolid = m_trace.startsolid;
				m_pTotalTrace->allsolid = m_trace.allsolid;
				m_totalTraceLength = m_traceLength;
				m_pTotalTrace->fraction = m_traceLength * m_ray->m_ooBaseLength;
				Assert(m_pTotalTrace->fraction >= 0 && m_pTotalTrace->fraction <= 1.0f);
#if !DEBUG_KEEP_FULL_RAY
				// shrink the ray to the shortened length, but leave a buffer of collisionSweepEpsilon units
				// at the end to make sure that precision doesn't make you miss something slightly closer
				float testFraction = (m_traceLength + m_epsilon*2) * m_ray->m_ooBaseLength;
				if ( testFraction < 1.0f )
				{
					m_ray->Reset( testFraction );
					// Update OS ray to limit tests
					m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length );
					m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f * testFraction );
				}
#endif
				m_pTotalTrace->contents = ledgeContents;
			}
		}
	}
}


void CTraceSolverSweptObject::SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node )
{
	IVP_U_Float_Point center; 
	center.set(node->center.k);
	if ( !SweepHitsSphereOS( &center, node->radius ) )
		return;

	// fast path for single leaf collision models
	if ( node->is_terminal() == IVP_TRUE )
	{
		SweepAgainstNode(node);
		return;
	}

	// use an array to implement a simple stack
	CUtlVectorFixedGrowable<const IVP_Compact_Ledgetree_Node *, 64> list;

	// pull the last item in the array (top of stack)
	// this is nearly a priority queue, but not actually, but it's cheaper (and faster in the benchmarks)
	// this code is trying to visit the nodes closest to the ray start first - which helps performance
	// since we're only interested in the first intersection of the swept object with the physcollide.
	while ( 1 )
	{
		// don't use the temp storage unless you have to.
loop_without_store:
		if ( node->is_terminal() == IVP_TRUE )
		{
			// leaf, do the test
			SweepAgainstNode(node);
		}
		else
		{
			// check node's children
			const IVP_Compact_Ledgetree_Node *node0 = node->left_son();
			center.set(node0->center.k);
			// if we don't insert, this is larger than any quad distance
			float lastDist = 1e24f;
			if ( SweepHitsSphereOS( &center, node0->radius ) )
			{
				lastDist = m_rayStartOS.quad_distance_to(&center);
			}
			else
			{
				node0 = NULL;
			}
			const IVP_Compact_Ledgetree_Node *node1 = node->right_son();
			center.set(node1->center.k);
			if ( SweepHitsSphereOS( &center, node1->radius ) )
			{
				if ( node0 )
				{
					// can hit, push on stack
					int index = list.AddToTail();
					float dist1 = m_rayStartOS.quad_distance_to(&center);
					if ( lastDist < dist1 )
					{
						node = node0;
						list[index] = node1;
					}
					else
					{
						node = node1;
						list[index] = node0;
					}
				}
				else
				{
					node = node1;
				}
				goto loop_without_store;
			}
			if ( node0 )
			{
				node = node0;
				goto loop_without_store;
			}
		}
		int last = list.Count()-1;
		if ( last < 0 )
			break;
		node = list[last];
		list.FastRemove(last);
	}
}


void CTraceSolverSweptObject::InitOSRay( void )
{
	// transform ray into object space
	m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length );
	m_obstacleIVP->TransformPositionToLocal( m_ray->m_start, m_rayStartOS );

	// no translation on matrix mult because this is a vector
	m_obstacleIVP->RotateRelativePositionToLocal( m_ray->m_delta, m_rayDeltaOS );
	m_rayDirOS.set(&m_rayDeltaOS);
	m_rayDirOS.normize();

	// add_multiple with 3 params assumes no initial value (should be set_add_multiple)
	m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f );
}


void CTraceSolverSweptObject::DoSweep( void )
{
	VPROF("TraceSolver::DoSweep");
	InitOSRay();

	// iterate ledge tree of obstacle
	const IVP_Compact_Surface *pSurface = m_obstacleIVP->m_pSurface;

	const IVP_Compact_Ledgetree_Node *lt_node_root;
	lt_node_root = pSurface->get_compact_ledge_tree_root();
	SweepLedgeTree_r( lt_node_root );
}

void CPhysicsTrace::SweepBoxIVP( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
	Ray_t ray;
	ray.Init( start, end, mins, maxs );
	SweepBoxIVP( ray, MASK_ALL, NULL, pCollide, surfaceOrigin, surfaceAngles, ptr );
}

void CPhysicsTrace::SweepBoxIVP( const Ray_t &raySrc, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
	CM_ClearTrace( ptr );

	CTraceAABB box( -raySrc.m_Extents, raySrc.m_Extents, raySrc.m_IsRay );
	CTraceIVP ivp( pCollide, vec3_origin, surfaceAngles );

	// offset the space of this sweep so that the surface is at the origin of the solution space
	CTraceRay ray( raySrc, -surfaceOrigin );

	CTraceSolverSweptObject solver( ptr, &box, &ray, &ivp, ray.m_start, contentsMask, pConvexInfo );
	solver.DoSweep();

	VectorAdd( raySrc.m_Start, raySrc.m_StartOffset, ptr->startpos );
	VectorMA( ptr->startpos, ptr->fraction, raySrc.m_Delta, ptr->endpos );
	// The plane was shifted because we shifted everything over by surfaceOrigin, shift it back
	if ( ptr->DidHit() )
	{
		ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal );
	}
}

void CPhysicsTrace::SweepIVP( const Vector &start, const Vector &end, const CPhysCollide *pSweptSurface, const QAngle &sweptAngles, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
	CM_ClearTrace( ptr );

	CTraceIVP sweptObject( pSweptSurface, vec3_origin, sweptAngles );

	// offset the space of this sweep so that the surface is at the origin of the solution space
	CTraceIVP ivp( pSurface, vec3_origin, surfaceAngles );
	CTraceRay ray( start - surfaceOrigin, end - surfaceOrigin );

	IVP_U_BigVector<IVP_Compact_Ledge> objectLedges(32);
	IVP_Compact_Ledge_Solver::get_all_ledges( pSweptSurface->GetCompactSurface(), &objectLedges );
	for ( int i = objectLedges.len() - 1; i >= 0; --i )
	{
		trace_t tr;
		CM_ClearTrace( &tr );
		sweptObject.SetLedge( objectLedges.element_at(i) );
		CTraceSolverSweptObject solver( &tr, &sweptObject, &ray, &ivp, start - surfaceOrigin, MASK_ALL, NULL );
		// UNDONE: Need just more than 0.25" tolerance here because the output position will be used by vphysics
		// UNDONE: Really this should be the collision radius from the environment.
		solver.SetEpsilon( g_PhysicsUnits.globalCollisionTolerance );
		solver.DoSweep();
		if ( tr.fraction < ptr->fraction )
		{
			*ptr = tr;
		}
	}
	ptr->endpos = start*(1.f-ptr->fraction) + end * ptr->fraction;
	if ( ptr->DidHit() )
	{
		ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal );
	}
}


static void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex );


//-----------------------------------------------------------------------------
// What is this doing? It's going to be hard to understand without reading a 
// reference on the GJK algorithm.  But here's a quick overview:
// Basically (remember this is glossing over a ton of details!) the 
// algorithm is building up a simplex that is trying to contain the origin.  
// A simplex is a point, line segment, triangle, or tetrahedron - depending on 
// how many verts you have.  
// Anyway it slowly builds one of these one vert at a time with a directed search. 
// So you start out with a point, then it guesses the next point that would be 
// most likely to form a line through the origin.  If the line doesn't go quite 
// through the origin it tries to find a third point to capture the origin 
// within a triangle.  If that doesn't work it tries to make a 
// tent (tetrahedron) out of the triangle to capture the origin.
//
// But at each step if the origin is not contained within, it tries to 
// find which sub-feature is most likely to be in the solution.  In 
// the point case it's always just the point.  In the line/edge case it 
// can reduce back to a point (origin is closest to one of the points) 
// or be the line (origin is closest to some point between them).  
// Same with the triangle (origin is closest to one vert - vert, origin is 
// closest to one edge - reduce to that edge, origin is closes to some point 
// in the triangle's surface - keep the whole triangle).  With a tetrahedron 
// keeping the whole isn't possible unless the origin is inside and you're 
// done (the origin has been captured).
//
// "You're done"  means that there is an intersection between the two 
// volumes.  Assuming you're testing a sweep, it still has to test whether that 
// sweep can be shrunk back until there is no intersection.  So it checks that.
// If it's a swept test so it does the search with SolveMeshIntersection
// Otherwise, there's nothing to shrink, so you set startsolid and allsolid 
// because it's a point/box in solid test, not a swept box/ray hits solid test.
//
// Why is it trying to capture the origin? Basically GJK sets up a space 
// and a convex hull (the minkowski sum) in that space.  The convex hull
// represents a field of the distances between different features of the pair
// of objects (e.g. for two circles, this minkowski sum is just a circle).  
// So the origin is the point in the field where the distance between the 
// objects is zero.  This means they intersect.
//-----------------------------------------------------------------------------
#if defined(_X360)
inline void VectorNormalize_FastLowPrecision( Vector &a ) 
{
	float quad = (a.x*a.x) + (a.y*a.y) + (a.z*a.z);
	float ilen = __frsqrte(quad);
	a.x *= ilen;
	a.y *= ilen;
	a.z *= ilen;
}
#else
#define VectorNormalize_FastLowPrecision VectorNormalize
#endif

bool CTraceSolver::SweepSingleConvex( void )
{
	VPROF("TraceSolver::SweepSingleConvex");
	simplex_t simplex;
	simplexvert_t	vert;
	Vector tmp;

	simplex.vertCount = 0;

	if ( m_pointClosestToIntersection == vec3_origin )
	{
		m_pointClosestToIntersection.Init(1,0,0);
	}

	float testLen = 1;
	Vector dir = -m_pointClosestToIntersection;
	VectorNormalize_FastLowPrecision(dir);
	// safe loop, max 100 iterations
	for ( int i = 0; i < 100; i++ )
	{
		// map the direction into the minkowski sum, get a new surface point
		vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position );
		vert.sweepIndex = m_ray->SupportMap( dir, &tmp );
		VectorAdd( vert.position, tmp, vert.position );
		vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp );
		VectorSubtract( vert.position, tmp, vert.position );

		testLen = DotProduct( dir, vert.position );
		// found a separating axis, no intersection
		if ( testLen < 0 )
		{
			VPROF("SolveSeparation");
			// make sure we're separated by at least m_epsilon
			testLen = fabs(testLen);
			if ( testLen < m_epsilon && m_ray->m_length > 0 )
			{
				// not separated by enough
				Vector normal = dir;
				if ( testLen > 0 )
				{
					// try to find a better separating plane or clip the ray to the current one
					for ( int j = 0; j < 20; j++ )
					{
						Vector lastVert = vert.position;
						simplex.SolveGJKSet( vert, &m_pointClosestToIntersection );
						dir = -m_pointClosestToIntersection;
						VectorNormalize_FastLowPrecision( dir );
						// map the direction into the minkowski sum, get a new surface point
						vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position );
						vert.sweepIndex = m_ray->SupportMap( dir, &tmp );
						VectorAdd( vert.position, tmp, vert.position );
						vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp );
						VectorSubtract( vert.position, tmp, vert.position );

						// found a separating axis, no intersection
						float est = -DotProduct( dir, vert.position );
						if ( est > m_epsilon ) // big enough separation, no hit
							return false;
						// take plane with the most separation
						if ( est > testLen )
						{
							testLen = est;
							normal = dir;
						}
						float last = -DotProduct( dir, lastVert );
						// search is not converging, exit.
						if ( (est - last) > -1e-4f )
							break;
					}
				}

				// This trace is going to miss, but not by enough.
				// Hit the separating plane instead 
				float dot = -DotProduct( m_ray->m_delta, normal );
				if ( dot < -(m_epsilon*0.1) || (dot < -1e-4f && testLen < (m_epsilon*0.9)) )
				{
					CM_ClearTrace( &m_trace );
					float backupDistance = m_epsilon - testLen;
					backupDistance = -(backupDistance * m_ray->m_baseLength) / dot;
					m_traceLength = m_ray->m_length - backupDistance;
					if ( m_traceLength < 0 )
					{
						m_traceLength = 0;
						// try sliding along the surface of the minkowski sum
						backupDistance = SolveMeshIntersection2D( simplex );
						if ( m_ray->m_length > backupDistance )
						{
							m_traceLength = m_ray->m_length - backupDistance;
						}
					}
					m_trace.plane.normal = -normal;
					// this is fixed up by the outer code
					//m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction;
					m_trace.contents = CONTENTS_SOLID;
					return true;
				}
			}
			return false;
		}

		// contains the origin
		if ( simplex.SolveGJKSet( vert, &m_pointClosestToIntersection ) )
		{
			VPROF("TraceSolver::SolveMeshIntersection");
			CM_ClearTrace( &m_trace );
			// now solve for t along the sweep
			if ( m_ray->m_length != 0 )
			{
				float dist = SolveMeshIntersection( simplex );
				if ( dist < m_ray->m_length && dist > 0.f )
				{
					m_traceLength = (m_ray->m_length - dist);
					CalculateSeparatingPlane( &m_trace, m_sweepObject, m_ray, m_obstacle, simplex );
					float dot = DotProduct( m_ray->m_dir, m_trace.plane.normal );
					if ( dot < 0 )
					{
						m_traceLength += (m_epsilon / dot);
					}

					if ( m_traceLength < 0 )
					{
						m_traceLength = 0;
					}
					//m_trace.fraction = m_traceLength * m_ray->m_ooBaseLength;
					//m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction;
					m_trace.contents = CONTENTS_SOLID;
				}
				else
				{
					// UNDONE: This case happens when you start solid as well as when a false 
					// intersection is detected at the very end of the trace
					m_trace.startsolid = true;
					m_trace.allsolid = true;
					m_traceLength = 0;
				}
			}
			else
			{
				m_trace.startsolid = true;
				m_trace.allsolid = true;
				m_traceLength = 0;
			}
			return true;
		}
		dir = -m_pointClosestToIntersection;
		VectorNormalize_FastLowPrecision( dir );
	}

	// BUGBUG: The solution never converged - something is probably wrong!
	AssertMsg( false, "Solution never converged.");
	return false;
}


// NEW SWEPT GJK SOLVER 2/16/2006
// convenience routines - just makes the code a little simpler.
FORCEINLINE bool simplex_t::TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut )
{
	vertCount = 3;
	verts[outIndex] = newPoint;
	*pOut = -faceNormal;
	return false;
}
FORCEINLINE bool simplex_t::EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut )
{
	vertCount = 2;
	verts[outIndex] = newPoint;
	Vector cross;
	CrossProduct( edge, newPoint.position, cross );
	CrossProduct( cross, edge, *pOut );
	return false;
}

FORCEINLINE bool simplex_t::PointSimplex( const simplexvert_t &newPoint, Vector *pOut )
{
	vertCount = 1;
	verts[0] = newPoint;
	*pOut = newPoint.position;
	return false;
}

// In general the voronoi region routines have comments referring to the verts
// All of the code assumes that vert A is the new vert being added to the set
// verts B, C, D are the previous set.  If BCD is a triangle it is assumed to be
// counter-clockwise winding order.  This must be maintained by the code!

// parametric value for closes point on a line segment (p0->p1) to the origin.
bool simplex_t::SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut )
{
	// solve the line segment AB (where A is the new point)
	Vector AB = verts[0].position - newPoint.position;
	float d = DotProduct(AB, newPoint.position);
	if ( d < 0 )
	{
		return EdgeSimplex(newPoint, 1, AB, pOut);
	}
	else
	{
		return PointSimplex(newPoint, pOut);
	}
}

// UNDONE: Collapse these routines into a single general routine?
bool simplex_t::SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut )
{
	// solve the triangle ABC (where A is the new point)
	Vector AB = verts[0].position - newPoint.position;
	Vector AC = verts[1].position - newPoint.position;
	Vector ABC;
	CrossProduct(AB, AC, ABC);
	Vector ABCxAC;
	CrossProduct(ABC, AC, ABCxAC);
	float d = DotProduct(ABCxAC, newPoint.position);
	// edge AC or edgeAB / A?
	if ( d < 0 )
	{
		d = DotProduct(AC, newPoint.position);
		if ( d < 0 )
		{
			// edge AC
			return EdgeSimplex(newPoint, 0, AC, pOut);
		}
	}
	else
	{
		// face or A / edge AB?
		Vector ABxABC;
		CrossProduct(AB, ABC, ABxABC);
		d = DotProduct(ABxABC, newPoint.position);
		if ( d > 0 )
		{
			// closest to face
			vertCount = 3;
			d = DotProduct(ABC, newPoint.position);
			// in front of face, return opposite direction
			if ( d < 0 )
			{
				verts[2] = newPoint;
				*pOut = -ABC;
				return false;
			}
			verts[2] = verts[1]; // swap to keep CCW
			verts[1] = newPoint;
			*pOut = ABC;
			return false;
		}
	}
	// edge AB or A
	d = DotProduct(AB, newPoint.position);
	if ( d < 0 )
	{
		return EdgeSimplex(newPoint, 1, AB, pOut);
	}
	return PointSimplex(newPoint, pOut);
}

bool simplex_t::SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut )
{
	// solve the tetrahedron ABCD (where A is the new point)

	// compute edge vectors
	Vector AB = verts[0].position - newPoint.position;
	Vector AC = verts[1].position - newPoint.position;
	Vector AD = verts[2].position - newPoint.position;

	// compute face normals
	Vector ABC, ACD, ADB;
	CrossProduct( AB, AC, ABC );
	CrossProduct( AC, AD, ACD );
	CrossProduct( AD, AB, ADB );

	// edge plane normals
	Vector ABCxAC, ABxABC;
	CrossProduct( ABC, AC, ABCxAC );
	CrossProduct( AB, ABC, ABxABC );
	Vector ACDxAD, ACxACD;
	CrossProduct( ACD, AD, ACDxAD );
	CrossProduct( AC, ACD, ACxACD );
	Vector ADBxAB, ADxADB;
	CrossProduct( ADB, AB, ADBxAB );
	CrossProduct( AD, ADB, ADxADB );

	int faceFlags = 0;
	float d;
	d = DotProduct(ABC,newPoint.position);
	if ( d < 0 )
	{
		faceFlags |= 0x1;
	}
	d = DotProduct(ACD,newPoint.position);
	if ( d < 0 )
	{
		faceFlags |= 0x2;
	}
	d = DotProduct(ADB,newPoint.position);
	if ( d < 0 )
	{
		faceFlags |= 0x4;
	}

	switch( faceFlags )
	{
	case 0:
		// inside all 3 faces, we're done
		verts[3] = newPoint;
		vertCount = 4;
		return true;
	case 1:
		// ABC only, solve as a triangle
		return SolveVoronoiRegion3(newPoint, pOut);
	case 2:
		// ACD only, solve as a triangle
		verts[0] = verts[2]; // collapse BCD to DC
		return SolveVoronoiRegion3(newPoint, pOut);
	case 4:
		// ADB only, solve as a triangle
		verts[1] = verts[2]; // collapse BCD to BD
		return SolveVoronoiRegion3(newPoint, pOut);
	case 3:
		{
			// in front of ABC & ACD
			d = DotProduct(ABCxAC, newPoint.position);
			if ( d < 0 )
			{
				d = DotProduct(ACxACD, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AC,newPoint.position);
					if ( d < 0 )
					{
						// edge AC
						return EdgeSimplex( newPoint, 0, AC, pOut);
					}
					// point A
					return PointSimplex(newPoint, pOut);
				}
				else
				{
					d = DotProduct(ACDxAD, newPoint.position);
					if ( d < 0 )
					{
						// edge AD
						verts[0] = verts[2]; // collapse BCD to D
						return EdgeSimplex(newPoint, 1, AD, pOut);
					}
					// face ACD
					return TriangleSimplex(newPoint,0,ACD, pOut);
				}
			}
			else
			{
				d = DotProduct(ABxABC, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AB, newPoint.position);
					if ( d < 0 )
					{
						// edge AB
						return EdgeSimplex(newPoint, 1, AB, pOut);
					}
					return PointSimplex(newPoint, pOut);
				}
				return TriangleSimplex(newPoint,2,ABC,pOut);
			}
		}
		break;
	case 5:
		{
			// in front of ADB & ABC
			d = DotProduct(ADBxAB, newPoint.position);
			if ( d < 0 )
			{
				d = DotProduct(ABxABC, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AB,newPoint.position);
					if ( d < 0 )
					{
						// edge AB
						return EdgeSimplex( newPoint, 1, AB , pOut);
					}
					// point A
					return PointSimplex(newPoint, pOut);
				}
				else
				{
					d = DotProduct(ABCxAC, newPoint.position);
					if ( d < 0 )
					{
						// edge AC
						return EdgeSimplex(newPoint, 0, AC, pOut);
					}
					// face ABC
					return TriangleSimplex(newPoint,2,ABC,pOut);
				}
			}
			else
			{
				d = DotProduct(ADxADB, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AD, newPoint.position);
					if ( d < 0 )
					{
						// edge AD
						verts[0] = verts[2]; // collapse BCD to D
						return EdgeSimplex(newPoint, 1, AD, pOut);
					}
					return PointSimplex(newPoint, pOut);
				}
				return TriangleSimplex(newPoint,1,ADB,pOut);
			}
		}
		break;
	case 6:
		{
			// in front of ACD & ADB
			d = DotProduct(ACDxAD, newPoint.position);
			if ( d < 0 )
			{
				d = DotProduct(ADxADB, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AD,newPoint.position);
					if ( d < 0 )
					{
						// edge AD
						verts[0] = verts[2]; // collapse BCD to D
						return EdgeSimplex(newPoint, 1, AD, pOut);
					}
					// point A
					return PointSimplex(newPoint, pOut);
				}
				else
				{
					d = DotProduct(ADBxAB, newPoint.position);
					if ( d < 0 )
					{
						// edge AB
						return EdgeSimplex(newPoint, 1, AB, pOut);
					}
					// face ADB
					return TriangleSimplex(newPoint,1,ADB, pOut);
				}
			}
			else
			{
				d = DotProduct(ACxACD, newPoint.position);
				if ( d < 0 )
				{
					d = DotProduct(AC, newPoint.position);
					if ( d < 0 )
					{
						// edge AC
						return EdgeSimplex(newPoint, 0, AC, pOut);
					}
					return PointSimplex(newPoint, pOut);
				}
				return TriangleSimplex(newPoint,0,ACD, pOut);
			}
		}
		break;
	case 7:
		{
			d = DotProduct(AB, newPoint.position);
			if ( d < 0 )
			{
				return EdgeSimplex(newPoint, 1, AB, pOut);
			}
			else
			{
				d = DotProduct(AC, newPoint.position);
				if ( d < 0 )
				{
					return EdgeSimplex(newPoint, 0, AC, pOut);
				}
				else
				{
					d = DotProduct(AD, newPoint.position);
					if ( d < 0 )
					{
						verts[0] = verts[2]; // collapse BCD to D
						return EdgeSimplex(newPoint, 1, AD, pOut);
					}
					return PointSimplex(newPoint, pOut);
				}
			}
		}
	}
	verts[3] = newPoint;
	vertCount = 4;
	return true;
}


bool simplex_t::SolveGJKSet( const simplexvert_t &w, Vector *pOut )
{
	VPROF("TraceSolver::simplex::SolveGJKSet");

#if 0
	for ( int v = 0; v < vertCount; v++ )
	{
		for ( int v2 = v+1; v2 < vertCount; v2++ )
		{
			if ( (verts[v].obstacleIndex == verts[v2].obstacleIndex) &&
				(verts[v].sweepIndex == verts[v2].sweepIndex) &&
				(verts[v].testIndex == verts[v2].testIndex) )
			{
				// same vert in the list twice!  degenerate
				Assert(0);
			}
		}
	}
#endif

	switch( vertCount )
	{
	case 0:
		vertCount = 1;
		verts[0] = w;
		*pOut = w.position;
		return false;
	case 1:
		return SolveVoronoiRegion2( w, pOut );
	case 2:
		return SolveVoronoiRegion3( w, pOut );
	case 3:
		return SolveVoronoiRegion4( w, pOut );
	}

	return true;
}


void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex )
{
	int testCount = 1, obstacleCount = 1;
	unsigned int testIndex[4], obstacleIndex[4];
	testIndex[0] = simplex.verts[0].testIndex;
	obstacleIndex[0] = simplex.verts[0].obstacleIndex;
	Assert( simplex.vertCount <= 4 );
	int i, j;
	for ( i = 1; i < simplex.vertCount; i++ )
	{
		for ( j = 0; j < obstacleCount; j++ )
		{
			if ( obstacleIndex[j] == simplex.verts[i].obstacleIndex )
				break;
		}
		if ( j == obstacleCount )
		{
			obstacleIndex[obstacleCount++] = simplex.verts[i].obstacleIndex;
		}

		for ( j = 0; j < testCount; j++ )
		{
			if ( testIndex[j] == simplex.verts[i].testIndex )
				break;
		}
		if ( j == testCount )
		{
			testIndex[testCount++] = simplex.verts[i].testIndex;
		}
	}

	if ( simplex.vertCount < 3 )
	{
		if ( simplex.vertCount == 2 && testCount == 2 )
		{
			// edge / point
			Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
			Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
			Vector edge = t1-t0;
			Vector tangent = CrossProduct( edge, ray->m_delta );
			ptr->plane.normal = CrossProduct( edge, tangent );
			VectorNormalize( ptr->plane.normal );
			ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
			return;
		}
	}
	if ( testCount == 3 )
	{
		// face / xxx
		Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
		Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
		Vector t2 = sweepObject->GetVertByIndex( testIndex[2] );
		ptr->plane.normal = CrossProduct( t1-t0, t2-t0 );
		VectorNormalize( ptr->plane.normal );
		if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
		{
			ptr->plane.normal = -ptr->plane.normal;
		}
		ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
	}
	else if ( testCount == 2 && obstacleCount == 2 )
	{
		// edge / edge
		Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
		Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
		Vector t2 = obstacle->GetVertByIndex( obstacleIndex[0] );
		Vector t3 = obstacle->GetVertByIndex( obstacleIndex[1] );
		ptr->plane.normal = CrossProduct( t1-t0, t3-t2 );
		VectorNormalize( ptr->plane.normal );
		if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
		{
			ptr->plane.normal = -ptr->plane.normal;
		}
		ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
	}
	else if ( obstacleCount == 3 )
	{
		// xxx / face
		Vector t0 = obstacle->GetVertByIndex( obstacleIndex[0] );
		Vector t1 = obstacle->GetVertByIndex( obstacleIndex[1] );
		Vector t2 = obstacle->GetVertByIndex( obstacleIndex[2] );
		ptr->plane.normal = CrossProduct( t1-t0, t2-t0 );
		VectorNormalize( ptr->plane.normal );
		if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
		{
			ptr->plane.normal = -ptr->plane.normal;
		}
		ptr->plane.dist = DotProduct( t0, ptr->plane.normal );
	}
	else
	{
		ptr->plane.normal = -ray->m_dir;
		if ( simplex.vertCount )
		{
			ptr->plane.dist = DotProduct( ptr->plane.normal, obstacle->GetVertByIndex( simplex.verts[0].obstacleIndex ) );
		}
		else
			ptr->plane.dist = 0.f;
	}
}

// clip a direction vector to a plane (ray begins at the origin)
inline float Clip( const Vector &dir, const Vector &pos, const Vector &normal )
{
	float dist = DotProduct(pos, normal);
	float cosTheta = DotProduct(dir,normal);
	if ( cosTheta > 0 )
		return dist / cosTheta;

	// parallel or not facing the plane
	return 1e24f;
}

// This is the first iteration of solving time of intersection.
// It needs to test all 4 faces of the tetrahedron to find the one the ray leaves through
// this is done by finding the closest plane by clipping the ray to each plane
Vector simplex_t::ClipRayToTetrahedronBase( const Vector &dir )
{
	Vector AB = verts[0].position - verts[3].position;
	Vector AC = verts[1].position - verts[3].position;
	Vector AD = verts[2].position - verts[3].position;
	Vector BC = verts[1].position - verts[0].position;
	Vector BD = verts[2].position - verts[0].position;

	// compute face normals
	Vector ABC, ACD, ADB, BCD;
	CrossProduct( AB, AC, ABC );
	CrossProduct( AC, AD, ACD );
	CrossProduct( AD, AB, ADB );
	CrossProduct( BD, BC, BCD );	// flipped to point out of the tetrahedron

	// NOTE: These cancel out in the clipping equation
	//VectorNormalize(ABC);
	//VectorNormalize(ACD);
	//VectorNormalize(ADB);
	//VectorNormalize(BCD);

	// Assert valid tetrahedron/winding order
#if CHECK_TOI_CALCS
	Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC ));
	Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD ));
	Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB ));
	Assert(DotProduct(verts[3].position, BCD) < DotProduct(verts[0].position, BCD ));
#endif

	float dists[4];
	dists[0] = Clip( dir, verts[3].position, ABC );
	dists[1] = Clip( dir, verts[3].position, ACD );
	dists[2] = Clip( dir, verts[3].position, ADB );
	dists[3] = Clip( dir, verts[0].position, BCD );
	float dmin = dists[3];
	int best = 3;
	for ( int i = 0; i < 3; i++ )
	{
		if ( dists[i] < dmin )
		{
			best = i;
			dmin = dists[i];
		}
	}

	vertCount = 3;
	// push back down to a triangle
	// Note that we need to preserve winding so that the vector order assumptions above are still valid!
	switch( best )
	{
	case 0:
		verts[2] = verts[3];
		return ABC;
	case 1:
		verts[0] = verts[3];
		return ACD;
	case 2:
		verts[1] = verts[3];
		return ADB;
	case 3:
		// swap 1 & 2
		verts[3] = verts[1];
		verts[1] = verts[2];
		verts[2] = verts[3];
		return BCD;
	}
	Assert(0); // failed!
	return vec3_origin;
}

// for subsequent iterations you don't need to test one of the faces (face you previously left through)
// so only test the three faces involving the new point A
Vector simplex_t::ClipRayToTetrahedron( const Vector &dir )
{
	Vector AB = verts[0].position - verts[3].position;
	Vector AC = verts[1].position - verts[3].position;
	Vector AD = verts[2].position - verts[3].position;

	// compute face normals
	Vector ABC, ACD, ADB;
	CrossProduct( AB, AC, ABC );
	CrossProduct( AC, AD, ACD );
	CrossProduct( AD, AB, ADB );

	// NOTE: These cancel out in the clipping equation
	//VectorNormalize(ABC);
	//VectorNormalize(ACD);
	//VectorNormalize(ADB);

	// Assert valid tetrahedron/winding order
#if CHECK_TOI_CALCS
	Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC ));
	Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD ));
	Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB ));
#endif

	float dists[3];
	dists[0] = Clip( dir, verts[3].position, ABC );
	dists[1] = Clip( dir, verts[3].position, ACD );
	dists[2] = Clip( dir, verts[3].position, ADB );

	float dmin = dists[2];
	int best = 2;
	for ( int i = 0; i < 2; i++ )
	{
		if ( dists[i] < dmin )
		{
			best = i;
			dmin = dists[i];
		}
	}

	vertCount = 3;
	// push back down to a triangle
	// Note that we need to preserve winding so that the vector order assumptions above are still valid!
	switch( best )
	{
	case 0:
		verts[2] = verts[3];
		return ABC;
	case 1:
		verts[0] = verts[3];
		return ACD;
	case 2:
		verts[1] = verts[3];
		return ADB;
	}
	return vec3_origin;
}

float simplex_t::ClipRayToTriangle( const Vector &dir, float epsilon )
{
	Vector AB = verts[0].position - verts[2].position;
	Vector AC = verts[1].position - verts[2].position;
	Vector BC = verts[1].position - verts[0].position;

	// compute face normal
	Vector ABC;
	CrossProduct( AB, AC, ABC );  // this points toward the origin
	VectorNormalize(ABC);
	Vector edgeAB, edgeAC, edgeBC;
	// these should point out of the triangle
	CrossProduct( AB, ABC, edgeAB );
	CrossProduct( ABC, AC, edgeAC );
	CrossProduct( BC, ABC, edgeBC );

	// NOTE: These cancel out in the clipping equation
	VectorNormalize(edgeAB);
	VectorNormalize(edgeAC);
	VectorNormalize(edgeBC);

#if CHECK_TOI_CALCS
	Assert(DotProduct(verts[2].position, ABC) < 0); // points toward
	// Assert valid triangle/winding order (all normals point away from the origin)
	Assert(DotProduct(verts[2].position, edgeAB) > 0);
	Assert(DotProduct(verts[2].position, edgeAC) > 0);
	Assert(DotProduct(verts[0].position, edgeBC) > 0);
#endif

	float dists[3];
	dists[0] = Clip( dir, verts[0].position, edgeAB );
	dists[1] = Clip( dir, verts[1].position, edgeAC );
	dists[2] = Clip( dir, verts[1].position, edgeBC );
	Vector *normals[3] = {&edgeAB, &edgeAC, &edgeBC};

	float dmin = dists[0];
	int best = 0;
	if ( dists[1] < dmin )
	{
		dmin = dists[1];
		best = 1;
	}
	if ( dists[2] < dmin )
	{
		best = 2;
		dmin = dists[2];
	}
	float dot = DotProduct( dir, *normals[best] );
	if ( dot <= 0 )
		return 1e24f;
	dmin += epsilon/dot;

	return dmin;
}

// Solve for time of intersection along the sweep
// Do this by iteratively clipping the ray to the tetrahedron containing the origin
// when a triangle is found intersecting the ray, reduce the simplex to that triangle
// and then re-expand it to a tetrahedron using the support point normal to the triangle (away from the origin)
// iterate until no new points can be found.  That's the surface of the sum.
float CTraceSolver::SolveMeshIntersection( simplex_t &simplex )
{
	Vector tmp;
	Assert( simplex.vertCount == 4 );
	if ( simplex.vertCount < 4 )
		return 0.0f;

	Vector v = simplex.ClipRayToTetrahedronBase( m_ray->m_dir );

	simplexvert_t vert;
	// safe loop, max 100 iterations
	for ( int i = 0; i < 100; i++ )
	{
		VectorNormalize(v);
		vert.testIndex = m_sweepObject->SupportMap( v, &vert.position );
		vert.sweepIndex = m_ray->SupportMap( v, &tmp );
		VectorAdd( vert.position, tmp, vert.position );
		vert.obstacleIndex = m_obstacle->SupportMap( -v, &tmp );
		VectorSubtract( vert.position, tmp, vert.position );

		// map the new separating axis (NOTE: This test is inverted from the GJK - we are trying to get out, not in)
		// found a separating axis, we've moved the sweep back enough
		float dist = DotProduct( v, simplex.verts[0].position ) + TEST_EPSILON;
		if ( DotProduct( v, vert.position ) <= dist )
		{
			Vector BC = simplex.verts[1].position - simplex.verts[0].position;
			Vector BD = simplex.verts[2].position - simplex.verts[0].position;

			// compute face normals
			Vector BCD;
			CrossProduct( BC, BD, BCD );
			// NOTE: This cancels out inside Clip()
			//VectorNormalize( BCD );
			// clip ray to triangle
			return Clip( m_ray->m_dir, simplex.verts[0].position, BCD );
		}

		// add the new vert
		simplex.verts[simplex.vertCount] = vert;
		simplex.vertCount++;
		v = simplex.ClipRayToTetrahedron( m_ray->m_dir );
	}

	Assert(0);
	return 0.0f;
}

// similar to SolveMeshIntersection, but solves projected into the 2D triangle simplex remaining
// this is used for the near miss case
float CTraceSolver::SolveMeshIntersection2D( simplex_t &simplex )
{
	AssertMsg( simplex.vertCount == 3, "simplex.vertCount != 3: %d", simplex.vertCount );
	if ( simplex.vertCount != 3 )
		return 0.0f;

	// note: This should really do this iteratively in case the triangle is coplanar with another triangle that 
	// is between this one and the edge of the sum in this plane
	float dist = simplex.ClipRayToTriangle( m_ray->m_dir, m_epsilon );
	return dist;
}


static const Vector g_xpos(1,0,0), g_xneg(-1,0,0);
static const Vector g_ypos(0,1,0), g_yneg(0,-1,0);
static const Vector g_zpos(0,0,1), g_zneg(0,0,-1);

void TraceGetAABB_r( Vector *pMins, Vector *pMaxs, const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp )
{
	if ( node->is_terminal() == IVP_TRUE )
	{
		Vector tmp;
		ivp.SetLedge( node->get_compact_ledge() );
		ivp.SupportMap( g_xneg, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		ivp.SupportMap( g_yneg, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		ivp.SupportMap( g_zneg, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		ivp.SupportMap( g_xpos, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		ivp.SupportMap( g_ypos, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		ivp.SupportMap( g_zpos, &tmp );
		AddPointToBounds( tmp, *pMins, *pMaxs );
		return;
	}

	TraceGetAABB_r( pMins, pMaxs, node->left_son(), ivp );
	TraceGetAABB_r( pMins, pMaxs, node->right_son(), ivp );
}

void CPhysicsTrace::GetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles )
{
	CTraceIVP ivp( pCollide, collideOrigin, collideAngles );

	if ( ivp.SetSingleConvex() )
	{
		Vector tmp;
		ivp.SupportMap( g_xneg, &tmp );
		pMins->x = tmp.x;
		ivp.SupportMap( g_yneg, &tmp );
		pMins->y = tmp.y;
		ivp.SupportMap( g_zneg, &tmp );
		pMins->z = tmp.z;

		ivp.SupportMap( g_xpos, &tmp );
		pMaxs->x = tmp.x;
		ivp.SupportMap( g_ypos, &tmp );
		pMaxs->y = tmp.y;
		ivp.SupportMap( g_zpos, &tmp );
		pMaxs->z = tmp.z;
	}
	else
	{
		const IVP_Compact_Ledgetree_Node *lt_node_root;
		lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root();
		ClearBounds( *pMins, *pMaxs );
		TraceGetAABB_r( pMins, pMaxs, lt_node_root, ivp );
	}
	// JAY: Disable this here, do it in the engine instead.  That way the tools get
	// accurate bboxes
#if 0
	const float radius = g_PhysicsUnits.collisionSweepEpsilon;
	mins -= Vector(radius,radius,radius);
	maxs += Vector(radius,radius,radius);
#endif
}

void TraceGetExtent_r( const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp, const Vector &dir, float &dot, Vector &point )
{
	if ( node->is_terminal() == IVP_TRUE )
	{
		ivp.SetLedge( node->get_compact_ledge() );
		Vector tmp;
		ivp.SupportMap( dir, &tmp );
		float newDot = DotProduct( tmp, dir );

		if ( newDot > dot )
		{
			dot = newDot;
			point = tmp;
		}
		return;
	}

	TraceGetExtent_r( node->left_son(), ivp, dir, dot, point );
	TraceGetExtent_r( node->right_son(), ivp, dir, dot, point );
}


Vector CPhysicsTrace::GetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction )
{
	CTraceIVP ivp( pCollide, collideOrigin, collideAngles );

	if ( ivp.SetSingleConvex() )
	{
		Vector tmp;
		ivp.SupportMap( direction, &tmp );
		return tmp;
	}
	else
	{
		const IVP_Compact_Ledgetree_Node *lt_node_root;
		lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root();
		Vector out = vec3_origin;
		float tmp = -1e6f;
		TraceGetExtent_r( lt_node_root, ivp, direction, tmp, out );
		return out;
	}
}

bool CPhysicsTrace::IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone )
{
	trace_t tr;
	CM_ClearTrace( &tr );
	bool bPoint = (boxAbsMins == boxAbsMaxs) ? true : false;
	CTraceAABB box( boxAbsMins - cone.origin, boxAbsMaxs - cone.origin, bPoint );
	CTraceCone traceCone( cone, -cone.origin );

	// offset the space of this sweep so that the surface is at the origin of the solution space
	CTraceRay ray( vec3_origin, vec3_origin );

	CTraceSolver solver( &tr, &box, &ray, &traceCone, boxAbsMaxs );
	solver.DoSweep();

	return tr.startsolid;
}



CPhysicsTrace::CPhysicsTrace()
{
}

CPhysicsTrace::~CPhysicsTrace()
{
}

CVisitHash::CVisitHash()
{
	m_vertVisitID = 1;
	memset( m_vertVisit, 0, sizeof(m_vertVisit) );
}