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

Doom 3 GPL Source Code
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 

This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").

Doom 3 Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

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

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

	Trace model vs. polygonal model collision detection.

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

#include "../idlib/precompiled.h"
#pragma hdrstop

#include "CollisionModel_local.h"

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

Collision detection for translational motion

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

/*
================
idCollisionModelManagerLocal::TranslateEdgeThroughEdge

  calculates fraction of the translation completed at which the edges collide
================
*/
ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cross, idPluecker &l1, idPluecker &l2, float *fraction ) {

	float d, t;

	/*

	a = start of line
	b = end of line
	dir = movement direction
	l1 = pluecker coordinate for line
	l2 = pluecker coordinate for edge we might collide with
	a+dir = start of line after movement
	b+dir = end of line after movement
	t = scale factor
	solve pluecker inner product for t of line (a+t*dir : b+t*dir) and line l2

	v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
	v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
	v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
	v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
	v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
	v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);

	l2[0] * v[4] + l2[1] * v[5] + l2[2] * v[3] + l2[4] * v[0] + l2[5] * v[1] + l2[3] * v[2] = 0;

	solve t

	v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
	v[0] = (a[0]*b[1]) + a[0]*t*dir[1] + b[1]*t*dir[0] + (t*t*dir[0]*dir[1]) -
			((b[0]*a[1]) + b[0]*t*dir[1] + a[1]*t*dir[0] + (t*t*dir[0]*dir[1]));
	v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];

	v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
	v[1] = (a[0]*b[2]) + a[0]*t*dir[2] + b[2]*t*dir[0] + (t*t*dir[0]*dir[2]) -
			((b[0]*a[2]) + b[0]*t*dir[2] + a[2]*t*dir[0] + (t*t*dir[0]*dir[2]));
	v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];

	v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
	v[2] = a[0] - b[0];

	v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
	v[3] = (a[1]*b[2]) + a[1]*t*dir[2] + b[2]*t*dir[1] + (t*t*dir[1]*dir[2]) -
			((b[1]*a[2]) + b[1]*t*dir[2] + a[2]*t*dir[1] + (t*t*dir[1]*dir[2]));
	v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];

	v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
	v[4] = a[2] - b[2];

	v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
	v[5] = b[1] - a[1];


	v[0] = a[0]*b[1] + a[0]*t*dir[1] + b[1]*t*dir[0] - b[0]*a[1] - b[0]*t*dir[1] - a[1]*t*dir[0];
	v[1] = a[0]*b[2] + a[0]*t*dir[2] + b[2]*t*dir[0] - b[0]*a[2] - b[0]*t*dir[2] - a[2]*t*dir[0];
	v[2] = a[0] - b[0];
	v[3] = a[1]*b[2] + a[1]*t*dir[2] + b[2]*t*dir[1] - b[1]*a[2] - b[1]*t*dir[2] - a[2]*t*dir[1];
	v[4] = a[2] - b[2];
	v[5] = b[1] - a[1];

	v[0] = (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + a[0]*b[1] - b[0]*a[1];
	v[1] = (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + a[0]*b[2] - b[0]*a[2];
	v[2] = a[0] - b[0];
	v[3] = (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + a[1]*b[2] - b[1]*a[2];
	v[4] = a[2] - b[2];
	v[5] = b[1] - a[1];

	l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) * t + l2[4] * (a[0]*b[1] - b[0]*a[1])
		+ l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) * t + l2[5] * (a[0]*b[2] - b[0]*a[2])
		+ l2[3] * (a[0] - b[0])
		+ l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]) * t + l2[2] * (a[1]*b[2] - b[1]*a[2])
		+ l2[0] * (a[2] - b[2])
		+ l2[1] * (b[1] - a[1]) = 0

	t = (- l2[4] * (a[0]*b[1] - b[0]*a[1]) -
			l2[5] * (a[0]*b[2] - b[0]*a[2]) -
			l2[3] * (a[0] - b[0]) -
			l2[2] * (a[1]*b[2] - b[1]*a[2]) -
			l2[0] * (a[2] - b[2]) -
			l2[1] * (b[1] - a[1])) /
				(l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
				l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
				l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]));

	d = l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
		l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
		l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]);

	t = - ( l2[4] * (a[0]*b[1] - b[0]*a[1]) +
			l2[5] * (a[0]*b[2] - b[0]*a[2]) +
			l2[3] * (a[0] - b[0]) +
			l2[2] * (a[1]*b[2] - b[1]*a[2]) +
			l2[0] * (a[2] - b[2]) +
			l2[1] * (b[1] - a[1]));
	t /= d;

	MrE pats Pluecker on the head.. good monkey

	edgeDir = a - b;
	d = l2[4] * (edgeDir[0]*dir[1] - edgeDir[1]*dir[0]) +
		l2[5] * (edgeDir[0]*dir[2] - edgeDir[2]*dir[0]) +
		l2[2] * (edgeDir[1]*dir[2] - edgeDir[2]*dir[1]);
	*/

	d = l2[4] * cross[0] + l2[5] * cross[1] + l2[2] * cross[2];

	if ( d == 0.0f ) {
		*fraction = 1.0f;
		// no collision ever
		return false;
	}

	t = -l1.PermutedInnerProduct( l2 );
	// if the lines cross each other to begin with
	if ( t == 0.0f ) {
		*fraction = 0.0f;
		return true;
	}
	// fraction of movement at the time the lines cross each other
	*fraction = t / d;
	return true;
}

/*
================
CM_AddContact
================
*/
ID_INLINE void CM_AddContact( cm_traceWork_t *tw ) {

	if ( tw->numContacts >= tw->maxContacts ) {
		return;
	}
	// copy contact information from trace_t
	tw->contacts[tw->numContacts] = tw->trace.c;
	tw->numContacts++;
	// set fraction back to 1 to find all other contacts
	tw->trace.fraction = 1.0f;
}

/*
================
CM_SetVertexSidedness

  stores for the given model vertex at which side of one of the trm edges it passes
================
*/
ID_INLINE void CM_SetVertexSidedness( cm_vertex_t *v, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
	if ( !(v->sideSet & (1<<bitNum)) ) {
		float fl;
		fl = vpl.PermutedInnerProduct( epl );
		v->side = (v->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
		v->sideSet |= (1 << bitNum);
	}
}

/*
================
CM_SetEdgeSidedness

  stores for the given model edge at which side one of the trm vertices
================
*/
ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
	if ( !(edge->sideSet & (1<<bitNum)) ) {
		float fl;
		fl = vpl.PermutedInnerProduct( epl );
		edge->side = (edge->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
		edge->sideSet |= (1 << bitNum);
	}
}

/*
================
idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon
================
*/
void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) {
	int i, edgeNum;
	float f1, f2, dist, d1, d2;
	idVec3 start, end, normal;
	cm_edge_t *edge;
	cm_vertex_t *v1, *v2;
	idPluecker *pl, epsPl;

	// check edges for a collision
	for ( i = 0; i < poly->numEdges; i++) {
		edgeNum = poly->edges[i];
		edge = tw->model->edges + abs(edgeNum);
		// if this edge is already checked
		if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
			continue;
		}
		// can never collide with internal edges
		if ( edge->internal ) {
			continue;
		}
		pl = &tw->polygonEdgePlueckerCache[i];
		// get the sides at which the trm edge vertices pass the polygon edge
		CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[0]].pl, trmEdge->vertexNum[0] );
		CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[1]].pl, trmEdge->vertexNum[1] );
		// if the trm edge start and end vertex do not pass the polygon edge at different sides
		if ( !(((edge->side >> trmEdge->vertexNum[0]) ^ (edge->side >> trmEdge->vertexNum[1])) & 1) ) {
			continue;
		}
		// get the sides at which the polygon edge vertices pass the trm edge
		v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)];
		CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum );
		v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)];
		CM_SetVertexSidedness( v2, tw->polygonVertexPlueckerCache[i+1], trmEdge->pl, trmEdge->bitNum );
		// if the polygon edge start and end vertex do not pass the trm edge at different sides
		if ( !((v1->side ^ v2->side) & (1<<trmEdge->bitNum)) ) {
			continue;
		}
		// if there is no possible collision between the trm edge and the polygon edge
		if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, *pl, &f1 ) ) {
			continue;
		}
		// if moving away from edge
		if ( f1 < 0.0f ) {
			continue;
		}

		// pluecker coordinate for epsilon expanded edge
		epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + edge->normal * CM_CLIP_EPSILON,
						tw->model->vertices[edge->vertexNum[1]].p + edge->normal * CM_CLIP_EPSILON );
		// calculate collision fraction with epsilon expanded edge
		if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, epsPl, &f2 ) ) {
			continue;
		}
		// if no collision with epsilon edge or moving away from edge
		if ( f2 > 1.0f || f1 < f2 ) {
			continue;
		}

		if ( f2 < 0.0f ) {
			f2 = 0.0f;
		}

		if ( f2 < tw->trace.fraction ) {
			tw->trace.fraction = f2;
			// create plane with normal vector orthogonal to both the polygon edge and the trm edge
			start = tw->model->vertices[edge->vertexNum[0]].p;
			end = tw->model->vertices[edge->vertexNum[1]].p;
			tw->trace.c.normal = ( end - start ).Cross( trmEdge->end - trmEdge->start );
			// FIXME: do this normalize when we know the first collision
			tw->trace.c.normal.Normalize();
			tw->trace.c.dist = tw->trace.c.normal * start;
			// make sure the collision plane faces the trace model
			if ( tw->trace.c.normal * trmEdge->start - tw->trace.c.dist < 0.0f ) {
				tw->trace.c.normal = -tw->trace.c.normal;
				tw->trace.c.dist = -tw->trace.c.dist;
			}
			tw->trace.c.contents = poly->contents;
			tw->trace.c.material = poly->material;
			tw->trace.c.type = CONTACT_EDGE;
			tw->trace.c.modelFeature = edgeNum;
			tw->trace.c.trmFeature = trmEdge - tw->edges;
			// calculate collision point
			normal[0] = trmEdge->cross[2];
			normal[1] = -trmEdge->cross[1];
			normal[2] = trmEdge->cross[0];
			dist = normal * trmEdge->start;
			d1 = normal * start - dist;
			d2 = normal * end - dist;
			f1 = d1 / ( d1 - d2 );
			//assert( f1 >= 0.0f && f1 <= 1.0f );
			tw->trace.c.point = start + f1 * ( end - start );
			// if retrieving contacts
			if ( tw->getContacts ) {
				CM_AddContact( tw );
			}
		}
	}
}

/*
================
CM_TranslationPlaneFraction
================
*/

#if 0

float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
	float d1, d2;

	d2 = plane.Distance( end );
	// if the end point is closer to the plane than an epsilon we still take it for a collision
	if ( d2 >= CM_CLIP_EPSILON ) {
		return 1.0f;
	}
	d1 = plane.Distance( start );

	// if completely behind the polygon
	if ( d1 <= 0.0f ) {
		return 1.0f;
	}
	// leaves polygon
	if ( d1 <= d2 ) {
		return 1.0f;
	}
	return (d1-CM_CLIP_EPSILON) / (d1-d2);
}

#else

float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
	float d1, d2, d2eps;

	d2 = plane.Distance( end );
	// if the end point is closer to the plane than an epsilon we still take it for a collision
	// if ( d2 >= CM_CLIP_EPSILON ) {
	d2eps = d2 - CM_CLIP_EPSILON;
	if ( FLOATSIGNBITNOTSET(d2eps) ) {
		return 1.0f;
	}
	d1 = plane.Distance( start );

	// if completely behind the polygon
	if ( FLOATSIGNBITSET(d1) ) {
		return 1.0f;
	}
	// if going towards the front of the plane and
	// the start and end point are not at equal distance from the plane
	// if ( d1 > d2 )
	d2 = d1 - d2;
	if ( d2 <= 0.0f ) {
		return 1.0f;
	}
	return (d1-CM_CLIP_EPSILON) / d2;
}

#endif

/*
================
idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon
================
*/
void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int bitNum ) {
	int i, edgeNum;
	float f;
	cm_edge_t *edge;

	f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
	if ( f < tw->trace.fraction ) {

		for ( i = 0; i < poly->numEdges; i++ ) {
			edgeNum = poly->edges[i];
			edge = tw->model->edges + abs(edgeNum);
			CM_SetEdgeSidedness( edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum );
			if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> bitNum) & 1) ) {
				return;
			}
		}
		if ( f < 0.0f ) {
			f = 0.0f;
		}
		tw->trace.fraction = f;
		// collision plane is the polygon plane
		tw->trace.c.normal = poly->plane.Normal();
		tw->trace.c.dist = poly->plane.Dist();
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_TRMVERTEX;
		tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
		tw->trace.c.trmFeature = v - tw->vertices;
		tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
		// if retrieving contacts
		if ( tw->getContacts ) {
			CM_AddContact( tw );
			// no need to store the trm vertex more than once as a contact
			v->used = false;
		}
	}
}

/*
================
idCollisionModelManagerLocal::TranslatePointThroughPolygon
================
*/
void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v ) {
	int i, edgeNum;
	float f;
	cm_edge_t *edge;
	idPluecker pl;

	f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
	if ( f < tw->trace.fraction ) {

		for ( i = 0; i < poly->numEdges; i++ ) {
			edgeNum = poly->edges[i];
			edge = tw->model->edges + abs(edgeNum);
			// if we didn't yet calculate the sidedness for this edge
			if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) {
				float fl;
				edge->checkcount = idCollisionModelManagerLocal::checkCount;
				pl.FromLine(tw->model->vertices[edge->vertexNum[0]].p, tw->model->vertices[edge->vertexNum[1]].p);
				fl = v->pl.PermutedInnerProduct( pl );
				edge->side = FLOATSIGNBITSET(fl);
			}
			// if the point passes the edge at the wrong side
			//if ( (edgeNum > 0) == edge->side ) {
			if ( INTSIGNBITSET(edgeNum) ^ edge->side ) {
				return;
			}
		}
		if ( f < 0.0f ) {
			f = 0.0f;
		}
		tw->trace.fraction = f;
		// collision plane is the polygon plane
		tw->trace.c.normal = poly->plane.Normal();
		tw->trace.c.dist = poly->plane.Dist();
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_TRMVERTEX;
		tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
		tw->trace.c.trmFeature = v - tw->vertices;
		tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
		// if retrieving contacts
		if ( tw->getContacts ) {
			CM_AddContact( tw );
			// no need to store the trm vertex more than once as a contact
			v->used = false;
		}
	}
}

/*
================
idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon
================
*/
void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &endp, idPluecker &pl ) {
	int i, edgeNum;
	float f;
	cm_trmEdge_t *edge;

	f = CM_TranslationPlaneFraction( trmpoly->plane, v->p, endp );
	if ( f < tw->trace.fraction ) {

		for ( i = 0; i < trmpoly->numEdges; i++ ) {
			edgeNum = trmpoly->edges[i];
			edge = tw->edges + abs(edgeNum);

			CM_SetVertexSidedness( v, pl, edge->pl, edge->bitNum );
			if ( INTSIGNBITSET(edgeNum) ^ ((v->side >> edge->bitNum) & 1) ) {
				return;
			}
		}
		if ( f < 0.0f ) {
			f = 0.0f;
		}
		tw->trace.fraction = f;
		// collision plane is the inverse trm polygon plane
		tw->trace.c.normal = -trmpoly->plane.Normal();
		tw->trace.c.dist = -trmpoly->plane.Dist();
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_MODELVERTEX;
		tw->trace.c.modelFeature = v - tw->model->vertices;
		tw->trace.c.trmFeature = trmpoly - tw->polys;
		tw->trace.c.point = v->p + tw->trace.fraction * ( endp - v->p );
		// if retrieving contacts
		if ( tw->getContacts ) {
			CM_AddContact( tw );
		}
	}
}

/*
================
idCollisionModelManagerLocal::TranslateTrmThroughPolygon

  returns true if the polygon blocks the complete translation
================
*/
bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
	int i, j, k, edgeNum;
	float fraction, d;
	idVec3 endp;
	idPluecker *pl;
	cm_trmVertex_t *bv;
	cm_trmEdge_t *be;
	cm_trmPolygon_t *bp;
	cm_vertex_t *v;
	cm_edge_t *e;

	// if already checked this polygon
	if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
		return false;
	}
	p->checkcount = idCollisionModelManagerLocal::checkCount;

	// if this polygon does not have the right contents behind it
	if ( !(p->contents & tw->contents) ) {
		return false;
	}

	// if the the trace bounds do not intersect the polygon bounds
	if ( !tw->bounds.IntersectsBounds( p->bounds ) ) {
		return false;
	}

	// only collide with the polygon if approaching at the front
	if ( ( p->plane.Normal() * tw->dir ) > 0.0f ) {
		return false;
	}

	// if the polygon is too far from the first heart plane
	d = p->bounds.PlaneDistance( tw->heartPlane1 );
	if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) {
		return false;
	}

	// if the polygon is too far from the second heart plane
	d = p->bounds.PlaneDistance( tw->heartPlane2 );
	if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane2 ) {
		return false;
	}
	fraction = tw->trace.fraction;

	// fast point trace
	if ( tw->pointTrace ) {
		idCollisionModelManagerLocal::TranslatePointThroughPolygon( tw, p, &tw->vertices[0] );
	}
	else {

		// trace bounds should cross polygon plane
		switch ( tw->bounds.PlaneSide( p->plane ) ) {
			case PLANESIDE_CROSS:
				break;
			case PLANESIDE_FRONT:
				if ( tw->model->isConvex ) {
					tw->quickExit = true;
					return true;
				}
			default:
				return false;
		}

		// calculate pluecker coordinates for the polygon edges and polygon vertices
		for ( i = 0; i < p->numEdges; i++ ) {
			edgeNum = p->edges[i];
			e = tw->model->edges + abs(edgeNum);
			// reset sidedness cache if this is the first time we encounter this edge during this trace
			if ( e->checkcount != idCollisionModelManagerLocal::checkCount ) {
				e->sideSet = 0;
			}
			// pluecker coordinate for edge
			tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p,
														tw->model->vertices[e->vertexNum[1]].p );

			v = &tw->model->vertices[e->vertexNum[INTSIGNBITSET(edgeNum)]];
			// reset sidedness cache if this is the first time we encounter this vertex during this trace
			if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) {
				v->sideSet = 0;
			}
			// pluecker coordinate for vertex movement vector
			tw->polygonVertexPlueckerCache[i].FromRay( v->p, -tw->dir );
		}
		// copy first to last so we can easily cycle through for the edges
		tw->polygonVertexPlueckerCache[p->numEdges] = tw->polygonVertexPlueckerCache[0];

		// trace trm vertices through polygon
		for ( i = 0; i < tw->numVerts; i++ ) {
			bv = tw->vertices + i;
			if ( bv->used ) {
				idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( tw, p, bv, i );
			}
		}

		// trace trm edges through polygon
		for ( i = 1; i <= tw->numEdges; i++ ) {
			be = tw->edges + i;
			if ( be->used ) {
				idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( tw, p, be);
			}
		}

		// trace all polygon vertices through the trm
		for ( i = 0; i < p->numEdges; i++ ) {
			edgeNum = p->edges[i];
			e = tw->model->edges + abs(edgeNum);

			if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) {
				continue;
			}
			// set edge check count
			e->checkcount = idCollisionModelManagerLocal::checkCount;
			// can never collide with internal edges
			if ( e->internal ) {
				continue;
			}
			// got to check both vertices because we skip internal edges
			for ( k = 0; k < 2; k++ ) {

				v = tw->model->vertices + e->vertexNum[k ^ INTSIGNBITSET(edgeNum)];
				// if this vertex is already checked
				if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
					continue;
				}
				// set vertex check count
				v->checkcount = idCollisionModelManagerLocal::checkCount;

				// if the vertex is outside the trace bounds
				if ( !tw->bounds.ContainsPoint( v->p ) ) {
					continue;
				}

				// vertex end point after movement
				endp = v->p - tw->dir;
				// pluecker coordinate for vertex movement vector
				pl = &tw->polygonVertexPlueckerCache[i+k];

				for ( j = 0; j < tw->numPolys; j++ ) {
					bp = tw->polys + j;
					if ( bp->used ) {
						idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( tw, bp, p, v, endp, *pl );
					}
				}
			}
		}
	}

	// if there was a collision with this polygon and we are not retrieving contacts
	if ( tw->trace.fraction < fraction && !tw->getContacts ) {
		fraction = tw->trace.fraction;
		endp = tw->start + fraction * tw->dir;
		// decrease bounds
		for ( i = 0; i < 3; i++ ) {
			if ( tw->start[i] < endp[i] ) {
				tw->bounds[0][i] = tw->start[i] + tw->size[0][i] - CM_BOX_EPSILON;
				tw->bounds[1][i] = endp[i] + tw->size[1][i] + CM_BOX_EPSILON;
			}
			else {
				tw->bounds[0][i] = endp[i] + tw->size[0][i] - CM_BOX_EPSILON;
				tw->bounds[1][i] = tw->start[i] + tw->size[1][i] + CM_BOX_EPSILON;
			}
		}
	}

	return ( tw->trace.fraction == 0.0f );
}

/*
================
idCollisionModelManagerLocal::SetupTrm
================
*/
void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceModel *trm ) {
	int i, j;

	// vertices
	tw->numVerts = trm->numVerts;
	for ( i = 0; i < trm->numVerts; i++ ) {
		tw->vertices[i].p = trm->verts[i];
		tw->vertices[i].used = false;
	}
	// edges
	tw->numEdges = trm->numEdges;
	for ( i = 1; i <= trm->numEdges; i++ ) {
		tw->edges[i].vertexNum[0] = trm->edges[i].v[0];
		tw->edges[i].vertexNum[1] = trm->edges[i].v[1];
		tw->edges[i].used = false;
	}
	// polygons
	tw->numPolys = trm->numPolys;
	for ( i = 0; i < trm->numPolys; i++ ) {
		tw->polys[i].numEdges = trm->polys[i].numEdges;
		for ( j = 0; j < trm->polys[i].numEdges; j++ ) {
			tw->polys[i].edges[j] = trm->polys[i].edges[j];
		}
		tw->polys[i].plane.SetNormal( trm->polys[i].normal );
		tw->polys[i].used = false;
	}
	// is the trace model convex or not
	tw->isConvex = trm->isConvex;
}

/*
================
idCollisionModelManagerLocal::SetupTranslationHeartPlanes
================
*/
void idCollisionModelManagerLocal::SetupTranslationHeartPlanes( cm_traceWork_t *tw ) {
	idVec3 dir, normal1, normal2;

	// calculate trace heart planes
	dir = tw->dir;
	dir.Normalize();
	dir.NormalVectors( normal1, normal2 );
	tw->heartPlane1.SetNormal( normal1 );
	tw->heartPlane1.FitThroughPoint( tw->start );
	tw->heartPlane2.SetNormal( normal2 );
	tw->heartPlane2.FitThroughPoint( tw->start );
}

/*
================
idCollisionModelManagerLocal::Translation
================
*/
#ifdef _DEBUG
static int entered = 0;
#endif

void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 &start, const idVec3 &end,
										const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
										cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {

	int i, j;
	float dist;
	bool model_rotated, trm_rotated;
	idVec3 dir1, dir2, dir;
	idMat3 invModelAxis, tmpAxis;
	cm_trmPolygon_t *poly;
	cm_trmEdge_t *edge;
	cm_trmVertex_t *vert;
	ALIGN16( static cm_traceWork_t tw );

	assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) >= (((byte *)results) + sizeof( trace_t )) );
	assert( ((byte *)&end) < ((byte *)results) || ((byte *)&end) >= (((byte *)results) + sizeof( trace_t )) );
	assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) >= (((byte *)results) + sizeof( trace_t )) );

	memset( results, 0, sizeof( *results ) );

	if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) {
		common->Printf("idCollisionModelManagerLocal::Translation: invalid model handle\n");
		return;
	}
	if ( !idCollisionModelManagerLocal::models[model] ) {
		common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n");
		return;
	}

	// if case special position test
	if ( start[0] == end[0] && start[1] == end[1] && start[2] == end[2] ) {
		idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
		return;
	}

#ifdef _DEBUG
	bool startsolid = false;
	// test whether or not stuck to begin with
	if ( cm_debugCollision.GetBool() ) {
		if ( !entered && !idCollisionModelManagerLocal::getContacts ) {
			entered = 1;
			// if already messed up to begin with
			if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
				startsolid = true;
			}
			entered = 0;
		}
	}
#endif

	idCollisionModelManagerLocal::checkCount++;

	tw.trace.fraction = 1.0f;
	tw.trace.c.contents = 0;
	tw.trace.c.type = CONTACT_NONE;
	tw.contents = contentMask;
	tw.isConvex = true;
	tw.rotation = false;
	tw.positionTest = false;
	tw.quickExit = false;
	tw.getContacts = idCollisionModelManagerLocal::getContacts;
	tw.contacts = idCollisionModelManagerLocal::contacts;
	tw.maxContacts = idCollisionModelManagerLocal::maxContacts;
	tw.numContacts = 0;
	tw.model = idCollisionModelManagerLocal::models[model];
	tw.start = start - modelOrigin;
	tw.end = end - modelOrigin;
	tw.dir = end - start;

	model_rotated = modelAxis.IsRotated();
	if ( model_rotated ) {
		invModelAxis = modelAxis.Transpose();
	}

	// if optimized point trace
	if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f &&
					trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f &&
					trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) {

		if ( model_rotated ) {
			// rotate trace instead of model
			tw.start *= invModelAxis;
			tw.end *= invModelAxis;
			tw.dir *= invModelAxis;
		}

		// trace bounds
		for ( i = 0; i < 3; i++ ) {
			if ( tw.start[i] < tw.end[i] ) {
				tw.bounds[0][i] = tw.start[i] - CM_BOX_EPSILON;
				tw.bounds[1][i] = tw.end[i] + CM_BOX_EPSILON;
			}
			else {
				tw.bounds[0][i] = tw.end[i] - CM_BOX_EPSILON;
				tw.bounds[1][i] = tw.start[i] + CM_BOX_EPSILON;
			}
		}
		tw.extents[0] = tw.extents[1] = tw.extents[2] = CM_BOX_EPSILON;
		tw.size.Zero();

		// setup trace heart planes
		idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
		tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON;
		tw.maxDistFromHeartPlane2 = CM_BOX_EPSILON;
		// collision with single point
		tw.numVerts = 1;
		tw.vertices[0].p = tw.start;
		tw.vertices[0].endp = tw.vertices[0].p + tw.dir;
		tw.vertices[0].pl.FromRay( tw.vertices[0].p, tw.dir );
		tw.numEdges = tw.numPolys = 0;
		tw.pointTrace = true;
		// trace through the model
		idCollisionModelManagerLocal::TraceThroughModel( &tw );
		// store results
		*results = tw.trace;
		results->endpos = start + results->fraction * (end - start);
		results->endAxis = mat3_identity;

		if ( results->fraction < 1.0f ) {
			// rotate trace plane normal if there was a collision with a rotated model
			if ( model_rotated ) {
				results->c.normal *= modelAxis;
				results->c.point *= modelAxis;
			}
			results->c.point += modelOrigin;
			results->c.dist += modelOrigin * results->c.normal;
		}
		idCollisionModelManagerLocal::numContacts = tw.numContacts;
		return;
	}

	// the trace fraction is too inaccurate to describe translations over huge distances
	if ( tw.dir.LengthSqr() > Square( CM_MAX_TRACE_DIST ) ) {
		results->fraction = 0.0f;
		results->endpos = start;
		results->endAxis = trmAxis;
		results->c.normal = vec3_origin;
		results->c.material = NULL;
		results->c.point = start;
		if ( session->rw ) {
			session->rw->DebugArrow( colorRed, start, end, 1 );
		}
		common->Printf( "idCollisionModelManagerLocal::Translation: huge translation\n" );
		return;
	}

	tw.pointTrace = false;
	tw.size.Clear();

	// setup trm structure
	idCollisionModelManagerLocal::SetupTrm( &tw, trm );

	trm_rotated = trmAxis.IsRotated();

	// calculate vertex positions
	if ( trm_rotated ) {
		for ( i = 0; i < tw.numVerts; i++ ) {
			// rotate trm around the start position
			tw.vertices[i].p *= trmAxis;
		}
	}
	for ( i = 0; i < tw.numVerts; i++ ) {
		// set trm at start position
		tw.vertices[i].p += tw.start;
	}
	if ( model_rotated ) {
		for ( i = 0; i < tw.numVerts; i++ ) {
			// rotate trm around model instead of rotating the model
			tw.vertices[i].p *= invModelAxis;
		}
	}

	// add offset to start point
	if ( trm_rotated ) {
		dir = trm->offset * trmAxis;
		tw.start += dir;
		tw.end += dir;
	} else {
		tw.start += trm->offset;
		tw.end += trm->offset;
	}
	if ( model_rotated ) {
		// rotate trace instead of model
		tw.start *= invModelAxis;
		tw.end *= invModelAxis;
		tw.dir *= invModelAxis;
	}

	// rotate trm polygon planes
	if ( trm_rotated & model_rotated ) {
		tmpAxis = trmAxis * invModelAxis;
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= tmpAxis;
		}
	} else if ( trm_rotated ) {
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= trmAxis;
		}
	} else if ( model_rotated ) {
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= invModelAxis;
		}
	}

	// setup trm polygons
	for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
		// if the trm poly plane is facing in the movement direction
		dist = poly->plane.Normal() * tw.dir;
		if ( dist > 0.0f || ( !trm->isConvex && dist == 0.0f ) ) {
			// this trm poly and it's edges and vertices need to be used for collision
			poly->used = true;
			for ( j = 0; j < poly->numEdges; j++ ) {
				edge = &tw.edges[abs( poly->edges[j] )];
				edge->used = true;
				tw.vertices[edge->vertexNum[0]].used = true;
				tw.vertices[edge->vertexNum[1]].used = true;
			}
		}
	}

	// setup trm vertices
	for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
		if ( !vert->used ) {
			continue;
		}
		// get axial trm size after rotations
		tw.size.AddPoint( vert->p - tw.start );
		// calculate the end position of each vertex for a full trace
		vert->endp = vert->p + tw.dir;
		// pluecker coordinate for vertex movement line
		vert->pl.FromRay( vert->p, tw.dir );
	}

	// setup trm edges
	for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) {
		if ( !edge->used ) {
			continue;
		}
		// edge start, end and pluecker coordinate
		edge->start = tw.vertices[edge->vertexNum[0]].p;
		edge->end = tw.vertices[edge->vertexNum[1]].p;
		edge->pl.FromLine( edge->start, edge->end );
		// calculate normal of plane through movement plane created by the edge
		dir = edge->start - edge->end;
		edge->cross[0] = dir[0] * tw.dir[1] - dir[1] * tw.dir[0];
		edge->cross[1] = dir[0] * tw.dir[2] - dir[2] * tw.dir[0];
		edge->cross[2] = dir[1] * tw.dir[2] - dir[2] * tw.dir[1];
		// bit for vertex sidedness bit cache
		edge->bitNum = i;
	}

	// set trm plane distances
	for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
		if ( poly->used ) {
			poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start );
		}
	}

	// bounds for full trace, a little bit larger for epsilons
	for ( i = 0; i < 3; i++ ) {
		if ( tw.start[i] < tw.end[i] ) {
			tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
			tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
		} else {
			tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
			tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
		}
		if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) {
			tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON;
		} else {
			tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON;
		}
	}

	// setup trace heart planes
	idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
	tw.maxDistFromHeartPlane1 = 0;
	tw.maxDistFromHeartPlane2 = 0;
	// calculate maximum trm vertex distance from both heart planes
	for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
		if ( !vert->used ) {
			continue;
		}
		dist = idMath::Fabs( tw.heartPlane1.Distance( vert->p ) );
		if ( dist > tw.maxDistFromHeartPlane1 ) {
			tw.maxDistFromHeartPlane1 = dist;
		}
		dist = idMath::Fabs( tw.heartPlane2.Distance( vert->p ) );
		if ( dist > tw.maxDistFromHeartPlane2 ) {
			tw.maxDistFromHeartPlane2 = dist;
		}
	}
	// for epsilons
	tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON;
	tw.maxDistFromHeartPlane2 += CM_BOX_EPSILON;

	// trace through the model
	idCollisionModelManagerLocal::TraceThroughModel( &tw );

	// if we're getting contacts
	if ( tw.getContacts ) {
		// move all contacts to world space
		if ( model_rotated ) {
			for ( i = 0; i < tw.numContacts; i++ ) {
				tw.contacts[i].normal *= modelAxis;
				tw.contacts[i].point *= modelAxis;
			}
		}
		if ( modelOrigin != vec3_origin ) {
			for ( i = 0; i < tw.numContacts; i++ ) {
				tw.contacts[i].point += modelOrigin;
				tw.contacts[i].dist += modelOrigin * tw.contacts[i].normal;
			}
		}
		idCollisionModelManagerLocal::numContacts = tw.numContacts;
	} else {
		// store results
		*results = tw.trace;
		results->endpos = start + results->fraction * ( end - start );
		results->endAxis = trmAxis;

		if ( results->fraction < 1.0f ) {
			// if the fraction is tiny the actual movement could end up zero
			if ( results->fraction > 0.0f && results->endpos.Compare( start ) ) {
				results->fraction = 0.0f;
			}
			// rotate trace plane normal if there was a collision with a rotated model
			if ( model_rotated ) {
				results->c.normal *= modelAxis;
				results->c.point *= modelAxis;
			}
			results->c.point += modelOrigin;
			results->c.dist += modelOrigin * results->c.normal;
		}
	}

#ifdef _DEBUG
	// test for missed collisions
	if ( cm_debugCollision.GetBool() ) {
		if ( !entered && !idCollisionModelManagerLocal::getContacts ) {
			entered = 1;
			// if the trm is stuck in the model
			if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
				trace_t tr;

				// test where the trm is stuck in the model
				idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis );
				// re-run collision detection to find out where it failed
				idCollisionModelManagerLocal::Translation( &tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
			}
			entered = 0;
		}
	}
#endif
}