2 ===========================================================================
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
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.
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.
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/>.
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.
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.
26 ===========================================================================
30 ===============================================================================
32 Trace model vs. polygonal model collision detection.
34 ===============================================================================
37 #include "../idlib/precompiled.h"
40 #include "CollisionModel_local.h"
43 ===============================================================================
47 ===============================================================================
52 idCollisionModelManagerLocal::TestTrmVertsInBrush
54 returns true if any of the trm vertices is inside the brush
57 bool idCollisionModelManagerLocal::TestTrmVertsInBrush( cm_traceWork_t *tw, cm_brush_t *b ) {
58 int i, j, numVerts, bestPlane;
62 if ( b->checkcount == idCollisionModelManagerLocal::checkCount ) {
65 b->checkcount = idCollisionModelManagerLocal::checkCount;
67 if ( !(b->contents & tw->contents) ) {
71 // if the brush bounds don't intersect the trace bounds
72 if ( !b->bounds.IntersectsBounds( tw->bounds ) ) {
76 if ( tw->pointTrace ) {
80 numVerts = tw->numVerts;
83 for ( j = 0; j < numVerts; j++ ) {
84 p = &tw->vertices[j].p;
86 // see if the point is inside the brush
88 bestd = -idMath::INFINITY;
89 for ( i = 0; i < b->numPlanes; i++ ) {
90 d = b->planes[i].Distance( *p );
99 if ( i >= b->numPlanes ) {
100 tw->trace.fraction = 0.0f;
101 tw->trace.c.type = CONTACT_TRMVERTEX;
102 tw->trace.c.normal = b->planes[bestPlane].Normal();
103 tw->trace.c.dist = b->planes[bestPlane].Dist();
104 tw->trace.c.contents = b->contents;
105 tw->trace.c.material = b->material;
106 tw->trace.c.point = *p;
107 tw->trace.c.modelFeature = 0;
108 tw->trace.c.trmFeature = j;
117 CM_SetTrmEdgeSidedness
120 #define CM_SetTrmEdgeSidedness( edge, bpl, epl, bitNum ) { \
121 if ( !(edge->sideSet & (1<<bitNum)) ) { \
123 fl = (bpl).PermutedInnerProduct( epl ); \
124 edge->side = (edge->side & ~(1<<bitNum)) | (FLOATSIGNBITSET(fl) << bitNum); \
125 edge->sideSet |= (1 << bitNum); \
131 CM_SetTrmPolygonSidedness
134 #define CM_SetTrmPolygonSidedness( v, plane, bitNum ) { \
135 if ( !((v)->sideSet & (1<<bitNum)) ) { \
137 fl = plane.Distance( (v)->p ); \
138 /* cannot use float sign bit because it is undetermined when fl == 0.0f */ \
140 (v)->side |= (1 << bitNum); \
143 (v)->side &= ~(1 << bitNum); \
145 (v)->sideSet |= (1 << bitNum); \
151 idCollisionModelManagerLocal::TestTrmInPolygon
153 returns true if the trm intersects the polygon
156 bool idCollisionModelManagerLocal::TestTrmInPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
157 int i, j, k, edgeNum, flip, trmEdgeNum, bitNum, bestPlane;
158 int sides[MAX_TRACEMODEL_VERTS];
160 cm_trmEdge_t *trmEdge;
162 cm_vertex_t *v, *v1, *v2;
164 // if already checked this polygon
165 if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
168 p->checkcount = idCollisionModelManagerLocal::checkCount;
170 // if this polygon does not have the right contents behind it
171 if ( !(p->contents & tw->contents) ) {
175 // if the polygon bounds don't intersect the trace bounds
176 if ( !p->bounds.IntersectsBounds( tw->bounds ) ) {
180 // bounds should cross polygon plane
181 switch( tw->bounds.PlaneSide( p->plane ) ) {
182 case PLANESIDE_CROSS:
184 case PLANESIDE_FRONT:
185 if ( tw->model->isConvex ) {
186 tw->quickExit = true;
193 // if the trace model is convex
194 if ( tw->isConvex ) {
195 // test if any polygon vertices are inside the trm
196 for ( i = 0; i < p->numEdges; i++ ) {
197 edgeNum = p->edges[i];
198 edge = tw->model->edges + abs(edgeNum);
199 // if this edge is already tested
200 if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
204 for ( j = 0; j < 2; j++ ) {
205 v = &tw->model->vertices[edge->vertexNum[j]];
206 // if this vertex is already tested
207 if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
212 bestd = -idMath::INFINITY;
213 for ( k = 0; k < tw->numPolys; k++ ) {
214 d = tw->polys[k].plane.Distance( v->p );
223 if ( k >= tw->numPolys ) {
224 tw->trace.fraction = 0.0f;
225 tw->trace.c.type = CONTACT_MODELVERTEX;
226 tw->trace.c.normal = -tw->polys[bestPlane].plane.Normal();
227 tw->trace.c.dist = -tw->polys[bestPlane].plane.Dist();
228 tw->trace.c.contents = p->contents;
229 tw->trace.c.material = p->material;
230 tw->trace.c.point = v->p;
231 tw->trace.c.modelFeature = edge->vertexNum[j];
232 tw->trace.c.trmFeature = 0;
239 for ( i = 0; i < p->numEdges; i++ ) {
240 edgeNum = p->edges[i];
241 edge = tw->model->edges + abs(edgeNum);
242 // reset sidedness cache if this is the first time we encounter this edge
243 if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) {
246 // pluecker coordinate for edge
247 tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[edge->vertexNum[0]].p,
248 tw->model->vertices[edge->vertexNum[1]].p );
249 v = &tw->model->vertices[edge->vertexNum[INTSIGNBITSET(edgeNum)]];
250 // reset sidedness cache if this is the first time we encounter this vertex
251 if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) {
254 v->checkcount = idCollisionModelManagerLocal::checkCount;
257 // get side of polygon for each trm vertex
258 for ( i = 0; i < tw->numVerts; i++ ) {
259 d = p->plane.Distance( tw->vertices[i].p );
260 sides[i] = d < 0.0f ? -1 : 1;
263 // test if any trm edges go through the polygon
264 for ( i = 1; i <= tw->numEdges; i++ ) {
265 // if the trm edge does not cross the polygon plane
266 if ( sides[tw->edges[i].vertexNum[0]] == sides[tw->edges[i].vertexNum[1]] ) {
269 // check from which side to which side the trm edge goes
270 flip = INTSIGNBITSET( sides[tw->edges[i].vertexNum[0]] );
271 // test if trm edge goes through the polygon between the polygon edges
272 for ( j = 0; j < p->numEdges; j++ ) {
273 edgeNum = p->edges[j];
274 edge = tw->model->edges + abs(edgeNum);
276 CM_SetTrmEdgeSidedness( edge, tw->edges[i].pl, tw->polygonEdgePlueckerCache[j], i );
277 if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> i) & 1) ^ flip ) {
281 d = tw->edges[i].pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[j] );
297 if ( j >= p->numEdges ) {
298 tw->trace.fraction = 0.0f;
299 tw->trace.c.type = CONTACT_EDGE;
300 tw->trace.c.normal = p->plane.Normal();
301 tw->trace.c.dist = p->plane.Dist();
302 tw->trace.c.contents = p->contents;
303 tw->trace.c.material = p->material;
304 tw->trace.c.point = tw->vertices[tw->edges[i].vertexNum[ !flip ]].p;
305 tw->trace.c.modelFeature = *reinterpret_cast<int *>(&p);
306 tw->trace.c.trmFeature = i;
311 // test if any polygon edges go through the trm polygons
312 for ( i = 0; i < p->numEdges; i++ ) {
313 edgeNum = p->edges[i];
314 edge = tw->model->edges + abs(edgeNum);
315 if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
318 edge->checkcount = idCollisionModelManagerLocal::checkCount;
320 for ( j = 0; j < tw->numPolys; j++ ) {
322 v1 = tw->model->vertices + edge->vertexNum[0];
323 CM_SetTrmPolygonSidedness( v1, tw->polys[j].plane, j );
324 v2 = tw->model->vertices + edge->vertexNum[1];
325 CM_SetTrmPolygonSidedness( v2, tw->polys[j].plane, j );
326 // if the polygon edge does not cross the trm polygon plane
327 if ( !(((v1->side ^ v2->side) >> j) & 1) ) {
330 flip = (v1->side >> j) & 1;
334 v1 = tw->model->vertices + edge->vertexNum[0];
335 d1 = tw->polys[j].plane.Distance( v1->p );
336 v2 = tw->model->vertices + edge->vertexNum[1];
337 d2 = tw->polys[j].plane.Distance( v2->p );
338 // if the polygon edge does not cross the trm polygon plane
339 if ( (d1 >= 0.0f && d2 >= 0.0f) || (d1 <= 0.0f && d2 <= 0.0f) ) {
347 // test if polygon edge goes through the trm polygon between the trm polygon edges
348 for ( k = 0; k < tw->polys[j].numEdges; k++ ) {
349 trmEdgeNum = tw->polys[j].edges[k];
350 trmEdge = tw->edges + abs(trmEdgeNum);
352 bitNum = abs(trmEdgeNum);
353 CM_SetTrmEdgeSidedness( edge, trmEdge->pl, tw->polygonEdgePlueckerCache[i], bitNum );
354 if ( INTSIGNBITSET(trmEdgeNum) ^ ((edge->side >> bitNum) & 1) ^ flip ) {
358 d = trmEdge->pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] );
362 if ( trmEdgeNum > 0 ) {
374 if ( k >= tw->polys[j].numEdges ) {
375 tw->trace.fraction = 0.0f;
376 tw->trace.c.type = CONTACT_EDGE;
377 tw->trace.c.normal = -tw->polys[j].plane.Normal();
378 tw->trace.c.dist = -tw->polys[j].plane.Dist();
379 tw->trace.c.contents = p->contents;
380 tw->trace.c.material = p->material;
381 tw->trace.c.point = tw->model->vertices[edge->vertexNum[ !flip ]].p;
382 tw->trace.c.modelFeature = edgeNum;
383 tw->trace.c.trmFeature = j;
393 idCollisionModelManagerLocal::PointNode
396 cm_node_t *idCollisionModelManagerLocal::PointNode( const idVec3 &p, cm_model_t *model ) {
400 while ( node->planeType != -1 ) {
401 if (p[node->planeType] > node->planeDist) {
402 node = node->children[0];
405 node = node->children[1];
408 assert( node != NULL );
415 idCollisionModelManagerLocal::PointContents
418 int idCollisionModelManagerLocal::PointContents( const idVec3 p, cmHandle_t model ) {
426 node = idCollisionModelManagerLocal::PointNode( p, idCollisionModelManagerLocal::models[model] );
427 for ( bref = node->brushes; bref; bref = bref->next ) {
429 // test if the point is within the brush bounds
430 for ( i = 0; i < 3; i++ ) {
431 if ( p[i] < b->bounds[0][i] ) {
434 if ( p[i] > b->bounds[1][i] ) {
441 // test if the point is inside the brush
443 for ( i = 0; i < b->numPlanes; i++, plane++ ) {
444 d = plane->Distance( p );
449 if ( i >= b->numPlanes ) {
458 idCollisionModelManagerLocal::TransformedPointContents
461 int idCollisionModelManagerLocal::TransformedPointContents( const idVec3 &p, cmHandle_t model, const idVec3 &origin, const idMat3 &modelAxis ) {
464 // subtract origin offset
466 if ( modelAxis.IsRotated() ) {
469 return idCollisionModelManagerLocal::PointContents( p_l, model );
475 idCollisionModelManagerLocal::ContentsTrm
478 int idCollisionModelManagerLocal::ContentsTrm( trace_t *results, const idVec3 &start,
479 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
480 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
482 bool model_rotated, trm_rotated;
483 idMat3 invModelAxis, tmpAxis;
485 ALIGN16( cm_traceWork_t tw );
488 if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f &&
489 trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f &&
490 trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) {
492 results->c.contents = idCollisionModelManagerLocal::TransformedPointContents( start, model, modelOrigin, modelAxis );
493 results->fraction = ( results->c.contents == 0 );
494 results->endpos = start;
495 results->endAxis = trmAxis;
497 return results->c.contents;
500 idCollisionModelManagerLocal::checkCount++;
502 tw.trace.fraction = 1.0f;
503 tw.trace.c.contents = 0;
504 tw.trace.c.type = CONTACT_NONE;
505 tw.contents = contentMask;
508 tw.positionTest = true;
509 tw.pointTrace = false;
510 tw.quickExit = false;
512 tw.model = idCollisionModelManagerLocal::models[model];
513 tw.start = start - modelOrigin;
516 model_rotated = modelAxis.IsRotated();
517 if ( model_rotated ) {
518 invModelAxis = modelAxis.Transpose();
521 // setup trm structure
522 idCollisionModelManagerLocal::SetupTrm( &tw, trm );
524 trm_rotated = trmAxis.IsRotated();
526 // calculate vertex positions
528 for ( i = 0; i < tw.numVerts; i++ ) {
529 // rotate trm around the start position
530 tw.vertices[i].p *= trmAxis;
533 for ( i = 0; i < tw.numVerts; i++ ) {
534 // set trm at start position
535 tw.vertices[i].p += tw.start;
537 if ( model_rotated ) {
538 for ( i = 0; i < tw.numVerts; i++ ) {
539 // rotate trm around model instead of rotating the model
540 tw.vertices[i].p *= invModelAxis;
544 // add offset to start point
546 dir = trm->offset * trmAxis;
550 tw.start += trm->offset;
551 tw.end += trm->offset;
553 if ( model_rotated ) {
554 // rotate trace instead of model
555 tw.start *= invModelAxis;
556 tw.end *= invModelAxis;
560 // setup trm vertices
562 for ( i = 0; i < tw.numVerts; i++ ) {
563 // get axial trm size after rotations
564 tw.size.AddPoint( tw.vertices[i].p - tw.start );
568 for ( i = 1; i <= tw.numEdges; i++ ) {
569 // edge start, end and pluecker coordinate
570 tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
571 tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
572 tw.edges[i].pl.FromLine( tw.edges[i].start, tw.edges[i].end );
575 // setup trm polygons
576 if ( trm_rotated & model_rotated ) {
577 tmpAxis = trmAxis * invModelAxis;
578 for ( i = 0; i < tw.numPolys; i++ ) {
579 tw.polys[i].plane *= tmpAxis;
581 } else if ( trm_rotated ) {
582 for ( i = 0; i < tw.numPolys; i++ ) {
583 tw.polys[i].plane *= trmAxis;
585 } else if ( model_rotated ) {
586 for ( i = 0; i < tw.numPolys; i++ ) {
587 tw.polys[i].plane *= invModelAxis;
590 for ( i = 0; i < tw.numPolys; i++ ) {
591 tw.polys[i].plane.FitThroughPoint( tw.edges[abs(tw.polys[i].edges[0])].start );
594 // bounds for full trace, a little bit larger for epsilons
595 for ( i = 0; i < 3; i++ ) {
596 if ( tw.start[i] < tw.end[i] ) {
597 tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
598 tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
600 tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
601 tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
603 if ( idMath::Fabs(tw.size[0][i]) > idMath::Fabs(tw.size[1][i]) ) {
604 tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON;
606 tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON;
610 // trace through the model
611 idCollisionModelManagerLocal::TraceThroughModel( &tw );
614 results->fraction = ( results->c.contents == 0 );
615 results->endpos = start;
616 results->endAxis = trmAxis;
618 return results->c.contents;
623 idCollisionModelManagerLocal::Contents
626 int idCollisionModelManagerLocal::Contents( const idVec3 &start,
627 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
628 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
631 if ( model < 0 || model > idCollisionModelManagerLocal::maxModels || model > MAX_SUBMODELS ) {
632 common->Printf("idCollisionModelManagerLocal::Contents: invalid model handle\n");
635 if ( !idCollisionModelManagerLocal::models || !idCollisionModelManagerLocal::models[model] ) {
636 common->Printf("idCollisionModelManagerLocal::Contents: invalid model\n");
640 return ContentsTrm( &results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );