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

IceTech GPL Source Code
Copyright (C) 2026 Justin Marshall

This file is part of the IceTech GPL Source Code (?IceTech Source Code?).

IceTech 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.

IceTech 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 IceTech Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the IceTech 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 IceTech 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 Justin Marshall, justinmarshall20@gmail.com

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

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

	Trace model vs. polygonal model collision detection.

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

#include "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 against the trace direction
			if ((tw->dir * tw->trace.c.normal) > 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);
			}
			else if (tw->trace.fraction == 0.0f) {
				return;
			}
		}
	}
}

/*
================
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 = tw->trace.c.normal * v->p;
		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;
		// 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, 0.1f)) {
		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");
		results->fraction = 1.0f;
		results->endpos = end;
		results->endAxis = trmAxis;
		return;
	}
	if (!idCollisionModelManagerLocal::models[model]) {
		common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n");
		results->fraction = 1.0f;
		results->endpos = end;
		results->endAxis = trmAxis;
		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.trace.c.material = NULL;
	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
}