src/public/collisionutils.cpp

//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose: Common collision utility methods
//
// $Header: $
// $NoKeywords: $
//=============================================================================//

#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)

#include "collisionutils.h"
#include "cmodel.h"
#include "mathlib/mathlib.h"
#include "mathlib/vector.h"
#include "tier0/dbg.h"
#include <float.h>
#include "mathlib/vector4d.h"
#include "trace.h"

// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"

#define UNINIT		-99999.0

//-----------------------------------------------------------------------------
// Clears the trace
//-----------------------------------------------------------------------------
static void Collision_ClearTrace( const Vector &vecRayStart, const Vector &vecRayDelta, CBaseTrace *pTrace )
{
	pTrace->startpos = vecRayStart;
	pTrace->endpos = vecRayStart;
	pTrace->endpos += vecRayDelta;
	pTrace->startsolid = false;
	pTrace->allsolid = false;
	pTrace->fraction = 1.0f;
	pTrace->contents = 0;
}


//-----------------------------------------------------------------------------
// Compute the offset in t along the ray that we'll use for the collision
//-----------------------------------------------------------------------------
static float ComputeBoxOffset( const Ray_t& ray )
{
	if (ray.m_IsRay)
		return 1e-3f;

	// Find the projection of the box diagonal along the ray...
	float offset = FloatMakePositive(ray.m_Extents[0] * ray.m_Delta[0]) +
					FloatMakePositive(ray.m_Extents[1] * ray.m_Delta[1]) +
					FloatMakePositive(ray.m_Extents[2] * ray.m_Delta[2]);

	// We need to divide twice: Once to normalize the computation above
	// so we get something in units of extents, and the second to normalize
	// that with respect to the entire raycast.
	offset *= InvRSquared( ray.m_Delta );

	// 1e-3 is an epsilon
	return offset + 1e-3;
}


//-----------------------------------------------------------------------------
// Intersects a swept box against a triangle
//-----------------------------------------------------------------------------
float IntersectRayWithTriangle( const Ray_t& ray, 
		const Vector& v1, const Vector& v2, const Vector& v3, bool oneSided )
{
	// This is cute: Use barycentric coordinates to represent the triangle
	// Vo(1-u-v) + V1u + V2v and intersect that with a line Po + Dt
	// This gives us 3 equations + 3 unknowns, which we can solve with
	// Cramer's rule...
	//		E1x u + E2x v - Dx t = Pox - Vox
	// There's a couple of other optimizations, Cramer's rule involves
	// computing the determinant of a matrix which has been constructed
	// by three vectors. It turns out that 
	// det | A B C | = -( A x C ) dot B or -(C x B) dot A
	// which we'll use below..

	Vector edge1, edge2, org;
	VectorSubtract( v2, v1, edge1 );
	VectorSubtract( v3, v1, edge2 );

	// Cull out one-sided stuff
	if (oneSided)
	{
		Vector normal;
		CrossProduct( edge1, edge2, normal );
		if (DotProduct( normal, ray.m_Delta ) >= 0.0f)
			return -1.0f;
	}

	// FIXME: This is inaccurate, but fast for boxes
	// We want to do a fast separating axis implementation here
	// with a swept triangle along the reverse direction of the ray.

	// Compute some intermediary terms
	Vector dirCrossEdge2, orgCrossEdge1;
	CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 );

	// Compute the denominator of Cramer's rule:
	//		| -Dx E1x E2x |
	// det	| -Dy E1y E2y | = (D x E2) dot E1
	//		| -Dz E1z E2z |
	float denom = DotProduct( dirCrossEdge2, edge1 );
	if( FloatMakePositive( denom ) < 1e-6 )
		return -1.0f;
	denom = 1.0f / denom;

	// Compute u. It's gotta lie in the range of 0 to 1.
	//				   | -Dx orgx E2x |
	// u = denom * det | -Dy orgy E2y | = (D x E2) dot org
	//				   | -Dz orgz E2z |
	VectorSubtract( ray.m_Start, v1, org );
	float u = DotProduct( dirCrossEdge2, org ) * denom;
	if ((u < 0.0f) || (u > 1.0f))
		return -1.0f;

	// Compute t and v the same way...
	// In barycentric coords, u + v < 1
	CrossProduct( org, edge1, orgCrossEdge1 );
	float v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom;
	if ((v < 0.0f) || (v + u > 1.0f))
		return -1.0f;

	// Compute the distance along the ray direction that we need to fudge 
	// when using swept boxes
	float boxt = ComputeBoxOffset( ray );
	float t = DotProduct( orgCrossEdge1, edge2 ) * denom;
	if ((t < -boxt) || (t > 1.0f + boxt))
		return -1.0f;

	return clamp( t, 0.f, 1.f );
}

//-----------------------------------------------------------------------------
// computes the barycentric coordinates of an intersection
//-----------------------------------------------------------------------------

bool ComputeIntersectionBarycentricCoordinates( const Ray_t& ray, 
		const Vector& v1, const Vector& v2, const Vector& v3, float& u, float& v,
		float *t )
{
	Vector edge1, edge2, org;
	VectorSubtract( v2, v1, edge1 );
	VectorSubtract( v3, v1, edge2 );

	// Compute some intermediary terms
	Vector dirCrossEdge2, orgCrossEdge1;
	CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 );

	// Compute the denominator of Cramer's rule:
	//		| -Dx E1x E2x |
	// det	| -Dy E1y E2y | = (D x E2) dot E1
	//		| -Dz E1z E2z |
	float denom = DotProduct( dirCrossEdge2, edge1 );
	if( FloatMakePositive( denom ) < 1e-6 )
		return false;
	denom = 1.0f / denom;

	// Compute u. It's gotta lie in the range of 0 to 1.
	//				   | -Dx orgx E2x |
	// u = denom * det | -Dy orgy E2y | = (D x E2) dot org
	//				   | -Dz orgz E2z |
	VectorSubtract( ray.m_Start, v1, org );
	u = DotProduct( dirCrossEdge2, org ) * denom;

	// Compute t and v the same way...
	// In barycentric coords, u + v < 1
	CrossProduct( org, edge1, orgCrossEdge1 );
	v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom;

	// Compute the distance along the ray direction that we need to fudge 
	// when using swept boxes
	if( t )
	{
		float boxt = ComputeBoxOffset( ray );
		*t = DotProduct( orgCrossEdge1, edge2 ) * denom;
		if( ( *t < -boxt ) || ( *t > 1.0f + boxt ) )
			return false;
	}

	return true;
}

//-----------------------------------------------------------------------------
// Intersects a plane with a triangle (requires barycentric definition)
//-----------------------------------------------------------------------------

int IntersectTriangleWithPlaneBarycentric( const Vector& org, const Vector& edgeU,
		const Vector& edgeV, const Vector4D& plane, Vector2D* pIntersection )
{
	// This uses a barycentric method, since we need that to determine
	// interpolated points, alphas, and normals
	// Given the plane equation P dot N + d = 0
	// and the barycentric coodinate equation P = Org + EdgeU * u + EdgeV * v
	// Plug em in. Intersection occurs at u = 0 or v = 0 or u + v = 1

	float orgDotNormal = DotProduct( org, plane.AsVector3D() );
	float edgeUDotNormal = DotProduct( edgeU, plane.AsVector3D() );
	float edgeVDotNormal = DotProduct( edgeV, plane.AsVector3D() );

	int ptIdx = 0;

	// u = 0
	if ( edgeVDotNormal != 0.0f )
	{
		pIntersection[ptIdx].x = 0.0f;
		pIntersection[ptIdx].y = - ( orgDotNormal - plane.w ) / edgeVDotNormal;
		if ((pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f))
			++ptIdx;
	}

	// v = 0
	if ( edgeUDotNormal != 0.0f )
	{
		pIntersection[ptIdx].x = - ( orgDotNormal - plane.w ) / edgeUDotNormal;
		pIntersection[ptIdx].y = 0.0f;
		if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f))
			++ptIdx;
	}

	// u + v = 1
	if (ptIdx == 2)
		return ptIdx;

	if ( edgeVDotNormal != edgeUDotNormal )
	{
		pIntersection[ptIdx].x = - ( orgDotNormal - plane.w + edgeVDotNormal) / 
			( edgeUDotNormal - edgeVDotNormal);
		pIntersection[ptIdx].y = 1.0f - pIntersection[ptIdx].x;
		if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f) &&
			 (pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f))
			++ptIdx;
	}

	Assert( ptIdx < 3 );
	return ptIdx;
}


//-----------------------------------------------------------------------------
// Returns true if a box intersects with a sphere
//-----------------------------------------------------------------------------
bool IsSphereIntersectingSphere( const Vector& center1, float radius1, 
								 const Vector& center2, float radius2 )
{
	Vector delta;
	VectorSubtract( center2, center1, delta );
	float distSq = delta.LengthSqr();
	float radiusSum = radius1 + radius2;
	return (distSq <= (radiusSum * radiusSum));
}


//-----------------------------------------------------------------------------
// Returns true if a box intersects with a sphere
//-----------------------------------------------------------------------------
bool IsBoxIntersectingSphere( const Vector& boxMin, const Vector& boxMax, 
						const Vector& center, float radius )
{
	// See Graphics Gems, box-sphere intersection
	float dmin = 0.0f;
	float flDelta;

	// Unrolled the loop.. this is a big cycle stealer...
	if (center[0] < boxMin[0])
	{
		flDelta = center[0] - boxMin[0];
		dmin += flDelta * flDelta;
	}
	else if (center[0] > boxMax[0])
	{
		flDelta = boxMax[0] - center[0];
		dmin += flDelta * flDelta;
	}

	if (center[1] < boxMin[1])
	{
		flDelta = center[1] - boxMin[1];
		dmin += flDelta * flDelta;
	}
	else if (center[1] > boxMax[1])
	{
		flDelta = boxMax[1] - center[1];
		dmin += flDelta * flDelta;
	}

	if (center[2] < boxMin[2])
	{
		flDelta = center[2] - boxMin[2];
		dmin += flDelta * flDelta;
	}
	else if (center[2] > boxMax[2])
	{
		flDelta = boxMax[2] - center[2];
		dmin += flDelta * flDelta;
	}

	return dmin < radius * radius;
}

bool IsBoxIntersectingSphereExtents( const Vector& boxCenter, const Vector& boxHalfDiag, 
						const Vector& center, float radius )
{
	// See Graphics Gems, box-sphere intersection
	float dmin = 0.0f;
	float flDelta, flDiff;

	// Unrolled the loop.. this is a big cycle stealer...
	flDiff = FloatMakePositive( center.x - boxCenter.x );
	if (flDiff > boxHalfDiag.x)
	{
		flDelta = flDiff - boxHalfDiag.x;
		dmin += flDelta * flDelta;
	}

	flDiff = FloatMakePositive( center.y - boxCenter.y );
	if (flDiff > boxHalfDiag.y)
	{
		flDelta = flDiff - boxHalfDiag.y;
		dmin += flDelta * flDelta;
	}

	flDiff = FloatMakePositive( center.z - boxCenter.z );
	if (flDiff > boxHalfDiag.z)
	{
		flDelta = flDiff - boxHalfDiag.z;
		dmin += flDelta * flDelta;
	}

	return dmin < radius * radius;
}


//-----------------------------------------------------------------------------
// Returns true if a rectangle intersects with a circle
//-----------------------------------------------------------------------------
bool IsCircleIntersectingRectangle( const Vector2D& boxMin, const Vector2D& boxMax, 
						      const Vector2D& center, float radius )
{
	// See Graphics Gems, box-sphere intersection
	float dmin = 0.0f;
	float flDelta;

	if (center[0] < boxMin[0])
	{
		flDelta = center[0] - boxMin[0];
		dmin += flDelta * flDelta;
	}
	else if (center[0] > boxMax[0])
	{
		flDelta = boxMax[0] - center[0];
		dmin += flDelta * flDelta;
	}

	if (center[1] < boxMin[1])
	{
		flDelta = center[1] - boxMin[1];
		dmin += flDelta * flDelta;
	}
	else if (center[1] > boxMax[1])
	{
		flDelta = boxMax[1] - center[1];
		dmin += flDelta * flDelta;
	}

	return dmin < radius * radius;
}


//-----------------------------------------------------------------------------
// returns true if there's an intersection between ray and sphere
//-----------------------------------------------------------------------------
bool IsRayIntersectingSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, 
		const Vector& vecCenter, float flRadius, float flTolerance )
{
	// For this algorithm, find a point on the ray  which is closest to the sphere origin
	// Do this by making a plane passing through the sphere origin
	// whose normal is parallel to the ray. Intersect that plane with the ray.
	// Plane: N dot P = I, N = D (ray direction), I = C dot N = C dot D
	// Ray: P = O + D * t
	// D dot ( O + D * t ) = C dot D
	// D dot O + D dot D * t = C dot D
	// t = (C - O) dot D / D dot D
	// Clamp t to (0,1)
	// Find distance of the point on the ray to the sphere center.
	Assert( flTolerance >= 0.0f );
	flRadius += flTolerance;

	Vector vecRayToSphere;
	VectorSubtract( vecCenter, vecRayOrigin, vecRayToSphere );
	float flNumerator = DotProduct( vecRayToSphere, vecRayDelta );
	
	float t;
	if (flNumerator <= 0.0f)
	{
		t = 0.0f;
	}
	else
	{
		float flDenominator = DotProduct( vecRayDelta, vecRayDelta );
		if ( flNumerator > flDenominator )
			t = 1.0f;
		else
			t = flNumerator / flDenominator;
	}
	
	Vector vecClosestPoint;
	VectorMA( vecRayOrigin, t, vecRayDelta, vecClosestPoint );
	return ( vecClosestPoint.DistToSqr( vecCenter ) <= flRadius * flRadius );

	// NOTE: This in an alternate algorithm which I didn't use because I'd have to use a sqrt
	// So it's probably faster to do this other algorithm. I'll leave the comments here
	// for how to go back if we want to

	// Solve using the ray equation + the sphere equation
	// P = o + dt
	// (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
	// (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2
	// (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 +
	//		(oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 +
	//		(oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2
	// (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t +
	//		(ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0
	// or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a
	// a = DotProduct( vecRayDelta, vecRayDelta );
	// b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta )
	// c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius;
	// Valid solutions are possible only if b^2 - 4ac >= 0
	// Therefore, compute that value + see if we got it	
}


//-----------------------------------------------------------------------------
//
// IntersectInfiniteRayWithSphere
//
// Returns whether or not there was an intersection. 
// Returns the two intersection points
//
//-----------------------------------------------------------------------------
bool IntersectInfiniteRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, 
	const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 )
{
	// Solve using the ray equation + the sphere equation
	// P = o + dt
	// (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
	// (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2
	// (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 +
	//		(oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 +
	//		(oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2
	// (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t +
	//		(ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0
	// or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a
	// a = DotProduct( vecRayDelta, vecRayDelta );
	// b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta )
	// c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius;

	Vector vecSphereToRay;
	VectorSubtract(	vecRayOrigin, vecSphereCenter, vecSphereToRay );

	float a = DotProduct( vecRayDelta, vecRayDelta );

	// This would occur in the case of a zero-length ray
	if ( a == 0.0f )
	{
		*pT1 = *pT2 = 0.0f;
		return vecSphereToRay.LengthSqr() <= flRadius * flRadius;
	}

	float b = 2 * DotProduct( vecSphereToRay, vecRayDelta );
	float c = DotProduct( vecSphereToRay, vecSphereToRay ) - flRadius * flRadius;
	float flDiscrim = b * b - 4 * a * c;
	if ( flDiscrim < 0.0f )
		return false;

	flDiscrim = sqrt( flDiscrim );
	float oo2a = 0.5f / a;
	*pT1 = ( - b - flDiscrim ) * oo2a;
	*pT2 = ( - b + flDiscrim ) * oo2a;
	return true;
}


//-----------------------------------------------------------------------------
//
// IntersectRayWithSphere
//
// Returns whether or not there was an intersection. 
// Returns the two intersection points, clamped to (0,1)
//
//-----------------------------------------------------------------------------
bool IntersectRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, 
	const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 )
{
	if ( !IntersectInfiniteRayWithSphere( vecRayOrigin, vecRayDelta, vecSphereCenter, flRadius, pT1, pT2 ) )
		return false;

	if (( *pT1 > 1.0f ) || ( *pT2 < 0.0f ))
		return false;

	// Clamp it!
	if ( *pT1 < 0.0f )
		*pT1 = 0.0f;
	if ( *pT2 > 1.0f )
		*pT2 = 1.0f;

	return true;
}


// returns true if the sphere and cone intersect
// NOTE: cone sine/cosine are the half angle of the cone
bool IsSphereIntersectingCone( const Vector &sphereCenter, float sphereRadius, const Vector &coneOrigin, const Vector &coneNormal, float coneSine, float coneCosine )
{
	Vector backCenter = coneOrigin - (sphereRadius / coneSine) * coneNormal;
	Vector delta = sphereCenter - backCenter;
	float deltaLen = delta.Length();
	if ( DotProduct(coneNormal, delta) >= deltaLen*coneCosine )
	{
		delta = sphereCenter - coneOrigin;
		deltaLen = delta.Length();
		if ( -DotProduct(coneNormal, delta) >= deltaLen * coneSine )
		{
			return ( deltaLen <= sphereRadius ) ? true : false;
		}
		return true;
	}
	return false;
}


//-----------------------------------------------------------------------------
// returns true if the point is in the box
//-----------------------------------------------------------------------------
bool IsPointInBox( const Vector& pt, const Vector& boxMin, const Vector& boxMax )
{
	Assert( boxMin[0] <= boxMax[0] );
	Assert( boxMin[1] <= boxMax[1] );
	Assert( boxMin[2] <= boxMax[2] );

	// on x360, force use of SIMD version.
	if (IsX360())
	{
		return IsPointInBox( LoadUnaligned3SIMD(pt.Base()), LoadUnaligned3SIMD(boxMin.Base()), LoadUnaligned3SIMD(boxMax.Base()) ) ;
	}

	if ( (pt[0] > boxMax[0]) || (pt[0] < boxMin[0]) )
		return false;
	if ( (pt[1] > boxMax[1]) || (pt[1] < boxMin[1]) )
		return false;
	if ( (pt[2] > boxMax[2]) || (pt[2] < boxMin[2]) )
		return false;
	return true;
}


bool IsPointInCone( const Vector &pt, const Vector &origin, const Vector &axis, float cosAngle, float length )
{
	Vector delta = pt - origin;
	float dist = VectorNormalize( delta );
	float dot = DotProduct( delta, axis );
	if ( dot < cosAngle )
		return false;
	if ( dist * dot > length )
		return false;

	return true;
}


//-----------------------------------------------------------------------------
// returns true if there's an intersection between two boxes
//-----------------------------------------------------------------------------
bool IsBoxIntersectingBox( const Vector& boxMin1, const Vector& boxMax1, 
						const Vector& boxMin2, const Vector& boxMax2 )
{
	Assert( boxMin1[0] <= boxMax1[0] );
	Assert( boxMin1[1] <= boxMax1[1] );
	Assert( boxMin1[2] <= boxMax1[2] );
	Assert( boxMin2[0] <= boxMax2[0] );
	Assert( boxMin2[1] <= boxMax2[1] );
	Assert( boxMin2[2] <= boxMax2[2] );

	if ( (boxMin1[0] > boxMax2[0]) || (boxMax1[0] < boxMin2[0]) )
		return false;
	if ( (boxMin1[1] > boxMax2[1]) || (boxMax1[1] < boxMin2[1]) )
		return false;
	if ( (boxMin1[2] > boxMax2[2]) || (boxMax1[2] < boxMin2[2]) )
		return false;
	return true;
}

bool IsBoxIntersectingBoxExtents( const Vector& boxCenter1, const Vector& boxHalfDiagonal1, 
						   const Vector& boxCenter2, const Vector& boxHalfDiagonal2 )
{
	Vector vecDelta, vecSize;
	VectorSubtract( boxCenter1, boxCenter2, vecDelta );
	VectorAdd( boxHalfDiagonal1, boxHalfDiagonal2, vecSize );
	return ( FloatMakePositive( vecDelta.x ) <= vecSize.x ) &&
			( FloatMakePositive( vecDelta.y ) <= vecSize.y ) &&
			( FloatMakePositive( vecDelta.z ) <= vecSize.z );
}


//-----------------------------------------------------------------------------
// 
// IsOBBIntersectingOBB
//
// returns true if there's an intersection between two OBBs
//
//-----------------------------------------------------------------------------
bool IsOBBIntersectingOBB( const Vector &vecOrigin1, const QAngle &vecAngles1, const Vector& boxMin1, const Vector& boxMax1, 
						   const Vector &vecOrigin2, const QAngle &vecAngles2, const Vector& boxMin2, const Vector& boxMax2, float flTolerance )
{
	// FIXME: Simple case AABB check doesn't work because the min and max extents are not oriented based on the angle
	// this fast check would only be good for cubes.
	/*if ( vecAngles1 == vecAngles2 )
	{
		const Vector &vecDelta = vecOrigin2 - vecOrigin1;
		Vector vecOtherMins, vecOtherMaxs;
		VectorAdd( boxMin2, vecDelta, vecOtherMins );
		VectorAdd( boxMax2, vecDelta, vecOtherMaxs );
		return IsBoxIntersectingBox( boxMin1, boxMax1, vecOtherMins, vecOtherMaxs );
	}*/

	// OBB test...
	cplane_t plane;
	bool bFoundPlane = ComputeSeparatingPlane( vecOrigin1, vecAngles1, boxMin1, boxMax1, 
		vecOrigin2, vecAngles2, boxMin2, boxMax2, flTolerance, &plane );
	return (bFoundPlane == false);
}

// NOTE: This is only very slightly faster on high end PCs and x360
#define USE_SIMD_RAY_CHECKS 1
//-----------------------------------------------------------------------------
// returns true if there's an intersection between box and ray
//-----------------------------------------------------------------------------
bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, 
									const Vector& origin, const Vector& vecDelta, float flTolerance )
{
	
#if USE_SIMD_RAY_CHECKS
	// Load the unaligned ray/box parameters into SIMD registers
	fltx4 start = LoadUnaligned3SIMD(origin.Base());
	fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base());
	fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() );
	fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() );
	fltx4 epsilon = ReplicateX4(flTolerance);
	// compute the mins/maxs of the box expanded by the ray extents
	// relocate the problem so that the ray start is at the origin.
	fltx4 offsetMins = SubSIMD(boxMins, start);
	fltx4 offsetMaxs = SubSIMD(boxMaxs, start);
	fltx4 offsetMinsExpanded = SubSIMD(offsetMins, epsilon);
	fltx4 offsetMaxsExpanded = AddSIMD(offsetMaxs, epsilon);

	// Check to see if both the origin (start point) and the end point (delta) are on the front side
	// of any of the box sides - if so there can be no intersection
	fltx4 startOutMins = CmpLtSIMD(Four_Zeros, offsetMinsExpanded);
	fltx4 endOutMins = CmpLtSIMD(delta,offsetMinsExpanded);
	fltx4 minsMask = AndSIMD( startOutMins, endOutMins );
	fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, offsetMaxsExpanded);
	fltx4 endOutMaxs = CmpGtSIMD(delta,offsetMaxsExpanded);
	fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs );
	if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask))))
		return false;

	// now build the per-axis interval of t for intersections
	fltx4 invDelta = ReciprocalSaturateSIMD(delta);
	fltx4 tmins = MulSIMD( offsetMinsExpanded, invDelta );
	fltx4 tmaxs = MulSIMD( offsetMaxsExpanded, invDelta );
	fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs));

	// only consider axes where we crossed a plane
	tmins = MaskedAssign( crossPlane, tmins, Four_Negative_FLT_MAX );
	tmaxs = MaskedAssign( crossPlane, tmaxs, Four_FLT_MAX );

	// now sort the interval per axis
	fltx4 mint = MinSIMD( tmins, tmaxs );
	fltx4 maxt = MaxSIMD( tmins, tmaxs );

	// now find the intersection of the intervals on all axes
	fltx4 firstOut = FindLowestSIMD3(maxt);
	fltx4 lastIn = FindHighestSIMD3(mint);
	// NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut]
	firstOut = MinSIMD(firstOut, Four_Ones);
	lastIn = MaxSIMD(lastIn, Four_Zeros);

	// If the final interval is valid lastIn<firstOut, check for separation
	fltx4 separation = CmpGtSIMD(lastIn, firstOut);

	return IsAllZeros(separation);
#else
	// On the x360, we force use of the SIMD functions.
#if defined(_X360) 
	if (IsX360())
	{
		fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base());
		return IsBoxIntersectingRay( 
			LoadUnaligned3SIMD(boxMin.Base()), LoadUnaligned3SIMD(boxMax.Base()),
			LoadUnaligned3SIMD(origin.Base()), delta, ReciprocalSIMD(delta), // ray parameters
			ReplicateX4(flTolerance) ///< eg from ReplicateX4(flTolerance)
			);
	}
#endif
	Assert( boxMin[0] <= boxMax[0] );
	Assert( boxMin[1] <= boxMax[1] );
	Assert( boxMin[2] <= boxMax[2] );

	// FIXME: Surely there's a faster way
	float tmin = -FLT_MAX;
	float tmax = FLT_MAX;

	for (int i = 0; i < 3; ++i)
	{
		// Parallel case...
		if (FloatMakePositive(vecDelta[i]) < 1e-8)
		{
			// Check that origin is in the box
			// if not, then it doesn't intersect..
			if ( (origin[i] < boxMin[i] - flTolerance) || (origin[i] > boxMax[i] + flTolerance) )
				return false;

			continue;
		}

		// non-parallel case
		// Find the t's corresponding to the entry and exit of
		// the ray along x, y, and z. The find the furthest entry
		// point, and the closest exit point. Once that is done,
		// we know we don't collide if the closest exit point
		// is behind the starting location. We also don't collide if
		// the closest exit point is in front of the furthest entry point

		float invDelta = 1.0f / vecDelta[i];
		float t1 = (boxMin[i] - flTolerance - origin[i]) * invDelta;
		float t2 = (boxMax[i] + flTolerance - origin[i]) * invDelta;
		if (t1 > t2)
		{
			float temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tmin)
			tmin = t1;
		if (t2 < tmax)
			tmax = t2;
		if (tmin > tmax)
			return false;
		if (tmax < 0)
			return false;
		if (tmin > 1)
			return false;
	}

	return true;
#endif
}

//-----------------------------------------------------------------------------
// returns true if there's an intersection between box and ray
//-----------------------------------------------------------------------------
bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, 
									const Vector& origin, const Vector& vecDelta,
									const Vector& vecInvDelta, float flTolerance )
{	
#if USE_SIMD_RAY_CHECKS
	// Load the unaligned ray/box parameters into SIMD registers
	fltx4 start = LoadUnaligned3SIMD(origin.Base());
	fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base());
	fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() );
	fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() );
	// compute the mins/maxs of the box expanded by the ray extents
	// relocate the problem so that the ray start is at the origin.
	boxMins = SubSIMD(boxMins, start);
	boxMaxs = SubSIMD(boxMaxs, start);

	// Check to see if both the origin (start point) and the end point (delta) are on the front side
	// of any of the box sides - if so there can be no intersection
	fltx4 startOutMins = CmpLtSIMD(Four_Zeros, boxMins);
	fltx4 endOutMins = CmpLtSIMD(delta,boxMins);
	fltx4 minsMask = AndSIMD( startOutMins, endOutMins );
	fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, boxMaxs);
	fltx4 endOutMaxs = CmpGtSIMD(delta,boxMaxs);
	fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs );
	if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask))))
		return false;

	// now build the per-axis interval of t for intersections
	fltx4 epsilon = ReplicateX4(flTolerance);
	fltx4 invDelta = LoadUnaligned3SIMD(vecInvDelta.Base());
	boxMins = SubSIMD(boxMins, epsilon);
	boxMaxs = AddSIMD(boxMaxs, epsilon);

	boxMins = MulSIMD( boxMins, invDelta );
	boxMaxs = MulSIMD( boxMaxs, invDelta );

	fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs));
	// only consider axes where we crossed a plane
	boxMins = MaskedAssign( crossPlane, boxMins, Four_Negative_FLT_MAX );
	boxMaxs = MaskedAssign( crossPlane, boxMaxs, Four_FLT_MAX );

	// now sort the interval per axis
	fltx4 mint = MinSIMD( boxMins, boxMaxs );
	fltx4 maxt = MaxSIMD( boxMins, boxMaxs );

	// now find the intersection of the intervals on all axes
	fltx4 firstOut = FindLowestSIMD3(maxt);
	fltx4 lastIn = FindHighestSIMD3(mint);
	// NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut]
	firstOut = MinSIMD(firstOut, Four_Ones);
	lastIn = MaxSIMD(lastIn, Four_Zeros);

	// If the final interval is valid lastIn<firstOut, check for separation
	fltx4 separation = CmpGtSIMD(lastIn, firstOut);

	return IsAllZeros(separation);
#else
	// On the x360, we force use of the SIMD functions.
#if defined(_X360) && !defined(PARANOID_SIMD_ASSERTING)
	if (IsX360())
	{
		return IsBoxIntersectingRay( 
			LoadUnaligned3SIMD(boxMin.Base()), LoadUnaligned3SIMD(boxMax.Base()),
			LoadUnaligned3SIMD(origin.Base()), LoadUnaligned3SIMD(vecDelta.Base()), LoadUnaligned3SIMD(vecInvDelta.Base()), // ray parameters
			ReplicateX4(flTolerance) ///< eg from ReplicateX4(flTolerance)
			);
	}
#endif

	Assert( boxMin[0] <= boxMax[0] );
	Assert( boxMin[1] <= boxMax[1] );
	Assert( boxMin[2] <= boxMax[2] );

	// FIXME: Surely there's a faster way
	float tmin = -FLT_MAX;
	float tmax = FLT_MAX;

	for ( int i = 0; i < 3; ++i )
	{
		// Parallel case...
		if ( FloatMakePositive( vecDelta[i] ) < 1e-8 )
		{
			// Check that origin is in the box, if not, then it doesn't intersect..
			if ( ( origin[i] < boxMin[i] - flTolerance ) || ( origin[i] > boxMax[i] + flTolerance ) )
				return false;

			continue;
		}

		// Non-parallel case
		// Find the t's corresponding to the entry and exit of
		// the ray along x, y, and z. The find the furthest entry
		// point, and the closest exit point. Once that is done,
		// we know we don't collide if the closest exit point
		// is behind the starting location. We also don't collide if
		// the closest exit point is in front of the furthest entry point
		float t1 = ( boxMin[i] - flTolerance - origin[i] ) * vecInvDelta[i];
		float t2 = ( boxMax[i] + flTolerance - origin[i] ) * vecInvDelta[i];
		if ( t1 > t2 )
		{
			float temp = t1;
			t1 = t2;
			t2 = temp;
		}

		if (t1 > tmin)
			tmin = t1;

		if (t2 < tmax)
			tmax = t2;

		if (tmin > tmax)
			return false;

		if (tmax < 0)
			return false;

		if (tmin > 1)
			return false;
	}

	return true;
#endif
}

//-----------------------------------------------------------------------------
// Intersects a ray with a aabb, return true if they intersect
//-----------------------------------------------------------------------------
bool FASTCALL IsBoxIntersectingRay( const Vector& vecBoxMin, const Vector& vecBoxMax, const Ray_t& ray, float flTolerance )
{
	// On the x360, we force use of the SIMD functions.
#if defined(_X360) 
	if (IsX360())
	{
		return IsBoxIntersectingRay( 
			LoadUnaligned3SIMD(vecBoxMin.Base()), LoadUnaligned3SIMD(vecBoxMax.Base()),
			ray, flTolerance);
	}
#endif

	if ( !ray.m_IsSwept )
	{
		Vector rayMins, rayMaxs;
		VectorSubtract( ray.m_Start, ray.m_Extents, rayMins );
		VectorAdd( ray.m_Start, ray.m_Extents, rayMaxs );
		if ( flTolerance != 0.0f )
		{
			rayMins.x -= flTolerance; rayMins.y -= flTolerance; rayMins.z -= flTolerance;
			rayMaxs.x += flTolerance; rayMaxs.y += flTolerance; rayMaxs.z += flTolerance;
		}
		return IsBoxIntersectingBox( vecBoxMin, vecBoxMax, rayMins, rayMaxs );
	}

	Vector vecExpandedBoxMin, vecExpandedBoxMax;
	VectorSubtract( vecBoxMin, ray.m_Extents, vecExpandedBoxMin );
	VectorAdd( vecBoxMax, ray.m_Extents, vecExpandedBoxMax );
	return IsBoxIntersectingRay( vecExpandedBoxMin, vecExpandedBoxMax, ray.m_Start, ray.m_Delta, flTolerance );
}


//-----------------------------------------------------------------------------
// returns true if there's an intersection between box and ray (SIMD version)
//-----------------------------------------------------------------------------


#ifdef _X360
bool FASTCALL IsBoxIntersectingRay( fltx4 boxMin, fltx4 boxMax, 
								    fltx4 origin, fltx4 delta, fltx4 invDelta, // ray parameters
									fltx4 vTolerance ///< eg from ReplicateX4(flTolerance)
									)
#else
bool FASTCALL IsBoxIntersectingRay( const fltx4 &inBoxMin, const fltx4 & inBoxMax, 
								   const fltx4 & origin, const fltx4 & delta, const fltx4 & invDelta, // ray parameters
								   const fltx4 & vTolerance ///< eg from ReplicateX4(flTolerance)
								   )
#endif
{
	// Load the unaligned ray/box parameters into SIMD registers
	// compute the mins/maxs of the box expanded by the ray extents
	// relocate the problem so that the ray start is at the origin.

#ifdef _X360
	boxMin = SubSIMD(boxMin, origin);
	boxMax = SubSIMD(boxMax, origin);
#else
	fltx4 boxMin = SubSIMD(inBoxMin, origin);
	fltx4 boxMax = SubSIMD(inBoxMax, origin);
#endif

	// Check to see if the origin (start point) and the end point (delta) are on the same side
	// of any of the box sides - if so there can be no intersection
	fltx4 startOutMins = AndSIMD( CmpLtSIMD(Four_Zeros, boxMin), CmpLtSIMD(delta,boxMin) );
	fltx4 startOutMaxs = AndSIMD( CmpGtSIMD(Four_Zeros, boxMax), CmpGtSIMD(delta,boxMax) );
	if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(startOutMaxs,startOutMins))))
		return false;

	// now build the per-axis interval of t for intersections
	boxMin = SubSIMD(boxMin, vTolerance);
	boxMax = AddSIMD(boxMax, vTolerance);

	boxMin = MulSIMD( boxMin, invDelta );
	boxMax = MulSIMD( boxMax, invDelta );

	// now sort the interval per axis
	fltx4 mint = MinSIMD( boxMin, boxMax );
	fltx4 maxt = MaxSIMD( boxMin, boxMax );

	// now find the intersection of the intervals on all axes
	fltx4 firstOut = FindLowestSIMD3(maxt);
	fltx4 lastIn = FindHighestSIMD3(mint);
	// NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut]
	firstOut = MinSIMD(firstOut, Four_Ones);
	lastIn = MaxSIMD(lastIn, Four_Zeros);

	// If the final interval is valid lastIn<firstOut, check for separation
	fltx4 separation = CmpGtSIMD(lastIn, firstOut);

	return IsAllZeros(separation);
}


bool FASTCALL IsBoxIntersectingRay( const fltx4& boxMin, const fltx4& boxMax, 
								   const Ray_t& ray, float flTolerance )
{
	fltx4 vTolerance = ReplicateX4(flTolerance);
	fltx4 rayStart = LoadAlignedSIMD(ray.m_Start);
	fltx4 rayExtents = LoadAlignedSIMD(ray.m_Extents);
	if ( !ray.m_IsSwept )
	{

		fltx4 rayMins, rayMaxs;
		rayMins = SubSIMD(rayStart, rayExtents);
		rayMaxs = AddSIMD(rayStart, rayExtents);
		rayMins = AddSIMD(rayMins, vTolerance);
		rayMaxs = AddSIMD(rayMaxs, vTolerance);

		VectorAligned vecBoxMin, vecBoxMax, vecRayMins, vecRayMaxs;
		StoreAlignedSIMD( vecBoxMin.Base(), boxMin );
		StoreAlignedSIMD( vecBoxMax.Base(), boxMax );
		StoreAlignedSIMD( vecRayMins.Base(), rayMins );
		StoreAlignedSIMD( vecRayMaxs.Base(), rayMaxs );

		return IsBoxIntersectingBox( vecBoxMin, vecBoxMax, vecRayMins, vecRayMaxs );
	}

	fltx4 rayDelta = LoadAlignedSIMD(ray.m_Delta);
	fltx4 vecExpandedBoxMin, vecExpandedBoxMax;
	vecExpandedBoxMin = SubSIMD( boxMin, rayExtents );
	vecExpandedBoxMax = AddSIMD( boxMax, rayExtents );

	return IsBoxIntersectingRay( vecExpandedBoxMin, vecExpandedBoxMax, rayStart, rayDelta, ReciprocalSIMD(rayDelta), ReplicateX4(flTolerance) );
}


//-----------------------------------------------------------------------------
// Intersects a ray with a ray, return true if they intersect
// t, s = parameters of closest approach (if not intersecting!)
//-----------------------------------------------------------------------------
bool IntersectRayWithRay( const Ray_t &ray0, const Ray_t &ray1, float &t, float &s )
{
	Assert( ray0.m_IsRay && ray1.m_IsRay );

	//
	// r0 = p0 + v0t
	// r1 = p1 + v1s
	//
	// intersection : r0 = r1 :: p0 + v0t = p1 + v1s
	// NOTE: v(0,1) are unit direction vectors
	//
	// subtract p0 from both sides and cross with v1 (NOTE: v1 x v1 = 0)
	//  (v0 x v1)t = ((p1 - p0 ) x v1)
	//
	// dotting  with (v0 x v1) and dividing by |v0 x v1|^2
	//	t = Det | (p1 - p0) , v1 , (v0 x v1) | / |v0 x v1|^2
	//  s = Det | (p1 - p0) , v0 , (v0 x v1) | / |v0 x v1|^2
	//
	//  Det | A B C | = -( A x C ) dot B or -( C x B ) dot A
	// 
	//  NOTE: if |v0 x v1|^2 = 0, then the lines are parallel
	//
	Vector v0( ray0.m_Delta );
	Vector v1( ray1.m_Delta );
	VectorNormalize( v0 );
	VectorNormalize( v1 );

	Vector v0xv1 = v0.Cross( v1 );
	float lengthSq = v0xv1.LengthSqr();
	if( lengthSq == 0.0f )
	{
		t = 0; s = 0;
		return false;		// parallel
	}

	Vector p1p0 = ray1.m_Start - ray0.m_Start;

	Vector AxC = p1p0.Cross( v0xv1 );
	AxC.Negate();
	float detT = AxC.Dot( v1 );
	
	AxC = p1p0.Cross( v0xv1 );
	AxC.Negate();
	float detS = AxC.Dot( v0 );

	t = detT / lengthSq;
	s = detS / lengthSq;

	// intersection????
	Vector i0, i1;
	i0 = v0 * t;
	i1 = v1 * s;
	i0 += ray0.m_Start;
	i1 += ray1.m_Start;
	if( i0.x == i1.x && i0.y == i1.y && i0.z == i1.z )
		return true;

	return false;
}

//-----------------------------------------------------------------------------
// Intersects a ray with a plane, returns distance t along ray.
//-----------------------------------------------------------------------------
float IntersectRayWithPlane( const Ray_t& ray, const cplane_t& plane )
{
	float denom	= DotProduct( ray.m_Delta, plane.normal );
	if (denom == 0.0f)
		return 0.0f;

	denom = 1.0f / denom;
	return (plane.dist - DotProduct( ray.m_Start, plane.normal )) * denom;
}

float IntersectRayWithPlane( const Vector& org, const Vector& dir, const cplane_t& plane )
{
	float denom	= DotProduct( dir, plane.normal );
	if (denom == 0.0f)
		return 0.0f;

	denom = 1.0f / denom;
	return (plane.dist - DotProduct( org, plane.normal )) * denom;
}

float IntersectRayWithPlane( const Vector& org, const Vector& dir, const Vector& normal, float dist )
{
	float denom	= DotProduct( dir, normal );
	if (denom == 0.0f)
		return 0.0f;

	denom = 1.0f / denom;
	return (dist - DotProduct( org, normal )) * denom;
}

float IntersectRayWithAAPlane( const Vector& vecStart, const Vector& vecEnd, int nAxis, float flSign, float flDist )
{
	float denom	= flSign * (vecEnd[nAxis] - vecStart[nAxis]);
	if (denom == 0.0f)
		return 0.0f;

	denom = 1.0f / denom;
	return (flDist - flSign * vecStart[nAxis]) * denom;
}


//-----------------------------------------------------------------------------
// Intersects a ray against a box
//-----------------------------------------------------------------------------
bool IntersectRayWithBox( const Vector &vecRayStart, const Vector &vecRayDelta, 
	const Vector &boxMins, const Vector &boxMaxs, float flTolerance, BoxTraceInfo_t *pTrace )
{
	int			i;
	float		d1, d2;
	float		f;

	pTrace->t1 = -1.0f;
	pTrace->t2 = 1.0f;
	pTrace->hitside = -1;

	// UNDONE: This makes this code a little messy
	pTrace->startsolid = true;

	for ( i = 0; i < 6; ++i )
	{
		if ( i >= 3 )
		{
			d1 = vecRayStart[i-3] - boxMaxs[i-3];
			d2 = d1 + vecRayDelta[i-3];
		}
		else
		{
			d1 = -vecRayStart[i] + boxMins[i];
			d2 = d1 - vecRayDelta[i];
		}

		// if completely in front of face, no intersection
		if (d1 > 0 && d2 > 0)
		{
			// UNDONE: Have to revert this in case it's still set
			// UNDONE: Refactor to have only 2 return points (true/false) from this function
			pTrace->startsolid = false;
			return false;
		}

		// completely inside, check next face
		if (d1 <= 0 && d2 <= 0)
			continue;

		if (d1 > 0)
		{
			pTrace->startsolid = false;
		}

		// crosses face
		if (d1 > d2)
		{
			f = d1 - flTolerance;
			if ( f < 0 )
			{
				f = 0;
			}
			f = f / (d1-d2);
			if (f > pTrace->t1)
			{
				pTrace->t1 = f;
				pTrace->hitside = i;
			}
		}
		else
		{ 
			// leave
			f = (d1 + flTolerance) / (d1-d2);
			if (f < pTrace->t2)
			{
				pTrace->t2 = f;
			}
		}
	}

	return pTrace->startsolid || (pTrace->t1 < pTrace->t2 && pTrace->t1 >= 0.0f);
}


//-----------------------------------------------------------------------------
// Intersects a ray against a box
//-----------------------------------------------------------------------------
bool IntersectRayWithBox( const Vector &vecRayStart, const Vector &vecRayDelta, 
	const Vector &boxMins, const Vector &boxMaxs, float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid )
{
	Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace );

	BoxTraceInfo_t trace;

	if ( IntersectRayWithBox( vecRayStart, vecRayDelta, boxMins, boxMaxs, flTolerance, &trace ) )
	{
		pTrace->startsolid = trace.startsolid;
		if (trace.t1 < trace.t2 && trace.t1 >= 0.0f)
		{
			pTrace->fraction = trace.t1;
			VectorMA( pTrace->startpos, trace.t1, vecRayDelta, pTrace->endpos );
			pTrace->contents = CONTENTS_SOLID;
			pTrace->plane.normal = vec3_origin;
			if ( trace.hitside >= 3 )
			{
				trace.hitside -= 3;
				pTrace->plane.dist = boxMaxs[trace.hitside];
				pTrace->plane.normal[trace.hitside] = 1.0f;
				pTrace->plane.type = trace.hitside;
			}
			else
			{
				pTrace->plane.dist = -boxMins[trace.hitside];
				pTrace->plane.normal[trace.hitside] = -1.0f;
				pTrace->plane.type = trace.hitside;
			}
			return true;
		}

		if ( pTrace->startsolid )
		{
			pTrace->allsolid = (trace.t2 <= 0.0f) || (trace.t2 >= 1.0f);
			pTrace->fraction = 0;
			if ( pFractionLeftSolid )
			{
				*pFractionLeftSolid = trace.t2;
			}
			pTrace->endpos = pTrace->startpos;
			pTrace->contents = CONTENTS_SOLID;
			pTrace->plane.dist = pTrace->startpos[0];
			pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f );
			pTrace->plane.type = 0;
			pTrace->startpos = vecRayStart + (trace.t2 * vecRayDelta);
			return true;
		}
	}

	return false;
}


//-----------------------------------------------------------------------------
// Intersects a ray against a box
//-----------------------------------------------------------------------------
bool IntersectRayWithBox( const Ray_t &ray, const Vector &boxMins, const Vector &boxMaxs, 
						 float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid )
{
	if ( !ray.m_IsRay )
	{
		Vector vecExpandedMins = boxMins - ray.m_Extents;
		Vector vecExpandedMaxs = boxMaxs + ray.m_Extents;
		bool bIntersects = IntersectRayWithBox( ray.m_Start, ray.m_Delta, vecExpandedMins, vecExpandedMaxs, flTolerance, pTrace, pFractionLeftSolid );
		pTrace->startpos += ray.m_StartOffset;
		pTrace->endpos += ray.m_StartOffset;
		return bIntersects;
	}
	return IntersectRayWithBox( ray.m_Start, ray.m_Delta, boxMins, boxMaxs, flTolerance, pTrace, pFractionLeftSolid );
}


//-----------------------------------------------------------------------------
// Intersects a ray against an OBB, returns t1 and t2
//-----------------------------------------------------------------------------
bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, 
	const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, 
	float flTolerance, BoxTraceInfo_t *pTrace )
{
	// FIXME: Two transforms is pretty expensive. Should we optimize this?
	Vector start, delta;
	VectorITransform( vecRayStart, matOBBToWorld, start );
	VectorIRotate( vecRayDelta, matOBBToWorld, delta );

	return IntersectRayWithBox( start, delta, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); 
}


//-----------------------------------------------------------------------------
// Intersects a ray against an OBB
//-----------------------------------------------------------------------------
bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, 
	const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, 
	float flTolerance, CBaseTrace *pTrace )
{
	Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace );

	// FIXME: Make it work with tolerance
	Assert( flTolerance == 0.0f );

	// OPTIMIZE: Store this in the box instead of computing it here
	// compute center in local space
	Vector vecBoxExtents = (vecOBBMins + vecOBBMaxs) * 0.5; 
	Vector vecBoxCenter;

	// transform to world space
	VectorTransform( vecBoxExtents, matOBBToWorld, vecBoxCenter );

	// calc extents from local center
	vecBoxExtents = vecOBBMaxs - vecBoxExtents;

	// OPTIMIZE: This is optimized for world space.  If the transform is fast enough, it may make more
	// sense to just xform and call UTIL_ClipToBox() instead.  MEASURE THIS.

	// save the extents of the ray along 
	Vector extent, uextent;
	Vector segmentCenter = vecRayStart + vecRayDelta - vecBoxCenter;

	extent.Init();

	// check box axes for separation
	for ( int j = 0; j < 3; j++ )
	{
		extent[j] = vecRayDelta.x * matOBBToWorld[0][j] + vecRayDelta.y * matOBBToWorld[1][j] +	vecRayDelta.z * matOBBToWorld[2][j];
		uextent[j] = fabsf(extent[j]);
		float coord = segmentCenter.x * matOBBToWorld[0][j] + segmentCenter.y * matOBBToWorld[1][j] +	segmentCenter.z * matOBBToWorld[2][j];
		coord = fabsf(coord);

		if ( coord > (vecBoxExtents[j] + uextent[j]) )
			return false;
	}

	// now check cross axes for separation
	float tmp, cextent;
	Vector cross = vecRayDelta.Cross( segmentCenter );
	cextent = cross.x * matOBBToWorld[0][0] + cross.y * matOBBToWorld[1][0] + cross.z * matOBBToWorld[2][0];
	cextent = fabsf(cextent);
	tmp = vecBoxExtents[1]*uextent[2] + vecBoxExtents[2]*uextent[1];
	if ( cextent > tmp )
		return false;

	cextent = cross.x * matOBBToWorld[0][1] + cross.y * matOBBToWorld[1][1] + cross.z * matOBBToWorld[2][1];
	cextent = fabsf(cextent);
	tmp = vecBoxExtents[0]*uextent[2] + vecBoxExtents[2]*uextent[0];
	if ( cextent > tmp )
		return false;

	cextent = cross.x * matOBBToWorld[0][2] + cross.y * matOBBToWorld[1][2] + cross.z * matOBBToWorld[2][2];
	cextent = fabsf(cextent);
	tmp = vecBoxExtents[0]*uextent[1] + vecBoxExtents[1]*uextent[0];
	if ( cextent > tmp )
		return false;

	// !!! We hit this box !!! compute intersection point and return
	// Compute ray start in bone space
	Vector start;
	VectorITransform( vecRayStart, matOBBToWorld, start );

	// extent is ray.m_Delta in bone space, recompute delta in bone space
	extent *= 2.0f;

	// delta was prescaled by the current t, so no need to see if this intersection
	// is closer
	trace_t boxTrace;
	if ( !IntersectRayWithBox( start, extent, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ) )
		return false;

	// Fix up the start/end pos and fraction
	Vector vecTemp;
	VectorTransform( pTrace->endpos, matOBBToWorld, vecTemp );
	pTrace->endpos = vecTemp;

	pTrace->startpos = vecRayStart;
	pTrace->fraction *= 2.0f;

	// Fix up the plane information
	float flSign = pTrace->plane.normal[ pTrace->plane.type ];
	pTrace->plane.normal[0] = flSign * matOBBToWorld[0][pTrace->plane.type];
	pTrace->plane.normal[1] = flSign * matOBBToWorld[1][pTrace->plane.type];
	pTrace->plane.normal[2] = flSign * matOBBToWorld[2][pTrace->plane.type];
	pTrace->plane.dist = DotProduct( pTrace->endpos, pTrace->plane.normal );
	pTrace->plane.type = 3;

	return true;
}


//-----------------------------------------------------------------------------
// Intersects a ray against an OBB
//-----------------------------------------------------------------------------
bool IntersectRayWithOBB( const Vector &vecRayOrigin, const Vector &vecRayDelta, 
	const Vector &vecBoxOrigin, const QAngle &angBoxRotation,
	const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace )
{
	if (angBoxRotation == vec3_angle)
	{
		Vector vecAbsMins, vecAbsMaxs;
		VectorAdd( vecBoxOrigin, vecOBBMins, vecAbsMins );
		VectorAdd( vecBoxOrigin, vecOBBMaxs, vecAbsMaxs );
		return IntersectRayWithBox( vecRayOrigin, vecRayDelta, vecAbsMins, vecAbsMaxs, flTolerance, pTrace ); 
	}

	matrix3x4_t obbToWorld;
	AngleMatrix( angBoxRotation, vecBoxOrigin, obbToWorld );
	return IntersectRayWithOBB( vecRayOrigin, vecRayDelta, obbToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace );
}


//-----------------------------------------------------------------------------
// Box support map
//-----------------------------------------------------------------------------
inline void ComputeSupportMap( const Vector &vecDirection, const Vector &vecBoxMins, 
				const Vector &vecBoxMaxs, float pDist[2] )
{
	int nIndex = (vecDirection.x > 0.0f);
	pDist[nIndex] = vecBoxMaxs.x * vecDirection.x;
	pDist[1 - nIndex] = vecBoxMins.x * vecDirection.x;

	nIndex = (vecDirection.y > 0.0f);
	pDist[nIndex] += vecBoxMaxs.y * vecDirection.y;
	pDist[1 - nIndex] += vecBoxMins.y * vecDirection.y;

	nIndex = (vecDirection.z > 0.0f);
	pDist[nIndex] += vecBoxMaxs.z * vecDirection.z;
	pDist[1 - nIndex] += vecBoxMins.z * vecDirection.z;
}

inline void ComputeSupportMap( const Vector &vecDirection, int i1, int i2, 
	const Vector &vecBoxMins, const Vector &vecBoxMaxs, float pDist[2] )
{
	int nIndex = (vecDirection[i1] > 0.0f);
	pDist[nIndex] = vecBoxMaxs[i1] * vecDirection[i1];
	pDist[1 - nIndex] = vecBoxMins[i1] * vecDirection[i1];

	nIndex = (vecDirection[i2] > 0.0f);
	pDist[nIndex] += vecBoxMaxs[i2] * vecDirection[i2];
	pDist[1 - nIndex] += vecBoxMins[i2] * vecDirection[i2];
}

//-----------------------------------------------------------------------------
// Intersects a ray against an OBB
//-----------------------------------------------------------------------------
static int s_ExtIndices[3][2] = 
{
	{ 2, 1 },
	{ 0, 2 },
	{ 0, 1 },
};

static int s_MatIndices[3][2] = 
{
	{ 1, 2 },
	{ 2, 0 },
	{ 1, 0 },
};

bool IntersectRayWithOBB( const Ray_t &ray, const matrix3x4_t &matOBBToWorld,
	const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace )
{
	if ( ray.m_IsRay )
	{
		return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, matOBBToWorld, 
			vecOBBMins, vecOBBMaxs, flTolerance, pTrace );
	}

	Collision_ClearTrace( ray.m_Start + ray.m_StartOffset, ray.m_Delta, pTrace );

	// Compute a bounding sphere around the bloated OBB
	Vector vecOBBCenter;
	VectorAdd( vecOBBMins, vecOBBMaxs, vecOBBCenter );
	vecOBBCenter *= 0.5f;
	vecOBBCenter.x += matOBBToWorld[0][3];
	vecOBBCenter.y += matOBBToWorld[1][3];
	vecOBBCenter.z += matOBBToWorld[2][3];

	Vector vecOBBHalfDiagonal;
	VectorSubtract( vecOBBMaxs, vecOBBMins, vecOBBHalfDiagonal );
	vecOBBHalfDiagonal *= 0.5f;

	float flRadius = vecOBBHalfDiagonal.Length() + ray.m_Extents.Length();
	if ( !IsRayIntersectingSphere( ray.m_Start, ray.m_Delta, vecOBBCenter, flRadius, flTolerance ) )
		return false;

	// Ok, we passed the trivial reject, so lets do the dirty deed.
	// Basically we're going to do the GJK thing explicitly. We'll shrink the ray down
	// to a point, and bloat the OBB by the ray's extents. This will generate facet
	// planes which are perpendicular to all of the separating axes typically seen in
	// a standard seperating axis implementation.

	// We're going to create a number of planes through various vertices in the OBB
	// which represent all of the separating planes. Then we're going to bloat the planes
	// by the ray extents.
	
	// We're going to do all work in OBB-space because it's easier to do the
	// support-map in this case

	// First, transform the ray into the space of the OBB
	Vector vecLocalRayOrigin, vecLocalRayDirection;
	VectorITransform( ray.m_Start, matOBBToWorld, vecLocalRayOrigin );
	VectorIRotate( ray.m_Delta, matOBBToWorld, vecLocalRayDirection );

	// Next compute all separating planes
	Vector pPlaneNormal[15];
	float ppPlaneDist[15][2];

	int i;
	for ( i = 0; i < 3; ++i )
	{
		// Each plane needs to be bloated an amount = to the abs dot product of 
		// the ray extents with the plane normal
		// For the OBB planes, do it in world space; 
		// and use the direction of the OBB (the ith column of matOBBToWorld) in world space vs extents
		pPlaneNormal[i].Init( );
		pPlaneNormal[i][i] = 1.0f;

		float flExtentDotNormal = 
			FloatMakePositive( matOBBToWorld[0][i] * ray.m_Extents.x ) +
			FloatMakePositive( matOBBToWorld[1][i] * ray.m_Extents.y ) +
			FloatMakePositive( matOBBToWorld[2][i] * ray.m_Extents.z );

		ppPlaneDist[i][0] = vecOBBMins[i] - flExtentDotNormal; 
		ppPlaneDist[i][1] = vecOBBMaxs[i] + flExtentDotNormal;

		// For the ray-extents planes, they are bloated by the extents
		// Use the support map to determine which
		VectorCopy( matOBBToWorld[i], pPlaneNormal[i+3].Base() ); 
		ComputeSupportMap( pPlaneNormal[i+3], vecOBBMins, vecOBBMaxs, ppPlaneDist[i+3] );
		ppPlaneDist[i+3][0] -= ray.m_Extents[i];
		ppPlaneDist[i+3][1] += ray.m_Extents[i];

		// Now the edge cases... (take the cross product of x,y,z axis w/ ray extent axes
		// given by the rows of the obb to world matrix. 
		// Compute the ray extent bloat in world space because it's easier...

		// These are necessary to compute the world-space versions of
		// the edges so we can compute the extent dot products
		float flRayExtent0 = ray.m_Extents[s_ExtIndices[i][0]];
		float flRayExtent1 = ray.m_Extents[s_ExtIndices[i][1]];
		const float *pMatRow0 = matOBBToWorld[s_MatIndices[i][0]];
		const float *pMatRow1 = matOBBToWorld[s_MatIndices[i][1]];

		// x axis of the OBB + world ith axis
		pPlaneNormal[i+6].Init( 0.0f, -matOBBToWorld[i][2], matOBBToWorld[i][1] );
		ComputeSupportMap( pPlaneNormal[i+6], 1, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+6] );
		flExtentDotNormal = 
			FloatMakePositive( pMatRow0[0] ) * flRayExtent0 +
			FloatMakePositive( pMatRow1[0] ) * flRayExtent1;
		ppPlaneDist[i+6][0] -= flExtentDotNormal;
		ppPlaneDist[i+6][1] += flExtentDotNormal;
		
		// y axis of the OBB + world ith axis
		pPlaneNormal[i+9].Init( matOBBToWorld[i][2], 0.0f, -matOBBToWorld[i][0] );
		ComputeSupportMap( pPlaneNormal[i+9], 0, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+9] );
		flExtentDotNormal = 
			FloatMakePositive( pMatRow0[1] ) * flRayExtent0 +
			FloatMakePositive( pMatRow1[1] ) * flRayExtent1;
		ppPlaneDist[i+9][0] -= flExtentDotNormal;
		ppPlaneDist[i+9][1] += flExtentDotNormal;

		// z axis of the OBB + world ith axis
		pPlaneNormal[i+12].Init( -matOBBToWorld[i][1], matOBBToWorld[i][0], 0.0f );
		ComputeSupportMap( pPlaneNormal[i+12], 0, 1, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+12] );
		flExtentDotNormal = 
			FloatMakePositive( pMatRow0[2] ) * flRayExtent0 +
			FloatMakePositive( pMatRow1[2] ) * flRayExtent1;
		ppPlaneDist[i+12][0] -= flExtentDotNormal;
		ppPlaneDist[i+12][1] += flExtentDotNormal;
	}

	float enterfrac, leavefrac;
	float d1[2], d2[2];
	float f;

	int hitplane = -1;
	int hitside = -1;
	enterfrac = -1.0f;
	leavefrac = 1.0f;

	pTrace->startsolid = true;

	Vector vecLocalRayEnd;
	VectorAdd( vecLocalRayOrigin, vecLocalRayDirection, vecLocalRayEnd );

	for ( i = 0; i < 15; ++i )
	{
		// FIXME: Not particularly optimal since there's a lot of 0's in the plane normals
		float flStartDot = DotProduct( pPlaneNormal[i], vecLocalRayOrigin );
		float flEndDot = DotProduct( pPlaneNormal[i], vecLocalRayEnd );

		// NOTE: Negative here is because the plane normal + dist 
		// are defined in negative terms for the far plane (plane dist index 0)
		d1[0] = -(flStartDot - ppPlaneDist[i][0]);
		d2[0] = -(flEndDot - ppPlaneDist[i][0]);

		d1[1] = flStartDot - ppPlaneDist[i][1];
		d2[1] = flEndDot - ppPlaneDist[i][1];

		int j;
		for ( j = 0; j < 2; ++j )
		{
			// if completely in front near plane or behind far plane no intersection
			if (d1[j] > 0 && d2[j] > 0)
				return false;

			// completely inside, check next plane set
			if (d1[j] <= 0 && d2[j] <= 0)
				continue;

			if (d1[j] > 0)
			{
				pTrace->startsolid = false;
			}

			// crosses face
			float flDenom = 1.0f / (d1[j] - d2[j]);
			if (d1[j] > d2[j])
			{
				f = d1[j] - flTolerance;
				if ( f < 0 )
				{
					f = 0;
				}
				f *= flDenom;
				if (f > enterfrac)
				{
					enterfrac = f;
					hitplane = i;
					hitside = j;
				}
			}
			else
			{ 
				// leave
				f = (d1[j] + flTolerance) * flDenom;
				if (f < leavefrac)
				{
					leavefrac = f;
				}
			}
		}
	}

	if (enterfrac < leavefrac && enterfrac >= 0.0f)
	{
		pTrace->fraction = enterfrac;
		VectorMA( pTrace->startpos, enterfrac, ray.m_Delta, pTrace->endpos );
		pTrace->contents = CONTENTS_SOLID;

		// Need to transform the plane into world space...
		cplane_t temp;
		temp.normal = pPlaneNormal[hitplane];
		temp.dist = ppPlaneDist[hitplane][hitside];
		if (hitside == 0)
		{
			temp.normal *= -1.0f;
			temp.dist *= -1.0f;
		}
		temp.type = 3;

		MatrixITransformPlane( matOBBToWorld, temp, pTrace->plane );
		return true;
	}

	if ( pTrace->startsolid )
	{
		pTrace->allsolid = (leavefrac <= 0.0f) || (leavefrac >= 1.0f);
		pTrace->fraction = 0;
		pTrace->endpos = pTrace->startpos;
		pTrace->contents = CONTENTS_SOLID;
		pTrace->plane.dist = pTrace->startpos[0];
		pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f );
		pTrace->plane.type = 0;
		return true;
	}

	return false;
}


//-----------------------------------------------------------------------------
// Intersects a ray against an OBB
//-----------------------------------------------------------------------------
bool IntersectRayWithOBB( const Ray_t &ray, const Vector &vecBoxOrigin, const QAngle &angBoxRotation,
	const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace )
{
	if ( angBoxRotation == vec3_angle )
	{
		Vector vecWorldMins, vecWorldMaxs;
		VectorAdd( vecBoxOrigin, vecOBBMins, vecWorldMins );
		VectorAdd( vecBoxOrigin, vecOBBMaxs, vecWorldMaxs );
		return IntersectRayWithBox( ray, vecWorldMins, vecWorldMaxs, flTolerance, pTrace );
	}

	if ( ray.m_IsRay )
	{
		return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, vecBoxOrigin, angBoxRotation, vecOBBMins, vecOBBMaxs, flTolerance, pTrace );
	}

	matrix3x4_t matOBBToWorld;
	AngleMatrix( angBoxRotation, vecBoxOrigin, matOBBToWorld );
	return IntersectRayWithOBB( ray, matOBBToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace );
}

	
//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------
void GetNonMajorAxes( const Vector &vNormal, Vector2D &axes )
{
	axes[0] = 0;
	axes[1] = 1;

	if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.y ) )
	{
		if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.z ) )
		{
			axes[0] = 1;
			axes[1] = 2;
		}
	}
	else
	{
		if( FloatMakePositive( vNormal.y ) > FloatMakePositive( vNormal.z ) )
		{
			axes[0] = 0;
			axes[1] = 2;
		}
	}
}


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
QuadBarycentricRetval_t QuadWithParallelEdges( const Vector &vecOrigin, 
	const Vector &vecU, float lengthU, const Vector &vecV, float lengthV,
	const Vector &pt, Vector2D &vecUV )
{
	Ray_t rayAxis;
	Ray_t rayPt;

	//
	// handle the u axis
	//
	rayAxis.m_Start = vecOrigin;
	rayAxis.m_Delta = vecU;
	rayAxis.m_IsRay = true;

	rayPt.m_Start = pt;
	rayPt.m_Delta = vecV * -( lengthV * 10.0f );
	rayPt.m_IsRay = true;

	float s, t;
	IntersectRayWithRay( rayAxis, rayPt, t, s );
	vecUV[0] = t / lengthU;

	//
	// handle the v axis
	//
	rayAxis.m_Delta = vecV;

	rayPt.m_Delta = vecU * -( lengthU * 10.0f );

	IntersectRayWithRay( rayAxis, rayPt, t, s );
	vecUV[1] = t / lengthV;

	// inside of the quad??
	if( ( vecUV[0] < 0.0f ) || ( vecUV[0] > 1.0f ) || 
		( vecUV[1] < 0.0f ) || ( vecUV[1] > 1.0f ) )
		return BARY_QUADRATIC_FALSE;

	return BARY_QUADRATIC_TRUE;
}


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void ResolveQuadratic( double tPlus, double tMinus, 
					   const Vector& axisU0, const Vector& axisU1,
					   const Vector& axisV0, const Vector& axisV1,
					   const Vector& axisOrigin, const Vector& pt,
					   int projU, double &s, double &t )
{
	// calculate the sPlus, sMinus pair(s)
	double sDenomPlus = ( axisU0[projU] * ( 1 - tPlus ) ) + ( axisU1[projU] * tPlus );
	double sDenomMinus = ( axisU0[projU] * ( 1 - tMinus ) ) + ( axisU1[projU] * tMinus );

	double sPlus = UNINIT, sMinus = UNINIT;
	if( FloatMakePositive( sDenomPlus ) >= 1e-5 )
	{
		sPlus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tPlus ) ) / sDenomPlus; 
	}

	if( FloatMakePositive( sDenomMinus ) >= 1e-5 )
	{
		sMinus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tMinus ) ) / sDenomMinus; 
	}
	
	if( ( tPlus >= 0.0 ) && ( tPlus <= 1.0 ) && ( sPlus >= 0.0 ) && ( sPlus <= 1.0 ) )
	{
		s = sPlus;
		t = tPlus;
		return;
	}

	if( ( tMinus >= 0.0 ) && ( tMinus <= 1.0 ) && ( sMinus >= 0.0 ) && ( sMinus <= 1.0 ) )
	{
		s = sMinus;
		t = tMinus;
		return;
	}

	double s0, t0, s1, t1;

	s0 = sPlus;
	t0 = tPlus;
	if( s0 >= 1.0 ) { s0 -= 1.0; }
	if( t0 >= 1.0 ) { t0 -= 1.0; }

	s1 = sMinus;
	t1 = tMinus;
	if( s1 >= 1.0 ) { s1 -= 1.0; }
	if( t1 >= 1.0 ) { t1 -= 1.0; }

	s0 = FloatMakePositive( s0 );
	t0 = FloatMakePositive( t0 );
	s1 = FloatMakePositive( s1 );
	t1 = FloatMakePositive( t1 );

	double max0, max1;
	max0 = s0;
	if( t0 > max0 ) { max0 = t0; }
	max1 = s1;
	if( t1 > max1 ) { max1 = t1; }

	if( max0 > max1 )
	{
		s = sMinus;
		t = tMinus;
	}
	else
	{
		s = sPlus;
		t = tPlus;
	}
}


//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------

QuadBarycentricRetval_t PointInQuadToBarycentric( const Vector &v1, const Vector &v2, 
	const Vector &v3, const Vector &v4, const Vector &point, Vector2D &uv )
{
#define PIQ_TEXTURE_EPSILON		0.001
#define PIQ_PLANE_EPSILON		0.1
#define PIQ_DOT_EPSILON			0.99f

	//
	// Think of a quad with points v1, v2, v3, v4 and u, v line segments
	// u0 = v2 - v1
	// u1 = v3 - v4
	// v0 = v4 - v1
	// v1 = v3 - v2
	//
	Vector axisU[2], axisV[2];
	Vector axisUNorm[2], axisVNorm[2];
	axisU[0] = axisUNorm[0] = v2 - v1;
	axisU[1] = axisUNorm[1] = v3 - v4;
	axisV[0] = axisVNorm[0] = v4 - v1;
	axisV[1] = axisVNorm[1] = v3 - v2;

	float lengthU[2], lengthV[2];
	lengthU[0] = VectorNormalize( axisUNorm[0] );
	lengthU[1] = VectorNormalize( axisUNorm[1] );
	lengthV[0] = VectorNormalize( axisVNorm[0] );
	lengthV[1] = VectorNormalize( axisVNorm[1] );

	//
	// check for an early out - parallel opposite edges!
	// NOTE: quad property if 1 set of opposite edges is parallel and equal
	//       in length, then the other set of edges is as well
	//
	if( axisUNorm[0].Dot( axisUNorm[1] ) > PIQ_DOT_EPSILON )
	{
		if( FloatMakePositive( lengthU[0] - lengthU[1] ) < PIQ_PLANE_EPSILON )
		{
			return QuadWithParallelEdges( v1, axisUNorm[0], lengthU[0], axisVNorm[0], lengthV[0], point, uv );
		}
	}

	//
	// since we are solving for s in our equations below we need to ensure that
	// the v axes are non-parallel
	//
	bool bFlipped = false;
	if( axisVNorm[0].Dot( axisVNorm[1] ) > PIQ_DOT_EPSILON )
	{
		Vector tmp[2];
		tmp[0] = axisV[0];
		tmp[1] = axisV[1];
		axisV[0] = axisU[0];
		axisV[1] = axisU[1];
		axisU[0] = tmp[0];
		axisU[1] = tmp[1];
		bFlipped = true;
	}

	//
	// get the "projection" axes
	//
	Vector2D projAxes;
	Vector vNormal = axisU[0].Cross( axisV[0] );
	GetNonMajorAxes( vNormal, projAxes );

	//
	// NOTE: axisU[0][projAxes[0]] < axisU[0][projAxes[1]], 
	//       this is done to decrease error when dividing later
	//
	if( FloatMakePositive( axisU[0][projAxes[0]] ) < FloatMakePositive( axisU[0][projAxes[1]] ) )
	{
		int tmp = projAxes[0];
		projAxes[0] = projAxes[1];
		projAxes[1] = tmp;
	}

	// Here's how we got these equations:
	//
	// Given the points and u,v line segments above...
	//
	// Then:
	//
	// (1.0) PT = P0 + U0 * s + V * t
	//
	// where
	//
	// (1.1) V = V0 + s * (V1 - V0)
	// (1.2) U = U0 + t * (U1 - U0)
	// 
	// Therefore (from 1.1 + 1.0):
	// PT - P0 = U0 * s + (V0 + s * (V1-V0)) * t
	// Group s's:
	// PT - P0 - t * V0 = s * (U0 + t * (V1-V0))
	// Two equations and two unknowns in x and y get you the following quadratic:
	//
	// solve the quadratic
	//
	double s = 0.0, t = 0.0;
	double A, negB, C;

	A = ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - 
		( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] ) - 
		( axisU[1][projAxes[1]] * axisV[0][projAxes[0]] ) + 
		( axisU[1][projAxes[0]] * axisV[0][projAxes[1]] );
	C = ( v1[projAxes[1]] * axisU[0][projAxes[0]] ) - 
		( point[projAxes[1]] * axisU[0][projAxes[0]] ) - 
		( v1[projAxes[0]] * axisU[0][projAxes[1]] ) + 
		( point[projAxes[0]] * axisU[0][projAxes[1]] );
	negB = C - 
		  ( v1[projAxes[1]] * axisU[1][projAxes[0]] ) + 
		  ( point[projAxes[1]] * axisU[1][projAxes[0]] ) + 
		  ( v1[projAxes[0]] * axisU[1][projAxes[1]] ) - 
		  ( point[projAxes[0]] * axisU[1][projAxes[1]] ) + 
		  ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - 
		  ( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] );

	if( ( A > -PIQ_PLANE_EPSILON ) && ( A < PIQ_PLANE_EPSILON ) )
	{
		// shouldn't be here -- this should have been take care of in the "early out"
//		Assert( 0 );

		Vector vecUAvg, vecVAvg;
		vecUAvg = ( axisUNorm[0] + axisUNorm[1] ) * 0.5f;
		vecVAvg = ( axisVNorm[0] + axisVNorm[1] ) * 0.5f;
		
		float fLengthUAvg = ( lengthU[0] + lengthU[1] ) * 0.5f;
		float fLengthVAvg = ( lengthV[0] + lengthV[1] ) * 0.5f;

		return QuadWithParallelEdges( v1, vecUAvg, fLengthUAvg, vecVAvg, fLengthVAvg, point, uv );

#if 0
		// legacy code -- kept here for completeness!

		// not a quadratic -- solve linearly
		t = C / negB;

		// See (1.2) above
		float ui = axisU[0][projAxes[0]] + t * ( axisU[1][projAxes[0]] - axisU[0][projAxes[0]] );
		if( FloatMakePositive( ui ) >= 1e-5 )
		{
			// See (1.0) above
			s = ( point[projAxes[0]] - v1[projAxes[0]] - axisV[0][projAxes[0]] * t ) / ui;
		}
#endif
	}
	else
	{
		// (-b +/- sqrt( b^2 - 4ac )) / 2a
		double discriminant = (negB*negB) - (4.0f * A * C);
		if( discriminant < 0.0f )
		{
			uv[0] = -99999.0f;
			uv[1] = -99999.0f;
			return BARY_QUADRATIC_NEGATIVE_DISCRIMINANT;
		}

		double quad = sqrt( discriminant );
		double QPlus = ( negB + quad ) / ( 2.0f * A );
		double QMinus = ( negB - quad ) / ( 2.0f * A );

		ResolveQuadratic( QPlus, QMinus, axisU[0], axisU[1], axisV[0], axisV[1], v1, point, projAxes[0], s, t );
	}

	if( !bFlipped )
	{
		uv[0] = ( float )s;
		uv[1] = ( float )t;
	}
	else
	{
		uv[0] = ( float )t;
		uv[1] = ( float )s;
	}

	// inside of the quad??
	if( ( uv[0] < 0.0f ) || ( uv[0] > 1.0f ) || ( uv[1] < 0.0f ) || ( uv[1] > 1.0f ) )
		return BARY_QUADRATIC_FALSE;
	
	return BARY_QUADRATIC_TRUE;

#undef PIQ_TEXTURE_EPSILON
#undef PIQ_PLANE_EPSILON
}


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void PointInQuadFromBarycentric( const Vector &v1, const Vector &v2, const Vector &v3, const Vector &v4,
								 const Vector2D &uv, Vector &point )
{
	//
	// Think of a quad with points v1, v2, v3, v4 and u, v line segments
	// find the ray from v0 edge to v1 edge at v
	//
	Vector vPts[2];
	VectorLerp( v1, v4, uv[1], vPts[0] );
	VectorLerp( v2, v3, uv[1], vPts[1] );
	VectorLerp( vPts[0], vPts[1], uv[0], point ); 
}


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void TexCoordInQuadFromBarycentric( const Vector2D &v1, const Vector2D &v2, const Vector2D &v3, const Vector2D &v4,
								    const Vector2D &uv, Vector2D &texCoord )
{
	//
	// Think of a quad with points v1, v2, v3, v4 and u, v line segments
	// find the ray from v0 edge to v1 edge at v
	//
	Vector2D vCoords[2];
	Vector2DLerp( v1, v4, uv[1], vCoords[0] );
	Vector2DLerp( v2, v3, uv[1], vCoords[1] );
	Vector2DLerp( vCoords[0], vCoords[1], uv[0], texCoord ); 
}


//-----------------------------------------------------------------------------
// Compute point from barycentric specification
// Edge u goes from v0 to v1, edge v goes from v0 to v2
//-----------------------------------------------------------------------------
void ComputePointFromBarycentric( const Vector& v0, const Vector& v1, const Vector& v2, 
								 float u, float v, Vector& pt )
{
	Vector edgeU, edgeV;
	VectorSubtract( v1, v0, edgeU );
	VectorSubtract( v2, v0, edgeV );
	VectorMA( v0, u, edgeU, pt );
	VectorMA( pt, v, edgeV, pt );
}

void ComputePointFromBarycentric( const Vector2D& v0, const Vector2D& v1, const Vector2D& v2, 
								 float u, float v, Vector2D& pt )
{
	Vector2D edgeU, edgeV;
	Vector2DSubtract( v1, v0, edgeU );
	Vector2DSubtract( v2, v0, edgeV );
	Vector2DMA( v0, u, edgeU, pt );
	Vector2DMA( pt, v, edgeV, pt );
}


//-----------------------------------------------------------------------------
// Compute a matrix that has the correct orientation but which has an origin at
// the center of the bounds
//-----------------------------------------------------------------------------
static void ComputeCenterMatrix( const Vector& origin, const QAngle& angles, 
	const Vector& mins, const Vector& maxs, matrix3x4_t& matrix )
{
	Vector centroid;
	VectorAdd( mins, maxs, centroid );
	centroid *= 0.5f;
	AngleMatrix( angles, matrix );

	Vector worldCentroid;
	VectorRotate( centroid, matrix, worldCentroid );
	worldCentroid += origin;
	MatrixSetColumn( worldCentroid, 3, matrix );
}

static void ComputeCenterIMatrix( const Vector& origin, const QAngle& angles, 
	const Vector& mins, const Vector& maxs, matrix3x4_t& matrix )
{
	Vector centroid;
	VectorAdd( mins, maxs, centroid );
	centroid *= -0.5f;
	AngleIMatrix( angles, matrix );

	// For the translational component here, note that the origin in world space
	// is T = R * C + O, (R = rotation matrix, C = centroid in local space, O = origin in world space)
	// The IMatrix translation = - transpose(R) * T = -C - transpose(R) * 0
	Vector localOrigin;
	VectorRotate( origin, matrix, localOrigin );
	centroid -= localOrigin;
	MatrixSetColumn( centroid, 3, matrix );
}


//-----------------------------------------------------------------------------
// Compute a matrix which is the absolute value of another
//-----------------------------------------------------------------------------
static inline void ComputeAbsMatrix( const matrix3x4_t& in, matrix3x4_t& out )
{
	FloatBits(out[0][0]) = FloatAbsBits(in[0][0]);
	FloatBits(out[0][1]) = FloatAbsBits(in[0][1]);
	FloatBits(out[0][2]) = FloatAbsBits(in[0][2]);
	FloatBits(out[1][0]) = FloatAbsBits(in[1][0]);
	FloatBits(out[1][1]) = FloatAbsBits(in[1][1]);
	FloatBits(out[1][2]) = FloatAbsBits(in[1][2]);
	FloatBits(out[2][0]) = FloatAbsBits(in[2][0]);
	FloatBits(out[2][1]) = FloatAbsBits(in[2][1]);
	FloatBits(out[2][2]) = FloatAbsBits(in[2][2]);
}


//-----------------------------------------------------------------------------
// Compute a separating plane between two boxes (expensive!)
// Returns false if no separating plane exists
//-----------------------------------------------------------------------------
static bool ComputeSeparatingPlane( const matrix3x4_t &worldToBox1, const matrix3x4_t &box2ToWorld, 
	const Vector& box1Size, const Vector& box2Size, float tolerance, cplane_t* pPlane )
{
	// The various separating planes can be either
	// 1) A plane parallel to one of the box face planes
	// 2) A plane parallel to the cross-product of an edge from each box

	// First, compute the basis of second box in the space of the first box
	// NOTE: These basis place the origin at the centroid of each box!
	matrix3x4_t	box2ToBox1;
	ConcatTransforms( worldToBox1, box2ToWorld, box2ToBox1 );

	// We're going to be using the origin of box2 in the space of box1 alot,
	// lets extract it from the matrix....
	Vector box2Origin;
	MatrixGetColumn( box2ToBox1, 3, box2Origin );

	// Next get the absolute values of these entries and store in absbox2ToBox1.
	matrix3x4_t absBox2ToBox1;
	ComputeAbsMatrix( box2ToBox1, absBox2ToBox1 );

	// There are 15 tests to make.  The first 3 involve trying planes parallel
	// to the faces of the first box.

	// NOTE: The algorithm here involves finding the projections of the two boxes
	// onto a particular line. If the projections on the line do not overlap,
	// that means that there's a plane perpendicular to the line which separates 
	// the two boxes; and we've therefore found a separating plane.

	// The way we check for overlay is we find the projections of the two boxes
	// onto the line, and add them up. We compare the sum with the projection
	// of the relative center of box2 onto the same line.

	Vector tmp;
	float boxProjectionSum;
	float originProjection;
	
	// NOTE: For these guys, we're taking advantage of the fact that the ith
	// row of the box2ToBox1 is the direction of the box1 (x,y,z)-axis
	// transformed into the space of box2.

	// First side of box 1
	boxProjectionSum = box1Size.x + MatrixRowDotProduct( absBox2ToBox1, 0, box2Size );
	originProjection = FloatMakePositive( box2Origin.x ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		VectorCopy( worldToBox1[0], pPlane->normal.Base() );
		return true;
	}
	
	// Second side of box 1
	boxProjectionSum = box1Size.y + MatrixRowDotProduct( absBox2ToBox1, 1, box2Size );
	originProjection = FloatMakePositive( box2Origin.y ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		VectorCopy( worldToBox1[1], pPlane->normal.Base() );
		return true;
	}
	
	// Third side of box 1
	boxProjectionSum = box1Size.z + MatrixRowDotProduct( absBox2ToBox1, 2, box2Size );
	originProjection = FloatMakePositive( box2Origin.z ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		VectorCopy( worldToBox1[2], pPlane->normal.Base() );
		return true;
	}
	
	// The next three involve checking splitting planes parallel to the
	// faces of the second box.

	// NOTE: For these guys, we're taking advantage of the fact that the 0th
	// column of the box2ToBox1 is the direction of the box2 x-axis
	// transformed into the space of box1.
	// Here, we're determining the distance of box2's center from box1's center
	// by projecting it onto a line parallel to box2's axis
	
	// First side of box 2
	boxProjectionSum = box2Size.x +	MatrixColumnDotProduct( absBox2ToBox1, 0, box1Size );
	originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 0, box2Origin ) ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		MatrixGetColumn( box2ToWorld, 0, pPlane->normal );	
		return true;
	}
	
	// Second side of box 2
	boxProjectionSum = box2Size.y +	MatrixColumnDotProduct( absBox2ToBox1, 1, box1Size );
	originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 1, box2Origin ) ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		MatrixGetColumn( box2ToWorld, 1, pPlane->normal );	
		return true;
	}
	
	// Third side of box 2
	boxProjectionSum = box2Size.z +	MatrixColumnDotProduct( absBox2ToBox1, 2, box1Size );
	originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 2, box2Origin ) ) + tolerance;
	if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
	{
		MatrixGetColumn( box2ToWorld, 2, pPlane->normal );	
		return true;
	}
	
	// Next check the splitting planes which are orthogonal to the pairs
	// of edges, one from box1 and one from box2.  As only direction matters,
	// there are 9 pairs since each box has 3 distinct edge directions.
	
	// Here, we take advantage of the fact that the edges from box 1 are all
	// axis aligned; therefore the crossproducts are simplified. Let's walk through
	// the example of b1e1 x b2e1:

	// In this example, the line to check is perpendicular to b1e1 + b2e2
	// we can compute this line by taking the cross-product:
	//
	// [  i  j  k ]
	// [  1  0  0 ] = - ez j + ey k = l1
	// [ ex ey ez ]

	// Where ex, ey, ez is the components of box2's x axis in the space of box 1,
	// which is == to the 0th column of of box2toBox1

	// The projection of box1 onto this line = the absolute dot product of the box size
	// against the line, which =
	// AbsDot( box1Size, l1 ) = abs( -ez * box1.y ) + abs( ey * box1.z )

	// To compute the projection of box2 onto this line, we'll do it in the space of box 2
	//
	// [  i  j  k ]
	// [ fx fy fz ] = fz j - fy k = l2
	// [  1  0  0 ]

	// Where fx, fy, fz is the components of box1's x axis in the space of box 2,
	// which is == to the 0th row of of box2toBox1

	// The projection of box2 onto this line = the absolute dot product of the box size
	// against the line, which =
	// AbsDot( box2Size, l2 ) = abs( fz * box2.y ) + abs ( fy * box2.z )

	// The projection of the relative origin position on this line is done in the 
	// space of box 1:
	//
	// originProjection = DotProduct( <-ez j + ey k>, box2Origin ) =
	//		-ez * box2Origin.y + ey * box2Origin.z

	// NOTE: These checks can be bogus if both edges are parallel. The if
	// checks at the beginning of each block are designed to catch that case
	
	// b1e1 x b2e1
	if ( absBox2ToBox1[0][0] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.y * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[1][0] +
			box2Size.y * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][1];
		originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][0] + box2Origin.z * box2ToBox1[1][0] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 0, tmp );	
			CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e1 x b2e2
	if ( absBox2ToBox1[0][1] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.y * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[1][1] +
			box2Size.x * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][0];
		originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][1] + box2Origin.z * box2ToBox1[1][1] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 1, tmp );	
			CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e1 x b2e3
	if ( absBox2ToBox1[0][2] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.y * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[1][2] +
			box2Size.x * absBox2ToBox1[0][1] + box2Size.y * absBox2ToBox1[0][0];
		originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][2] + box2Origin.z * box2ToBox1[1][2] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 2, tmp );	
			CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e2 x b2e1
	if ( absBox2ToBox1[1][0] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[0][0] +
			box2Size.y * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][1];
		originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][0] - box2Origin.z * box2ToBox1[0][0] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 0, tmp );	
			CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e2 x b2e2
	if ( absBox2ToBox1[1][1] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[0][1] +
			box2Size.x * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][0];
		originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][1] - box2Origin.z * box2ToBox1[0][1] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 1, tmp );	
			CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e2 x b2e3
	if ( absBox2ToBox1[1][2] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[0][2] +
			box2Size.x * absBox2ToBox1[1][1] + box2Size.y * absBox2ToBox1[1][0];
		originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][2] - box2Origin.z * box2ToBox1[0][2] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 2, tmp );	
			CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e3 x b2e1
	if ( absBox2ToBox1[2][0] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[1][0] + box1Size.y * absBox2ToBox1[0][0] +
			box2Size.y * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][1];
		originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][0] + box2Origin.y * box2ToBox1[0][0] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 0, tmp );	
			CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e3 x b2e2
	if ( absBox2ToBox1[2][1] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[1][1] + box1Size.y * absBox2ToBox1[0][1] +
			box2Size.x * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][0];
		originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][1] + box2Origin.y * box2ToBox1[0][1] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 1, tmp );	
			CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}

	// b1e3 x b2e3
	if ( absBox2ToBox1[2][2] < 1.0f - 1e-3f )
	{
		boxProjectionSum =
			box1Size.x * absBox2ToBox1[1][2] + box1Size.y * absBox2ToBox1[0][2] +
			box2Size.x * absBox2ToBox1[2][1] + box2Size.y * absBox2ToBox1[2][0];
		originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][2] + box2Origin.y * box2ToBox1[0][2] ) + tolerance;
		if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) )
		{
			MatrixGetColumn( box2ToWorld, 2, tmp );	
			CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); 
			return true;
		}
	}
	return false;
}


//-----------------------------------------------------------------------------
// Compute a separating plane between two boxes (expensive!)
// Returns false if no separating plane exists
//-----------------------------------------------------------------------------
bool ComputeSeparatingPlane( const Vector& org1, const QAngle& angles1, const Vector& min1, const Vector& max1, 
	const Vector& org2, const QAngle& angles2, const Vector& min2, const Vector& max2, 
	float tolerance, cplane_t* pPlane )
{
	matrix3x4_t	worldToBox1, box2ToWorld;
	ComputeCenterIMatrix( org1, angles1, min1, max1, worldToBox1 );
	ComputeCenterMatrix( org2, angles2, min2, max2, box2ToWorld );

	// Then compute the size of the two boxes
	Vector box1Size, box2Size;
	VectorSubtract( max1, min1, box1Size );
	VectorSubtract( max2, min2, box2Size );
	box1Size *= 0.5f;
	box2Size *= 0.5f;

	return ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, tolerance, pPlane );
}


//-----------------------------------------------------------------------------
// Swept OBB test
//-----------------------------------------------------------------------------
bool IsRayIntersectingOBB( const Ray_t &ray, const Vector& org, const QAngle& angles, 
						  const Vector& mins, const Vector& maxs )
{
	if ( angles == vec3_angle )
	{
		Vector vecWorldMins, vecWorldMaxs;
		VectorAdd( org, mins, vecWorldMins );
		VectorAdd( org, maxs, vecWorldMaxs );
		return IsBoxIntersectingRay( vecWorldMins, vecWorldMaxs, ray ); 
	}

	if ( ray.m_IsRay )
	{
		matrix3x4_t worldToBox;
		AngleIMatrix( angles, org, worldToBox );

		Ray_t rotatedRay;
		VectorTransform( ray.m_Start, worldToBox, rotatedRay.m_Start );
		VectorRotate( ray.m_Delta, worldToBox, rotatedRay.m_Delta );
		rotatedRay.m_StartOffset = vec3_origin;
		rotatedRay.m_Extents = vec3_origin;
		rotatedRay.m_IsRay = ray.m_IsRay;
		rotatedRay.m_IsSwept = ray.m_IsSwept;

		return IsBoxIntersectingRay( mins, maxs, rotatedRay ); 
	}

	if ( !ray.m_IsSwept )
	{
		cplane_t plane;
		return ComputeSeparatingPlane( ray.m_Start, vec3_angle, -ray.m_Extents, ray.m_Extents,
			org, angles, mins, maxs, 0.0f, &plane ) == false;
	}

	// NOTE: See the comments in ComputeSeparatingPlane to understand this math

	// First, compute the basis of box in the space of the ray
	// NOTE: These basis place the origin at the centroid of each box!
	matrix3x4_t	worldToBox1, box2ToWorld;
	ComputeCenterMatrix( org, angles, mins, maxs, box2ToWorld );

	// Find the center + extents of an AABB surrounding the ray
	Vector vecRayCenter;
	VectorMA( ray.m_Start, 0.5, ray.m_Delta, vecRayCenter );
	vecRayCenter *= -1.0f;
	SetIdentityMatrix( worldToBox1 );
	MatrixSetColumn( vecRayCenter, 3, worldToBox1 );

	Vector box1Size;
	box1Size.x = ray.m_Extents.x + FloatMakePositive( ray.m_Delta.x ) * 0.5f;
	box1Size.y = ray.m_Extents.y + FloatMakePositive( ray.m_Delta.y ) * 0.5f;
	box1Size.z = ray.m_Extents.z + FloatMakePositive( ray.m_Delta.z ) * 0.5f;

	// Then compute the size of the box
	Vector box2Size;
	VectorSubtract( maxs, mins, box2Size );
	box2Size *= 0.5f;

	// Do an OBB test of the box with the AABB surrounding the ray
	cplane_t plane;
	if ( ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, 0.0f, &plane ) )
		return false;

	// Now deal with the planes which are the cross products of the ray sweep direction vs box edges
	Vector vecRayDirection = ray.m_Delta;
	VectorNormalize( vecRayDirection );

	// Need a vector between ray center vs box center measured in the space of the ray (world)
	Vector vecCenterDelta;
	vecCenterDelta.x = box2ToWorld[0][3] - ray.m_Start.x;
	vecCenterDelta.y = box2ToWorld[1][3] - ray.m_Start.y;
	vecCenterDelta.z = box2ToWorld[2][3] - ray.m_Start.z;

	// Rotate the ray direction into the space of the OBB
	Vector vecAbsRayDirBox2;
	VectorIRotate( vecRayDirection, box2ToWorld, vecAbsRayDirBox2 );

	// Make abs versions of the ray in world space + ray in box2 space
	VectorAbs( vecAbsRayDirBox2, vecAbsRayDirBox2 );

	// Now do the work for the planes which are perpendicular to the edges of the AABB
	// and the sweep direction edges...

	// In this example, the line to check is perpendicular to box edge x + ray delta
	// we can compute this line by taking the cross-product:
	//
	// [  i  j  k ]
	// [  1  0  0 ] = - dz j + dy k = l1
	// [ dx dy dz ]

	// Where dx, dy, dz is the ray delta (normalized)

	// The projection of the box onto this line = the absolute dot product of the box size
	// against the line, which =
	// AbsDot( vecBoxHalfDiagonal, l1 ) = abs( -dz * vecBoxHalfDiagonal.y ) + abs( dy * vecBoxHalfDiagonal.z )

	// Because the plane contains the sweep direction, the sweep will produce
	// no extra projection onto the line normal to the plane. 
	// Therefore all we need to do is project the ray extents onto this line also:
	// AbsDot( ray.m_Extents, l1 ) = abs( -dz * ray.m_Extents.y ) + abs( dy * ray.m_Extents.z )

	Vector vecPlaneNormal;

	// box x x ray delta
	CrossProduct( vecRayDirection, Vector( box2ToWorld[0][0], box2ToWorld[1][0], box2ToWorld[2][0] ), vecPlaneNormal );
	float flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) );
	float flBoxProjectionSum = 
		vecAbsRayDirBox2.z * box2Size.y + vecAbsRayDirBox2.y * box2Size.z +
		DotProductAbs( vecPlaneNormal, ray.m_Extents );
	if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) )
		return false;

	// box y x ray delta
	CrossProduct( vecRayDirection, Vector( box2ToWorld[0][1], box2ToWorld[1][1], box2ToWorld[2][1] ), vecPlaneNormal );
	flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) );
	flBoxProjectionSum = 
		vecAbsRayDirBox2.z * box2Size.x + vecAbsRayDirBox2.x * box2Size.z +
		DotProductAbs( vecPlaneNormal, ray.m_Extents );
	if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) )
		return false;

	// box z x ray delta
	CrossProduct( vecRayDirection, Vector( box2ToWorld[0][2], box2ToWorld[1][2], box2ToWorld[2][2] ), vecPlaneNormal );
	flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) );
	flBoxProjectionSum = 
		vecAbsRayDirBox2.y * box2Size.x + vecAbsRayDirBox2.x * box2Size.y +
		DotProductAbs( vecPlaneNormal, ray.m_Extents );
	if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) )
		return false;
	
	return true;
}

//--------------------------------------------------------------------------
// Purpose:
//
// NOTE:
//	triangle points are given in clockwise order (aabb-triangle test)
//
//    1				edge0 = 1 - 0
//    | \           edge1 = 2 - 1
//    |  \          edge2 = 0 - 2
//    |   \			.
//    |    \		.
//    0-----2		.
//
//--------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// Purpose: find the minima and maxima of the 3 given values
//-----------------------------------------------------------------------------
inline void FindMinMax( float v1, float v2, float v3, float &min, float &max )
{
	min = max = v1;
	if ( v2 < min ) { min = v2; }
	if ( v2 > max ) { max = v2; }
	if ( v3 < min ) { min = v3; }
	if ( v3 > max ) { max = v3; }
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
inline bool AxisTestEdgeCrossX2( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY,
							     const Vector &p1, const Vector &p3, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y
	// Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z
	float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z;
	float flDist3 = flEdgeZ * p3.y - flEdgeY * p3.z;

	// Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z
	float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z;

	// Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist1 < flDist3 )
	{
		if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) )
			return false;
	}
	else
	{
		if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//--------------------------------------------------------------------------
// Purpose:
//--------------------------------------------------------------------------
inline bool AxisTestEdgeCrossX3( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY,
								 const Vector &p1, const Vector &p2, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y 
	// Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z
	float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z;
	float flDist2 = flEdgeZ * p2.y - flEdgeY * p2.z;

	// Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z
	float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z;

	// Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist1 < flDist2 )
	{
		if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) )
			return false;
	}
	else
	{
		if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
inline bool AxisTestEdgeCrossY2( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX,
								 const Vector &p1, const Vector &p3, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x
	// Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z
	float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z;
	float flDist3 = -flEdgeZ * p3.x + flEdgeX * p3.z;

	// Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z
	float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z;

	// Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist1 < flDist3 )
	{
		if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) )
			return false;
	}
	else
	{
		if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
inline bool AxisTestEdgeCrossY3( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX,
								 const Vector &p1, const Vector &p2, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x
	// Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z
	float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z;
	float flDist2 = -flEdgeZ * p2.x + flEdgeX * p2.z;

	// Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z
	float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z;

	// Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist1 < flDist2 )
	{
		if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) )
			return false;
	}
	else
	{
		if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
inline bool AxisTestEdgeCrossZ1( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX,
								 const Vector &p2, const Vector &p3, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f
	// Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y
	float flDist2 = flEdgeY * p2.x - flEdgeX * p2.y;
	float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y;

	// Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y
	float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; 

	// Either dist2, dist3 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist3 < flDist2 )
	{
		if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) 
			return false;
	}
	else
	{
		if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
inline bool AxisTestEdgeCrossZ2( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX,
								 const Vector &p1, const Vector &p3, const Vector &vecExtents,
								 float flTolerance )
{
	// Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f
	// Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y
	float flDist1 = flEdgeY * p1.x - flEdgeX * p1.y;
	float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y;

	// Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y
	float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; 

	// Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB).
	if ( flDist1 < flDist3 )
	{
		if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) 
			return false;
	}
	else
	{
		if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) )
			return false;
	}

	return true;
}

//-----------------------------------------------------------------------------
// Purpose: Test for an intersection (overlap) between an axial-aligned bounding 
//          box (AABB) and a triangle.
//
// Using the "Separating-Axis Theorem" to test for intersections between
// a triangle and an axial-aligned bounding box (AABB).
// 1. 3 Axis Planes - x, y, z
// 2. 9 Edge Planes Tests - the 3 edges of the triangle crossed with all 3 axial 
//                          planes (x, y, z)
// 3. 1 Face Plane - the triangle plane (cplane_t plane below)
// Output: false = separating axis (no intersection)
//         true = intersection
//-----------------------------------------------------------------------------
bool IsBoxIntersectingTriangle( const Vector &vecBoxCenter, const Vector &vecBoxExtents,
						        const Vector &v1, const Vector &v2, const Vector &v3,
						        const cplane_t &plane, float flTolerance )
{
	// Test the axial planes (x,y,z) against the min, max of the triangle.
	float flMin, flMax;
	Vector p1, p2, p3;

	// x plane
	p1.x = v1.x - vecBoxCenter.x;
	p2.x = v2.x - vecBoxCenter.x;
	p3.x = v3.x - vecBoxCenter.x;
	FindMinMax( p1.x, p2.x, p3.x, flMin, flMax );
	if ( ( flMin > ( vecBoxExtents.x + flTolerance ) ) || ( flMax < -( vecBoxExtents.x + flTolerance ) ) ) 
		return false; 

	// y plane
	p1.y = v1.y - vecBoxCenter.y;
	p2.y = v2.y - vecBoxCenter.y;
	p3.y = v3.y - vecBoxCenter.y;
	FindMinMax( p1.y, p2.y, p3.y, flMin, flMax );
	if ( ( flMin > ( vecBoxExtents.y + flTolerance ) ) || ( flMax < -( vecBoxExtents.y + flTolerance ) ) ) 
		return false; 

	// z plane
	p1.z = v1.z - vecBoxCenter.z;
	p2.z = v2.z - vecBoxCenter.z;
	p3.z = v3.z - vecBoxCenter.z;
	FindMinMax( p1.z, p2.z, p3.z, flMin, flMax );
	if ( ( flMin > ( vecBoxExtents.z + flTolerance ) ) || ( flMax < -( vecBoxExtents.z + flTolerance ) ) ) 
		return false; 

	// Test the 9 edge cases.
	Vector vecEdge, vecAbsEdge;

	// edge 0 (cross x,y,z)
	vecEdge = p2 - p1;
	vecAbsEdge.y = FloatMakePositive( vecEdge.y );
	vecAbsEdge.z = FloatMakePositive( vecEdge.z );
	if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p3, vecBoxExtents, flTolerance ) ) 
		return false; 

	vecAbsEdge.x = FloatMakePositive( vecEdge.x );
	if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) 
		return false; 

	if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) 
		return false; 

	// edge 1 (cross x,y,z)
	vecEdge = p3 - p2;
	vecAbsEdge.y = FloatMakePositive( vecEdge.y );
	vecAbsEdge.z = FloatMakePositive( vecEdge.z );
	if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) 
		return false; 

	vecAbsEdge.x = FloatMakePositive( vecEdge.x );
	if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) 
		return false; 

	if ( !AxisTestEdgeCrossZ2( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) 
		return false; 

	// edge 2 (cross x,y,z)
	vecEdge = p1 - p3;
	vecAbsEdge.y = FloatMakePositive( vecEdge.y );
	vecAbsEdge.z = FloatMakePositive( vecEdge.z );
	if ( !AxisTestEdgeCrossX3( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) 
		return false; 

	vecAbsEdge.x = FloatMakePositive( vecEdge.x );
	if ( !AxisTestEdgeCrossY3( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) 
		return false; 

	if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) 
		return false; 

	// Test against the triangle face plane.
	Vector vecMin, vecMax;
	VectorSubtract( vecBoxCenter, vecBoxExtents, vecMin );
	VectorAdd( vecBoxCenter, vecBoxExtents, vecMax );
	if ( BoxOnPlaneSide( vecMin, vecMax, &plane ) != 3 ) 
		return false; 

	return true;
}

// NOTE: JAY: This is untested code based on Real-time Collision Detection by Ericson
#if 0
Vector CalcClosestPointOnTriangle( const Vector &P, const Vector &v0, const Vector &v1, const Vector &v2 )
{
	Vector e0 = v1 - v0;
	Vector e1 = v2 - v0;
	Vector p0 = P - v0;

	// voronoi region of v0
	float d1 = DotProduct( e0, p0 );
	float d2 = DotProduct( e1, p0 );
	if (d1 <= 0.0f && d2 <= 0.0f)
		return v0;

	// voronoi region of v1
	Vector p1 = P - v1;
	float d3 = DotProduct( e0, p1 );
	float d4 = DotProduct( e1, p1 );
	if (d3 >=0.0f && d4 <= d3)
		return v1;

	// voronoi region of e0 (v0-v1)
	float ve2 = d1*d4 - d3*d2;
	if ( ve2 <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
	{
		float v = d1 / (d1-d3);
		return v0 + v * e0;
	}
	// voronoi region of v2
	Vector p2 = P - v2;
	float d5 = DotProduct( e0, p2 );
	float d6 = DotProduct( e1, p2 );
	if (d6 >= 0.0f && d5 <= d6)
		return v2;
	// voronoi region of e1
	float ve1 = d5*d2 - d1*d6;
	if (ve1 <= 0.0f && d2 >= 0.0f && d6 >= 0.0f)
	{
		float w = d2 / (d2-d6);
		return v0 + w * e1;
	}
	// voronoi region on e2
	float ve0 = d3*d6 - d5*d4;
	if ( ve0 <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
	{
		float w = (d4-d3)/((d4-d3) + (d5-d6));
		return v1 + w * (v2-v1);
	}
	// voronoi region of v0v1v2 triangle
	float denom = 1.0f / (ve0+ve1+ve2);
	float v = ve1*denom;
	float w = ve2 * denom;
	return v0 + e0 * v + e1 * w;
}
#endif


bool OBBHasFullyContainedIntersectionWithQuad( const Vector &vOBBExtent1_Scaled, const Vector &vOBBExtent2_Scaled, const Vector &vOBBExtent3_Scaled, const Vector &ptOBBCenter,
											  const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter,
											  const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, 
											  const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length )
{
	Vector ptOBB[8]; //this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's)
	ptOBB[0] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled;
	ptOBB[1] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled;
	ptOBB[2] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled;
	ptOBB[3] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled;
	ptOBB[4] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled;
	ptOBB[5] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled;
	ptOBB[6] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled;
	ptOBB[7] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled;

	float fDists[8];
	for( int i = 0; i != 8; ++i )
		fDists[i] = vQuadNormal.Dot( ptOBB[i] ) - fQuadPlaneDist;

	int iSides[8];
	int iSideMask = 0;
	for( int i = 0; i != 8; ++i )
	{
		if( fDists[i] > 0.0f )
		{
			iSides[i] = 1;
			iSideMask |= 1;
		}
		else
		{
			iSides[i] = 2;
			iSideMask |= 2;
		}
	}

	if( iSideMask != 3 ) //points reside entirely on one side of the quad's plane
		return false;

	Vector ptPlaneIntersections[12]; //only have 12 lines, can only possibly generate 12 split points
	int iPlaneIntersectionsCount = 0;

	for( int i = 0; i != 8; ++i )
	{
		if( iSides[i] == 2 ) //point behind the plane
		{
			int iAxisCrossings[3];
			iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses vOBBExtent1 axis
			iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either vOBBExtent2 or vOBBExtent3 axis, we don't care which
			iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross

			for( int j = 0; j != 3; ++j )
			{
				if( iSides[iAxisCrossings[j]] == 1 ) //point in front of the plane
				{
					//line between ptOBB[i] and ptOBB[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing
					float fTotalDist = fDists[iAxisCrossings[j]] - fDists[i]; //remember that fDists[i] is a negative value
					ptPlaneIntersections[iPlaneIntersectionsCount] = (ptOBB[iAxisCrossings[j]] * (-fDists[i]/fTotalDist)) + (ptOBB[i] * (fDists[iAxisCrossings[j]]/fTotalDist));

					Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane

					++iPlaneIntersectionsCount;
				}
			}
		}
	}

	Assert( iPlaneIntersectionsCount != 0 );

	for( int i = 0; i != iPlaneIntersectionsCount; ++i )
	{
		//these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents
		Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter;

		float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter );
		if( fabs( fExt1Dist ) > fQuadExtent1Length )
			return false; //point is outside boundaries

		//vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads

		float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter );
		if( fabs( fExt2Dist ) > fQuadExtent2Length )
			return false; //point is outside boundaries
	}

	return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries
}

//-----------------------------------------------------------------------------
// Compute if the Ray intersects the quad plane, and whether the entire
// Ray/Quad intersection is contained within the quad itself
//
// False if no intersection exists, or if part of the intersection is
// outside the quad's extents
//-----------------------------------------------------------------------------
bool RayHasFullyContainedIntersectionWithQuad( const Ray_t &ray,
											  const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter,
											  const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, 
											  const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length )
{
	Vector ptPlaneIntersections[(12 + 12 + 8)]; //absolute max possible: 12 lines to connect the start box, 12 more to connect the end box, 8 to connect the boxes to eachother
	
	//8 points to make an AABB, 8 lines to connect each point from it's start to end point along the ray, 8 possible intersections
	int iPlaneIntersectionsCount = 0;

	if( ray.m_IsRay )
	{
		//just 1 line
		if( ray.m_IsSwept )
		{
			Vector ptEndPoints[2];
			ptEndPoints[0] = ray.m_Start;
			ptEndPoints[1] = ptEndPoints[0] + ray.m_Delta;

			int i;
			float fDists[2];
			for( i = 0; i != 2; ++i )
				fDists[i] = vQuadNormal.Dot( ptEndPoints[i] ) - fQuadPlaneDist;
	       
			for( i = 0; i != 2; ++i )
			{
				if( fDists[i] <= 0.0f )
				{
					int j = 1-i;
					if( fDists[j] >= 0.0f )
					{
						float fInvTotalDist = 1.0f / (fDists[j] - fDists[i]); //fDists[i] <= 0, ray is swept so no chance that the denom was 0
						ptPlaneIntersections[0] = (ptEndPoints[i] * (fDists[j] * fInvTotalDist)) - (ptEndPoints[j] * (fDists[i] * fInvTotalDist)); //fDists[i] <= 0 
						Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane
						iPlaneIntersectionsCount = 1;
					}
					else 
					{
						return false;
					}
					break;
				}
			}

			if( i == 2 )
				return false;
		}
		else //not swept, so this is actually a point on quad question
		{
			if( fabs( vQuadNormal.Dot( ray.m_Start ) - fQuadPlaneDist ) < 1e-6 )
			{
				ptPlaneIntersections[0] = ray.m_Start;
				iPlaneIntersectionsCount = 1;
			}
			else
			{
				return false;
			}
		}
	}
	else
	{
		Vector ptEndPoints[2][8];
		//this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's)
		ptEndPoints[0][0] = ray.m_Start; ptEndPoints[0][0].x -= ray.m_Extents.x; ptEndPoints[0][0].y -= ray.m_Extents.y; ptEndPoints[0][0].z -= ray.m_Extents.z;
		ptEndPoints[0][1] = ray.m_Start; ptEndPoints[0][1].x -= ray.m_Extents.x; ptEndPoints[0][1].y -= ray.m_Extents.y; ptEndPoints[0][1].z += ray.m_Extents.z;
		ptEndPoints[0][2] = ray.m_Start; ptEndPoints[0][2].x -= ray.m_Extents.x; ptEndPoints[0][2].y += ray.m_Extents.y; ptEndPoints[0][2].z += ray.m_Extents.z;
		ptEndPoints[0][3] = ray.m_Start; ptEndPoints[0][3].x -= ray.m_Extents.x; ptEndPoints[0][3].y += ray.m_Extents.y; ptEndPoints[0][3].z -= ray.m_Extents.z;
		ptEndPoints[0][4] = ray.m_Start; ptEndPoints[0][4].x += ray.m_Extents.x; ptEndPoints[0][4].y -= ray.m_Extents.y; ptEndPoints[0][4].z -= ray.m_Extents.z;
		ptEndPoints[0][5] = ray.m_Start; ptEndPoints[0][5].x += ray.m_Extents.x; ptEndPoints[0][5].y -= ray.m_Extents.y; ptEndPoints[0][5].z += ray.m_Extents.z;
		ptEndPoints[0][6] = ray.m_Start; ptEndPoints[0][6].x += ray.m_Extents.x; ptEndPoints[0][6].y += ray.m_Extents.y; ptEndPoints[0][6].z += ray.m_Extents.z;
		ptEndPoints[0][7] = ray.m_Start; ptEndPoints[0][7].x += ray.m_Extents.x; ptEndPoints[0][7].y += ray.m_Extents.y; ptEndPoints[0][7].z -= ray.m_Extents.z;

		float fDists[2][8];
		int iSides[2][8];
		int iSideMask[2] = { 0, 0 };
		for( int i = 0; i != 8; ++i )
		{
			fDists[0][i] = vQuadNormal.Dot( ptEndPoints[0][i] ) - fQuadPlaneDist;
			if( fDists[0][i] > 0.0f )
			{
				iSides[0][i] = 1;
				iSideMask[0] |= 1;
			}
			else
			{
				iSides[0][i] = 2;
				iSideMask[0] |= 2;
			}
		}

		if( ray.m_IsSwept )
		{
			for( int i = 0; i != 8; ++i )
				ptEndPoints[1][i] = ptEndPoints[0][i] + ray.m_Delta;

			for( int i = 0; i != 8; ++i )
			{
				fDists[1][i] = vQuadNormal.Dot( ptEndPoints[1][i] ) - fQuadPlaneDist;
				if( fDists[1][i] > 0.0f )
				{
					iSides[1][i] = 1;
					iSideMask[1] |= 1;
				}
				else
				{
					iSides[1][i] = 2;
					iSideMask[1] |= 2;
				}
			}
		}

		if( (iSideMask[0] | iSideMask[1]) != 3 )
		{
			//Assert( (iSideMask[0] | iSideMask[1]) != 2 );
			return false; //all points resides entirely on one side of the quad
		}


		//generate intersections for boxes split by the plane at either end of the ray
		for( int k = 0; k != 2; ++k )
		{
			if( iSideMask[k] == 3 ) //box is split by the plane
			{
				for( int i = 0; i != 8; ++i )
				{
					if( iSides[k][i] == 2 ) //point behind the plane
					{
						int iAxisCrossings[3];
						iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses X axis
						iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either Y or Z axis, we don't care which
						iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross

						for( int j = 0; j != 3; ++j )
						{
							if( iSides[k][iAxisCrossings[j]] == 1 ) //point in front of the plane
							{
								//line between ptEndPoints[i] and ptEndPoints[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing
								float fInvTotalDist = 1.0f / (fDists[k][iAxisCrossings[j]] - fDists[k][i]); //remember that fDists[k][i] is a negative value
								ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[k][iAxisCrossings[j]] * (-fDists[k][i] * fInvTotalDist)) + (ptEndPoints[k][i] * (fDists[k][iAxisCrossings[j]] * fInvTotalDist));

								Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane

								++iPlaneIntersectionsCount;
							}
						}
					}
				}
			}
		}		

		if( ray.m_IsSwept )
		{
			for( int i = 0; i != 8; ++i )
			{
				if( iSides[0][i] != iSides[1][i] )
				{
					int iPosSide, iNegSide;
					if( iSides[0][i] == 1 )
					{
						iPosSide = 0;
						iNegSide = 1;
					}
					else
					{
						iPosSide = 1;
						iNegSide = 0;
					}

					Assert( (fDists[iPosSide][i] >= 0.0f) && (fDists[iNegSide][i] <= 0.0f) );

					float fInvTotalDist = 1.0f / (fDists[iPosSide][i] - fDists[iNegSide][i]); //remember that fDists[iNegSide][i] is a negative value
					ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[iPosSide][i] * (-fDists[iNegSide][i] * fInvTotalDist)) + (ptEndPoints[iNegSide][i] * (fDists[iPosSide][i] * fInvTotalDist));

					Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane

					++iPlaneIntersectionsCount;
				}
			}
		}
	}

	//down here, we should simply have a collection of plane intersections, now we see if they reside within the quad
	Assert( iPlaneIntersectionsCount != 0 );

	for( int i = 0; i != iPlaneIntersectionsCount; ++i )
	{
		//these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents
		Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter;

		float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter );
		if( fabs( fExt1Dist ) > fQuadExtent1Length )
			return false; //point is outside boundaries

		//vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads

		float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter );
		if( fabs( fExt2Dist ) > fQuadExtent2Length )
			return false; //point is outside boundaries
	}

	return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries
}

#endif // !_STATIC_LINKED || _SHARED_LIB