neo/cm/CollisionModel_translate.cpp

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