CollisionModel_rotate.cpp (50703B)
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 rotational motion 47 48 =============================================================================== 49 */ 50 51 // epsilon for round-off errors in epsilon calculations 52 #define CM_PL_RANGE_EPSILON 1e-4f 53 // if the collision point is this close to the rotation axis it is not considered a collision 54 #define ROTATION_AXIS_EPSILON (CM_CLIP_EPSILON*0.25f) 55 56 57 /* 58 ================ 59 CM_RotatePoint 60 61 rotates a point about an arbitrary axis using the tangent of half the rotation angle 62 ================ 63 */ 64 void CM_RotatePoint( idVec3 &point, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) { 65 double d, t, s, c; 66 idVec3 proj, v1, v2; 67 68 point -= origin; 69 proj = axis * ( point * axis ); 70 v1 = point - proj; 71 v2 = axis.Cross( v1 ); 72 73 // r = tan( a / 2 ); 74 // sin(a) = 2*r/(1+r*r); 75 // cos(a) = (1-r*r)/(1+r*r); 76 t = tanHalfAngle * tanHalfAngle; 77 d = 1.0f / ( 1.0f + t ); 78 s = 2.0f * tanHalfAngle * d; 79 c = ( 1.0f - t ) * d; 80 81 point = v1 * c - v2 * s + proj + origin; 82 } 83 84 /* 85 ================ 86 CM_RotateEdge 87 88 rotates an edge about an arbitrary axis using the tangent of half the rotation angle 89 ================ 90 */ 91 void CM_RotateEdge( idVec3 &start, idVec3 &end, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) { 92 double d, t, s, c; 93 idVec3 proj, v1, v2; 94 95 // r = tan( a / 2 ); 96 // sin(a) = 2*r/(1+r*r); 97 // cos(a) = (1-r*r)/(1+r*r); 98 t = tanHalfAngle * tanHalfAngle; 99 d = 1.0f / ( 1.0f + t ); 100 s = 2.0f * tanHalfAngle * d; 101 c = ( 1.0f - t ) * d; 102 103 start -= origin; 104 proj = axis * ( start * axis ); 105 v1 = start - proj; 106 v2 = axis.Cross( v1 ); 107 start = v1 * c - v2 * s + proj + origin; 108 109 end -= origin; 110 proj = axis * ( end * axis ); 111 v1 = end - proj; 112 v2 = axis.Cross( v1 ); 113 end = v1 * c - v2 * s + proj + origin; 114 } 115 116 /* 117 ================ 118 idCollisionModelManagerLocal::CollisionBetweenEdgeBounds 119 120 verifies if the collision of two edges occurs between the edge bounds 121 also calculates the collision point and collision plane normal if the collision occurs between the bounds 122 ================ 123 */ 124 int idCollisionModelManagerLocal::CollisionBetweenEdgeBounds( cm_traceWork_t *tw, const idVec3 &va, const idVec3 &vb, 125 const idVec3 &vc, const idVec3 &vd, float tanHalfAngle, 126 idVec3 &collisionPoint, idVec3 &collisionNormal ) { 127 float d1, d2, d; 128 idVec3 at, bt, dir, dir1, dir2; 129 idPluecker pl1, pl2; 130 131 at = va; 132 bt = vb; 133 if ( tanHalfAngle != 0.0f ) { 134 CM_RotateEdge( at, bt, tw->origin, tw->axis, tanHalfAngle ); 135 } 136 137 dir1 = (at - tw->origin).Cross( tw->axis ); 138 dir2 = (bt - tw->origin).Cross( tw->axis ); 139 if ( dir1 * dir1 > dir2 * dir2 ) { 140 dir = dir1; 141 } 142 else { 143 dir = dir2; 144 } 145 if ( tw->angle < 0.0f ) { 146 dir = -dir; 147 } 148 149 pl1.FromLine( at, bt ); 150 pl2.FromRay( vc, dir ); 151 d1 = pl1.PermutedInnerProduct( pl2 ); 152 pl2.FromRay( vd, dir ); 153 d2 = pl1.PermutedInnerProduct( pl2 ); 154 if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) { 155 return false; 156 } 157 158 pl1.FromLine( vc, vd ); 159 pl2.FromRay( at, dir ); 160 d1 = pl1.PermutedInnerProduct( pl2 ); 161 pl2.FromRay( bt, dir ); 162 d2 = pl1.PermutedInnerProduct( pl2 ); 163 if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) { 164 return false; 165 } 166 167 // collision point on the edge at-bt 168 dir1 = (vd - vc).Cross( dir ); 169 d = dir1 * vc; 170 d1 = dir1 * at - d; 171 d2 = dir1 * bt - d; 172 if ( d1 == d2 ) { 173 return false; 174 } 175 collisionPoint = at + ( d1 / (d1 - d2) ) * ( bt - at ); 176 177 // normal is cross product of the rotated edge va-vb and the edge vc-vd 178 collisionNormal.Cross( bt-at, vd-vc ); 179 180 return true; 181 } 182 183 /* 184 ================ 185 idCollisionModelManagerLocal::RotateEdgeThroughEdge 186 187 calculates the tangent of half the rotation angle at which the edges collide 188 ================ 189 */ 190 int idCollisionModelManagerLocal::RotateEdgeThroughEdge( cm_traceWork_t *tw, const idPluecker &pl1, 191 const idVec3 &vc, const idVec3 &vd, 192 const float minTan, float &tanHalfAngle ) { 193 double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2; 194 idVec3 ct, dt; 195 idPluecker pl2; 196 197 /* 198 199 a = start of line being rotated 200 b = end of line being rotated 201 pl1 = pluecker coordinate for line (a - b) 202 pl2 = pluecker coordinate for edge we might collide with (c - d) 203 t = rotation angle around the z-axis 204 solve pluecker inner product for t of rotating line a-b and line l2 205 206 // start point of rotated line during rotation 207 an[0] = a[0] * cos(t) + a[1] * sin(t) 208 an[1] = a[0] * -sin(t) + a[1] * cos(t) 209 an[2] = a[2]; 210 // end point of rotated line during rotation 211 bn[0] = b[0] * cos(t) + b[1] * sin(t) 212 bn[1] = b[0] * -sin(t) + b[1] * cos(t) 213 bn[2] = b[2]; 214 215 pl1[0] = a[0] * b[1] - b[0] * a[1]; 216 pl1[1] = a[0] * b[2] - b[0] * a[2]; 217 pl1[2] = a[0] - b[0]; 218 pl1[3] = a[1] * b[2] - b[1] * a[2]; 219 pl1[4] = a[2] - b[2]; 220 pl1[5] = b[1] - a[1]; 221 222 v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t)); 223 v[1] = (a[0] * cos(t) + a[1] * sin(t)) * b[2] - (b[0] * cos(t) + b[1] * sin(t)) * a[2]; 224 v[2] = (a[0] * cos(t) + a[1] * sin(t)) - (b[0] * cos(t) + b[1] * sin(t)); 225 v[3] = (a[0] * -sin(t) + a[1] * cos(t)) * b[2] - (b[0] * -sin(t) + b[1] * cos(t)) * a[2]; 226 v[4] = a[2] - b[2]; 227 v[5] = (b[0] * -sin(t) + b[1] * cos(t)) - (a[0] * -sin(t) + a[1] * cos(t)); 228 229 pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0; 230 231 v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t)); 232 v[0] = (a[1] * b[1] - a[0] * b[0]) * cos(t) * sin(t) + (a[0] * b[1] + a[1] * b[0] * cos(t)^2) - (a[1] * b[0]) - ((b[1] * a[1] - b[0] * a[0]) * cos(t) * sin(t) + (b[0] * a[1] + b[1] * a[0]) * cos(t)^2 - (b[1] * a[0])) 233 v[0] = - (a[1] * b[0]) - ( - (b[1] * a[0])) 234 v[0] = (b[1] * a[0]) - (a[1] * b[0]) 235 236 v[0] = (a[0]*b[1]) - (a[1]*b[0]); 237 v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t); 238 v[2] = (a[0]-b[0]) * cos(t) + (a[1]-b[1]) * sin(t); 239 v[3] = (b[0]*a[2] - a[0]*b[2]) * sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t); 240 v[4] = a[2] - b[2]; 241 v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t); 242 243 v[0] = (a[0]*b[1]) - (a[1]*b[0]); 244 v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t); 245 v[2] = (a[0]-b[0]) * cos(t) - (b[1]-a[1]) * sin(t); 246 v[3] = (a[0]*b[2] - b[0]*a[2]) * -sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t); 247 v[4] = a[2] - b[2]; 248 v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t); 249 250 v[0] = pl1[0]; 251 v[1] = pl1[1] * cos(t) + pl1[3] * sin(t); 252 v[2] = pl1[2] * cos(t) - pl1[5] * sin(t); 253 v[3] = pl1[3] * cos(t) - pl1[1] * sin(t); 254 v[4] = pl1[4]; 255 v[5] = pl1[5] * cos(t) + pl1[2] * sin(t); 256 257 pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0; 258 259 0 = pl2[0] * pl1[4] + 260 pl2[1] * (pl1[5] * cos(t) + pl1[2] * sin(t)) + 261 pl2[2] * (pl1[3] * cos(t) - pl1[1] * sin(t)) + 262 pl2[4] * pl1[0] + 263 pl2[5] * (pl1[1] * cos(t) + pl1[3] * sin(t)) + 264 pl2[3] * (pl1[2] * cos(t) - pl1[5] * sin(t)); 265 266 v2 * cos(t) + v1 * sin(t) + v0 = 0; 267 268 // rotation about the z-axis 269 v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0]; 270 v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5]; 271 v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2]; 272 273 // rotation about the x-axis 274 //v0 = pl2[3] * pl1[2] + pl2[2] * pl1[3]; 275 //v1 = -pl2[5] * pl1[0] + pl2[4] * pl1[1] - pl2[1] * pl1[4] + pl2[0] * pl1[5]; 276 //v2 = pl2[4] * pl1[0] + pl2[5] * pl1[1] + pl2[0] * pl1[4] + pl2[1] * pl1[5]; 277 278 r = tan(t / 2); 279 sin(t) = 2*r/(1+r*r); 280 cos(t) = (1-r*r)/(1+r*r); 281 282 v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0 283 (v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0 284 (v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0 285 v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r) 286 v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r 287 (v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0; 288 289 MrE gives Pluecker a banana.. good monkey 290 291 */ 292 293 tanHalfAngle = tw->maxTan; 294 295 // transform rotation axis to z-axis 296 ct = (vc - tw->origin) * tw->matrix; 297 dt = (vd - tw->origin) * tw->matrix; 298 299 pl2.FromLine( ct, dt ); 300 301 v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0]; 302 v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5]; 303 v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2]; 304 305 a = v0 - v2; 306 b = v1; 307 c = v0 + v2; 308 if ( a == 0.0f ) { 309 if ( b == 0.0f ) { 310 return false; 311 } 312 frac1 = -c / ( 2.0f * b ); 313 frac2 = 1e10; // = tan( idMath::HALF_PI ) 314 } 315 else { 316 d = b * b - c * a; 317 if ( d <= 0.0f ) { 318 return false; 319 } 320 sqrtd = sqrt( d ); 321 if ( b > 0.0f ) { 322 q = - b + sqrtd; 323 } 324 else { 325 q = - b - sqrtd; 326 } 327 frac1 = q / a; 328 frac2 = c / q; 329 } 330 331 if ( tw->angle < 0.0f ) { 332 frac1 = -frac1; 333 frac2 = -frac2; 334 } 335 336 // get smallest tangent for which a collision occurs 337 if ( frac1 >= minTan && frac1 < tanHalfAngle ) { 338 tanHalfAngle = frac1; 339 } 340 if ( frac2 >= minTan && frac2 < tanHalfAngle ) { 341 tanHalfAngle = frac2; 342 } 343 344 if ( tw->angle < 0.0f ) { 345 tanHalfAngle = -tanHalfAngle; 346 } 347 348 return true; 349 } 350 351 /* 352 ================ 353 idCollisionModelManagerLocal::EdgeFurthestFromEdge 354 355 calculates the direction of motion at the initial position, where dir < 0 means the edges move towards each other 356 if the edges move away from each other the tangent of half the rotation angle at which 357 the edges are furthest apart is also calculated 358 ================ 359 */ 360 int idCollisionModelManagerLocal::EdgeFurthestFromEdge( cm_traceWork_t *tw, const idPluecker &pl1, 361 const idVec3 &vc, const idVec3 &vd, 362 float &tanHalfAngle, float &dir ) { 363 double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2; 364 idVec3 ct, dt; 365 idPluecker pl2; 366 367 /* 368 369 v2 * cos(t) + v1 * sin(t) + v0 = 0; 370 371 // rotation about the z-axis 372 v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0]; 373 v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5]; 374 v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2]; 375 376 derivative: 377 v1 * cos(t) - v2 * sin(t) = 0; 378 379 r = tan(t / 2); 380 sin(t) = 2*r/(1+r*r); 381 cos(t) = (1-r*r)/(1+r*r); 382 383 -v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r); 384 -v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0; 385 -v2 * 2 * r + v1 * (1 - r*r) = 0; 386 (-v1) * r * r + (-2 * v2) * r + (v1) = 0; 387 388 */ 389 390 tanHalfAngle = 0.0f; 391 392 // transform rotation axis to z-axis 393 ct = (vc - tw->origin) * tw->matrix; 394 dt = (vd - tw->origin) * tw->matrix; 395 396 pl2.FromLine( ct, dt ); 397 398 v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0]; 399 v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5]; 400 v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2]; 401 402 // get the direction of motion at the initial position 403 c = v0 + v2; 404 if ( tw->angle > 0.0f ) { 405 if ( c > 0.0f ) { 406 dir = v1; 407 } 408 else { 409 dir = -v1; 410 } 411 } 412 else { 413 if ( c > 0.0f ) { 414 dir = -v1; 415 } 416 else { 417 dir = v1; 418 } 419 } 420 // negative direction means the edges move towards each other at the initial position 421 if ( dir <= 0.0f ) { 422 return true; 423 } 424 425 a = -v1; 426 b = -v2; 427 c = v1; 428 if ( a == 0.0f ) { 429 if ( b == 0.0f ) { 430 return false; 431 } 432 frac1 = -c / ( 2.0f * b ); 433 frac2 = 1e10; // = tan( idMath::HALF_PI ) 434 } 435 else { 436 d = b * b - c * a; 437 if ( d <= 0.0f ) { 438 return false; 439 } 440 sqrtd = sqrt( d ); 441 if ( b > 0.0f ) { 442 q = - b + sqrtd; 443 } 444 else { 445 q = - b - sqrtd; 446 } 447 frac1 = q / a; 448 frac2 = c / q; 449 } 450 451 if ( tw->angle < 0.0f ) { 452 frac1 = -frac1; 453 frac2 = -frac2; 454 } 455 456 if ( frac1 < 0.0f && frac2 < 0.0f ) { 457 return false; 458 } 459 460 if ( frac1 > frac2 ) { 461 tanHalfAngle = frac1; 462 } 463 else { 464 tanHalfAngle = frac2; 465 } 466 467 if ( tw->angle < 0.0f ) { 468 tanHalfAngle = -tanHalfAngle; 469 } 470 471 return true; 472 } 473 474 /* 475 ================ 476 idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon 477 ================ 478 */ 479 void idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) { 480 int i, j, edgeNum; 481 float f1, f2, startTan, dir, tanHalfAngle; 482 cm_edge_t *edge; 483 cm_vertex_t *v1, *v2; 484 idVec3 collisionPoint, collisionNormal, origin, epsDir; 485 idPluecker epsPl; 486 idBounds bounds; 487 488 // if the trm is convex and the rotation axis intersects the trm 489 if ( tw->isConvex && tw->axisIntersectsTrm ) { 490 // if both points are behind the polygon the edge cannot collide within a 180 degrees rotation 491 if ( tw->vertices[trmEdge->vertexNum[0]].polygonSide & tw->vertices[trmEdge->vertexNum[1]].polygonSide ) { 492 return; 493 } 494 } 495 496 // if the trace model edge rotation bounds do not intersect the polygon bounds 497 if ( !trmEdge->rotationBounds.IntersectsBounds( poly->bounds ) ) { 498 return; 499 } 500 501 // edge rotation bounds should cross polygon plane 502 if ( trmEdge->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) { 503 return; 504 } 505 506 // check edges for a collision 507 for ( i = 0; i < poly->numEdges; i++ ) { 508 edgeNum = poly->edges[i]; 509 edge = tw->model->edges + abs(edgeNum); 510 511 // if this edge is already checked 512 if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) { 513 continue; 514 } 515 516 // can never collide with internal edges 517 if ( edge->internal ) { 518 continue; 519 } 520 521 v1 = tw->model->vertices + edge->vertexNum[INT32_SIGNBITSET( edgeNum )]; 522 v2 = tw->model->vertices + edge->vertexNum[INT32_SIGNBITNOTSET( edgeNum )]; 523 524 // edge bounds 525 for ( j = 0; j < 3; j++ ) { 526 if ( v1->p[j] > v2->p[j] ) { 527 bounds[0][j] = v2->p[j]; 528 bounds[1][j] = v1->p[j]; 529 } 530 else { 531 bounds[0][j] = v1->p[j]; 532 bounds[1][j] = v2->p[j]; 533 } 534 } 535 536 // if the trace model edge rotation bounds do not intersect the polygon edge bounds 537 if ( !trmEdge->rotationBounds.IntersectsBounds( bounds ) ) { 538 continue; 539 } 540 541 f1 = trmEdge->pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ); 542 543 // pluecker coordinate for epsilon expanded edge 544 epsDir = edge->normal * (CM_CLIP_EPSILON+CM_PL_RANGE_EPSILON); 545 epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + epsDir, 546 tw->model->vertices[edge->vertexNum[1]].p + epsDir ); 547 548 f2 = trmEdge->pl.PermutedInnerProduct( epsPl ); 549 550 // if the rotating edge is inbetween the polygon edge and the epsilon expanded edge 551 if ( ( f1 < 0.0f && f2 > 0.0f ) || ( f1 > 0.0f && f2 < 0.0f ) ) { 552 553 if ( !EdgeFurthestFromEdge( tw, trmEdge->plzaxis, v1->p, v2->p, startTan, dir ) ) { 554 continue; 555 } 556 557 if ( dir <= 0.0f ) { 558 // moving towards the polygon edge so stop immediately 559 tanHalfAngle = 0.0f; 560 } 561 else if ( idMath::Fabs( startTan ) >= tw->maxTan ) { 562 // never going to get beyond the start tangent during the current rotation 563 continue; 564 } 565 else { 566 // collide with the epsilon expanded edge 567 if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, idMath::Fabs( startTan ), tanHalfAngle ) ) { 568 tanHalfAngle = startTan; 569 } 570 } 571 } 572 else { 573 // collide with the epsilon expanded edge 574 epsDir = edge->normal * CM_CLIP_EPSILON; 575 if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, 0.0f, tanHalfAngle ) ) { 576 continue; 577 } 578 } 579 580 if ( idMath::Fabs( tanHalfAngle ) >= tw->maxTan ) { 581 continue; 582 } 583 584 // check if the collision is between the edge bounds 585 if ( !CollisionBetweenEdgeBounds( tw, trmEdge->start, trmEdge->end, v1->p, v2->p, 586 tanHalfAngle, collisionPoint, collisionNormal ) ) { 587 continue; 588 } 589 590 // allow rotation if the rotation axis goes through the collisionPoint 591 origin = tw->origin + tw->axis * ( tw->axis * ( collisionPoint - tw->origin ) ); 592 if ( ( collisionPoint - origin ).LengthSqr() < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) { 593 continue; 594 } 595 596 // fill in trace structure 597 tw->maxTan = idMath::Fabs( tanHalfAngle ); 598 tw->trace.c.normal = collisionNormal; 599 tw->trace.c.normal.Normalize(); 600 tw->trace.c.dist = tw->trace.c.normal * v1->p; 601 // make sure the collision plane faces the trace model 602 if ( (tw->trace.c.normal * trmEdge->start) - tw->trace.c.dist < 0 ) { 603 tw->trace.c.normal = -tw->trace.c.normal; 604 tw->trace.c.dist = -tw->trace.c.dist; 605 } 606 tw->trace.c.contents = poly->contents; 607 tw->trace.c.material = poly->material; 608 tw->trace.c.type = CONTACT_EDGE; 609 tw->trace.c.modelFeature = edgeNum; 610 tw->trace.c.trmFeature = trmEdge - tw->edges; 611 tw->trace.c.point = collisionPoint; 612 // if no collision can be closer 613 if ( tw->maxTan == 0.0f ) { 614 break; 615 } 616 } 617 } 618 619 /* 620 ================ 621 idCollisionModelManagerLocal::RotatePointThroughPlane 622 623 calculates the tangent of half the rotation angle at which the point collides with the plane 624 ================ 625 */ 626 int idCollisionModelManagerLocal::RotatePointThroughPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane, 627 const float angle, const float minTan, float &tanHalfAngle ) { 628 double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2; 629 idVec3 p, normal; 630 631 /* 632 633 p[0] = point[0] * cos(t) + point[1] * sin(t) 634 p[1] = point[0] * -sin(t) + point[1] * cos(t) 635 p[2] = point[2]; 636 637 normal[0] * (p[0] * cos(t) + p[1] * sin(t)) + 638 normal[1] * (p[0] * -sin(t) + p[1] * cos(t)) + 639 normal[2] * p[2] + dist = 0 640 641 normal[0] * p[0] * cos(t) + normal[0] * p[1] * sin(t) + 642 -normal[1] * p[0] * sin(t) + normal[1] * p[1] * cos(t) + 643 normal[2] * p[2] + dist = 0 644 645 v2 * cos(t) + v1 * sin(t) + v0 646 647 // rotation about the z-axis 648 v0 = normal[2] * p[2] + dist 649 v1 = normal[0] * p[1] - normal[1] * p[0] 650 v2 = normal[0] * p[0] + normal[1] * p[1] 651 652 r = tan(t / 2); 653 sin(t) = 2*r/(1+r*r); 654 cos(t) = (1-r*r)/(1+r*r); 655 656 v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0 657 (v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0 658 (v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0 659 v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r) 660 v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r 661 (v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0; 662 663 */ 664 665 tanHalfAngle = tw->maxTan; 666 667 // transform rotation axis to z-axis 668 p = (point - tw->origin) * tw->matrix; 669 d = plane[3] + plane.Normal() * tw->origin; 670 normal = plane.Normal() * tw->matrix; 671 672 v0 = normal[2] * p[2] + d; 673 v1 = normal[0] * p[1] - normal[1] * p[0]; 674 v2 = normal[0] * p[0] + normal[1] * p[1]; 675 676 a = v0 - v2; 677 b = v1; 678 c = v0 + v2; 679 if ( a == 0.0f ) { 680 if ( b == 0.0f ) { 681 return false; 682 } 683 frac1 = -c / ( 2.0f * b ); 684 frac2 = 1e10; // = tan( idMath::HALF_PI ) 685 } 686 else { 687 d = b * b - c * a; 688 if ( d <= 0.0f ) { 689 return false; 690 } 691 sqrtd = sqrt( d ); 692 if ( b > 0.0f ) { 693 q = - b + sqrtd; 694 } 695 else { 696 q = - b - sqrtd; 697 } 698 frac1 = q / a; 699 frac2 = c / q; 700 } 701 702 if ( angle < 0.0f ) { 703 frac1 = -frac1; 704 frac2 = -frac2; 705 } 706 707 // get smallest tangent for which a collision occurs 708 if ( frac1 >= minTan && frac1 < tanHalfAngle ) { 709 tanHalfAngle = frac1; 710 } 711 if ( frac2 >= minTan && frac2 < tanHalfAngle ) { 712 tanHalfAngle = frac2; 713 } 714 715 if ( angle < 0.0f ) { 716 tanHalfAngle = -tanHalfAngle; 717 } 718 719 return true; 720 } 721 722 /* 723 ================ 724 idCollisionModelManagerLocal::PointFurthestFromPlane 725 726 calculates the direction of motion at the initial position, where dir < 0 means the point moves towards the plane 727 if the point moves away from the plane the tangent of half the rotation angle at which 728 the point is furthest away from the plane is also calculated 729 ================ 730 */ 731 int idCollisionModelManagerLocal::PointFurthestFromPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane, 732 const float angle, float &tanHalfAngle, float &dir ) { 733 734 double v1, v2, a, b, c, d, sqrtd, q, frac1, frac2; 735 idVec3 p, normal; 736 737 /* 738 739 v2 * cos(t) + v1 * sin(t) + v0 = 0; 740 741 // rotation about the z-axis 742 v0 = normal[2] * p[2] + dist 743 v1 = normal[0] * p[1] - normal[1] * p[0] 744 v2 = normal[0] * p[0] + normal[1] * p[1] 745 746 derivative: 747 v1 * cos(t) - v2 * sin(t) = 0; 748 749 r = tan(t / 2); 750 sin(t) = 2*r/(1+r*r); 751 cos(t) = (1-r*r)/(1+r*r); 752 753 -v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r); 754 -v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0; 755 -v2 * 2 * r + v1 * (1 - r*r) = 0; 756 (-v1) * r * r + (-2 * v2) * r + (v1) = 0; 757 758 */ 759 760 tanHalfAngle = 0.0f; 761 762 // transform rotation axis to z-axis 763 p = (point - tw->origin) * tw->matrix; 764 normal = plane.Normal() * tw->matrix; 765 766 v1 = normal[0] * p[1] - normal[1] * p[0]; 767 v2 = normal[0] * p[0] + normal[1] * p[1]; 768 769 // the point will always start at the front of the plane, therefore v0 + v2 > 0 is always true 770 if ( angle < 0.0f ) { 771 dir = -v1; 772 } 773 else { 774 dir = v1; 775 } 776 // negative direction means the point moves towards the plane at the initial position 777 if ( dir <= 0.0f ) { 778 return true; 779 } 780 781 a = -v1; 782 b = -v2; 783 c = v1; 784 if ( a == 0.0f ) { 785 if ( b == 0.0f ) { 786 return false; 787 } 788 frac1 = -c / ( 2.0f * b ); 789 frac2 = 1e10; // = tan( idMath::HALF_PI ) 790 } 791 else { 792 d = b * b - c * a; 793 if ( d <= 0.0f ) { 794 return false; 795 } 796 sqrtd = sqrt( d ); 797 if ( b > 0.0f ) { 798 q = - b + sqrtd; 799 } 800 else { 801 q = - b - sqrtd; 802 } 803 frac1 = q / a; 804 frac2 = c / q; 805 } 806 807 if ( angle < 0.0f ) { 808 frac1 = -frac1; 809 frac2 = -frac2; 810 } 811 812 if ( frac1 < 0.0f && frac2 < 0.0f ) { 813 return false; 814 } 815 816 if ( frac1 > frac2 ) { 817 tanHalfAngle = frac1; 818 } 819 else { 820 tanHalfAngle = frac2; 821 } 822 823 if ( angle < 0.0f ) { 824 tanHalfAngle = -tanHalfAngle; 825 } 826 827 return true; 828 } 829 830 /* 831 ================ 832 idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane 833 ================ 834 */ 835 int idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane( const cm_traceWork_t *tw, const idVec3 &point, const idVec3 &endPoint, 836 const idPlane &plane, const float angle, const idVec3 &origin, 837 float &tanHalfAngle, idVec3 &collisionPoint, idVec3 &endDir ) { 838 float d, dir, startTan; 839 idVec3 vec, startDir; 840 idPlane epsPlane; 841 842 // epsilon expanded plane 843 epsPlane = plane; 844 epsPlane.SetDist( epsPlane.Dist() + CM_CLIP_EPSILON ); 845 846 // if the rotation sphere at the rotation origin is too far away from the polygon plane 847 d = epsPlane.Distance( origin ); 848 vec = point - origin; 849 if ( d * d > vec * vec ) { 850 return false; 851 } 852 853 // calculate direction of motion at vertex start position 854 startDir = ( point - origin ).Cross( tw->axis ); 855 if ( angle < 0.0f ) { 856 startDir = -startDir; 857 } 858 // if moving away from plane at start position 859 if ( startDir * epsPlane.Normal() >= 0.0f ) { 860 // if end position is outside epsilon range 861 d = epsPlane.Distance( endPoint ); 862 if ( d >= 0.0f ) { 863 return false; // no collision 864 } 865 // calculate direction of motion at vertex end position 866 endDir = ( endPoint - origin ).Cross( tw->axis ); 867 if ( angle < 0.0f ) { 868 endDir = -endDir; 869 } 870 // if also moving away from plane at end position 871 if ( endDir * epsPlane.Normal() > 0.0f ) { 872 return false; // no collision 873 } 874 } 875 876 // if the start position is in the epsilon range 877 d = epsPlane.Distance( point ); 878 if ( d <= CM_PL_RANGE_EPSILON ) { 879 880 // calculate tangent of half the rotation for which the vertex is furthest away from the plane 881 if ( !PointFurthestFromPlane( tw, point, plane, angle, startTan, dir ) ) { 882 return false; 883 } 884 885 if ( dir <= 0.0f ) { 886 // moving towards the polygon plane so stop immediately 887 tanHalfAngle = 0.0f; 888 } 889 else if ( idMath::Fabs( startTan ) >= tw->maxTan ) { 890 // never going to get beyond the start tangent during the current rotation 891 return false; 892 } 893 else { 894 // calculate collision with epsilon expanded plane 895 if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, idMath::Fabs( startTan ), tanHalfAngle ) ) { 896 tanHalfAngle = startTan; 897 } 898 } 899 } 900 else { 901 // calculate collision with epsilon expanded plane 902 if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, 0.0f, tanHalfAngle ) ) { 903 return false; 904 } 905 } 906 907 // calculate collision point 908 collisionPoint = point; 909 if ( tanHalfAngle != 0.0f ) { 910 CM_RotatePoint( collisionPoint, tw->origin, tw->axis, tanHalfAngle ); 911 } 912 // calculate direction of motion at collision point 913 endDir = ( collisionPoint - origin ).Cross( tw->axis ); 914 if ( angle < 0.0f ) { 915 endDir = -endDir; 916 } 917 return true; 918 } 919 920 /* 921 ================ 922 idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon 923 ================ 924 */ 925 void idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int vertexNum ) { 926 int i; 927 float tanHalfAngle; 928 idVec3 endDir, collisionPoint; 929 idPluecker pl; 930 931 // if the trm vertex is behind the polygon plane it cannot collide with the polygon within a 180 degrees rotation 932 if ( tw->isConvex && tw->axisIntersectsTrm && v->polygonSide ) { 933 return; 934 } 935 936 // if the trace model vertex rotation bounds do not intersect the polygon bounds 937 if ( !v->rotationBounds.IntersectsBounds( poly->bounds ) ) { 938 return; 939 } 940 941 // vertex rotation bounds should cross polygon plane 942 if ( v->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) { 943 return; 944 } 945 946 // rotate the vertex through the epsilon plane 947 if ( !RotatePointThroughEpsilonPlane( tw, v->p, v->endp, poly->plane, tw->angle, v->rotationOrigin, 948 tanHalfAngle, collisionPoint, endDir ) ) { 949 return; 950 } 951 952 if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) { 953 // verify if 'collisionPoint' moving along 'endDir' moves between polygon edges 954 pl.FromRay( collisionPoint, endDir ); 955 for ( i = 0; i < poly->numEdges; i++ ) { 956 if ( poly->edges[i] < 0 ) { 957 if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) > 0.0f ) { 958 return; 959 } 960 } 961 else { 962 if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) < 0.0f ) { 963 return; 964 } 965 } 966 } 967 tw->maxTan = idMath::Fabs( tanHalfAngle ); 968 // collision plane is the polygon plane 969 tw->trace.c.normal = poly->plane.Normal(); 970 tw->trace.c.dist = poly->plane.Dist(); 971 tw->trace.c.contents = poly->contents; 972 tw->trace.c.material = poly->material; 973 tw->trace.c.type = CONTACT_TRMVERTEX; 974 tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly); 975 tw->trace.c.trmFeature = v - tw->vertices; 976 tw->trace.c.point = collisionPoint; 977 } 978 } 979 980 /* 981 ================ 982 idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon 983 ================ 984 */ 985 void idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &rotationOrigin ) { 986 int i, edgeNum; 987 float tanHalfAngle; 988 idVec3 dir, endp, endDir, collisionPoint; 989 idPluecker pl; 990 cm_trmEdge_t *edge; 991 992 // if the polygon vertex is behind the trm plane it cannot collide with the trm polygon within a 180 degrees rotation 993 if ( tw->isConvex && tw->axisIntersectsTrm && trmpoly->plane.Distance( v->p ) < 0.0f ) { 994 return; 995 } 996 997 // if the model vertex is outside the trm polygon rotation bounds 998 if ( !trmpoly->rotationBounds.ContainsPoint( v->p ) ) { 999 return; 1000 } 1001 1002 // if the rotation axis goes through the polygon vertex 1003 dir = v->p - rotationOrigin; 1004 if ( dir * dir < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) { 1005 return; 1006 } 1007 1008 // calculate vertex end position 1009 endp = v->p; 1010 tw->modelVertexRotation.RotatePoint( endp ); 1011 1012 // rotate the vertex through the epsilon plane 1013 if ( !RotatePointThroughEpsilonPlane( tw, v->p, endp, trmpoly->plane, -tw->angle, rotationOrigin, 1014 tanHalfAngle, collisionPoint, endDir ) ) { 1015 return; 1016 } 1017 1018 if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) { 1019 // verify if 'collisionPoint' moving along 'endDir' moves between polygon edges 1020 pl.FromRay( collisionPoint, endDir ); 1021 for ( i = 0; i < trmpoly->numEdges; i++ ) { 1022 edgeNum = trmpoly->edges[i]; 1023 edge = tw->edges + abs(edgeNum); 1024 if ( edgeNum < 0 ) { 1025 if ( pl.PermutedInnerProduct( edge->pl ) > 0.0f ) { 1026 return; 1027 } 1028 } 1029 else { 1030 if ( pl.PermutedInnerProduct( edge->pl ) < 0.0f ) { 1031 return; 1032 } 1033 } 1034 } 1035 tw->maxTan = idMath::Fabs( tanHalfAngle ); 1036 // collision plane is the flipped trm polygon plane 1037 tw->trace.c.normal = -trmpoly->plane.Normal(); 1038 tw->trace.c.dist = tw->trace.c.normal * v->p; 1039 tw->trace.c.contents = poly->contents; 1040 tw->trace.c.material = poly->material; 1041 tw->trace.c.type = CONTACT_MODELVERTEX; 1042 tw->trace.c.modelFeature = v - tw->model->vertices; 1043 tw->trace.c.trmFeature = trmpoly - tw->polys; 1044 tw->trace.c.point = v->p; 1045 } 1046 } 1047 1048 /* 1049 ================ 1050 idCollisionModelManagerLocal::RotateTrmThroughPolygon 1051 1052 returns true if the polygon blocks the complete rotation 1053 ================ 1054 */ 1055 bool idCollisionModelManagerLocal::RotateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) { 1056 int i, j, k, edgeNum; 1057 float d; 1058 cm_trmVertex_t *bv; 1059 cm_trmEdge_t *be; 1060 cm_trmPolygon_t *bp; 1061 cm_vertex_t *v; 1062 cm_edge_t *e; 1063 idVec3 *rotationOrigin; 1064 1065 // if already checked this polygon 1066 if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) { 1067 return false; 1068 } 1069 p->checkcount = idCollisionModelManagerLocal::checkCount; 1070 1071 // if this polygon does not have the right contents behind it 1072 if ( !(p->contents & tw->contents) ) { 1073 return false; 1074 } 1075 1076 // if the the trace bounds do not intersect the polygon bounds 1077 if ( !tw->bounds.IntersectsBounds( p->bounds ) ) { 1078 return false; 1079 } 1080 1081 // back face culling 1082 if ( tw->isConvex ) { 1083 // if the center of the convex trm is behind the polygon plane 1084 if ( p->plane.Distance( tw->start ) < 0.0f ) { 1085 // if the rotation axis intersects the trace model 1086 if ( tw->axisIntersectsTrm ) { 1087 return false; 1088 } 1089 else { 1090 // if the direction of motion at the start and end position of the 1091 // center of the trm both go towards or away from the polygon plane 1092 // or if the intersections of the rotation axis with the expanded heart planes 1093 // are both in front of the polygon plane 1094 } 1095 } 1096 } 1097 1098 // if the polygon is too far from the first heart plane 1099 d = p->bounds.PlaneDistance( tw->heartPlane1 ); 1100 if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) { 1101 return false; 1102 } 1103 1104 // rotation bounds should cross polygon plane 1105 switch( tw->bounds.PlaneSide( p->plane ) ) { 1106 case PLANESIDE_CROSS: 1107 break; 1108 case PLANESIDE_FRONT: 1109 if ( tw->model->isConvex ) { 1110 tw->quickExit = true; 1111 return true; 1112 } 1113 default: 1114 return false; 1115 } 1116 1117 for ( i = 0; i < tw->numVerts; i++ ) { 1118 bv = tw->vertices + i; 1119 // calculate polygon side this vertex is on 1120 d = p->plane.Distance( bv->p ); 1121 bv->polygonSide = ( d < 0.0f ); 1122 } 1123 1124 for ( i = 0; i < p->numEdges; i++ ) { 1125 edgeNum = p->edges[i]; 1126 e = tw->model->edges + abs(edgeNum); 1127 v = tw->model->vertices + e->vertexNum[INT32_SIGNBITSET( edgeNum )]; 1128 1129 // pluecker coordinate for edge 1130 tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p, 1131 tw->model->vertices[e->vertexNum[1]].p ); 1132 1133 // calculate rotation origin projected into rotation plane through the vertex 1134 tw->polygonRotationOriginCache[i] = tw->origin + tw->axis * ( tw->axis * ( v->p - tw->origin ) ); 1135 } 1136 // copy first to last so we can easily cycle through 1137 tw->polygonRotationOriginCache[p->numEdges] = tw->polygonRotationOriginCache[0]; 1138 1139 // fast point rotation 1140 if ( tw->pointTrace ) { 1141 RotateTrmVertexThroughPolygon( tw, p, &tw->vertices[0], 0 ); 1142 } 1143 else { 1144 // rotate trm vertices through polygon 1145 for ( i = 0; i < tw->numVerts; i++ ) { 1146 bv = tw->vertices + i; 1147 if ( bv->used ) { 1148 RotateTrmVertexThroughPolygon( tw, p, bv, i ); 1149 } 1150 } 1151 1152 // rotate trm edges through polygon 1153 for ( i = 1; i <= tw->numEdges; i++ ) { 1154 be = tw->edges + i; 1155 if ( be->used ) { 1156 RotateTrmEdgeThroughPolygon( tw, p, be ); 1157 } 1158 } 1159 1160 // rotate all polygon vertices through the trm 1161 for ( i = 0; i < p->numEdges; i++ ) { 1162 edgeNum = p->edges[i]; 1163 e = tw->model->edges + abs(edgeNum); 1164 1165 if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) { 1166 continue; 1167 } 1168 // set edge check count 1169 e->checkcount = idCollisionModelManagerLocal::checkCount; 1170 // can never collide with internal edges 1171 if ( e->internal ) { 1172 continue; 1173 } 1174 // got to check both vertices because we skip internal edges 1175 for ( k = 0; k < 2; k++ ) { 1176 1177 v = tw->model->vertices + e->vertexNum[k ^ INT32_SIGNBITSET( edgeNum )]; 1178 1179 // if this vertex is already checked 1180 if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) { 1181 continue; 1182 } 1183 // set vertex check count 1184 v->checkcount = idCollisionModelManagerLocal::checkCount; 1185 1186 // if the vertex is outside the trm rotation bounds 1187 if ( !tw->bounds.ContainsPoint( v->p ) ) { 1188 continue; 1189 } 1190 1191 rotationOrigin = &tw->polygonRotationOriginCache[i+k]; 1192 1193 for ( j = 0; j < tw->numPolys; j++ ) { 1194 bp = tw->polys + j; 1195 if ( bp->used ) { 1196 RotateVertexThroughTrmPolygon( tw, bp, p, v, *rotationOrigin ); 1197 } 1198 } 1199 } 1200 } 1201 } 1202 1203 return ( tw->maxTan == 0.0f ); 1204 } 1205 1206 /* 1207 ================ 1208 idCollisionModelManagerLocal::BoundsForRotation 1209 1210 only for rotations < 180 degrees 1211 ================ 1212 */ 1213 void idCollisionModelManagerLocal::BoundsForRotation( const idVec3 &origin, const idVec3 &axis, const idVec3 &start, const idVec3 &end, idBounds &bounds ) { 1214 int i; 1215 float radiusSqr; 1216 idVec3 v1, v2; 1217 1218 radiusSqr = ( start - origin ).LengthSqr(); 1219 v1 = ( start - origin ).Cross( axis ); 1220 v2 = ( end - origin ).Cross( axis ); 1221 1222 for ( i = 0; i < 3; i++ ) { 1223 // if the derivative changes sign along this axis during the rotation from start to end 1224 if ( ( v1[i] > 0.0f && v2[i] < 0.0f ) || ( v1[i] < 0.0f && v2[i] > 0.0f ) ) { 1225 if ( ( 0.5f * (start[i] + end[i]) - origin[i] ) > 0.0f ) { 1226 bounds[0][i] = Min( start[i], end[i] ); 1227 bounds[1][i] = origin[i] + idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) ); 1228 } 1229 else { 1230 bounds[0][i] = origin[i] - idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) ); 1231 bounds[1][i] = Max( start[i], end[i] ); 1232 } 1233 } 1234 else if ( start[i] > end[i] ) { 1235 bounds[0][i] = end[i]; 1236 bounds[1][i] = start[i]; 1237 } 1238 else { 1239 bounds[0][i] = start[i]; 1240 bounds[1][i] = end[i]; 1241 } 1242 // expand for epsilons 1243 bounds[0][i] -= CM_BOX_EPSILON; 1244 bounds[1][i] += CM_BOX_EPSILON; 1245 } 1246 } 1247 1248 /* 1249 ================ 1250 idCollisionModelManagerLocal::Rotation180 1251 ================ 1252 */ 1253 void idCollisionModelManagerLocal::Rotation180( trace_t *results, const idVec3 &rorg, const idVec3 &axis, 1254 const float startAngle, const float endAngle, const idVec3 &start, 1255 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask, 1256 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) { 1257 int i, j, edgeNum; 1258 float d, maxErr, initialTan; 1259 bool model_rotated, trm_rotated; 1260 idVec3 dir, dir1, dir2, tmp, vr, vup, org, at, bt; 1261 idMat3 invModelAxis, endAxis, tmpAxis; 1262 idRotation startRotation, endRotation; 1263 idPluecker plaxis; 1264 cm_trmPolygon_t *poly; 1265 cm_trmEdge_t *edge; 1266 cm_trmVertex_t *vert; 1267 ALIGN16( static cm_traceWork_t tw ); 1268 1269 if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) { 1270 common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model handle\n"); 1271 return; 1272 } 1273 if ( !idCollisionModelManagerLocal::models[model] ) { 1274 common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model\n"); 1275 return; 1276 } 1277 1278 idCollisionModelManagerLocal::checkCount++; 1279 1280 tw.trace.fraction = 1.0f; 1281 tw.trace.c.contents = 0; 1282 tw.trace.c.type = CONTACT_NONE; 1283 tw.contents = contentMask; 1284 tw.isConvex = true; 1285 tw.rotation = true; 1286 tw.positionTest = false; 1287 tw.axisIntersectsTrm = false; 1288 tw.quickExit = false; 1289 tw.angle = endAngle - startAngle; 1290 assert( tw.angle > -180.0f && tw.angle < 180.0f ); 1291 tw.maxTan = initialTan = idMath::Fabs( tan( ( idMath::PI / 360.0f ) * tw.angle ) ); 1292 tw.model = idCollisionModelManagerLocal::models[model]; 1293 tw.start = start - modelOrigin; 1294 // rotation axis, axis is assumed to be normalized 1295 tw.axis = axis; 1296 // assert( tw.axis[0] * tw.axis[0] + tw.axis[1] * tw.axis[1] + tw.axis[2] * tw.axis[2] > 0.99f ); 1297 // rotation origin projected into rotation plane through tw.start 1298 tw.origin = rorg - modelOrigin; 1299 d = (tw.axis * tw.origin) - ( tw.axis * tw.start ); 1300 tw.origin = tw.origin - d * tw.axis; 1301 // radius of rotation 1302 tw.radius = ( tw.start - tw.origin ).Length(); 1303 // maximum error of the circle approximation traced through the axial BSP tree 1304 d = tw.radius * tw.radius - (CIRCLE_APPROXIMATION_LENGTH*CIRCLE_APPROXIMATION_LENGTH*0.25f); 1305 if ( d > 0.0f ) { 1306 maxErr = tw.radius - idMath::Sqrt( d ); 1307 } else { 1308 maxErr = tw.radius; 1309 } 1310 1311 model_rotated = modelAxis.IsRotated(); 1312 if ( model_rotated ) { 1313 invModelAxis = modelAxis.Transpose(); 1314 tw.axis *= invModelAxis; 1315 tw.origin *= invModelAxis; 1316 } 1317 1318 startRotation.Set( tw.origin, tw.axis, startAngle ); 1319 endRotation.Set( tw.origin, tw.axis, endAngle ); 1320 1321 // create matrix which rotates the rotation axis to the z-axis 1322 tw.axis.NormalVectors( vr, vup ); 1323 tw.matrix[0][0] = vr[0]; 1324 tw.matrix[1][0] = vr[1]; 1325 tw.matrix[2][0] = vr[2]; 1326 tw.matrix[0][1] = -vup[0]; 1327 tw.matrix[1][1] = -vup[1]; 1328 tw.matrix[2][1] = -vup[2]; 1329 tw.matrix[0][2] = tw.axis[0]; 1330 tw.matrix[1][2] = tw.axis[1]; 1331 tw.matrix[2][2] = tw.axis[2]; 1332 1333 // if optimized point trace 1334 if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f && 1335 trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f && 1336 trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) { 1337 1338 if ( model_rotated ) { 1339 // rotate trace instead of model 1340 tw.start *= invModelAxis; 1341 } 1342 tw.end = tw.start; 1343 // if we start at a specific angle 1344 if ( startAngle != 0.0f ) { 1345 startRotation.RotatePoint( tw.start ); 1346 } 1347 // calculate end position of rotation 1348 endRotation.RotatePoint( tw.end ); 1349 1350 // calculate rotation origin projected into rotation plane through the vertex 1351 tw.numVerts = 1; 1352 tw.vertices[0].p = tw.start; 1353 tw.vertices[0].endp = tw.end; 1354 tw.vertices[0].used = true; 1355 tw.vertices[0].rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( tw.vertices[0].p - tw.origin ) ); 1356 BoundsForRotation( tw.vertices[0].rotationOrigin, tw.axis, tw.start, tw.end, tw.vertices[0].rotationBounds ); 1357 // rotation bounds 1358 tw.bounds = tw.vertices[0].rotationBounds; 1359 tw.numEdges = tw.numPolys = 0; 1360 1361 // collision with single point 1362 tw.pointTrace = true; 1363 1364 // extents is set to maximum error of the circle approximation traced through the axial BSP tree 1365 tw.extents[0] = tw.extents[1] = tw.extents[2] = maxErr + CM_BOX_EPSILON; 1366 1367 // setup rotation heart plane 1368 tw.heartPlane1.SetNormal( tw.axis ); 1369 tw.heartPlane1.FitThroughPoint( tw.start ); 1370 tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON; 1371 1372 // trace through the model 1373 idCollisionModelManagerLocal::TraceThroughModel( &tw ); 1374 1375 // store results 1376 *results = tw.trace; 1377 results->endpos = start; 1378 if ( tw.maxTan == initialTan ) { 1379 results->fraction = 1.0f; 1380 } else { 1381 results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle ); 1382 } 1383 assert( results->fraction <= 1.0f ); 1384 endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction ); 1385 endRotation.RotatePoint( results->endpos ); 1386 results->endAxis.Identity(); 1387 1388 if ( results->fraction < 1.0f ) { 1389 // rotate trace plane normal if there was a collision with a rotated model 1390 if ( model_rotated ) { 1391 results->c.normal *= modelAxis; 1392 results->c.point *= modelAxis; 1393 } 1394 results->c.point += modelOrigin; 1395 results->c.dist += modelOrigin * results->c.normal; 1396 } 1397 return; 1398 } 1399 1400 tw.pointTrace = false; 1401 1402 // setup trm structure 1403 idCollisionModelManagerLocal::SetupTrm( &tw, trm ); 1404 1405 trm_rotated = trmAxis.IsRotated(); 1406 1407 // calculate vertex positions 1408 if ( trm_rotated ) { 1409 for ( i = 0; i < tw.numVerts; i++ ) { 1410 // rotate trm around the start position 1411 tw.vertices[i].p *= trmAxis; 1412 } 1413 } 1414 for ( i = 0; i < tw.numVerts; i++ ) { 1415 // set trm at start position 1416 tw.vertices[i].p += tw.start; 1417 } 1418 if ( model_rotated ) { 1419 for ( i = 0; i < tw.numVerts; i++ ) { 1420 tw.vertices[i].p *= invModelAxis; 1421 } 1422 } 1423 for ( i = 0; i < tw.numVerts; i++ ) { 1424 tw.vertices[i].endp = tw.vertices[i].p; 1425 } 1426 // if we start at a specific angle 1427 if ( startAngle != 0.0f ) { 1428 for ( i = 0; i < tw.numVerts; i++ ) { 1429 startRotation.RotatePoint( tw.vertices[i].p ); 1430 } 1431 } 1432 for ( i = 0; i < tw.numVerts; i++ ) { 1433 // end position of vertex 1434 endRotation.RotatePoint( tw.vertices[i].endp ); 1435 } 1436 1437 // add offset to start point 1438 if ( trm_rotated ) { 1439 tw.start += trm->offset * trmAxis; 1440 } else { 1441 tw.start += trm->offset; 1442 } 1443 // if the model is rotated 1444 if ( model_rotated ) { 1445 // rotate trace instead of model 1446 tw.start *= invModelAxis; 1447 } 1448 tw.end = tw.start; 1449 // if we start at a specific angle 1450 if ( startAngle != 0.0f ) { 1451 startRotation.RotatePoint( tw.start ); 1452 } 1453 // calculate end position of rotation 1454 endRotation.RotatePoint( tw.end ); 1455 1456 // setup trm vertices 1457 for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) { 1458 // calculate rotation origin projected into rotation plane through the vertex 1459 vert->rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( vert->p - tw.origin ) ); 1460 // calculate rotation bounds for this vertex 1461 BoundsForRotation( vert->rotationOrigin, tw.axis, vert->p, vert->endp, vert->rotationBounds ); 1462 // if the rotation axis goes through the vertex then the vertex is not used 1463 d = ( vert->p - vert->rotationOrigin ).LengthSqr(); 1464 if ( d > ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) { 1465 vert->used = true; 1466 } 1467 } 1468 1469 // setup trm edges 1470 for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) { 1471 // if the rotation axis goes through both the edge vertices then the edge is not used 1472 if ( tw.vertices[edge->vertexNum[0]].used | tw.vertices[edge->vertexNum[1]].used ) { 1473 edge->used = true; 1474 } 1475 // edge start, end and pluecker coordinate 1476 edge->start = tw.vertices[edge->vertexNum[0]].p; 1477 edge->end = tw.vertices[edge->vertexNum[1]].p; 1478 edge->pl.FromLine( edge->start, edge->end ); 1479 // pluecker coordinate for edge being rotated about the z-axis 1480 at = ( edge->start - tw.origin ) * tw.matrix; 1481 bt = ( edge->end - tw.origin ) * tw.matrix; 1482 edge->plzaxis.FromLine( at, bt ); 1483 // get edge rotation bounds from the rotation bounds of both vertices 1484 edge->rotationBounds = tw.vertices[edge->vertexNum[0]].rotationBounds; 1485 edge->rotationBounds.AddBounds( tw.vertices[edge->vertexNum[1]].rotationBounds ); 1486 // used to calculate if the rotation axis intersects the trm 1487 edge->bitNum = 0; 1488 } 1489 1490 tw.bounds.Clear(); 1491 1492 // rotate trm polygon planes 1493 if ( trm_rotated & model_rotated ) { 1494 tmpAxis = trmAxis * invModelAxis; 1495 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 1496 poly->plane *= tmpAxis; 1497 } 1498 } else if ( trm_rotated ) { 1499 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 1500 poly->plane *= trmAxis; 1501 } 1502 } else if ( model_rotated ) { 1503 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 1504 poly->plane *= invModelAxis; 1505 } 1506 } 1507 1508 // setup trm polygons 1509 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 1510 poly->used = true; 1511 // set trm polygon plane distance 1512 poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start ); 1513 // get polygon bounds from edge bounds 1514 poly->rotationBounds.Clear(); 1515 for ( j = 0; j < poly->numEdges; j++ ) { 1516 // add edge rotation bounds to polygon rotation bounds 1517 edge = &tw.edges[abs( poly->edges[j] )]; 1518 poly->rotationBounds.AddBounds( edge->rotationBounds ); 1519 } 1520 // get trace bounds from polygon bounds 1521 tw.bounds.AddBounds( poly->rotationBounds ); 1522 } 1523 1524 // extents including the maximum error of the circle approximation traced through the axial BSP tree 1525 for ( i = 0; i < 3; i++ ) { 1526 tw.size[0][i] = tw.bounds[0][i] - tw.start[i]; 1527 tw.size[1][i] = tw.bounds[1][i] - tw.start[i]; 1528 if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) { 1529 tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + maxErr + CM_BOX_EPSILON; 1530 } else { 1531 tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + maxErr + CM_BOX_EPSILON; 1532 } 1533 } 1534 1535 // for back-face culling 1536 if ( tw.isConvex ) { 1537 if ( tw.start == tw.origin ) { 1538 tw.axisIntersectsTrm = true; 1539 } else { 1540 // determine if the rotation axis intersects the trm 1541 plaxis.FromRay( tw.origin, tw.axis ); 1542 for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { 1543 // back face cull polygons 1544 if ( poly->plane.Normal() * tw.axis > 0.0f ) { 1545 continue; 1546 } 1547 // test if the axis goes between the polygon edges 1548 for ( j = 0; j < poly->numEdges; j++ ) { 1549 edgeNum = poly->edges[j]; 1550 edge = tw.edges + abs( edgeNum ); 1551 if ( ( edge->bitNum & 2 ) == 0 ) { 1552 d = plaxis.PermutedInnerProduct( edge->pl ); 1553 edge->bitNum = ( ( d < 0.0f ) ? 1 : 0 ) | 2; 1554 } 1555 if ( ( edge->bitNum ^ INT32_SIGNBITSET( edgeNum ) ) & 1 ) { 1556 break; 1557 } 1558 } 1559 if ( j >= poly->numEdges ) { 1560 tw.axisIntersectsTrm = true; 1561 break; 1562 } 1563 } 1564 } 1565 } 1566 1567 // setup rotation heart plane 1568 tw.heartPlane1.SetNormal( tw.axis ); 1569 tw.heartPlane1.FitThroughPoint( tw.start ); 1570 tw.maxDistFromHeartPlane1 = 0.0f; 1571 for ( i = 0; i < tw.numVerts; i++ ) { 1572 d = idMath::Fabs( tw.heartPlane1.Distance( tw.vertices[i].p ) ); 1573 if ( d > tw.maxDistFromHeartPlane1 ) { 1574 tw.maxDistFromHeartPlane1 = d; 1575 } 1576 } 1577 tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON; 1578 1579 // inverse rotation to rotate model vertices towards trace model 1580 tw.modelVertexRotation.Set( tw.origin, tw.axis, -tw.angle ); 1581 1582 // trace through the model 1583 idCollisionModelManagerLocal::TraceThroughModel( &tw ); 1584 1585 // store results 1586 *results = tw.trace; 1587 results->endpos = start; 1588 if ( tw.maxTan == initialTan ) { 1589 results->fraction = 1.0f; 1590 } else { 1591 results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle ); 1592 } 1593 assert( results->fraction <= 1.0f ); 1594 endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction ); 1595 endRotation.RotatePoint( results->endpos ); 1596 results->endAxis = trmAxis * endRotation.ToMat3(); 1597 1598 if ( results->fraction < 1.0f ) { 1599 // rotate trace plane normal if there was a collision with a rotated model 1600 if ( model_rotated ) { 1601 results->c.normal *= modelAxis; 1602 results->c.point *= modelAxis; 1603 } 1604 results->c.point += modelOrigin; 1605 results->c.dist += modelOrigin * results->c.normal; 1606 } 1607 } 1608 1609 /* 1610 ================ 1611 idCollisionModelManagerLocal::Rotation 1612 ================ 1613 */ 1614 #ifdef _DEBUG 1615 static int entered = 0; 1616 #endif 1617 1618 void idCollisionModelManagerLocal::Rotation( trace_t *results, const idVec3 &start, const idRotation &rotation, 1619 const idTraceModel *trm, const idMat3 &trmAxis, int contentMask, 1620 cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) { 1621 idVec3 tmp; 1622 float maxa, stepa, a, lasta; 1623 1624 assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) > (((byte *)results) + sizeof( trace_t )) ); 1625 assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) > (((byte *)results) + sizeof( trace_t )) ); 1626 1627 memset( results, 0, sizeof( *results ) ); 1628 1629 // if special position test 1630 if ( rotation.GetAngle() == 0.0f ) { 1631 idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 1632 return; 1633 } 1634 1635 #ifdef _DEBUG 1636 bool startsolid = false; 1637 // test whether or not stuck to begin with 1638 if ( cm_debugCollision.GetBool() ) { 1639 if ( !entered ) { 1640 entered = 1; 1641 // if already messed up to begin with 1642 if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { 1643 startsolid = true; 1644 } 1645 entered = 0; 1646 } 1647 } 1648 #endif 1649 1650 if ( rotation.GetAngle() >= 180.0f || rotation.GetAngle() <= -180.0f) { 1651 if ( rotation.GetAngle() >= 360.0f ) { 1652 maxa = 360.0f; 1653 stepa = 120.0f; // three steps strictly < 180 degrees 1654 } else if ( rotation.GetAngle() <= -360.0f ) { 1655 maxa = -360.0f; 1656 stepa = -120.0f; // three steps strictly < 180 degrees 1657 } else { 1658 maxa = rotation.GetAngle(); 1659 stepa = rotation.GetAngle() * 0.5f; // two steps strictly < 180 degrees 1660 } 1661 for ( lasta = 0.0f, a = stepa; fabs( a ) < fabs( maxa ) + 1.0f; lasta = a, a += stepa ) { 1662 // partial rotation 1663 idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), lasta, a, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 1664 // if there is a collision 1665 if ( results->fraction < 1.0f ) { 1666 // fraction of total rotation 1667 results->fraction = (lasta + stepa * results->fraction) / rotation.GetAngle(); 1668 return; 1669 } 1670 } 1671 results->fraction = 1.0f; 1672 return; 1673 } 1674 1675 idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), 0.0f, rotation.GetAngle(), start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 1676 1677 #ifdef _DEBUG 1678 // test for missed collisions 1679 if ( cm_debugCollision.GetBool() ) { 1680 if ( !entered ) { 1681 entered = 1; 1682 // if the trm is stuck in the model 1683 if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { 1684 trace_t tr; 1685 1686 // test where the trm is stuck in the model 1687 idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis ); 1688 // re-run collision detection to find out where it failed 1689 idCollisionModelManagerLocal::Rotation( &tr, start, rotation, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); 1690 } 1691 entered = 0; 1692 } 1693 } 1694 #endif 1695 }