1 /*
2 ===========================================================================
3 
4 Doom 3 GPL Source Code
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
6 
7 This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
8 
9 Doom 3 Source Code is free software: you can redistribute it and/or modify
10 it under the terms of the GNU General Public License as published by
11 the Free Software Foundation, either version 3 of the License, or
12 (at your option) any later version.
13 
14 Doom 3 Source Code is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 GNU General Public License for more details.
18 
19 You should have received a copy of the GNU General Public License
20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
21 
22 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.
23 
24 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.
25 
26 ===========================================================================
27 */
28 
29 /*
30 ===============================================================================
31 
32 	Trace model vs. polygonal model collision detection.
33 
34 ===============================================================================
35 */
36 
37 #include "sys/platform.h"
38 #include "framework/Session.h"
39 #include "renderer/RenderWorld.h"
40 
41 #include "cm/CollisionModel_local.h"
42 
43 /*
44 ===============================================================================
45 
46 Collision detection for translational motion
47 
48 ===============================================================================
49 */
50 
51 /*
52 ================
53 idCollisionModelManagerLocal::TranslateEdgeThroughEdge
54 
55   calculates fraction of the translation completed at which the edges collide
56 ================
57 */
TranslateEdgeThroughEdge(idVec3 & cross,idPluecker & l1,idPluecker & l2,float * fraction)58 ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cross, idPluecker &l1, idPluecker &l2, float *fraction ) {
59 
60 	float d, t;
61 
62 	/*
63 
64 	a = start of line
65 	b = end of line
66 	dir = movement direction
67 	l1 = pluecker coordinate for line
68 	l2 = pluecker coordinate for edge we might collide with
69 	a+dir = start of line after movement
70 	b+dir = end of line after movement
71 	t = scale factor
72 	solve pluecker inner product for t of line (a+t*dir : b+t*dir) and line l2
73 
74 	v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
75 	v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
76 	v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
77 	v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
78 	v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
79 	v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
80 
81 	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;
82 
83 	solve t
84 
85 	v[0] = (a[0]+t*dir[0]) * (b[1]+t*dir[1]) - (b[0]+t*dir[0]) * (a[1]+t*dir[1]);
86 	v[0] = (a[0]*b[1]) + a[0]*t*dir[1] + b[1]*t*dir[0] + (t*t*dir[0]*dir[1]) -
87 			((b[0]*a[1]) + b[0]*t*dir[1] + a[1]*t*dir[0] + (t*t*dir[0]*dir[1]));
88 	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];
89 
90 	v[1] = (a[0]+t*dir[0]) * (b[2]+t*dir[2]) - (b[0]+t*dir[0]) * (a[2]+t*dir[2]);
91 	v[1] = (a[0]*b[2]) + a[0]*t*dir[2] + b[2]*t*dir[0] + (t*t*dir[0]*dir[2]) -
92 			((b[0]*a[2]) + b[0]*t*dir[2] + a[2]*t*dir[0] + (t*t*dir[0]*dir[2]));
93 	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];
94 
95 	v[2] = (a[0]+t*dir[0]) - (b[0]+t*dir[0]);
96 	v[2] = a[0] - b[0];
97 
98 	v[3] = (a[1]+t*dir[1]) * (b[2]+t*dir[2]) - (b[1]+t*dir[1]) * (a[2]+t*dir[2]);
99 	v[3] = (a[1]*b[2]) + a[1]*t*dir[2] + b[2]*t*dir[1] + (t*t*dir[1]*dir[2]) -
100 			((b[1]*a[2]) + b[1]*t*dir[2] + a[2]*t*dir[1] + (t*t*dir[1]*dir[2]));
101 	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];
102 
103 	v[4] = (a[2]+t*dir[2]) - (b[2]+t*dir[2]);
104 	v[4] = a[2] - b[2];
105 
106 	v[5] = (b[1]+t*dir[1]) - (a[1]+t*dir[1]);
107 	v[5] = b[1] - a[1];
108 
109 
110 	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];
111 	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];
112 	v[2] = a[0] - b[0];
113 	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];
114 	v[4] = a[2] - b[2];
115 	v[5] = b[1] - a[1];
116 
117 	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];
118 	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];
119 	v[2] = a[0] - b[0];
120 	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];
121 	v[4] = a[2] - b[2];
122 	v[5] = b[1] - a[1];
123 
124 	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])
125 		+ 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])
126 		+ l2[3] * (a[0] - b[0])
127 		+ 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])
128 		+ l2[0] * (a[2] - b[2])
129 		+ l2[1] * (b[1] - a[1]) = 0
130 
131 	t = (- l2[4] * (a[0]*b[1] - b[0]*a[1]) -
132 			l2[5] * (a[0]*b[2] - b[0]*a[2]) -
133 			l2[3] * (a[0] - b[0]) -
134 			l2[2] * (a[1]*b[2] - b[1]*a[2]) -
135 			l2[0] * (a[2] - b[2]) -
136 			l2[1] * (b[1] - a[1])) /
137 				(l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
138 				l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
139 				l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]));
140 
141 	d = l2[4] * (a[0]*dir[1] + b[1]*dir[0] - b[0]*dir[1] - a[1]*dir[0]) +
142 		l2[5] * (a[0]*dir[2] + b[2]*dir[0] - b[0]*dir[2] - a[2]*dir[0]) +
143 		l2[2] * (a[1]*dir[2] + b[2]*dir[1] - b[1]*dir[2] - a[2]*dir[1]);
144 
145 	t = - ( l2[4] * (a[0]*b[1] - b[0]*a[1]) +
146 			l2[5] * (a[0]*b[2] - b[0]*a[2]) +
147 			l2[3] * (a[0] - b[0]) +
148 			l2[2] * (a[1]*b[2] - b[1]*a[2]) +
149 			l2[0] * (a[2] - b[2]) +
150 			l2[1] * (b[1] - a[1]));
151 	t /= d;
152 
153 	MrE pats Pluecker on the head.. good monkey
154 
155 	edgeDir = a - b;
156 	d = l2[4] * (edgeDir[0]*dir[1] - edgeDir[1]*dir[0]) +
157 		l2[5] * (edgeDir[0]*dir[2] - edgeDir[2]*dir[0]) +
158 		l2[2] * (edgeDir[1]*dir[2] - edgeDir[2]*dir[1]);
159 	*/
160 
161 	d = l2[4] * cross[0] + l2[5] * cross[1] + l2[2] * cross[2];
162 
163 	if ( d == 0.0f ) {
164 		*fraction = 1.0f;
165 		// no collision ever
166 		return false;
167 	}
168 
169 	t = -l1.PermutedInnerProduct( l2 );
170 	// if the lines cross each other to begin with
171 	if ( t == 0.0f ) {
172 		*fraction = 0.0f;
173 		return true;
174 	}
175 	// fraction of movement at the time the lines cross each other
176 	*fraction = t / d;
177 	return true;
178 }
179 
180 /*
181 ================
182 CM_AddContact
183 ================
184 */
CM_AddContact(cm_traceWork_t * tw)185 ID_INLINE void CM_AddContact( cm_traceWork_t *tw ) {
186 
187 	if ( tw->numContacts >= tw->maxContacts ) {
188 		return;
189 	}
190 	// copy contact information from trace_t
191 	tw->contacts[tw->numContacts] = tw->trace.c;
192 	tw->numContacts++;
193 	// set fraction back to 1 to find all other contacts
194 	tw->trace.fraction = 1.0f;
195 }
196 
197 /*
198 ================
199 CM_SetVertexSidedness
200 
201   stores for the given model vertex at which side of one of the trm edges it passes
202 ================
203 */
CM_SetVertexSidedness(cm_vertex_t * v,const idPluecker & vpl,const idPluecker & epl,const int bitNum)204 ID_INLINE void CM_SetVertexSidedness( cm_vertex_t *v, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
205 	if ( !(v->sideSet & (1<<bitNum)) ) {
206 		float fl;
207 		fl = vpl.PermutedInnerProduct( epl );
208 		v->side = (v->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
209 		v->sideSet |= (1 << bitNum);
210 	}
211 }
212 
213 /*
214 ================
215 CM_SetEdgeSidedness
216 
217   stores for the given model edge at which side one of the trm vertices
218 ================
219 */
CM_SetEdgeSidedness(cm_edge_t * edge,const idPluecker & vpl,const idPluecker & epl,const int bitNum)220 ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) {
221 	if ( !(edge->sideSet & (1<<bitNum)) ) {
222 		float fl;
223 		fl = vpl.PermutedInnerProduct( epl );
224 		edge->side = (edge->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum);
225 		edge->sideSet |= (1 << bitNum);
226 	}
227 }
228 
229 /*
230 ================
231 idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon
232 ================
233 */
TranslateTrmEdgeThroughPolygon(cm_traceWork_t * tw,cm_polygon_t * poly,cm_trmEdge_t * trmEdge)234 void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) {
235 	int i, edgeNum;
236 	float f1, f2, dist, d1, d2;
237 	idVec3 start, end, normal;
238 	cm_edge_t *edge;
239 	cm_vertex_t *v1, *v2;
240 	idPluecker *pl, epsPl;
241 
242 	// check edges for a collision
243 	for ( i = 0; i < poly->numEdges; i++) {
244 		edgeNum = poly->edges[i];
245 		edge = tw->model->edges + abs(edgeNum);
246 		// if this edge is already checked
247 		if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
248 			continue;
249 		}
250 		// can never collide with internal edges
251 		if ( edge->internal ) {
252 			continue;
253 		}
254 		pl = &tw->polygonEdgePlueckerCache[i];
255 		// get the sides at which the trm edge vertices pass the polygon edge
256 		CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[0]].pl, trmEdge->vertexNum[0] );
257 		CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[1]].pl, trmEdge->vertexNum[1] );
258 		// if the trm edge start and end vertex do not pass the polygon edge at different sides
259 		if ( !(((edge->side >> trmEdge->vertexNum[0]) ^ (edge->side >> trmEdge->vertexNum[1])) & 1) ) {
260 			continue;
261 		}
262 		// get the sides at which the polygon edge vertices pass the trm edge
263 		v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)];
264 		CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum );
265 		v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)];
266 		CM_SetVertexSidedness( v2, tw->polygonVertexPlueckerCache[i+1], trmEdge->pl, trmEdge->bitNum );
267 		// if the polygon edge start and end vertex do not pass the trm edge at different sides
268 		if ( !((v1->side ^ v2->side) & (1<<trmEdge->bitNum)) ) {
269 			continue;
270 		}
271 		// if there is no possible collision between the trm edge and the polygon edge
272 		if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, *pl, &f1 ) ) {
273 			continue;
274 		}
275 		// if moving away from edge
276 		if ( f1 < 0.0f ) {
277 			continue;
278 		}
279 
280 		// pluecker coordinate for epsilon expanded edge
281 		epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + edge->normal * CM_CLIP_EPSILON,
282 						tw->model->vertices[edge->vertexNum[1]].p + edge->normal * CM_CLIP_EPSILON );
283 		// calculate collision fraction with epsilon expanded edge
284 		if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, epsPl, &f2 ) ) {
285 			continue;
286 		}
287 		// if no collision with epsilon edge or moving away from edge
288 		if ( f2 > 1.0f || f1 < f2 ) {
289 			continue;
290 		}
291 
292 		if ( f2 < 0.0f ) {
293 			f2 = 0.0f;
294 		}
295 
296 		if ( f2 < tw->trace.fraction ) {
297 			tw->trace.fraction = f2;
298 			// create plane with normal vector orthogonal to both the polygon edge and the trm edge
299 			start = tw->model->vertices[edge->vertexNum[0]].p;
300 			end = tw->model->vertices[edge->vertexNum[1]].p;
301 			tw->trace.c.normal = ( end - start ).Cross( trmEdge->end - trmEdge->start );
302 			// FIXME: do this normalize when we know the first collision
303 			tw->trace.c.normal.Normalize();
304 			tw->trace.c.dist = tw->trace.c.normal * start;
305 			// make sure the collision plane faces the trace model
306 			if ( tw->trace.c.normal * trmEdge->start - tw->trace.c.dist < 0.0f ) {
307 				tw->trace.c.normal = -tw->trace.c.normal;
308 				tw->trace.c.dist = -tw->trace.c.dist;
309 			}
310 			tw->trace.c.contents = poly->contents;
311 			tw->trace.c.material = poly->material;
312 			tw->trace.c.type = CONTACT_EDGE;
313 			tw->trace.c.modelFeature = edgeNum;
314 			tw->trace.c.trmFeature = trmEdge - tw->edges;
315 			// calculate collision point
316 			normal[0] = trmEdge->cross[2];
317 			normal[1] = -trmEdge->cross[1];
318 			normal[2] = trmEdge->cross[0];
319 			dist = normal * trmEdge->start;
320 			d1 = normal * start - dist;
321 			d2 = normal * end - dist;
322 			f1 = d1 / ( d1 - d2 );
323 			//assert( f1 >= 0.0f && f1 <= 1.0f );
324 			tw->trace.c.point = start + f1 * ( end - start );
325 			// if retrieving contacts
326 			if ( tw->getContacts ) {
327 				CM_AddContact( tw );
328 			}
329 		}
330 	}
331 }
332 
333 /*
334 ================
335 CM_TranslationPlaneFraction
336 ================
337 */
338 
339 #if 0
340 
341 float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
342 	float d1, d2;
343 
344 	d2 = plane.Distance( end );
345 	// if the end point is closer to the plane than an epsilon we still take it for a collision
346 	if ( d2 >= CM_CLIP_EPSILON ) {
347 		return 1.0f;
348 	}
349 	d1 = plane.Distance( start );
350 
351 	// if completely behind the polygon
352 	if ( d1 <= 0.0f ) {
353 		return 1.0f;
354 	}
355 	// leaves polygon
356 	if ( d1 <= d2 ) {
357 		return 1.0f;
358 	}
359 	return (d1-CM_CLIP_EPSILON) / (d1-d2);
360 }
361 
362 #else
363 
CM_TranslationPlaneFraction(idPlane & plane,idVec3 & start,idVec3 & end)364 float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) {
365 	float d1, d2, d2eps;
366 
367 	d2 = plane.Distance( end );
368 	// if the end point is closer to the plane than an epsilon we still take it for a collision
369 	// if ( d2 >= CM_CLIP_EPSILON ) {
370 	d2eps = d2 - CM_CLIP_EPSILON;
371 	if ( FLOATSIGNBITNOTSET(d2eps) ) {
372 		return 1.0f;
373 	}
374 	d1 = plane.Distance( start );
375 
376 	// if completely behind the polygon
377 	if ( FLOATSIGNBITSET(d1) ) {
378 		return 1.0f;
379 	}
380 	// if going towards the front of the plane and
381 	// the start and end point are not at equal distance from the plane
382 	// if ( d1 > d2 )
383 	d2 = d1 - d2;
384 	if ( d2 <= 0.0f ) {
385 		return 1.0f;
386 	}
387 	return (d1-CM_CLIP_EPSILON) / d2;
388 }
389 
390 #endif
391 
392 /*
393 ================
394 idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon
395 ================
396 */
TranslateTrmVertexThroughPolygon(cm_traceWork_t * tw,cm_polygon_t * poly,cm_trmVertex_t * v,int bitNum)397 void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int bitNum ) {
398 	int i, edgeNum;
399 	float f;
400 	cm_edge_t *edge;
401 
402 	f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
403 	if ( f < tw->trace.fraction ) {
404 
405 		for ( i = 0; i < poly->numEdges; i++ ) {
406 			edgeNum = poly->edges[i];
407 			edge = tw->model->edges + abs(edgeNum);
408 			CM_SetEdgeSidedness( edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum );
409 			if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> bitNum) & 1) ) {
410 				return;
411 			}
412 		}
413 		if ( f < 0.0f ) {
414 			f = 0.0f;
415 		}
416 		tw->trace.fraction = f;
417 		// collision plane is the polygon plane
418 		tw->trace.c.normal = poly->plane.Normal();
419 		tw->trace.c.dist = poly->plane.Dist();
420 		tw->trace.c.contents = poly->contents;
421 		tw->trace.c.material = poly->material;
422 		tw->trace.c.type = CONTACT_TRMVERTEX;
423 		tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
424 		tw->trace.c.trmFeature = v - tw->vertices;
425 		tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
426 		// if retrieving contacts
427 		if ( tw->getContacts ) {
428 			CM_AddContact( tw );
429 			// no need to store the trm vertex more than once as a contact
430 			v->used = false;
431 		}
432 	}
433 }
434 
435 /*
436 ================
437 idCollisionModelManagerLocal::TranslatePointThroughPolygon
438 ================
439 */
TranslatePointThroughPolygon(cm_traceWork_t * tw,cm_polygon_t * poly,cm_trmVertex_t * v)440 void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v ) {
441 	int i, edgeNum;
442 	float f;
443 	cm_edge_t *edge;
444 	idPluecker pl;
445 
446 	f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp );
447 	if ( f < tw->trace.fraction ) {
448 
449 		for ( i = 0; i < poly->numEdges; i++ ) {
450 			edgeNum = poly->edges[i];
451 			edge = tw->model->edges + abs(edgeNum);
452 			// if we didn't yet calculate the sidedness for this edge
453 			if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) {
454 				float fl;
455 				edge->checkcount = idCollisionModelManagerLocal::checkCount;
456 				pl.FromLine(tw->model->vertices[edge->vertexNum[0]].p, tw->model->vertices[edge->vertexNum[1]].p);
457 				fl = v->pl.PermutedInnerProduct( pl );
458 				edge->side = FLOATSIGNBITSET(fl);
459 			}
460 			// if the point passes the edge at the wrong side
461 			//if ( (edgeNum > 0) == edge->side ) {
462 			if ( INTSIGNBITSET(edgeNum) ^ edge->side ) {
463 				return;
464 			}
465 		}
466 		if ( f < 0.0f ) {
467 			f = 0.0f;
468 		}
469 		tw->trace.fraction = f;
470 		// collision plane is the polygon plane
471 		tw->trace.c.normal = poly->plane.Normal();
472 		tw->trace.c.dist = poly->plane.Dist();
473 		tw->trace.c.contents = poly->contents;
474 		tw->trace.c.material = poly->material;
475 		tw->trace.c.type = CONTACT_TRMVERTEX;
476 		tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
477 		tw->trace.c.trmFeature = v - tw->vertices;
478 		tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p );
479 		// if retrieving contacts
480 		if ( tw->getContacts ) {
481 			CM_AddContact( tw );
482 			// no need to store the trm vertex more than once as a contact
483 			v->used = false;
484 		}
485 	}
486 }
487 
488 /*
489 ================
490 idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon
491 ================
492 */
TranslateVertexThroughTrmPolygon(cm_traceWork_t * tw,cm_trmPolygon_t * trmpoly,cm_polygon_t * poly,cm_vertex_t * v,idVec3 & endp,idPluecker & pl)493 void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &endp, idPluecker &pl ) {
494 	int i, edgeNum;
495 	float f;
496 	cm_trmEdge_t *edge;
497 
498 	f = CM_TranslationPlaneFraction( trmpoly->plane, v->p, endp );
499 	if ( f < tw->trace.fraction ) {
500 
501 		for ( i = 0; i < trmpoly->numEdges; i++ ) {
502 			edgeNum = trmpoly->edges[i];
503 			edge = tw->edges + abs(edgeNum);
504 
505 			CM_SetVertexSidedness( v, pl, edge->pl, edge->bitNum );
506 			if ( INTSIGNBITSET(edgeNum) ^ ((v->side >> edge->bitNum) & 1) ) {
507 				return;
508 			}
509 		}
510 		if ( f < 0.0f ) {
511 			f = 0.0f;
512 		}
513 		tw->trace.fraction = f;
514 		// collision plane is the inverse trm polygon plane
515 		tw->trace.c.normal = -trmpoly->plane.Normal();
516 		tw->trace.c.dist = -trmpoly->plane.Dist();
517 		tw->trace.c.contents = poly->contents;
518 		tw->trace.c.material = poly->material;
519 		tw->trace.c.type = CONTACT_MODELVERTEX;
520 		tw->trace.c.modelFeature = v - tw->model->vertices;
521 		tw->trace.c.trmFeature = trmpoly - tw->polys;
522 		tw->trace.c.point = v->p + tw->trace.fraction * ( endp - v->p );
523 		// if retrieving contacts
524 		if ( tw->getContacts ) {
525 			CM_AddContact( tw );
526 		}
527 	}
528 }
529 
530 /*
531 ================
532 idCollisionModelManagerLocal::TranslateTrmThroughPolygon
533 
534   returns true if the polygon blocks the complete translation
535 ================
536 */
TranslateTrmThroughPolygon(cm_traceWork_t * tw,cm_polygon_t * p)537 bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
538 	int i, j, k, edgeNum;
539 	float fraction, d;
540 	idVec3 endp;
541 	idPluecker *pl;
542 	cm_trmVertex_t *bv;
543 	cm_trmEdge_t *be;
544 	cm_trmPolygon_t *bp;
545 	cm_vertex_t *v;
546 	cm_edge_t *e;
547 
548 	// if already checked this polygon
549 	if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
550 		return false;
551 	}
552 	p->checkcount = idCollisionModelManagerLocal::checkCount;
553 
554 	// if this polygon does not have the right contents behind it
555 	if ( !(p->contents & tw->contents) ) {
556 		return false;
557 	}
558 
559 	// if the the trace bounds do not intersect the polygon bounds
560 	if ( !tw->bounds.IntersectsBounds( p->bounds ) ) {
561 		return false;
562 	}
563 
564 	// only collide with the polygon if approaching at the front
565 	if ( ( p->plane.Normal() * tw->dir ) > 0.0f ) {
566 		return false;
567 	}
568 
569 	// if the polygon is too far from the first heart plane
570 	d = p->bounds.PlaneDistance( tw->heartPlane1 );
571 	if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) {
572 		return false;
573 	}
574 
575 	// if the polygon is too far from the second heart plane
576 	d = p->bounds.PlaneDistance( tw->heartPlane2 );
577 	if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane2 ) {
578 		return false;
579 	}
580 	fraction = tw->trace.fraction;
581 
582 	// fast point trace
583 	if ( tw->pointTrace ) {
584 		idCollisionModelManagerLocal::TranslatePointThroughPolygon( tw, p, &tw->vertices[0] );
585 	}
586 	else {
587 
588 		// trace bounds should cross polygon plane
589 		switch ( tw->bounds.PlaneSide( p->plane ) ) {
590 			case PLANESIDE_CROSS:
591 				break;
592 			case PLANESIDE_FRONT:
593 				if ( tw->model->isConvex ) {
594 					tw->quickExit = true;
595 					return true;
596 				}
597 			default:
598 				return false;
599 		}
600 
601 		// calculate pluecker coordinates for the polygon edges and polygon vertices
602 		for ( i = 0; i < p->numEdges; i++ ) {
603 			edgeNum = p->edges[i];
604 			e = tw->model->edges + abs(edgeNum);
605 			// reset sidedness cache if this is the first time we encounter this edge during this trace
606 			if ( e->checkcount != idCollisionModelManagerLocal::checkCount ) {
607 				e->sideSet = 0;
608 			}
609 			// pluecker coordinate for edge
610 			tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p,
611 														tw->model->vertices[e->vertexNum[1]].p );
612 
613 			v = &tw->model->vertices[e->vertexNum[INTSIGNBITSET(edgeNum)]];
614 			// reset sidedness cache if this is the first time we encounter this vertex during this trace
615 			if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) {
616 				v->sideSet = 0;
617 			}
618 			// pluecker coordinate for vertex movement vector
619 			tw->polygonVertexPlueckerCache[i].FromRay( v->p, -tw->dir );
620 		}
621 		// copy first to last so we can easily cycle through for the edges
622 		tw->polygonVertexPlueckerCache[p->numEdges] = tw->polygonVertexPlueckerCache[0];
623 
624 		// trace trm vertices through polygon
625 		for ( i = 0; i < tw->numVerts; i++ ) {
626 			bv = tw->vertices + i;
627 			if ( bv->used ) {
628 				idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( tw, p, bv, i );
629 			}
630 		}
631 
632 		// trace trm edges through polygon
633 		for ( i = 1; i <= tw->numEdges; i++ ) {
634 			be = tw->edges + i;
635 			if ( be->used ) {
636 				idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( tw, p, be);
637 			}
638 		}
639 
640 		// trace all polygon vertices through the trm
641 		for ( i = 0; i < p->numEdges; i++ ) {
642 			edgeNum = p->edges[i];
643 			e = tw->model->edges + abs(edgeNum);
644 
645 			if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) {
646 				continue;
647 			}
648 			// set edge check count
649 			e->checkcount = idCollisionModelManagerLocal::checkCount;
650 			// can never collide with internal edges
651 			if ( e->internal ) {
652 				continue;
653 			}
654 			// got to check both vertices because we skip internal edges
655 			for ( k = 0; k < 2; k++ ) {
656 
657 				v = tw->model->vertices + e->vertexNum[k ^ INTSIGNBITSET(edgeNum)];
658 				// if this vertex is already checked
659 				if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
660 					continue;
661 				}
662 				// set vertex check count
663 				v->checkcount = idCollisionModelManagerLocal::checkCount;
664 
665 				// if the vertex is outside the trace bounds
666 				if ( !tw->bounds.ContainsPoint( v->p ) ) {
667 					continue;
668 				}
669 
670 				// vertex end point after movement
671 				endp = v->p - tw->dir;
672 				// pluecker coordinate for vertex movement vector
673 				pl = &tw->polygonVertexPlueckerCache[i+k];
674 
675 				for ( j = 0; j < tw->numPolys; j++ ) {
676 					bp = tw->polys + j;
677 					if ( bp->used ) {
678 						idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( tw, bp, p, v, endp, *pl );
679 					}
680 				}
681 			}
682 		}
683 	}
684 
685 	// if there was a collision with this polygon and we are not retrieving contacts
686 	if ( tw->trace.fraction < fraction && !tw->getContacts ) {
687 		fraction = tw->trace.fraction;
688 		endp = tw->start + fraction * tw->dir;
689 		// decrease bounds
690 		for ( i = 0; i < 3; i++ ) {
691 			if ( tw->start[i] < endp[i] ) {
692 				tw->bounds[0][i] = tw->start[i] + tw->size[0][i] - CM_BOX_EPSILON;
693 				tw->bounds[1][i] = endp[i] + tw->size[1][i] + CM_BOX_EPSILON;
694 			}
695 			else {
696 				tw->bounds[0][i] = endp[i] + tw->size[0][i] - CM_BOX_EPSILON;
697 				tw->bounds[1][i] = tw->start[i] + tw->size[1][i] + CM_BOX_EPSILON;
698 			}
699 		}
700 	}
701 
702 	return ( tw->trace.fraction == 0.0f );
703 }
704 
705 /*
706 ================
707 idCollisionModelManagerLocal::SetupTrm
708 ================
709 */
SetupTrm(cm_traceWork_t * tw,const idTraceModel * trm)710 void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceModel *trm ) {
711 	int i, j;
712 
713 	// vertices
714 	tw->numVerts = trm->numVerts;
715 	for ( i = 0; i < trm->numVerts; i++ ) {
716 		tw->vertices[i].p = trm->verts[i];
717 		tw->vertices[i].used = false;
718 	}
719 	// edges
720 	tw->numEdges = trm->numEdges;
721 	for ( i = 1; i <= trm->numEdges; i++ ) {
722 		tw->edges[i].vertexNum[0] = trm->edges[i].v[0];
723 		tw->edges[i].vertexNum[1] = trm->edges[i].v[1];
724 		tw->edges[i].used = false;
725 	}
726 	// polygons
727 	tw->numPolys = trm->numPolys;
728 	for ( i = 0; i < trm->numPolys; i++ ) {
729 		tw->polys[i].numEdges = trm->polys[i].numEdges;
730 		for ( j = 0; j < trm->polys[i].numEdges; j++ ) {
731 			tw->polys[i].edges[j] = trm->polys[i].edges[j];
732 		}
733 		tw->polys[i].plane.SetNormal( trm->polys[i].normal );
734 		tw->polys[i].used = false;
735 	}
736 	// is the trace model convex or not
737 	tw->isConvex = trm->isConvex;
738 }
739 
740 /*
741 ================
742 idCollisionModelManagerLocal::SetupTranslationHeartPlanes
743 ================
744 */
SetupTranslationHeartPlanes(cm_traceWork_t * tw)745 void idCollisionModelManagerLocal::SetupTranslationHeartPlanes( cm_traceWork_t *tw ) {
746 	idVec3 dir, normal1, normal2;
747 
748 	// calculate trace heart planes
749 	dir = tw->dir;
750 	dir.Normalize();
751 	dir.NormalVectors( normal1, normal2 );
752 	tw->heartPlane1.SetNormal( normal1 );
753 	tw->heartPlane1.FitThroughPoint( tw->start );
754 	tw->heartPlane2.SetNormal( normal2 );
755 	tw->heartPlane2.FitThroughPoint( tw->start );
756 }
757 
758 /*
759 ================
760 idCollisionModelManagerLocal::Translation
761 ================
762 */
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)763 void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 &start, const idVec3 &end,
764 										const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
765 										cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
766 
767 	int i, j;
768 	float dist;
769 	bool model_rotated, trm_rotated;
770 	idVec3 dir1, dir2, dir;
771 	idMat3 invModelAxis, tmpAxis;
772 	cm_trmPolygon_t *poly;
773 	cm_trmEdge_t *edge;
774 	cm_trmVertex_t *vert;
775 	ALIGN16( static cm_traceWork_t tw );
776 
777 	assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) >= (((byte *)results) + sizeof( trace_t )) );
778 	assert( ((byte *)&end) < ((byte *)results) || ((byte *)&end) >= (((byte *)results) + sizeof( trace_t )) );
779 	assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) >= (((byte *)results) + sizeof( trace_t )) );
780 
781 	memset( results, 0, sizeof( *results ) );
782 
783 	if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) {
784 		common->Printf("idCollisionModelManagerLocal::Translation: invalid model handle\n");
785 		return;
786 	}
787 	if ( !idCollisionModelManagerLocal::models[model] ) {
788 		common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n");
789 		return;
790 	}
791 
792 	// if case special position test
793 	if ( start[0] == end[0] && start[1] == end[1] && start[2] == end[2] ) {
794 		idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
795 		return;
796 	}
797 
798 	idCollisionModelManagerLocal::checkCount++;
799 
800 	tw.trace.fraction = 1.0f;
801 	tw.trace.c.contents = 0;
802 	tw.trace.c.type = CONTACT_NONE;
803 	tw.contents = contentMask;
804 	tw.isConvex = true;
805 	tw.rotation = false;
806 	tw.positionTest = false;
807 	tw.quickExit = false;
808 	tw.getContacts = idCollisionModelManagerLocal::getContacts;
809 	tw.contacts = idCollisionModelManagerLocal::contacts;
810 	tw.maxContacts = idCollisionModelManagerLocal::maxContacts;
811 	tw.numContacts = 0;
812 	tw.model = idCollisionModelManagerLocal::models[model];
813 	tw.start = start - modelOrigin;
814 	tw.end = end - modelOrigin;
815 	tw.dir = end - start;
816 
817 	model_rotated = modelAxis.IsRotated();
818 	if ( model_rotated ) {
819 		invModelAxis = modelAxis.Transpose();
820 	}
821 
822 	// if optimized point trace
823 	if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f &&
824 					trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f &&
825 					trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) {
826 
827 		if ( model_rotated ) {
828 			// rotate trace instead of model
829 			tw.start *= invModelAxis;
830 			tw.end *= invModelAxis;
831 			tw.dir *= invModelAxis;
832 		}
833 
834 		// trace bounds
835 		for ( i = 0; i < 3; i++ ) {
836 			if ( tw.start[i] < tw.end[i] ) {
837 				tw.bounds[0][i] = tw.start[i] - CM_BOX_EPSILON;
838 				tw.bounds[1][i] = tw.end[i] + CM_BOX_EPSILON;
839 			}
840 			else {
841 				tw.bounds[0][i] = tw.end[i] - CM_BOX_EPSILON;
842 				tw.bounds[1][i] = tw.start[i] + CM_BOX_EPSILON;
843 			}
844 		}
845 		tw.extents[0] = tw.extents[1] = tw.extents[2] = CM_BOX_EPSILON;
846 		tw.size.Zero();
847 
848 		// setup trace heart planes
849 		idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
850 		tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON;
851 		tw.maxDistFromHeartPlane2 = CM_BOX_EPSILON;
852 		// collision with single point
853 		tw.numVerts = 1;
854 		tw.vertices[0].p = tw.start;
855 		tw.vertices[0].endp = tw.vertices[0].p + tw.dir;
856 		tw.vertices[0].pl.FromRay( tw.vertices[0].p, tw.dir );
857 		tw.numEdges = tw.numPolys = 0;
858 		tw.pointTrace = true;
859 		// trace through the model
860 		idCollisionModelManagerLocal::TraceThroughModel( &tw );
861 		// store results
862 		*results = tw.trace;
863 		results->endpos = start + results->fraction * (end - start);
864 		results->endAxis = mat3_identity;
865 
866 		if ( results->fraction < 1.0f ) {
867 			// rotate trace plane normal if there was a collision with a rotated model
868 			if ( model_rotated ) {
869 				results->c.normal *= modelAxis;
870 				results->c.point *= modelAxis;
871 			}
872 			results->c.point += modelOrigin;
873 			results->c.dist += modelOrigin * results->c.normal;
874 		}
875 		idCollisionModelManagerLocal::numContacts = tw.numContacts;
876 		return;
877 	}
878 
879 	// the trace fraction is too inaccurate to describe translations over huge distances
880 	if ( tw.dir.LengthSqr() > Square( CM_MAX_TRACE_DIST ) ) {
881 		results->fraction = 0.0f;
882 		results->endpos = start;
883 		results->endAxis = trmAxis;
884 		results->c.normal = vec3_origin;
885 		results->c.material = NULL;
886 		results->c.point = start;
887 		if ( session->rw ) {
888 			session->rw->DebugArrow( colorRed, start, end, 1 );
889 		}
890 		common->Printf( "idCollisionModelManagerLocal::Translation: huge translation\n" );
891 		return;
892 	}
893 
894 	tw.pointTrace = false;
895 	tw.size.Clear();
896 
897 	// setup trm structure
898 	idCollisionModelManagerLocal::SetupTrm( &tw, trm );
899 
900 	trm_rotated = trmAxis.IsRotated();
901 
902 	// calculate vertex positions
903 	if ( trm_rotated ) {
904 		for ( i = 0; i < tw.numVerts; i++ ) {
905 			// rotate trm around the start position
906 			tw.vertices[i].p *= trmAxis;
907 		}
908 	}
909 	for ( i = 0; i < tw.numVerts; i++ ) {
910 		// set trm at start position
911 		tw.vertices[i].p += tw.start;
912 	}
913 	if ( model_rotated ) {
914 		for ( i = 0; i < tw.numVerts; i++ ) {
915 			// rotate trm around model instead of rotating the model
916 			tw.vertices[i].p *= invModelAxis;
917 		}
918 	}
919 
920 	// add offset to start point
921 	if ( trm_rotated ) {
922 		dir = trm->offset * trmAxis;
923 		tw.start += dir;
924 		tw.end += dir;
925 	} else {
926 		tw.start += trm->offset;
927 		tw.end += trm->offset;
928 	}
929 	if ( model_rotated ) {
930 		// rotate trace instead of model
931 		tw.start *= invModelAxis;
932 		tw.end *= invModelAxis;
933 		tw.dir *= invModelAxis;
934 	}
935 
936 	// rotate trm polygon planes
937 	if ( trm_rotated & model_rotated ) {
938 		tmpAxis = trmAxis * invModelAxis;
939 		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
940 			poly->plane *= tmpAxis;
941 		}
942 	} else if ( trm_rotated ) {
943 		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
944 			poly->plane *= trmAxis;
945 		}
946 	} else if ( model_rotated ) {
947 		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
948 			poly->plane *= invModelAxis;
949 		}
950 	}
951 
952 	// setup trm polygons
953 	for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
954 		// if the trm poly plane is facing in the movement direction
955 		dist = poly->plane.Normal() * tw.dir;
956 		if ( dist > 0.0f || ( !trm->isConvex && dist == 0.0f ) ) {
957 			// this trm poly and it's edges and vertices need to be used for collision
958 			poly->used = true;
959 			for ( j = 0; j < poly->numEdges; j++ ) {
960 				edge = &tw.edges[abs( poly->edges[j] )];
961 				edge->used = true;
962 				tw.vertices[edge->vertexNum[0]].used = true;
963 				tw.vertices[edge->vertexNum[1]].used = true;
964 			}
965 		}
966 	}
967 
968 	// setup trm vertices
969 	for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
970 		if ( !vert->used ) {
971 			continue;
972 		}
973 		// get axial trm size after rotations
974 		tw.size.AddPoint( vert->p - tw.start );
975 		// calculate the end position of each vertex for a full trace
976 		vert->endp = vert->p + tw.dir;
977 		// pluecker coordinate for vertex movement line
978 		vert->pl.FromRay( vert->p, tw.dir );
979 	}
980 
981 	// setup trm edges
982 	for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) {
983 		if ( !edge->used ) {
984 			continue;
985 		}
986 		// edge start, end and pluecker coordinate
987 		edge->start = tw.vertices[edge->vertexNum[0]].p;
988 		edge->end = tw.vertices[edge->vertexNum[1]].p;
989 		edge->pl.FromLine( edge->start, edge->end );
990 		// calculate normal of plane through movement plane created by the edge
991 		dir = edge->start - edge->end;
992 		edge->cross[0] = dir[0] * tw.dir[1] - dir[1] * tw.dir[0];
993 		edge->cross[1] = dir[0] * tw.dir[2] - dir[2] * tw.dir[0];
994 		edge->cross[2] = dir[1] * tw.dir[2] - dir[2] * tw.dir[1];
995 		// bit for vertex sidedness bit cache
996 		edge->bitNum = i;
997 	}
998 
999 	// set trm plane distances
1000 	for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
1001 		if ( poly->used ) {
1002 			poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start );
1003 		}
1004 	}
1005 
1006 	// bounds for full trace, a little bit larger for epsilons
1007 	for ( i = 0; i < 3; i++ ) {
1008 		if ( tw.start[i] < tw.end[i] ) {
1009 			tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
1010 			tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
1011 		} else {
1012 			tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
1013 			tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
1014 		}
1015 		if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) {
1016 			tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON;
1017 		} else {
1018 			tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON;
1019 		}
1020 	}
1021 
1022 	// setup trace heart planes
1023 	idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw );
1024 	tw.maxDistFromHeartPlane1 = 0;
1025 	tw.maxDistFromHeartPlane2 = 0;
1026 	// calculate maximum trm vertex distance from both heart planes
1027 	for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
1028 		if ( !vert->used ) {
1029 			continue;
1030 		}
1031 		dist = idMath::Fabs( tw.heartPlane1.Distance( vert->p ) );
1032 		if ( dist > tw.maxDistFromHeartPlane1 ) {
1033 			tw.maxDistFromHeartPlane1 = dist;
1034 		}
1035 		dist = idMath::Fabs( tw.heartPlane2.Distance( vert->p ) );
1036 		if ( dist > tw.maxDistFromHeartPlane2 ) {
1037 			tw.maxDistFromHeartPlane2 = dist;
1038 		}
1039 	}
1040 	// for epsilons
1041 	tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON;
1042 	tw.maxDistFromHeartPlane2 += CM_BOX_EPSILON;
1043 
1044 	// trace through the model
1045 	idCollisionModelManagerLocal::TraceThroughModel( &tw );
1046 
1047 	// if we're getting contacts
1048 	if ( tw.getContacts ) {
1049 		// move all contacts to world space
1050 		if ( model_rotated ) {
1051 			for ( i = 0; i < tw.numContacts; i++ ) {
1052 				tw.contacts[i].normal *= modelAxis;
1053 				tw.contacts[i].point *= modelAxis;
1054 			}
1055 		}
1056 		if ( modelOrigin != vec3_origin ) {
1057 			for ( i = 0; i < tw.numContacts; i++ ) {
1058 				tw.contacts[i].point += modelOrigin;
1059 				tw.contacts[i].dist += modelOrigin * tw.contacts[i].normal;
1060 			}
1061 		}
1062 		idCollisionModelManagerLocal::numContacts = tw.numContacts;
1063 	} else {
1064 		// store results
1065 		*results = tw.trace;
1066 		results->endpos = start + results->fraction * ( end - start );
1067 		results->endAxis = trmAxis;
1068 
1069 		if ( results->fraction < 1.0f ) {
1070 			// if the fraction is tiny the actual movement could end up zero
1071 			if ( results->fraction > 0.0f && results->endpos.Compare( start ) ) {
1072 				results->fraction = 0.0f;
1073 			}
1074 			// rotate trace plane normal if there was a collision with a rotated model
1075 			if ( model_rotated ) {
1076 				results->c.normal *= modelAxis;
1077 				results->c.point *= modelAxis;
1078 			}
1079 			results->c.point += modelOrigin;
1080 			results->c.dist += modelOrigin * results->c.normal;
1081 		}
1082 	}
1083 
1084 #ifdef _DEBUG
1085 	// test for collisions
1086 	if ( cm_debugCollision.GetBool() ) {
1087 		if (!idCollisionModelManagerLocal::getContacts ) {
1088 			// if the trm is stuck in the model
1089 			if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
1090 				trace_t tr;
1091 
1092 				// test where the trm is stuck in the model
1093 				idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis );
1094 				// re-run collision detection to find out where it failed
1095 				idCollisionModelManagerLocal::Translation( &tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
1096 			}
1097 		}
1098 	}
1099 #endif
1100 }
1101