CollisionModel_translate.cpp (35681B)
1 /* 2 =========================================================================== 3 4 Doom 3 BFG Edition GPL Source Code 5 Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 6 7 This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code"). 8 9 Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>. 21 22 In addition, the Doom 3 BFG Edition 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 BFG Edition 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 #pragma hdrstop 38 #include "../idlib/precompiled.h" 39 40 41 #include "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 */ 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 ( fabs( t ) < idMath::FLT_SMALLEST_NON_DENORMAL ) { 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 */ 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 */ 204 ID_INLINE void CM_SetVertexSidedness( cm_vertex_t *v, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) { 205 const int mask = 1 << bitNum; 206 if ( ( v->sideSet & mask ) == 0 ) { 207 const float fl = vpl.PermutedInnerProduct( epl ); 208 v->side = ( v->side & ~mask ) | ( ( fl < 0.0f ) ? mask : 0 ); 209 v->sideSet |= mask; 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 */ 220 ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) { 221 const int mask = 1 << bitNum; 222 if ( ( edge->sideSet & mask ) == 0 ) { 223 const float fl = vpl.PermutedInnerProduct( epl ); 224 edge->side = ( edge->side & ~mask ) | ( ( fl < 0.0f ) ? mask : 0 ); 225 edge->sideSet |= mask; 226 } 227 } 228 229 /* 230 ================ 231 idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon 232 ================ 233 */ 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[INT32_SIGNBITSET( edgeNum )]; 264 CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum ); 265 v2 = tw->model->vertices + edge->vertexNum[INT32_SIGNBITNOTSET( 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 float CM_TranslationPlaneFraction( const idPlane &plane, const idVec3 &start, const idVec3 &end ) { 339 const float d2 = plane.Distance( end ); 340 // if the end point is closer to the plane than an epsilon we still take it for a collision 341 if ( d2 >= CM_CLIP_EPSILON ) { 342 return 1.0f; 343 } 344 const float d1 = plane.Distance( start ); 345 346 // if completely behind the polygon 347 if ( d1 <= 0.0f ) { 348 return 1.0f; 349 } 350 // leaves polygon 351 if ( d1 - d2 < idMath::FLT_SMALLEST_NON_DENORMAL ) { 352 return 1.0f; 353 } 354 return ( d1 - CM_CLIP_EPSILON ) / ( d1 - d2 ); 355 } 356 357 /* 358 ================ 359 idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon 360 ================ 361 */ 362 void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int bitNum ) { 363 int i, edgeNum; 364 float f; 365 cm_edge_t *edge; 366 367 f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp ); 368 if ( f < tw->trace.fraction ) { 369 370 for ( i = 0; i < poly->numEdges; i++ ) { 371 edgeNum = poly->edges[i]; 372 edge = tw->model->edges + abs(edgeNum); 373 CM_SetEdgeSidedness( edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum ); 374 if ( INT32_SIGNBITSET( edgeNum ) ^ ( ( edge->side >> bitNum ) & 1 ) ) { 375 return; 376 } 377 } 378 if ( f < 0.0f ) { 379 f = 0.0f; 380 } 381 tw->trace.fraction = f; 382 // collision plane is the polygon plane 383 tw->trace.c.normal = poly->plane.Normal(); 384 tw->trace.c.dist = poly->plane.Dist(); 385 tw->trace.c.contents = poly->contents; 386 tw->trace.c.material = poly->material; 387 tw->trace.c.type = CONTACT_TRMVERTEX; 388 tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly); 389 tw->trace.c.trmFeature = v - tw->vertices; 390 tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p ); 391 // if retrieving contacts 392 if ( tw->getContacts ) { 393 CM_AddContact( tw ); 394 // no need to store the trm vertex more than once as a contact 395 v->used = false; 396 } 397 } 398 } 399 400 /* 401 ================ 402 idCollisionModelManagerLocal::TranslatePointThroughPolygon 403 ================ 404 */ 405 void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v ) { 406 int i, edgeNum; 407 float f; 408 cm_edge_t *edge; 409 idPluecker pl; 410 411 f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp ); 412 if ( f < tw->trace.fraction ) { 413 414 for ( i = 0; i < poly->numEdges; i++ ) { 415 edgeNum = poly->edges[i]; 416 edge = tw->model->edges + abs(edgeNum); 417 // if we didn't yet calculate the sidedness for this edge 418 if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) { 419 float fl; 420 edge->checkcount = idCollisionModelManagerLocal::checkCount; 421 pl.FromLine(tw->model->vertices[edge->vertexNum[0]].p, tw->model->vertices[edge->vertexNum[1]].p); 422 fl = v->pl.PermutedInnerProduct( pl ); 423 edge->side = ( fl < 0.0f ); 424 } 425 // if the point passes the edge at the wrong side 426 //if ( (edgeNum > 0) == edge->side ) { 427 if ( INT32_SIGNBITSET( edgeNum ) ^ edge->side ) { 428 return; 429 } 430 } 431 if ( f < 0.0f ) { 432 f = 0.0f; 433 } 434 tw->trace.fraction = f; 435 // collision plane is the polygon plane 436 tw->trace.c.normal = poly->plane.Normal(); 437 tw->trace.c.dist = poly->plane.Dist(); 438 tw->trace.c.contents = poly->contents; 439 tw->trace.c.material = poly->material; 440 tw->trace.c.type = CONTACT_TRMVERTEX; 441 tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly); 442 tw->trace.c.trmFeature = v - tw->vertices; 443 tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p ); 444 // if retrieving contacts 445 if ( tw->getContacts ) { 446 CM_AddContact( tw ); 447 // no need to store the trm vertex more than once as a contact 448 v->used = false; 449 } 450 } 451 } 452 453 /* 454 ================ 455 idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon 456 ================ 457 */ 458 void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &endp, idPluecker &pl ) { 459 int i, edgeNum; 460 float f; 461 cm_trmEdge_t *edge; 462 463 f = CM_TranslationPlaneFraction( trmpoly->plane, v->p, endp ); 464 if ( f < tw->trace.fraction ) { 465 466 for ( i = 0; i < trmpoly->numEdges; i++ ) { 467 edgeNum = trmpoly->edges[i]; 468 edge = tw->edges + abs(edgeNum); 469 470 CM_SetVertexSidedness( v, pl, edge->pl, edge->bitNum ); 471 if ( INT32_SIGNBITSET( edgeNum ) ^ ( ( v->side >> edge->bitNum ) & 1 ) ) { 472 return; 473 } 474 } 475 if ( f < 0.0f ) { 476 f = 0.0f; 477 } 478 tw->trace.fraction = f; 479 // collision plane is the inverse trm polygon plane 480 tw->trace.c.normal = -trmpoly->plane.Normal(); 481 tw->trace.c.dist = -trmpoly->plane.Dist(); 482 tw->trace.c.contents = poly->contents; 483 tw->trace.c.material = poly->material; 484 tw->trace.c.type = CONTACT_MODELVERTEX; 485 tw->trace.c.modelFeature = v - tw->model->vertices; 486 tw->trace.c.trmFeature = trmpoly - tw->polys; 487 tw->trace.c.point = v->p + tw->trace.fraction * ( endp - v->p ); 488 // if retrieving contacts 489 if ( tw->getContacts ) { 490 CM_AddContact( tw ); 491 } 492 } 493 } 494 495 /* 496 ================ 497 idCollisionModelManagerLocal::TranslateTrmThroughPolygon 498 499 returns true if the polygon blocks the complete translation 500 ================ 501 */ 502 bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) { 503 int i, j, k, edgeNum; 504 float fraction, d; 505 idVec3 endp; 506 idPluecker *pl; 507 cm_trmVertex_t *bv; 508 cm_trmEdge_t *be; 509 cm_trmPolygon_t *bp; 510 cm_vertex_t *v; 511 cm_edge_t *e; 512 513 // if already checked this polygon 514 if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) { 515 return false; 516 } 517 p->checkcount = idCollisionModelManagerLocal::checkCount; 518 519 // if this polygon does not have the right contents behind it 520 if ( !(p->contents & tw->contents) ) { 521 return false; 522 } 523 524 // if the the trace bounds do not intersect the polygon bounds 525 if ( !tw->bounds.IntersectsBounds( p->bounds ) ) { 526 return false; 527 } 528 529 // only collide with the polygon if approaching at the front 530 if ( ( p->plane.Normal() * tw->dir ) > 0.0f ) { 531 return false; 532 } 533 534 // if the polygon is too far from the first heart plane 535 d = p->bounds.PlaneDistance( tw->heartPlane1 ); 536 if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) { 537 return false; 538 } 539 540 // if the polygon is too far from the second heart plane 541 d = p->bounds.PlaneDistance( tw->heartPlane2 ); 542 if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane2 ) { 543 return false; 544 } 545 fraction = tw->trace.fraction; 546 547 // fast point trace 548 if ( tw->pointTrace ) { 549 idCollisionModelManagerLocal::TranslatePointThroughPolygon( tw, p, &tw->vertices[0] ); 550 } 551 else { 552 553 // trace bounds should cross polygon plane 554 switch ( tw->bounds.PlaneSide( p->plane ) ) { 555 case PLANESIDE_CROSS: 556 break; 557 case PLANESIDE_FRONT: 558 if ( tw->model->isConvex ) { 559 tw->quickExit = true; 560 return true; 561 } 562 default: 563 return false; 564 } 565 566 // calculate pluecker coordinates for the polygon edges and polygon vertices 567 for ( i = 0; i < p->numEdges; i++ ) { 568 edgeNum = p->edges[i]; 569 e = tw->model->edges + abs(edgeNum); 570 // reset sidedness cache if this is the first time we encounter this edge during this trace 571 if ( e->checkcount != idCollisionModelManagerLocal::checkCount ) { 572 e->sideSet = 0; 573 } 574 // pluecker coordinate for edge 575 tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p, 576 tw->model->vertices[e->vertexNum[1]].p ); 577 578 v = &tw->model->vertices[e->vertexNum[INT32_SIGNBITSET( edgeNum )]]; 579 // reset sidedness cache if this is the first time we encounter this vertex during this trace 580 if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) { 581 v->sideSet = 0; 582 } 583 // pluecker coordinate for vertex movement vector 584 tw->polygonVertexPlueckerCache[i].FromRay( v->p, -tw->dir ); 585 } 586 // copy first to last so we can easily cycle through for the edges 587 tw->polygonVertexPlueckerCache[p->numEdges] = tw->polygonVertexPlueckerCache[0]; 588 589 // trace trm vertices through polygon 590 for ( i = 0; i < tw->numVerts; i++ ) { 591 bv = tw->vertices + i; 592 if ( bv->used ) { 593 idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( tw, p, bv, i ); 594 } 595 } 596 597 // trace trm edges through polygon 598 for ( i = 1; i <= tw->numEdges; i++ ) { 599 be = tw->edges + i; 600 if ( be->used ) { 601 idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( tw, p, be); 602 } 603 } 604 605 // trace all polygon vertices through the trm 606 for ( i = 0; i < p->numEdges; i++ ) { 607 edgeNum = p->edges[i]; 608 e = tw->model->edges + abs(edgeNum); 609 610 if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) { 611 continue; 612 } 613 // set edge check count 614 e->checkcount = idCollisionModelManagerLocal::checkCount; 615 // can never collide with internal edges 616 if ( e->internal ) { 617 continue; 618 } 619 // got to check both vertices because we skip internal edges 620 for ( k = 0; k < 2; k++ ) { 621 622 v = tw->model->vertices + e->vertexNum[k ^ INT32_SIGNBITSET( edgeNum )]; 623 // if this vertex is already checked 624 if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) { 625 continue; 626 } 627 // set vertex check count 628 v->checkcount = idCollisionModelManagerLocal::checkCount; 629 630 // if the vertex is outside the trace bounds 631 if ( !tw->bounds.ContainsPoint( v->p ) ) { 632 continue; 633 } 634 635 // vertex end point after movement 636 endp = v->p - tw->dir; 637 // pluecker coordinate for vertex movement vector 638 pl = &tw->polygonVertexPlueckerCache[i+k]; 639 640 for ( j = 0; j < tw->numPolys; j++ ) { 641 bp = tw->polys + j; 642 if ( bp->used ) { 643 idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( tw, bp, p, v, endp, *pl ); 644 } 645 } 646 } 647 } 648 } 649 650 // if there was a collision with this polygon and we are not retrieving contacts 651 if ( tw->trace.fraction < fraction && !tw->getContacts ) { 652 fraction = tw->trace.fraction; 653 endp = tw->start + fraction * tw->dir; 654 // decrease bounds 655 for ( i = 0; i < 3; i++ ) { 656 if ( tw->start[i] < endp[i] ) { 657 tw->bounds[0][i] = tw->start[i] + tw->size[0][i] - CM_BOX_EPSILON; 658 tw->bounds[1][i] = endp[i] + tw->size[1][i] + CM_BOX_EPSILON; 659 } 660 else { 661 tw->bounds[0][i] = endp[i] + tw->size[0][i] - CM_BOX_EPSILON; 662 tw->bounds[1][i] = tw->start[i] + tw->size[1][i] + CM_BOX_EPSILON; 663 } 664 } 665 } 666 667 return ( tw->trace.fraction == 0.0f ); 668 } 669 670 /* 671 ================ 672 idCollisionModelManagerLocal::SetupTrm 673 ================ 674 */ 675 void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceModel *trm ) { 676 int i, j; 677 678 // vertices 679 tw->numVerts = trm->numVerts; 680 for ( i = 0; i < trm->numVerts; i++ ) { 681 tw->vertices[i].p = trm->verts[i]; 682 tw->vertices[i].used = false; 683 } 684 // edges 685 tw->numEdges = trm->numEdges; 686 for ( i = 1; i <= trm->numEdges; i++ ) { 687 tw->edges[i].vertexNum[0] = trm->edges[i].v[0]; 688 tw->edges[i].vertexNum[1] = trm->edges[i].v[1]; 689 tw->edges[i].used = false; 690 } 691 // polygons 692 tw->numPolys = trm->numPolys; 693 for ( i = 0; i < trm->numPolys; i++ ) { 694 tw->polys[i].numEdges = trm->polys[i].numEdges; 695 for ( j = 0; j < trm->polys[i].numEdges; j++ ) { 696 tw->polys[i].edges[j] = trm->polys[i].edges[j]; 697 } 698 tw->polys[i].plane.SetNormal( trm->polys[i].normal ); 699 tw->polys[i].used = false; 700 } 701 // is the trace model convex or not 702 tw->isConvex = trm->isConvex; 703 } 704 705 /* 706 ================ 707 idCollisionModelManagerLocal::SetupTranslationHeartPlanes 708 ================ 709 */ 710 void idCollisionModelManagerLocal::SetupTranslationHeartPlanes( cm_traceWork_t *tw ) { 711 idVec3 dir, normal1, normal2; 712 713 // calculate trace heart planes 714 dir = tw->dir; 715 dir.Normalize(); 716 dir.NormalVectors( normal1, normal2 ); 717 tw->heartPlane1.SetNormal( normal1 ); 718 tw->heartPlane1.FitThroughPoint( tw->start ); 719 tw->heartPlane2.SetNormal( normal2 ); 720 tw->heartPlane2.FitThroughPoint( tw->start ); 721 } 722 723 /* 724 ================ 725 idCollisionModelManagerLocal::Translation 726 ================ 727 */ 728 #ifdef _DEBUG 729 static int entered = 0; 730 #endif 731 732 void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 &start, const idVec3 &end, 733 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask, 734 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) { 735 736 int i, j; 737 float dist; 738 bool model_rotated, trm_rotated; 739 idVec3 dir1, dir2, dir; 740 idMat3 invModelAxis, tmpAxis; 741 cm_trmPolygon_t *poly; 742 cm_trmEdge_t *edge; 743 cm_trmVertex_t *vert; 744 ALIGN16( static cm_traceWork_t tw ); 745 746 assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) >= (((byte *)results) + sizeof( trace_t )) ); 747 assert( ((byte *)&end) < ((byte *)results) || ((byte *)&end) >= (((byte *)results) + sizeof( trace_t )) ); 748 assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) >= (((byte *)results) + sizeof( trace_t )) ); 749 750 memset( results, 0, sizeof( *results ) ); 751 752 if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) { 753 common->Printf("idCollisionModelManagerLocal::Translation: invalid model handle\n"); 754 return; 755 } 756 if ( !idCollisionModelManagerLocal::models[model] ) { 757 common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n"); 758 return; 759 } 760 761 // if case special position test 762 if ( start[0] == end[0] && start[1] == end[1] && start[2] == end[2] ) { 763 idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 764 return; 765 } 766 767 #ifdef _DEBUG 768 bool startsolid = false; 769 // test whether or not stuck to begin with 770 if ( cm_debugCollision.GetBool() ) { 771 if ( !entered && !idCollisionModelManagerLocal::getContacts ) { 772 entered = 1; 773 // if already messed up to begin with 774 if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { 775 startsolid = true; 776 } 777 entered = 0; 778 } 779 } 780 #endif 781 782 idCollisionModelManagerLocal::checkCount++; 783 784 tw.trace.fraction = 1.0f; 785 tw.trace.c.contents = 0; 786 tw.trace.c.type = CONTACT_NONE; 787 tw.contents = contentMask; 788 tw.isConvex = true; 789 tw.rotation = false; 790 tw.positionTest = false; 791 tw.quickExit = false; 792 tw.getContacts = idCollisionModelManagerLocal::getContacts; 793 tw.contacts = idCollisionModelManagerLocal::contacts; 794 tw.maxContacts = idCollisionModelManagerLocal::maxContacts; 795 tw.numContacts = 0; 796 tw.model = idCollisionModelManagerLocal::models[model]; 797 tw.start = start - modelOrigin; 798 tw.end = end - modelOrigin; 799 tw.dir = end - start; 800 801 model_rotated = modelAxis.IsRotated(); 802 if ( model_rotated ) { 803 invModelAxis = modelAxis.Transpose(); 804 } 805 806 // if optimized point trace 807 if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f && 808 trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f && 809 trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) { 810 811 if ( model_rotated ) { 812 // rotate trace instead of model 813 tw.start *= invModelAxis; 814 tw.end *= invModelAxis; 815 tw.dir *= invModelAxis; 816 } 817 818 // trace bounds 819 for ( i = 0; i < 3; i++ ) { 820 if ( tw.start[i] < tw.end[i] ) { 821 tw.bounds[0][i] = tw.start[i] - CM_BOX_EPSILON; 822 tw.bounds[1][i] = tw.end[i] + CM_BOX_EPSILON; 823 } 824 else { 825 tw.bounds[0][i] = tw.end[i] - CM_BOX_EPSILON; 826 tw.bounds[1][i] = tw.start[i] + CM_BOX_EPSILON; 827 } 828 } 829 tw.extents[0] = tw.extents[1] = tw.extents[2] = CM_BOX_EPSILON; 830 tw.size.Zero(); 831 832 // setup trace heart planes 833 idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw ); 834 tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON; 835 tw.maxDistFromHeartPlane2 = CM_BOX_EPSILON; 836 // collision with single point 837 tw.numVerts = 1; 838 tw.vertices[0].p = tw.start; 839 tw.vertices[0].endp = tw.vertices[0].p + tw.dir; 840 tw.vertices[0].pl.FromRay( tw.vertices[0].p, tw.dir ); 841 tw.numEdges = tw.numPolys = 0; 842 tw.pointTrace = true; 843 // trace through the model 844 idCollisionModelManagerLocal::TraceThroughModel( &tw ); 845 // store results 846 *results = tw.trace; 847 results->endpos = start + results->fraction * (end - start); 848 results->endAxis = mat3_identity; 849 850 if ( results->fraction < 1.0f ) { 851 // rotate trace plane normal if there was a collision with a rotated model 852 if ( model_rotated ) { 853 results->c.normal *= modelAxis; 854 results->c.point *= modelAxis; 855 } 856 results->c.point += modelOrigin; 857 results->c.dist += modelOrigin * results->c.normal; 858 } 859 idCollisionModelManagerLocal::numContacts = tw.numContacts; 860 return; 861 } 862 863 // the trace fraction is too inaccurate to describe translations over huge distances 864 if ( tw.dir.LengthSqr() > Square( CM_MAX_TRACE_DIST ) ) { 865 results->fraction = 0.0f; 866 results->endpos = start; 867 results->endAxis = trmAxis; 868 results->c.normal = vec3_origin; 869 results->c.material = NULL; 870 results->c.point = start; 871 if ( common->RW() ) { 872 common->RW()->DebugArrow( colorRed, start, end, 1 ); 873 } 874 common->Printf( "idCollisionModelManagerLocal::Translation: huge translation\n" ); 875 return; 876 } 877 878 tw.pointTrace = false; 879 tw.size.Clear(); 880 881 // setup trm structure 882 idCollisionModelManagerLocal::SetupTrm( &tw, trm ); 883 884 trm_rotated = trmAxis.IsRotated(); 885 886 // calculate vertex positions 887 if ( trm_rotated ) { 888 for ( i = 0; i < tw.numVerts; i++ ) { 889 // rotate trm around the start position 890 tw.vertices[i].p *= trmAxis; 891 } 892 } 893 for ( i = 0; i < tw.numVerts; i++ ) { 894 // set trm at start position 895 tw.vertices[i].p += tw.start; 896 } 897 if ( model_rotated ) { 898 for ( i = 0; i < tw.numVerts; i++ ) { 899 // rotate trm around model instead of rotating the model 900 tw.vertices[i].p *= invModelAxis; 901 } 902 } 903 904 // add offset to start point 905 if ( trm_rotated ) { 906 dir = trm->offset * trmAxis; 907 tw.start += dir; 908 tw.end += dir; 909 } else { 910 tw.start += trm->offset; 911 tw.end += trm->offset; 912 } 913 if ( model_rotated ) { 914 // rotate trace instead of model 915 tw.start *= invModelAxis; 916 tw.end *= invModelAxis; 917 tw.dir *= invModelAxis; 918 } 919 920 // rotate trm polygon planes 921 if ( trm_rotated & model_rotated ) { 922 tmpAxis = trmAxis * invModelAxis; 923 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 924 poly->plane *= tmpAxis; 925 } 926 } else if ( trm_rotated ) { 927 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 928 poly->plane *= trmAxis; 929 } 930 } else if ( model_rotated ) { 931 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 932 poly->plane *= invModelAxis; 933 } 934 } 935 936 // setup trm polygons 937 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 938 // if the trm poly plane is facing in the movement direction 939 dist = poly->plane.Normal() * tw.dir; 940 if ( dist > 0.0f || ( !trm->isConvex && dist == 0.0f ) ) { 941 // this trm poly and it's edges and vertices need to be used for collision 942 poly->used = true; 943 for ( j = 0; j < poly->numEdges; j++ ) { 944 edge = &tw.edges[abs( poly->edges[j] )]; 945 edge->used = true; 946 tw.vertices[edge->vertexNum[0]].used = true; 947 tw.vertices[edge->vertexNum[1]].used = true; 948 } 949 } 950 } 951 952 // setup trm vertices 953 for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) { 954 if ( !vert->used ) { 955 continue; 956 } 957 // get axial trm size after rotations 958 tw.size.AddPoint( vert->p - tw.start ); 959 // calculate the end position of each vertex for a full trace 960 vert->endp = vert->p + tw.dir; 961 // pluecker coordinate for vertex movement line 962 vert->pl.FromRay( vert->p, tw.dir ); 963 } 964 965 // setup trm edges 966 for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) { 967 if ( !edge->used ) { 968 continue; 969 } 970 // edge start, end and pluecker coordinate 971 edge->start = tw.vertices[edge->vertexNum[0]].p; 972 edge->end = tw.vertices[edge->vertexNum[1]].p; 973 edge->pl.FromLine( edge->start, edge->end ); 974 // calculate normal of plane through movement plane created by the edge 975 dir = edge->start - edge->end; 976 edge->cross[0] = dir[0] * tw.dir[1] - dir[1] * tw.dir[0]; 977 edge->cross[1] = dir[0] * tw.dir[2] - dir[2] * tw.dir[0]; 978 edge->cross[2] = dir[1] * tw.dir[2] - dir[2] * tw.dir[1]; 979 // bit for vertex sidedness bit cache 980 edge->bitNum = i; 981 } 982 983 // set trm plane distances 984 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 985 if ( poly->used ) { 986 poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start ); 987 } 988 } 989 990 // bounds for full trace, a little bit larger for epsilons 991 for ( i = 0; i < 3; i++ ) { 992 if ( tw.start[i] < tw.end[i] ) { 993 tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON; 994 tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON; 995 } else { 996 tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON; 997 tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON; 998 } 999 if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) { 1000 tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON; 1001 } else { 1002 tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON; 1003 } 1004 } 1005 1006 // setup trace heart planes 1007 idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw ); 1008 tw.maxDistFromHeartPlane1 = 0; 1009 tw.maxDistFromHeartPlane2 = 0; 1010 // calculate maximum trm vertex distance from both heart planes 1011 for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) { 1012 if ( !vert->used ) { 1013 continue; 1014 } 1015 dist = idMath::Fabs( tw.heartPlane1.Distance( vert->p ) ); 1016 if ( dist > tw.maxDistFromHeartPlane1 ) { 1017 tw.maxDistFromHeartPlane1 = dist; 1018 } 1019 dist = idMath::Fabs( tw.heartPlane2.Distance( vert->p ) ); 1020 if ( dist > tw.maxDistFromHeartPlane2 ) { 1021 tw.maxDistFromHeartPlane2 = dist; 1022 } 1023 } 1024 // for epsilons 1025 tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON; 1026 tw.maxDistFromHeartPlane2 += CM_BOX_EPSILON; 1027 1028 // trace through the model 1029 idCollisionModelManagerLocal::TraceThroughModel( &tw ); 1030 1031 // if we're getting contacts 1032 if ( tw.getContacts ) { 1033 // move all contacts to world space 1034 if ( model_rotated ) { 1035 for ( i = 0; i < tw.numContacts; i++ ) { 1036 tw.contacts[i].normal *= modelAxis; 1037 tw.contacts[i].point *= modelAxis; 1038 } 1039 } 1040 if ( modelOrigin != vec3_origin ) { 1041 for ( i = 0; i < tw.numContacts; i++ ) { 1042 tw.contacts[i].point += modelOrigin; 1043 tw.contacts[i].dist += modelOrigin * tw.contacts[i].normal; 1044 } 1045 } 1046 idCollisionModelManagerLocal::numContacts = tw.numContacts; 1047 } else { 1048 // store results 1049 *results = tw.trace; 1050 results->endpos = start + results->fraction * ( end - start ); 1051 results->endAxis = trmAxis; 1052 1053 if ( results->fraction < 1.0f ) { 1054 // if the fraction is tiny the actual movement could end up zero 1055 if ( results->fraction > 0.0f && results->endpos.Compare( start ) ) { 1056 results->fraction = 0.0f; 1057 } 1058 // rotate trace plane normal if there was a collision with a rotated model 1059 if ( model_rotated ) { 1060 results->c.normal *= modelAxis; 1061 results->c.point *= modelAxis; 1062 } 1063 results->c.point += modelOrigin; 1064 results->c.dist += modelOrigin * results->c.normal; 1065 } 1066 } 1067 1068 #ifdef _DEBUG 1069 // test for missed collisions 1070 if ( cm_debugCollision.GetBool() ) { 1071 if ( !entered && !idCollisionModelManagerLocal::getContacts ) { 1072 entered = 1; 1073 // if the trm is stuck in the model 1074 if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { 1075 trace_t tr; 1076 1077 // test where the trm is stuck in the model 1078 idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ); 1079 // re-run collision detection to find out where it failed 1080 idCollisionModelManagerLocal::Translation( &tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 1081 } 1082 entered = 0; 1083 } 1084 } 1085 #endif 1086 }