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
39 #include "cm/CollisionModel_local.h"
40
41 /*
42 ===============================================================================
43
44 Contents test
45
46 ===============================================================================
47 */
48
49 /*
50 ================
51 idCollisionModelManagerLocal::TestTrmVertsInBrush
52
53 returns true if any of the trm vertices is inside the brush
54 ================
55 */
TestTrmVertsInBrush(cm_traceWork_t * tw,cm_brush_t * b)56 bool idCollisionModelManagerLocal::TestTrmVertsInBrush( cm_traceWork_t *tw, cm_brush_t *b ) {
57 int i, j, numVerts, bestPlane;
58 float d, bestd;
59 idVec3 *p;
60
61 if ( b->checkcount == idCollisionModelManagerLocal::checkCount ) {
62 return false;
63 }
64 b->checkcount = idCollisionModelManagerLocal::checkCount;
65
66 if ( !(b->contents & tw->contents) ) {
67 return false;
68 }
69
70 // if the brush bounds don't intersect the trace bounds
71 if ( !b->bounds.IntersectsBounds( tw->bounds ) ) {
72 return false;
73 }
74
75 if ( tw->pointTrace ) {
76 numVerts = 1;
77 }
78 else {
79 numVerts = tw->numVerts;
80 }
81
82 for ( j = 0; j < numVerts; j++ ) {
83 p = &tw->vertices[j].p;
84
85 // see if the point is inside the brush
86 bestPlane = 0;
87 bestd = -idMath::INFINITY;
88 for ( i = 0; i < b->numPlanes; i++ ) {
89 d = b->planes[i].Distance( *p );
90 if ( d >= 0.0f ) {
91 break;
92 }
93 if ( d > bestd ) {
94 bestd = d;
95 bestPlane = i;
96 }
97 }
98 if ( i >= b->numPlanes ) {
99 tw->trace.fraction = 0.0f;
100 tw->trace.c.type = CONTACT_TRMVERTEX;
101 tw->trace.c.normal = b->planes[bestPlane].Normal();
102 tw->trace.c.dist = b->planes[bestPlane].Dist();
103 tw->trace.c.contents = b->contents;
104 tw->trace.c.material = b->material;
105 tw->trace.c.point = *p;
106 tw->trace.c.modelFeature = 0;
107 tw->trace.c.trmFeature = j;
108 return true;
109 }
110 }
111 return false;
112 }
113
114 /*
115 ================
116 CM_SetTrmEdgeSidedness
117 ================
118 */
119 #define CM_SetTrmEdgeSidedness( edge, bpl, epl, bitNum ) { \
120 if ( !(edge->sideSet & (1<<bitNum)) ) { \
121 float fl; \
122 fl = (bpl).PermutedInnerProduct( epl ); \
123 edge->side = (edge->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum); \
124 edge->sideSet |= (1 << bitNum); \
125 } \
126 }
127
128 /*
129 ================
130 CM_SetTrmPolygonSidedness
131 ================
132 */
133 #define CM_SetTrmPolygonSidedness( v, plane, bitNum ) { \
134 if ( !((v)->sideSet & (1<<bitNum)) ) { \
135 float fl; \
136 fl = plane.Distance( (v)->p ); \
137 /* cannot use float sign bit because it is undetermined when fl == 0.0f */ \
138 if ( fl < 0.0f ) { \
139 (v)->side |= (1 << bitNum); \
140 } \
141 else { \
142 (v)->side &= ~(1 << bitNum); \
143 } \
144 (v)->sideSet |= (1 << bitNum); \
145 } \
146 }
147
148 /*
149 ================
150 idCollisionModelManagerLocal::TestTrmInPolygon
151
152 returns true if the trm intersects the polygon
153 ================
154 */
TestTrmInPolygon(cm_traceWork_t * tw,cm_polygon_t * p)155 bool idCollisionModelManagerLocal::TestTrmInPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
156 int i, j, k, edgeNum, flip, trmEdgeNum, bitNum, bestPlane;
157 int sides[MAX_TRACEMODEL_VERTS];
158 float d, bestd;
159 cm_trmEdge_t *trmEdge;
160 cm_edge_t *edge;
161 cm_vertex_t *v, *v1, *v2;
162
163 // if already checked this polygon
164 if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
165 return false;
166 }
167 p->checkcount = idCollisionModelManagerLocal::checkCount;
168
169 // if this polygon does not have the right contents behind it
170 if ( !(p->contents & tw->contents) ) {
171 return false;
172 }
173
174 // if the polygon bounds don't intersect the trace bounds
175 if ( !p->bounds.IntersectsBounds( tw->bounds ) ) {
176 return false;
177 }
178
179 // bounds should cross polygon plane
180 switch( tw->bounds.PlaneSide( p->plane ) ) {
181 case PLANESIDE_CROSS:
182 break;
183 case PLANESIDE_FRONT:
184 if ( tw->model->isConvex ) {
185 tw->quickExit = true;
186 return true;
187 }
188 default:
189 return false;
190 }
191
192 // if the trace model is convex
193 if ( tw->isConvex ) {
194 // test if any polygon vertices are inside the trm
195 for ( i = 0; i < p->numEdges; i++ ) {
196 edgeNum = p->edges[i];
197 edge = tw->model->edges + abs(edgeNum);
198 // if this edge is already tested
199 if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
200 continue;
201 }
202
203 for ( j = 0; j < 2; j++ ) {
204 v = &tw->model->vertices[edge->vertexNum[j]];
205 // if this vertex is already tested
206 if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
207 continue;
208 }
209
210 bestPlane = 0;
211 bestd = -idMath::INFINITY;
212 for ( k = 0; k < tw->numPolys; k++ ) {
213 d = tw->polys[k].plane.Distance( v->p );
214 if ( d >= 0.0f ) {
215 break;
216 }
217 if ( d > bestd ) {
218 bestd = d;
219 bestPlane = k;
220 }
221 }
222 if ( k >= tw->numPolys ) {
223 tw->trace.fraction = 0.0f;
224 tw->trace.c.type = CONTACT_MODELVERTEX;
225 tw->trace.c.normal = -tw->polys[bestPlane].plane.Normal();
226 tw->trace.c.dist = -tw->polys[bestPlane].plane.Dist();
227 tw->trace.c.contents = p->contents;
228 tw->trace.c.material = p->material;
229 tw->trace.c.point = v->p;
230 tw->trace.c.modelFeature = edge->vertexNum[j];
231 tw->trace.c.trmFeature = 0;
232 return true;
233 }
234 }
235 }
236 }
237
238 for ( i = 0; i < p->numEdges; i++ ) {
239 edgeNum = p->edges[i];
240 edge = tw->model->edges + abs(edgeNum);
241 // reset sidedness cache if this is the first time we encounter this edge
242 if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) {
243 edge->sideSet = 0;
244 }
245 // pluecker coordinate for edge
246 tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[edge->vertexNum[0]].p,
247 tw->model->vertices[edge->vertexNum[1]].p );
248 v = &tw->model->vertices[edge->vertexNum[INTSIGNBITSET(edgeNum)]];
249 // reset sidedness cache if this is the first time we encounter this vertex
250 if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) {
251 v->sideSet = 0;
252 }
253 v->checkcount = idCollisionModelManagerLocal::checkCount;
254 }
255
256 // get side of polygon for each trm vertex
257 for ( i = 0; i < tw->numVerts; i++ ) {
258 d = p->plane.Distance( tw->vertices[i].p );
259 sides[i] = d < 0.0f ? -1 : 1;
260 }
261
262 // test if any trm edges go through the polygon
263 for ( i = 1; i <= tw->numEdges; i++ ) {
264 // if the trm edge does not cross the polygon plane
265 if ( sides[tw->edges[i].vertexNum[0]] == sides[tw->edges[i].vertexNum[1]] ) {
266 continue;
267 }
268 // check from which side to which side the trm edge goes
269 flip = INTSIGNBITSET( sides[tw->edges[i].vertexNum[0]] );
270 // test if trm edge goes through the polygon between the polygon edges
271 for ( j = 0; j < p->numEdges; j++ ) {
272 edgeNum = p->edges[j];
273 edge = tw->model->edges + abs(edgeNum);
274 #if 1
275 CM_SetTrmEdgeSidedness( edge, tw->edges[i].pl, tw->polygonEdgePlueckerCache[j], i );
276 if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> i) & 1) ^ flip ) {
277 break;
278 }
279 #else
280 d = tw->edges[i].pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[j] );
281 if ( flip ) {
282 d = -d;
283 }
284 if ( edgeNum > 0 ) {
285 if ( d <= 0.0f ) {
286 break;
287 }
288 }
289 else {
290 if ( d >= 0.0f ) {
291 break;
292 }
293 }
294 #endif
295 }
296 if ( j >= p->numEdges ) {
297 tw->trace.fraction = 0.0f;
298 tw->trace.c.type = CONTACT_EDGE;
299 tw->trace.c.normal = p->plane.Normal();
300 tw->trace.c.dist = p->plane.Dist();
301 tw->trace.c.contents = p->contents;
302 tw->trace.c.material = p->material;
303 tw->trace.c.point = tw->vertices[tw->edges[i].vertexNum[ !flip ]].p;
304 tw->trace.c.modelFeature = *reinterpret_cast<int *>(&p);
305 tw->trace.c.trmFeature = i;
306 return true;
307 }
308 }
309
310 // test if any polygon edges go through the trm polygons
311 for ( i = 0; i < p->numEdges; i++ ) {
312 edgeNum = p->edges[i];
313 edge = tw->model->edges + abs(edgeNum);
314 if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
315 continue;
316 }
317 edge->checkcount = idCollisionModelManagerLocal::checkCount;
318
319 for ( j = 0; j < tw->numPolys; j++ ) {
320 #if 1
321 v1 = tw->model->vertices + edge->vertexNum[0];
322 CM_SetTrmPolygonSidedness( v1, tw->polys[j].plane, j );
323 v2 = tw->model->vertices + edge->vertexNum[1];
324 CM_SetTrmPolygonSidedness( v2, tw->polys[j].plane, j );
325 // if the polygon edge does not cross the trm polygon plane
326 if ( !(((v1->side ^ v2->side) >> j) & 1) ) {
327 continue;
328 }
329 flip = (v1->side >> j) & 1;
330 #else
331 float d1, d2;
332
333 v1 = tw->model->vertices + edge->vertexNum[0];
334 d1 = tw->polys[j].plane.Distance( v1->p );
335 v2 = tw->model->vertices + edge->vertexNum[1];
336 d2 = tw->polys[j].plane.Distance( v2->p );
337 // if the polygon edge does not cross the trm polygon plane
338 if ( (d1 >= 0.0f && d2 >= 0.0f) || (d1 <= 0.0f && d2 <= 0.0f) ) {
339 continue;
340 }
341 flip = false;
342 if ( d1 < 0.0f ) {
343 flip = true;
344 }
345 #endif
346 // test if polygon edge goes through the trm polygon between the trm polygon edges
347 for ( k = 0; k < tw->polys[j].numEdges; k++ ) {
348 trmEdgeNum = tw->polys[j].edges[k];
349 trmEdge = tw->edges + abs(trmEdgeNum);
350 #if 1
351 bitNum = abs(trmEdgeNum);
352 CM_SetTrmEdgeSidedness( edge, trmEdge->pl, tw->polygonEdgePlueckerCache[i], bitNum );
353 if ( INTSIGNBITSET(trmEdgeNum) ^ ((edge->side >> bitNum) & 1) ^ flip ) {
354 break;
355 }
356 #else
357 d = trmEdge->pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] );
358 if ( flip ) {
359 d = -d;
360 }
361 if ( trmEdgeNum > 0 ) {
362 if ( d <= 0.0f ) {
363 break;
364 }
365 }
366 else {
367 if ( d >= 0.0f ) {
368 break;
369 }
370 }
371 #endif
372 }
373 if ( k >= tw->polys[j].numEdges ) {
374 tw->trace.fraction = 0.0f;
375 tw->trace.c.type = CONTACT_EDGE;
376 tw->trace.c.normal = -tw->polys[j].plane.Normal();
377 tw->trace.c.dist = -tw->polys[j].plane.Dist();
378 tw->trace.c.contents = p->contents;
379 tw->trace.c.material = p->material;
380 tw->trace.c.point = tw->model->vertices[edge->vertexNum[ !flip ]].p;
381 tw->trace.c.modelFeature = edgeNum;
382 tw->trace.c.trmFeature = j;
383 return true;
384 }
385 }
386 }
387 return false;
388 }
389
390 /*
391 ================
392 idCollisionModelManagerLocal::PointNode
393 ================
394 */
PointNode(const idVec3 & p,cm_model_t * model)395 cm_node_t *idCollisionModelManagerLocal::PointNode( const idVec3 &p, cm_model_t *model ) {
396 cm_node_t *node;
397
398 node = model->node;
399 while ( node->planeType != -1 ) {
400 if (p[node->planeType] > node->planeDist) {
401 node = node->children[0];
402 }
403 else {
404 node = node->children[1];
405 }
406
407 assert( node != NULL );
408 }
409 return node;
410 }
411
412 /*
413 ================
414 idCollisionModelManagerLocal::PointContents
415 ================
416 */
PointContents(const idVec3 p,cmHandle_t model)417 int idCollisionModelManagerLocal::PointContents( const idVec3 p, cmHandle_t model ) {
418 int i;
419 float d;
420 cm_node_t *node;
421 cm_brushRef_t *bref;
422 cm_brush_t *b;
423 idPlane *plane;
424
425 node = idCollisionModelManagerLocal::PointNode( p, idCollisionModelManagerLocal::models[model] );
426 for ( bref = node->brushes; bref; bref = bref->next ) {
427 b = bref->b;
428 // test if the point is within the brush bounds
429 for ( i = 0; i < 3; i++ ) {
430 if ( p[i] < b->bounds[0][i] ) {
431 break;
432 }
433 if ( p[i] > b->bounds[1][i] ) {
434 break;
435 }
436 }
437 if ( i < 3 ) {
438 continue;
439 }
440 // test if the point is inside the brush
441 plane = b->planes;
442 for ( i = 0; i < b->numPlanes; i++, plane++ ) {
443 d = plane->Distance( p );
444 if ( d >= 0 ) {
445 break;
446 }
447 }
448 if ( i >= b->numPlanes ) {
449 return b->contents;
450 }
451 }
452 return 0;
453 }
454
455 /*
456 ==================
457 idCollisionModelManagerLocal::TransformedPointContents
458 ==================
459 */
TransformedPointContents(const idVec3 & p,cmHandle_t model,const idVec3 & origin,const idMat3 & modelAxis)460 int idCollisionModelManagerLocal::TransformedPointContents( const idVec3 &p, cmHandle_t model, const idVec3 &origin, const idMat3 &modelAxis ) {
461 idVec3 p_l;
462
463 // subtract origin offset
464 p_l = p - origin;
465 if ( modelAxis.IsRotated() ) {
466 p_l *= modelAxis;
467 }
468 return idCollisionModelManagerLocal::PointContents( p_l, model );
469 }
470
471
472 /*
473 ==================
474 idCollisionModelManagerLocal::ContentsTrm
475 ==================
476 */
ContentsTrm(trace_t * results,const idVec3 & start,const idTraceModel * trm,const idMat3 & trmAxis,int contentMask,cmHandle_t model,const idVec3 & modelOrigin,const idMat3 & modelAxis)477 int idCollisionModelManagerLocal::ContentsTrm( trace_t *results, const idVec3 &start,
478 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
479 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
480 int i;
481 bool model_rotated, trm_rotated;
482 idMat3 invModelAxis, tmpAxis;
483 idVec3 dir;
484 ALIGN16( cm_traceWork_t tw );
485
486 // fast point case
487 if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f &&
488 trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f &&
489 trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) {
490
491 results->c.contents = idCollisionModelManagerLocal::TransformedPointContents( start, model, modelOrigin, modelAxis );
492 results->fraction = ( results->c.contents == 0 );
493 results->endpos = start;
494 results->endAxis = trmAxis;
495
496 return results->c.contents;
497 }
498
499 idCollisionModelManagerLocal::checkCount++;
500
501 tw.trace.fraction = 1.0f;
502 tw.trace.c.contents = 0;
503 tw.trace.c.type = CONTACT_NONE;
504 tw.contents = contentMask;
505 tw.isConvex = true;
506 tw.rotation = false;
507 tw.positionTest = true;
508 tw.pointTrace = false;
509 tw.quickExit = false;
510 tw.numContacts = 0;
511 tw.model = idCollisionModelManagerLocal::models[model];
512 tw.start = start - modelOrigin;
513 tw.end = tw.start;
514
515 model_rotated = modelAxis.IsRotated();
516 if ( model_rotated ) {
517 invModelAxis = modelAxis.Transpose();
518 }
519
520 // setup trm structure
521 idCollisionModelManagerLocal::SetupTrm( &tw, trm );
522
523 trm_rotated = trmAxis.IsRotated();
524
525 // calculate vertex positions
526 if ( trm_rotated ) {
527 for ( i = 0; i < tw.numVerts; i++ ) {
528 // rotate trm around the start position
529 tw.vertices[i].p *= trmAxis;
530 }
531 }
532 for ( i = 0; i < tw.numVerts; i++ ) {
533 // set trm at start position
534 tw.vertices[i].p += tw.start;
535 }
536 if ( model_rotated ) {
537 for ( i = 0; i < tw.numVerts; i++ ) {
538 // rotate trm around model instead of rotating the model
539 tw.vertices[i].p *= invModelAxis;
540 }
541 }
542
543 // add offset to start point
544 if ( trm_rotated ) {
545 dir = trm->offset * trmAxis;
546 tw.start += dir;
547 tw.end += dir;
548 } else {
549 tw.start += trm->offset;
550 tw.end += trm->offset;
551 }
552 if ( model_rotated ) {
553 // rotate trace instead of model
554 tw.start *= invModelAxis;
555 tw.end *= invModelAxis;
556 }
557
558
559 // setup trm vertices
560 tw.size.Clear();
561 for ( i = 0; i < tw.numVerts; i++ ) {
562 // get axial trm size after rotations
563 tw.size.AddPoint( tw.vertices[i].p - tw.start );
564 }
565
566 // setup trm edges
567 for ( i = 1; i <= tw.numEdges; i++ ) {
568 // edge start, end and pluecker coordinate
569 tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
570 tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
571 tw.edges[i].pl.FromLine( tw.edges[i].start, tw.edges[i].end );
572 }
573
574 // setup trm polygons
575 if ( trm_rotated & model_rotated ) {
576 tmpAxis = trmAxis * invModelAxis;
577 for ( i = 0; i < tw.numPolys; i++ ) {
578 tw.polys[i].plane *= tmpAxis;
579 }
580 } else if ( trm_rotated ) {
581 for ( i = 0; i < tw.numPolys; i++ ) {
582 tw.polys[i].plane *= trmAxis;
583 }
584 } else if ( model_rotated ) {
585 for ( i = 0; i < tw.numPolys; i++ ) {
586 tw.polys[i].plane *= invModelAxis;
587 }
588 }
589 for ( i = 0; i < tw.numPolys; i++ ) {
590 tw.polys[i].plane.FitThroughPoint( tw.edges[abs(tw.polys[i].edges[0])].start );
591 }
592
593 // bounds for full trace, a little bit larger for epsilons
594 for ( i = 0; i < 3; i++ ) {
595 if ( tw.start[i] < tw.end[i] ) {
596 tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
597 tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
598 } else {
599 tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
600 tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
601 }
602 if ( idMath::Fabs(tw.size[0][i]) > idMath::Fabs(tw.size[1][i]) ) {
603 tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON;
604 } else {
605 tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON;
606 }
607 }
608
609 // trace through the model
610 idCollisionModelManagerLocal::TraceThroughModel( &tw );
611
612 *results = tw.trace;
613 results->fraction = ( results->c.contents == 0 );
614 results->endpos = start;
615 results->endAxis = trmAxis;
616
617 return results->c.contents;
618 }
619
620 /*
621 ==================
622 idCollisionModelManagerLocal::Contents
623 ==================
624 */
Contents(const idVec3 & start,const idTraceModel * trm,const idMat3 & trmAxis,int contentMask,cmHandle_t model,const idVec3 & modelOrigin,const idMat3 & modelAxis)625 int idCollisionModelManagerLocal::Contents( const idVec3 &start,
626 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
627 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
628 trace_t results;
629
630 if ( model < 0 || model > idCollisionModelManagerLocal::maxModels || model > MAX_SUBMODELS ) {
631 common->Printf("idCollisionModelManagerLocal::Contents: invalid model handle\n");
632 return 0;
633 }
634 if ( !idCollisionModelManagerLocal::models || !idCollisionModelManagerLocal::models[model] ) {
635 common->Printf("idCollisionModelManagerLocal::Contents: invalid model\n");
636 return 0;
637 }
638
639 return ContentsTrm( &results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
640 }
641