//=======================================================================
//			Copyright XashXT Group 2007 ©
//			cm_trace.c - combined tracing
//=======================================================================

#include "cm_local.h"
#include "basefiles.h"

#define	DIST_EPSILON	(0.03125)			// 1/32 epsilon to keep floating point happy

/*
===============================================================================

CM INTERNAL MATH

===============================================================================
*/
/*
================
RotatePoint
================
*/
void RotatePoint(vec3_t point, const vec3_t matrix[3])
{
	vec3_t	tvec;

	VectorCopy( point, tvec );
	point[0] = DotProduct( matrix[0], tvec );
	point[1] = DotProduct( matrix[1], tvec );
	point[2] = DotProduct( matrix[2], tvec );
}

/*
================
TransposeMatrix
================
*/
void TransposeMatrix( const vec3_t matrix[3], vec3_t transpose[3])
{
	int i, j;

	for (i = 0; i < 3; i++)
		for( j = 0; j < 3; j++)
			transpose[i][j] = matrix[j][i];
}

/*
================
CreateRotationMatrix
================
*/
void CreateRotationMatrix(const vec3_t angles, vec3_t matrix[3])
{
	AngleVectors(angles, matrix[0], matrix[1], matrix[2]);
	VectorNegate( matrix[1], matrix[1] );
}

/*
================
CM_ProjectPointOntoVector
================
*/
void CM_ProjectPointOntoVector( vec3_t point, vec3_t vStart, vec3_t vDir, vec3_t vProj )
{
	vec3_t pVec;

	VectorSubtract( point, vStart, pVec );
	// project onto the directional vector for this segment
	VectorMA( vStart, DotProduct( pVec, vDir ), vDir, vProj );
}

/*
================
CM_DistanceFromLineSquared
================
*/
float CM_DistanceFromLineSquared(vec3_t p, vec3_t lp1, vec3_t lp2, vec3_t dir) {
	vec3_t proj, t;
	int j;

	CM_ProjectPointOntoVector(p, lp1, dir, proj);
	for (j = 0; j < 3; j++)
	{ 
		if ((proj[j] > lp1[j] && proj[j] > lp2[j]) || (proj[j] < lp1[j] && proj[j] < lp2[j]))
			break;
	}
	if (j < 3)
	{
		if (fabs(proj[j] - lp1[j]) < fabs(proj[j] - lp2[j]))
		{
			VectorSubtract(p, lp1, t);
		}
		else
		{
			VectorSubtract(p, lp2, t);
		}
		return VectorLength2(t);
	}
	VectorSubtract(p, proj, t);
	return VectorLength2( t );
}

/*
================
CM_VectorDistanceSquared
================
*/
float CM_VectorDistanceSquared( vec3_t p1, vec3_t p2 )
{
	vec3_t dir;

	VectorSubtract(p2, p1, dir);
	return VectorLength2( dir );
}

/*
================
SquareRootFloat
================
*/
float SquareRootFloat(float number) {
	long i;
	float x, y;
	const float f = 1.5F;

	x = number * 0.5F;
	y  = number;
	i  = * ( long * ) &y;
	i  = 0x5f3759df - ( i >> 1 );
	y  = * ( float * ) &i;
	y  = y * ( f - ( x * y * y ) );
	y  = y * ( f - ( x * y * y ) );
	return number * y;
}

/*
===============================================================================

BOX TRACING

===============================================================================
*/
/*
=============
CM_BoxLeafnums

Fills in a list of all the leafs touched
=============
*/
void CM_BoxLeafnums_r( int nodenum )
{
	cplane_t	*plane;
	cnode_t	*node;
	int	s;

	while( 1 )
	{
		if( nodenum < 0 )
		{
			if( leaf.count >= leaf.maxcount )
			{
				MsgDev(D_WARN, "CM_BoxLeafnums_r: overflow\n");
				return;
			}
			leaf.list[leaf.count++] = -1 - nodenum;
			return;
		}
	
		node = &cm.nodes[nodenum];
		plane = node->plane;
		s = BoxOnPlaneSide( leaf.mins, leaf.maxs, plane );
		if( s == SIDE_BACK ) nodenum = node->children[0];
		else if( s == SIDE_ON ) nodenum = node->children[1];
		else
		{	
			// go down both
			if (leaf.topnode == -1) leaf.topnode = nodenum;
			CM_BoxLeafnums_r (node->children[0]);
			nodenum = node->children[1];
		}
	}
}

int CM_BoxLeafnums_headnode( vec3_t mins, vec3_t maxs, int *list, int listsize, int headnode, int *topnode )
{
	leaf.list = list;
	leaf.count = 0;
	leaf.maxcount = listsize;
	leaf.mins = mins;
	leaf.maxs = maxs;
	leaf.topnode = -1;

	CM_BoxLeafnums_r( headnode );
	if( topnode ) *topnode = leaf.topnode;

	return leaf.count;
}

int CM_BoxLeafnums( vec3_t mins, vec3_t maxs, int *list, int listsize, int *topnode )
{
	return CM_BoxLeafnums_headnode( mins, maxs, list, listsize, cm.bmodels[0].headnode, topnode);
}

/*
================
CM_ClipBoxToBrush
================
*/
void CM_ClipBoxToBrush( vec3_t mins, vec3_t maxs, vec3_t p1, vec3_t p2, trace_t *trace, cbrush_t *brush )
{
	vec3_t		ofs;
	int		i, j;
	bool		getout, startout;
	cbrushside_t	*side, *leadside;
	cplane_t		*plane, *clipplane;
	float		dist, d1, d2, f, enterfrac, leavefrac;

	enterfrac = -1;
	leavefrac = 1;
	clipplane = NULL;

	if(!brush->numsides ) return;

	getout = false;
	startout = false;
	leadside = NULL;

	for( i = 0; i < brush->numsides; i++ )
	{
		side = &cm.brushsides[brush->firstbrushside + i];
		plane = side->plane;

		// FIXME: special case for axial
		if( !maptrace.ispoint )
		{	
			// general box case
			// push the plane out apropriately for mins/maxs
			// FIXME: use signbits into 8 way lookup for each mins/maxs
			for (j = 0; j < 3; j++)
			{
				if(plane->normal[j] < 0) ofs[j] = maxs[j];
				else ofs[j] = mins[j];
			}
			dist = DotProduct( ofs, plane->normal );
			dist = plane->dist - dist;
		}
		else dist = plane->dist; // special point case

		d1 = DotProduct( p1, plane->normal) - dist;
		d2 = DotProduct( p2, plane->normal) - dist;

		if(d2 > 0) getout = true;	// endpoint is not in solid
		if(d1 > 0) startout = true;

		// if completely in front of face, no intersection
		if(d1 > 0 && d2 >= d1) return;
		if(d1 <= 0 && d2 <= 0) continue;

		// crosses face
		if (d1 > d2)
		{	
			// enter
			f = (d1 - DIST_EPSILON) / (d1-d2);
			if (f > enterfrac)
			{
				enterfrac = f;
				clipplane = plane;
				leadside = side;
			}
		}
		else
		{	// leave
			f = (d1 + DIST_EPSILON) / (d1-d2);
			if (f < leavefrac) leavefrac = f;
		}
	}

	if(!startout)
	{	
		// original point was inside brush
		trace->startsolid = true;
		if (!getout) trace->allsolid = true;
		return;
	}
	if( enterfrac < leavefrac )
	{
		if( enterfrac > -1 && enterfrac < trace->fraction )
		{
			if (enterfrac < 0) enterfrac = 0;
			trace->fraction = enterfrac;
			trace->plane = *clipplane;
			trace->surface = leadside->surface;
			trace->contents = brush->contents;
		}
	}
}

/*
================
CM_TestBoxInBrush
================
*/
void CM_TestBoxInBrush ( vec3_t mins, vec3_t maxs, vec3_t p1, trace_t *trace, cbrush_t *brush )
{
	int		i, j;
	cplane_t		*plane;
	vec3_t		ofs;
	float		dist, d1;
	cbrushside_t	*side;

	if(!brush->numsides) return;

	for( i = 0; i < brush->numsides; i++)
	{
		side = &cm.brushsides[brush->firstbrushside + i];
		plane = side->plane;

		// FIXME: special case for axial
		// general box case
		// push the plane out apropriately for mins/maxs
		// FIXME: use signbits into 8 way lookup for each mins/maxs
		for (j = 0; j < 3; j++ )
		{
			if (plane->normal[j] < 0) ofs[j] = maxs[j];
			else ofs[j] = mins[j];
		}
		dist = DotProduct (ofs, plane->normal);
		dist = plane->dist - dist;

		d1 = DotProduct( p1, plane->normal) - dist;

		// if completely in front of face, no intersection
		if(d1 > 0) return;
	}

	// inside this brush
	trace->startsolid = trace->allsolid = true;
	trace->fraction = 0;
	trace->contents = brush->contents;
}

/*
================
CM_TraceToLeaf
================
*/
void CM_TraceToLeaf( int leafnum )
{
	cleaf_t		*leaf;
	cbrush_t		*b;
	int		k, brushnum;
	
	leaf = &cm.leafs[leafnum];
	if(!(leaf->contents & maptrace.contents)) return;

	// trace line against all brushes in the leaf
	for( k = 0; k < leaf->numleafbrushes; k++ )
	{
		brushnum = cm.leafbrushes[leaf->firstleafbrush + k];
		b = &cm.brushes[brushnum];
		// already checked this brush in another leaf
		if( b->checkcount == cm.checkcount ) continue;
		b->checkcount = cm.checkcount;
		if(!(b->contents & maptrace.contents)) continue;
		CM_ClipBoxToBrush(maptrace.mins, maptrace.maxs, maptrace.start, maptrace.end, &maptrace.result, b);
		if(!maptrace.result.fraction) return;
	}
}

/*
================
CM_TestInLeaf
================
*/
void CM_TestInLeaf( int leafnum )
{
	int		k;
	int		brushnum;
	cleaf_t		*leaf;
	cbrush_t		*b;

	leaf = &cm.leafs[leafnum];
	if(!(leaf->contents & maptrace.contents)) return;

	// trace line against all brushes in the leaf
	for( k = 0; k < leaf->numleafbrushes; k++ )
	{
		brushnum = cm.leafbrushes[leaf->firstleafbrush + k];
		b = &cm.brushes[brushnum];
		// already checked this brush in another leaf
		if(b->checkcount == cm.checkcount) continue;
		b->checkcount = cm.checkcount;
		if(!(b->contents & maptrace.contents)) continue;
		CM_TestBoxInBrush(maptrace.mins, maptrace.maxs, maptrace.start, &maptrace.result, b);
		if(!maptrace.result.fraction) return;
	}
}

/*
==================
CM_RecursiveHullCheck
==================
*/
void CM_RecursiveHullCheck( int num, float p1f, float p2f, vec3_t p1, vec3_t p2 )
{
	cnode_t		*node;
	cplane_t		*plane;
	float		t1, t2, offset;
	float		frac, frac2;
	float		midf, idist;
	int		i, side;
	vec3_t		mid;

	if( maptrace.result.fraction <= p1f ) return; // already hit something nearer

	// if < 0, we are in a leaf node
	if( num < 0 )
	{
		CM_TraceToLeaf( -1 - num );
		return;
	}

	// find the point distances to the seperating plane
	// and the offset for the size of the box
	node = cm.nodes + num;
	plane = node->plane;

	if( plane->type < 3 )
	{
		t1 = p1[plane->type] - plane->dist;
		t2 = p2[plane->type] - plane->dist;
		offset = maptrace.extents[plane->type];
	}
	else
	{
		t1 = DotProduct(plane->normal, p1) - plane->dist;
		t2 = DotProduct(plane->normal, p2) - plane->dist;
		if(maptrace.ispoint) offset = 0;
		else offset = fabs(maptrace.extents[0]*plane->normal[0])+fabs(maptrace.extents[1]*plane->normal[1])+fabs(maptrace.extents[2]*plane->normal[2]);
	}

	// see which sides we need to consider
	if(t1 >= offset && t2 >= offset)
	{
		CM_RecursiveHullCheck(node->children[0], p1f, p2f, p1, p2);
		return;
	}
	if(t1 < -offset && t2 < -offset)
	{
		CM_RecursiveHullCheck(node->children[1], p1f, p2f, p1, p2);
		return;
	}

	// put the crosspoint DIST_EPSILON pixels on the near side
	if (t1 < t2)
	{
		idist = 1.0/(t1-t2);
		side = 1;
		frac2 = (t1 + offset + DIST_EPSILON)*idist;
		frac = (t1 - offset + DIST_EPSILON)*idist;
	}
	else if (t1 > t2)
	{
		idist = 1.0/(t1-t2);
		side = 0;
		frac2 = (t1 - offset - DIST_EPSILON)*idist;
		frac = (t1 + offset + DIST_EPSILON)*idist;
	}
	else
	{
		side = 0;
		frac = 1;
		frac2 = 0;
	}

	// move up to the node
	if (frac < 0) frac = 0;
	if (frac > 1) frac = 1;
		
	midf = p1f + (p2f - p1f) * frac;
	for(i = 0; i < 3; i++) mid[i] = p1[i] + frac * (p2[i] - p1[i]);
	CM_RecursiveHullCheck(node->children[side], p1f, midf, p1, mid);


	// go past the node
	if (frac2 < 0) frac2 = 0;
	if (frac2 > 1) frac2 = 1;
		
	midf = p1f + (p2f - p1f) * frac2;
	for (i = 0; i < 3; i++) mid[i] = p1[i] + frac2 * (p2[i] - p1[i]);
	CM_RecursiveHullCheck(node->children[side^1], midf, p2f, mid, p2);
}

/*
===============================================================================

		PUBLIC FUNCTIONS

===============================================================================
*/

/*
==================
CM_BoxTrace
==================
*/
trace_t CM_BoxTrace( vec3_t start, vec3_t end, vec3_t mins, vec3_t maxs, int headnode, int brushmask )
{
	int	i;

	cm.checkcount++;	// for multi-check avoidance

	// fill in a default trace
	memset(&maptrace.result, 0, sizeof(maptrace.result));
	maptrace.result.surface = &(cm.nullsurface);
	maptrace.result.fraction = 1;

	if (!cm.numnodes) return maptrace.result; // map not loaded

	maptrace.contents = brushmask;
	VectorCopy(start, maptrace.start);
	VectorCopy(end, maptrace.end);
	VectorCopy(mins, maptrace.mins);
	VectorCopy(maxs, maptrace.maxs);

	// check for position test special case
	if( VectorCompare( start, end ))
	{
		vec3_t	c1, c2;
		int	leafs[1024];
		int	i, numleafs, topnode;

		VectorAdd(start, mins, c1);
		VectorAdd(start, maxs, c2);
		for (i = 0; i < 3; i++)
		{
			c1[i] -= 1;
			c2[i] += 1;
		}

		numleafs = CM_BoxLeafnums_headnode(c1, c2, leafs, 1024, headnode, &topnode);
		for (i = 0; i < numleafs; i++)
		{
			CM_TestInLeaf(leafs[i]);
			if(maptrace.result.allsolid)
				break;
		}
		VectorCopy (start, maptrace.result.endpos);
		return maptrace.result;
	}

	// check for point special case
	if( VectorIsNull( mins ) && VectorIsNull( maxs ))
	{
		maptrace.ispoint = true;
		VectorClear( maptrace.extents );
	}
	else
	{
		maptrace.ispoint = false;
		maptrace.extents[0] = -mins[0] > maxs[0] ? -mins[0] : maxs[0];
		maptrace.extents[1] = -mins[1] > maxs[1] ? -mins[1] : maxs[1];
		maptrace.extents[2] = -mins[2] > maxs[2] ? -mins[2] : maxs[2];
	}

	// general sweeping through world
	CM_RecursiveHullCheck( headnode, 0, 1, start, end );

	if (maptrace.result.fraction == 1)
	{
		VectorCopy(end, maptrace.result.endpos);
	}
	else
	{
		for(i = 0; i < 3; i++)
			maptrace.result.endpos[i] = start[i] + maptrace.result.fraction * (end[i] - start[i]);
	}
	return maptrace.result;
}

/*
==================
CM_TransformedBoxTrace

Handles offseting and rotation of the end points for moving and
rotating entities
==================
*/
#ifdef _WIN32
#pragma optimize( "", off )
#endif

trace_t CM_TransformedBoxTrace (vec3_t start, vec3_t end, vec3_t mins, vec3_t maxs, int headnode, int brushmask, vec3_t origin, vec3_t angles )
{
	trace_t	trace;
	vec3_t	start_l, end_l;
	vec3_t	forward, right, up;
	vec3_t	a, temp;
	bool	rotated = false;

	// subtract origin offset
	VectorSubtract(start, origin, start_l);
	VectorSubtract(end, origin, end_l);

	// rotate start and end into the models frame of reference
	if (headnode != box.headnode && !VectorIsNull( angles ))
		rotated = true;

	if( rotated )
	{
		AngleVectors( angles, forward, right, up );
		VectorCopy( start_l, temp );
		start_l[0] = DotProduct( temp, forward );
		start_l[1] = -DotProduct( temp, right );
		start_l[2] = DotProduct( temp, up );

		VectorCopy( end_l, temp );
		end_l[0] = DotProduct( temp, forward );
		end_l[1] = -DotProduct( temp, right );
		end_l[2] = DotProduct( temp, up );
	}

	// sweep the box through the model
	trace = CM_BoxTrace( start_l, end_l, mins, maxs, headnode, brushmask );

	if( rotated && trace.fraction != 1.0)
	{
		// FIXME: figure out how to do this with existing angles
		VectorNegate( angles, a );
		AngleVectors( a, forward, right, up);

		VectorCopy( trace.plane.normal, temp);
		trace.plane.normal[0] = DotProduct( temp, forward);
		trace.plane.normal[1] = -DotProduct( temp, right);
		trace.plane.normal[2] = DotProduct( temp, up);
	}

	trace.endpos[0] = start[0] + trace.fraction * (end[0] - start[0]);
	trace.endpos[1] = start[1] + trace.fraction * (end[1] - start[1]);
	trace.endpos[2] = start[2] + trace.fraction * (end[2] - start[2]);
	return trace;
}

#ifdef _WIN32
#pragma optimize( "", on )
#endif