collision.c

   1
   2 #include "quakedef.h"
   3
   4 typedef struct
   5 {
   6         // the hull we're tracing through
   7         const hull_t *hull;
   8
   9         // the trace structure to fill in
  10         trace_t *trace;
  11
  12         // start and end of the trace (in model space)
  13         double start[3];
  14         double end[3];
  15
  16         // end - start
  17         double dist[3];
  18 }
  19 RecursiveHullCheckTraceInfo_t;
  20
  21 // 1/32 epsilon to keep floating point happy
  22 #define DIST_EPSILON (0.03125)
  23
  24 #define HULLCHECKSTATE_EMPTY 0
  25 #define HULLCHECKSTATE_SOLID 1
  26 #define HULLCHECKSTATE_DONE 2
  27
  28 static int RecursiveHullCheck(RecursiveHullCheckTraceInfo_t *t, int num, double p1f, double p2f, double p1[3], double p2[3])
  29 {
  30         // status variables, these don't need to be saved on the stack when
  31         // recursing...  but are because this should be thread-safe
  32         // (note: tracing against a bbox is not thread-safe, yet)
  33         int ret;
  34         mplane_t *plane;
  35         double t1, t2;
  36
  37         // variables that need to be stored on the stack when recursing
  38         dclipnode_t *node;
  39         int side;
  40         double midf, mid[3];
  41
  42         // LordHavoc: a goto!  everyone flee in terror... :)
  43 loc0:
  44         // check for empty
  45         if (num < 0)
  46         {
  47                 t->trace->endcontents = num;
  48                 if (t->trace->thiscontents)
  49                 {
  50                         if (num == t->trace->thiscontents)
  51                                 t->trace->allsolid = false;
  52                         else
  53                         {
  54                                 // if the first leaf is solid, set startsolid
  55                                 if (t->trace->allsolid)
  56                                         t->trace->startsolid = true;
  57                                 return HULLCHECKSTATE_SOLID;
  58                         }
  59                         return HULLCHECKSTATE_EMPTY;
  60                 }
  61                 else
  62                 {
  63                         if (num != CONTENTS_SOLID)
  64                         {
  65                                 t->trace->allsolid = false;
  66                                 if (num == CONTENTS_EMPTY)
  67                                         t->trace->inopen = true;
  68                                 else
  69                                         t->trace->inwater = true;
  70                         }
  71                         else
  72                         {
  73                                 // if the first leaf is solid, set startsolid
  74                                 if (t->trace->allsolid)
  75                                         t->trace->startsolid = true;
  76                                 return HULLCHECKSTATE_SOLID;
  77                         }
  78                         return HULLCHECKSTATE_EMPTY;
  79                 }
  80         }
  81
  82         // find the point distances
  83         node = t->hull->clipnodes + num;
  84
  85         plane = t->hull->planes + node->planenum;
  86         if (plane->type < 3)
  87         {
  88                 t1 = p1[plane->type] - plane->dist;
  89                 t2 = p2[plane->type] - plane->dist;
  90         }
  91         else
  92         {
  93                 t1 = DotProduct (plane->normal, p1) - plane->dist;
  94                 t2 = DotProduct (plane->normal, p2) - plane->dist;
  95         }
  96
  97         if (t1 < 0)
  98         {
  99                 if (t2 < 0)
 100                 {
 101                         num = node->children[1];
 102                         goto loc0;
 103                 }
 104                 side = 1;
 105         }
 106         else
 107         {
 108                 if (t2 >= 0)
 109                 {
 110                         num = node->children[0];
 111                         goto loc0;
 112                 }
 113                 side = 0;
 114         }
 115
 116         // the line intersects, find intersection point
 117         // LordHavoc: this uses the original trace for maximum accuracy
 118         if (plane->type < 3)
 119         {
 120                 t1 = t->start[plane->type] - plane->dist;
 121                 t2 = t->end[plane->type] - plane->dist;
 122         }
 123         else
 124         {
 125                 t1 = DotProduct (plane->normal, t->start) - plane->dist;
 126                 t2 = DotProduct (plane->normal, t->end) - plane->dist;
 127         }
 128
 129         midf = t1 / (t1 - t2);
 130         midf = bound(p1f, midf, p2f);
 131         VectorMA(t->start, midf, t->dist, mid);
 132
 133         // recurse both sides, front side first
 134         ret = RecursiveHullCheck (t, node->children[side], p1f, midf, p1, mid);
 135         // if this side is not empty, return what it is (solid or done)
 136         if (ret != HULLCHECKSTATE_EMPTY)
 137                 return ret;
 138
 139         ret = RecursiveHullCheck (t, node->children[side ^ 1], midf, p2f, mid, p2);
 140         // if other side is not solid, return what it is (empty or done)
 141         if (ret != HULLCHECKSTATE_SOLID)
 142                 return ret;
 143
 144         // front is air and back is solid, this is the impact point...
 145         if (side)
 146         {
 147                 t->trace->plane.dist = -plane->dist;
 148                 VectorNegate (plane->normal, t->trace->plane.normal);
 149         }
 150         else
 151         {
 152                 t->trace->plane.dist = plane->dist;
 153                 VectorCopy (plane->normal, t->trace->plane.normal);
 154         }
 155
 156         // bias away from surface a bit
 157         t1 = DotProduct(t->trace->plane.normal, t->start) - (t->trace->plane.dist + DIST_EPSILON);
 158         t2 = DotProduct(t->trace->plane.normal, t->end) - (t->trace->plane.dist + DIST_EPSILON);
 159
 160         midf = t1 / (t1 - t2);
 161         t->trace->fraction = bound(0.0f, midf, 1.0);
 162
 163         VectorMA(t->start, t->trace->fraction, t->dist, t->trace->endpos);
 164
 165         return HULLCHECKSTATE_DONE;
 166 }
 167
 168 #if 0
 169 // used if start and end are the same
 170 static void RecursiveHullCheckPoint (RecursiveHullCheckTraceInfo_t *t, int num)
 171 {
 172         // If you can read this, you understand BSP trees
 173         while (num >= 0)
 174                 num = t->hull->clipnodes[num].children[((t->hull->planes[t->hull->clipnodes[num].planenum].type < 3) ? (t->start[t->hull->planes[t->hull->clipnodes[num].planenum].type]) : (DotProduct(t->hull->planes[t->hull->clipnodes[num].planenum].normal, t->start))) < t->hull->planes[t->hull->clipnodes[num].planenum].dist];
 175
 176         // check for empty
 177         t->trace->endcontents = num;
 178         if (t->trace->thiscontents)
 179         {
 180                 if (num == t->trace->thiscontents)
 181                         t->trace->allsolid = false;
 182                 else
 183                 {
 184                         // if the first leaf is solid, set startsolid
 185                         if (t->trace->allsolid)
 186                                 t->trace->startsolid = true;
 187                 }
 188         }
 189         else
 190         {
 191                 if (num != CONTENTS_SOLID)
 192                 {
 193                         t->trace->allsolid = false;
 194                         if (num == CONTENTS_EMPTY)
 195                                 t->trace->inopen = true;
 196                         else
 197                                 t->trace->inwater = true;
 198                 }
 199                 else
 200                 {
 201                         // if the first leaf is solid, set startsolid
 202                         if (t->trace->allsolid)
 203                                 t->trace->startsolid = true;
 204                 }
 205         }
 206 }
 207 #endif
 208
 209 void Collision_RoundUpToHullSize(const model_t *cmodel, const vec3_t inmins, const vec3_t inmaxs, vec3_t outmins, vec3_t outmaxs)
 210 {
 211         vec3_t size;
 212         const hull_t *hull;
 213
 214         VectorSubtract(inmaxs, inmins, size);
 215         if (cmodel->brushq1.ishlbsp)
 216         {
 217                 if (size[0] < 3)
 218                         hull = &cmodel->brushq1.hulls[0]; // 0x0x0
 219                 else if (size[0] <= 32)
 220                 {
 221                         if (size[2] < 54) // pick the nearest of 36 or 72
 222                                 hull = &cmodel->brushq1.hulls[3]; // 32x32x36
 223                         else
 224                                 hull = &cmodel->brushq1.hulls[1]; // 32x32x72
 225                 }
 226                 else
 227                         hull = &cmodel->brushq1.hulls[2]; // 64x64x64
 228         }
 229         else
 230         {
 231                 if (size[0] < 3)
 232                         hull = &cmodel->brushq1.hulls[0]; // 0x0x0
 233                 else if (size[0] <= 32)
 234                         hull = &cmodel->brushq1.hulls[1]; // 32x32x56
 235                 else
 236                         hull = &cmodel->brushq1.hulls[2]; // 64x64x88
 237         }
 238         VectorCopy(inmins, outmins);
 239         VectorAdd(inmins, hull->clip_size, outmaxs);
 240 }
 241
 242 static hull_t box_hull;
 243 static dclipnode_t box_clipnodes[6];
 244 static mplane_t box_planes[6];
 245
 246 void Collision_Init (void)
 247 {
 248         int             i;
 249         int             side;
 250
 251         //Set up the planes and clipnodes so that the six floats of a bounding box
 252         //can just be stored out and get a proper hull_t structure.
 253
 254         box_hull.clipnodes = box_clipnodes;
 255         box_hull.planes = box_planes;
 256         box_hull.firstclipnode = 0;
 257         box_hull.lastclipnode = 5;
 258
 259         for (i = 0;i < 6;i++)
 260         {
 261                 box_clipnodes[i].planenum = i;
 262
 263                 side = i&1;
 264
 265                 box_clipnodes[i].children[side] = CONTENTS_EMPTY;
 266                 if (i != 5)
 267                         box_clipnodes[i].children[side^1] = i + 1;
 268                 else
 269                         box_clipnodes[i].children[side^1] = CONTENTS_SOLID;
 270
 271                 box_planes[i].type = i>>1;
 272                 box_planes[i].normal[i>>1] = 1;
 273         }
 274 }
 275
 276 void Collision_ClipTrace_Box(trace_t *trace, const vec3_t cmins, const vec3_t cmaxs, const vec3_t start, const vec3_t mins, const vec3_t maxs, const vec3_t end)
 277 {
 278         RecursiveHullCheckTraceInfo_t rhc;
 279         // fill in a default trace
 280         memset(&rhc, 0, sizeof(rhc));
 281         memset(trace, 0, sizeof(trace_t));
 282         //To keep everything totally uniform, bounding boxes are turned into small
 283         //BSP trees instead of being compared directly.
 284         // create a temp hull from bounding box sizes
 285         box_planes[0].dist = cmaxs[0] - mins[0];
 286         box_planes[1].dist = cmins[0] - maxs[0];
 287         box_planes[2].dist = cmaxs[1] - mins[1];
 288         box_planes[3].dist = cmins[1] - maxs[1];
 289         box_planes[4].dist = cmaxs[2] - mins[2];
 290         box_planes[5].dist = cmins[2] - maxs[2];
 291         // trace a line through the generated clipping hull
 292         rhc.hull = &box_hull;
 293         rhc.trace = trace;
 294         rhc.trace->fraction = 1;
 295         rhc.trace->allsolid = true;
 296         VectorCopy(start, rhc.start);
 297         VectorCopy(end, rhc.end);
 298         VectorSubtract(rhc.end, rhc.start, rhc.dist);
 299         RecursiveHullCheck(&rhc, rhc.hull->firstclipnode, 0, 1, rhc.start, rhc.end);
 300 }
 301
 302
 303
 304
 305
 306
 307
 308
 309
 310
 311
 312
 313
 314
 315
 316 void Collision_PrintBrushAsQHull(colbrushf_t *brush, const char *name)
 317 {
 318         int i;
 319         Con_Printf("3 %s\n%i\n", name, brush->numpoints);
 320         for (i = 0;i < brush->numpoints;i++)
 321                 Con_Printf("%g %g %g\n", brush->points[i].v[0], brush->points[i].v[1], brush->points[i].v[2]);
 322         // FIXME: optimize!
 323         Con_Printf("4\n%i\n", brush->numplanes);
 324         for (i = 0;i < brush->numplanes;i++)
 325                 Con_Printf("%g %g %g %g\n", brush->planes[i].normal[0], brush->planes[i].normal[1], brush->planes[i].normal[2], brush->planes[i].dist);
 326 }
 327
 328
 329 colbrushf_t *Collision_AllocBrushFloat(mempool_t *mempool, int numpoints, int numplanes)
 330 {
 331         colbrushf_t *brush;
 332         brush = Mem_Alloc(mempool, sizeof(colbrushf_t) + sizeof(colpointf_t) * numpoints + sizeof(colplanef_t) * numplanes);
 333         brush->numpoints = numpoints;
 334         brush->numplanes = numplanes;
 335         brush->planes = (void *)(brush + 1);
 336         brush->points = (void *)(brush->planes + brush->numplanes);
 337         return brush;
 338 }
 339
 340 void Collision_CalcPlanesForPolygonBrushFloat(colbrushf_t *brush)
 341 {
 342         int i;
 343         float edge0[3], edge1[3], normal[3], dist, bestdist;
 344         colpointf_t *p, *p2;
 345
 346         // choose best surface normal for polygon's plane
 347         bestdist = 0;
 348         for (i = 0, p = brush->points + 1;i < brush->numpoints - 2;i++, p++)
 349         {
 350                 VectorSubtract(p[-1].v, p[0].v, edge0);
 351                 VectorSubtract(p[1].v, p[0].v, edge1);
 352                 CrossProduct(edge0, edge1, normal);
 353                 dist = DotProduct(normal, normal);
 354                 if (i == 0 || bestdist < dist)
 355                 {
 356                         bestdist = dist;
 357                         VectorCopy(normal, brush->planes->normal);
 358                 }
 359         }
 360
 361         VectorNormalize(brush->planes->normal);
 362         brush->planes->dist = DotProduct(brush->points->v, brush->planes->normal);
 363
 364         // negate plane to create other side
 365         VectorNegate(brush->planes[0].normal, brush->planes[1].normal);
 366         brush->planes[1].dist = -brush->planes[0].dist;
 367         for (i = 0, p = brush->points + (brush->numpoints - 1), p2 = brush->points;i < brush->numpoints;i++, p = p2, p2++)
 368         {
 369                 VectorSubtract(p->v, p2->v, edge0);
 370                 CrossProduct(edge0, brush->planes->normal, brush->planes[i + 2].normal);
 371                 VectorNormalize(brush->planes[i + 2].normal);
 372                 brush->planes[i + 2].dist = DotProduct(p->v, brush->planes[i + 2].normal);
 373         }
 374
 375 #if 1
 376         // validity check - will be disabled later
 377         for (i = 0;i < brush->numplanes;i++)
 378         {
 379                 int j;
 380                 for (j = 0, p = brush->points;j < brush->numpoints;j++, p++)
 381                         if (DotProduct(p->v, brush->planes[i].normal) > brush->planes[i].dist + (1.0 / 32.0))
 382                                 Con_Printf("Error in brush plane generation, plane %i\n", i);
 383         }
 384 #endif
 385 }
 386
 387 colbrushf_t *Collision_AllocBrushFromPermanentPolygonFloat(mempool_t *mempool, int numpoints, float *points)
 388 {
 389         colbrushf_t *brush;
 390         brush = Mem_Alloc(mempool, sizeof(colbrushf_t) + sizeof(colplanef_t) * (numpoints + 2));
 391         brush->numpoints = numpoints;
 392         brush->numplanes = numpoints + 2;
 393         brush->planes = (void *)(brush + 1);
 394         brush->points = (colpointf_t *)points;
 395         return brush;
 396 }
 397
 398 float nearestplanedist_float(const float *normal, const colpointf_t *points, int numpoints)
 399 {
 400         float dist, bestdist;
 401         bestdist = DotProduct(points->v, normal);
 402         points++;
 403         while(--numpoints)
 404         {
 405                 dist = DotProduct(points->v, normal);
 406                 if (bestdist > dist)
 407                         bestdist = dist;
 408                 points++;
 409         }
 410         return bestdist;
 411 }
 412
 413 float furthestplanedist_float(const float *normal, const colpointf_t *points, int numpoints)
 414 {
 415         float dist, bestdist;
 416         bestdist = DotProduct(points->v, normal);
 417         points++;
 418         while(--numpoints)
 419         {
 420                 dist = DotProduct(points->v, normal);
 421                 if (bestdist < dist)
 422                         bestdist = dist;
 423                 points++;
 424         }
 425         return bestdist;
 426 }
 427
 428 #define COLLISIONEPSILON (1.0f / 32.0f)
 429 #define COLLISIONEPSILON2 0//(1.0f / 32.0f)
 430
 431 // NOTE: start and end of each brush pair must have same numplanes/numpoints
 432 void Collision_TraceBrushBrushFloat(trace_t *trace, const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, const colbrushf_t *thatbrush_start, const colbrushf_t *thatbrush_end)
 433 {
 434         int nplane, nplane2, fstartsolid, fendsolid;
 435         float enterfrac, leavefrac, d1, d2, f, newimpactnormal[3];
 436         const colplanef_t *startplane, *endplane;
 437
 438         enterfrac = -1;
 439         leavefrac = 1;
 440         fstartsolid = true;
 441         fendsolid = true;
 442
 443         for (nplane = 0;nplane < thatbrush_start->numplanes + thisbrush_start->numplanes;nplane++)
 444         {
 445                 nplane2 = nplane;
 446                 if (nplane2 >= thatbrush_start->numplanes)
 447                 {
 448                         nplane2 -= thatbrush_start->numplanes;
 449                         startplane = thisbrush_start->planes + nplane2;
 450                         endplane = thisbrush_end->planes + nplane2;
 451                 }
 452                 else
 453                 {
 454                         startplane = thatbrush_start->planes + nplane2;
 455                         endplane = thatbrush_end->planes + nplane2;
 456                 }
 457                 d1 = nearestplanedist_float(startplane->normal, thisbrush_start->points, thisbrush_start->numpoints) - furthestplanedist_float(startplane->normal, thatbrush_start->points, thatbrush_start->numpoints);
 458                 d2 = nearestplanedist_float(endplane->normal, thisbrush_end->points, thisbrush_end->numpoints) - furthestplanedist_float(endplane->normal, thatbrush_end->points, thatbrush_end->numpoints) - COLLISIONEPSILON2;
 459                 //Con_Printf("%c%i: d1 = %f, d2 = %f, d1 / (d1 - d2) = %f\n", nplane2 != nplane ? 'b' : 'a', nplane2, d1, d2, d1 / (d1 - d2));
 460
 461                 f = d1 - d2;
 462                 if (f >= 0)
 463                 {
 464                         // moving into brush
 465                         if (d2 > 0)
 466                                 return;
 467                         if (d1 < 0)
 468                                 continue;
 469                         // enter
 470                         fstartsolid = false;
 471                         f = (d1 - COLLISIONEPSILON) / f;
 472                         f = bound(0, f, 1);
 473                         if (enterfrac < f)
 474                         {
 475                                 enterfrac = f;
 476                                 VectorBlend(startplane->normal, endplane->normal, enterfrac, newimpactnormal);
 477                         }
 478                 }
 479                 else if (f < 0)
 480                 {
 481                         // moving out of brush
 482                         if (d1 > 0)
 483                                 return;
 484                         if (d2 < 0)
 485                                 continue;
 486                         // leave
 487                         fendsolid = false;
 488                         f = (d1 + COLLISIONEPSILON) / f;
 489                         f = bound(0, f, 1);
 490                         if (leavefrac > f)
 491                                 leavefrac = f;
 492                 }
 493         }
 494
 495         if (fstartsolid)
 496         {
 497                 trace->startsolid = true;
 498                 if (fendsolid)
 499                         trace->allsolid = true;
 500         }
 501
 502         // LordHavoc: we need an epsilon nudge here because for a point trace the
 503         // penetrating line segment is normally zero length if this brush was
 504         // generated from a polygon (infinitely thin), and could even be slightly
 505         // positive or negative due to rounding errors in that case.
 506         if (enterfrac > -1 && enterfrac < trace->fraction && enterfrac - (1.0f / 1024.0f) <= leavefrac)
 507         {
 508                 trace->fraction = bound(0, enterfrac, 1);
 509                 VectorCopy(newimpactnormal, trace->plane.normal);
 510         }
 511 }
 512
 513 static colplanef_t polyf_planes[256 + 2];
 514 static colbrushf_t polyf_brush;
 515
 516 void Collision_TraceBrushPolygonFloat(trace_t *trace, const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, int numpoints, const float *points)
 517 {
 518         if (numpoints > 256)
 519         {
 520                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
 521                 return;
 522         }
 523         polyf_brush.numpoints = numpoints;
 524         polyf_brush.numplanes = numpoints + 2;
 525         polyf_brush.points = (colpointf_t *)points;
 526         polyf_brush.planes = polyf_planes;
 527         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brush);
 528         //Collision_PrintBrushAsQHull(&polyf_brush, "polyf_brush");
 529         Collision_TraceBrushBrushFloat(trace, thisbrush_start, thisbrush_end, &polyf_brush, &polyf_brush);
 530 }
 531
 532 static colpointf_t polyf_pointsstart[256], polyf_pointsend[256];
 533 static colplanef_t polyf_planesstart[256 + 2], polyf_planesend[256 + 2];
 534 static colbrushf_t polyf_brushstart, polyf_brushend;
 535
 536 void Collision_TraceBrushPolygonTransformFloat(trace_t *trace, const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, int numpoints, const float *points, const matrix4x4_t *polygonmatrixstart, const matrix4x4_t *polygonmatrixend)
 537 {
 538         int i;
 539         if (numpoints > 256)
 540         {
 541                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
 542                 return;
 543         }
 544         polyf_brushstart.numpoints = numpoints;
 545         polyf_brushstart.numplanes = numpoints + 2;
 546         polyf_brushstart.points = polyf_pointsstart;//(colpointf_t *)points;
 547         polyf_brushstart.planes = polyf_planesstart;
 548         for (i = 0;i < numpoints;i++)
 549                 Matrix4x4_Transform(polygonmatrixstart, points + i * 3, polyf_brushstart.points[i].v);
 550         polyf_brushend.numpoints = numpoints;
 551         polyf_brushend.numplanes = numpoints + 2;
 552         polyf_brushend.points = polyf_pointsend;//(colpointf_t *)points;
 553         polyf_brushend.planes = polyf_planesend;
 554         for (i = 0;i < numpoints;i++)
 555                 Matrix4x4_Transform(polygonmatrixend, points + i * 3, polyf_brushend.points[i].v);
 556         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brushstart);
 557         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brushend);
 558
 559         //Collision_PrintBrushAsQHull(&polyf_brushstart, "polyf_brushstart");
 560         //Collision_PrintBrushAsQHull(&polyf_brushend, "polyf_brushend");
 561
 562         Collision_TraceBrushBrushFloat(trace, thisbrush_start, thisbrush_end, &polyf_brushstart, &polyf_brushend);
 563 }
 564
 565 colbrushd_t *Collision_AllocBrushDouble(mempool_t *mempool, int numpoints, int numplanes)
 566 {
 567         colbrushd_t *brush;
 568         brush = Mem_Alloc(mempool, sizeof(colbrushd_t) + sizeof(colpointd_t) * numpoints + sizeof(colplaned_t) * numplanes);
 569         brush->numpoints = numpoints;
 570         brush->numplanes = numplanes;
 571         brush->planes = (void *)(brush + 1);
 572         brush->points = (void *)(brush->planes + brush->numplanes);
 573         return brush;
 574 }
 575
 576 void Collision_CalcPlanesForPolygonBrushDouble(colbrushd_t *brush)
 577 {
 578         int i;
 579         double edge0[3], edge1[3], normal[3], dist, bestdist;
 580         colpointd_t *p, *p2;
 581
 582         // choose best surface normal for polygon's plane
 583         bestdist = 0;
 584         for (i = 2, p = brush->points + 2;i < brush->numpoints;i++, p++)
 585         {
 586                 VectorSubtract(p[-1].v, p[0].v, edge0);
 587                 VectorSubtract(p[1].v, p[0].v, edge1);
 588                 CrossProduct(edge0, edge1, normal);
 589                 dist = DotProduct(normal, normal);
 590                 if (i == 2 || bestdist < dist)
 591                 {
 592                         bestdist = dist;
 593                         VectorCopy(normal, brush->planes->normal);
 594                 }
 595         }
 596
 597         VectorNormalize(brush->planes->normal);
 598         brush->planes->dist = DotProduct(brush->points->v, brush->planes->normal);
 599
 600         // negate plane to create other side
 601         VectorNegate(brush->planes[0].normal, brush->planes[1].normal);
 602         brush->planes[1].dist = -brush->planes[0].dist;
 603         for (i = 0, p = brush->points + (brush->numpoints - 1), p2 = brush->points + 2;i < brush->numpoints;i++, p = p2, p2++)
 604         {
 605                 VectorSubtract(p->v, p2->v, edge0);
 606                 CrossProduct(edge0, brush->planes->normal, brush->planes[i].normal);
 607                 VectorNormalize(brush->planes[i].normal);
 608                 brush->planes[i].dist = DotProduct(p->v, brush->planes[i].normal);
 609         }
 610
 611 #if 1
 612         // validity check - will be disabled later
 613         for (i = 0;i < brush->numplanes;i++)
 614         {
 615                 int j;
 616                 for (j = 0, p = brush->points;j < brush->numpoints;j++, p++)
 617                         if (DotProduct(p->v, brush->planes[i].normal) > brush->planes[i].dist + (1.0 / 32.0))
 618                                 Con_Printf("Error in brush plane generation, plane %i\n");
 619         }
 620 #endif
 621 }
 622
 623 colbrushd_t *Collision_AllocBrushFromPermanentPolygonDouble(mempool_t *mempool, int numpoints, double *points)
 624 {
 625         colbrushd_t *brush;
 626         brush = Mem_Alloc(mempool, sizeof(colbrushd_t) + sizeof(colplaned_t) * (numpoints + 2));
 627         brush->numpoints = numpoints;
 628         brush->numplanes = numpoints + 2;
 629         brush->planes = (void *)(brush + 1);
 630         brush->points = (colpointd_t *)points;
 631         return brush;
 632 }
 633
 634
 635 double nearestplanedist_double(const double *normal, const colpointd_t *points, int numpoints)
 636 {
 637         double dist, bestdist;
 638         bestdist = DotProduct(points->v, normal);
 639         points++;
 640         while(--numpoints)
 641         {
 642                 dist = DotProduct(points->v, normal);
 643                 if (bestdist > dist)
 644                         bestdist = dist;
 645                 points++;
 646         }
 647         return bestdist;
 648 }
 649
 650 double furthestplanedist_double(const double *normal, const colpointd_t *points, int numpoints)
 651 {
 652         double dist, bestdist;
 653         bestdist = DotProduct(points->v, normal);
 654         points++;
 655         while(--numpoints)
 656         {
 657                 dist = DotProduct(points->v, normal);
 658                 if (bestdist < dist)
 659                         bestdist = dist;
 660                 points++;
 661         }
 662         return bestdist;
 663 }
 664
 665 // NOTE: start and end of each brush pair must have same numplanes/numpoints
 666 void Collision_TraceBrushBrushDouble(trace_t *trace, const colbrushd_t *thisbrush_start, const colbrushd_t *thisbrush_end, const colbrushd_t *thatbrush_start, const colbrushd_t *thatbrush_end)
 667 {
 668         int nplane, nplane2, fstartsolid, fendsolid;
 669         double enterfrac, leavefrac, d1, d2, f, newimpactnormal[3];
 670         const colplaned_t *startplane, *endplane;
 671
 672         enterfrac = -1;
 673         leavefrac = 1;
 674         fstartsolid = true;
 675         fendsolid = true;
 676
 677         for (nplane = 0;nplane < thatbrush_start->numplanes + thisbrush_start->numplanes;nplane++)
 678         {
 679                 nplane2 = nplane;
 680                 if (nplane2 >= thatbrush_start->numplanes)
 681                 {
 682                         nplane2 -= thatbrush_start->numplanes;
 683                         startplane = thisbrush_start->planes + nplane2;
 684                         endplane = thisbrush_end->planes + nplane2;
 685                 }
 686                 else
 687                 {
 688                         startplane = thatbrush_start->planes + nplane2;
 689                         endplane = thatbrush_end->planes + nplane2;
 690                 }
 691                 d1 = nearestplanedist_double(startplane->normal, thisbrush_start->points, thisbrush_start->numpoints) - furthestplanedist_double(startplane->normal, thatbrush_start->points, thatbrush_start->numpoints);
 692                 d2 = nearestplanedist_double(endplane->normal, thisbrush_end->points, thisbrush_end->numpoints) - furthestplanedist_double(endplane->normal, thatbrush_end->points, thatbrush_end->numpoints) - COLLISIONEPSILON2;
 693                 //Con_Printf("%c%i: d1 = %f, d2 = %f, d1 / (d1 - d2) = %f\n", nplane2 != nplane ? 'b' : 'a', nplane2, d1, d2, d1 / (d1 - d2));
 694
 695                 f = d1 - d2;
 696                 if (f >= 0)
 697                 {
 698                         // moving into brush
 699                         if (d2 > 0)
 700                                 return;
 701                         if (d1 < 0)
 702                                 continue;
 703                         // enter
 704                         fstartsolid = false;
 705                         f = (d1 - COLLISIONEPSILON) / f;
 706                         f = bound(0, f, 1);
 707                         if (enterfrac < f)
 708                         {
 709                                 enterfrac = f;
 710                                 VectorBlend(startplane->normal, endplane->normal, enterfrac, newimpactnormal);
 711                         }
 712                 }
 713                 else if (f < 0)
 714                 {
 715                         // moving out of brush
 716                         if (d1 > 0)
 717                                 return;
 718                         if (d2 < 0)
 719                                 continue;
 720                         // leave
 721                         fendsolid = false;
 722                         f = (d1 + COLLISIONEPSILON) / f;
 723                         f = bound(0, f, 1);
 724                         if (leavefrac > f)
 725                                 leavefrac = f;
 726                 }
 727         }
 728
 729         if (fstartsolid)
 730         {
 731                 trace->startsolid = true;
 732                 if (fendsolid)
 733                         trace->allsolid = true;
 734         }
 735
 736         // LordHavoc: we need an epsilon nudge here because for a point trace the
 737         // penetrating line segment is normally zero length if this brush was
 738         // generated from a polygon (infinitely thin), and could even be slightly
 739         // positive or negative due to rounding errors in that case.
 740         if (enterfrac > -1 && enterfrac < trace->fraction && enterfrac - (1.0f / 1024.0f) <= leavefrac)
 741         {
 742                 trace->fraction = bound(0, enterfrac, 1);
 743                 VectorCopy(newimpactnormal, trace->plane.normal);
 744         }
 745 }
 746
 747 static colplaned_t polyd_planes[256 + 2];
 748 static colbrushd_t polyd_brush;
 749 void Collision_TraceBrushPolygonDouble(trace_t *trace, const colbrushd_t *thisbrush_start, const colbrushd_t *thisbrush_end, int numpoints, const double *points)
 750 {
 751         if (numpoints > 256)
 752         {
 753                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
 754                 return;
 755         }
 756         polyd_brush.numpoints = numpoints;
 757         polyd_brush.numplanes = numpoints + 2;
 758         polyd_brush.points = (colpointd_t *)points;
 759         polyd_brush.planes = polyd_planes;
 760         Collision_CalcPlanesForPolygonBrushDouble(&polyd_brush);
 761         Collision_TraceBrushBrushDouble(trace, thisbrush_start, thisbrush_end, &polyd_brush, &polyd_brush);
 762 }
 763
 764
 765
 766
 767 typedef struct colbrushbmodelinfo_s
 768 {
 769         model_t *model;
 770         trace_t *trace;
 771         const matrix4x4_t *modelmatrixstart;
 772         const matrix4x4_t *modelmatrixend;
 773         const colbrushf_t *thisbrush_start;
 774         const colbrushf_t *thisbrush_end;
 775 }
 776 colbrushbmodelinfo_t;
 777
 778 static int colframecount = 1;
 779
 780 void Collision_RecursiveTraceBrushNode(colbrushbmodelinfo_t *info, mnode_t *node)
 781 {
 782         if (node->contents)
 783         {
 784                 // collide with surfaces marked by this leaf
 785                 int i, *mark;
 786                 mleaf_t *leaf = (mleaf_t *)node;
 787                 msurface_t *surf;
 788                 for (i = 0, mark = leaf->firstmarksurface;i < leaf->nummarksurfaces;i++, mark++)
 789                 {
 790                         surf = info->model->brushq1.surfaces + *mark;
 791                         // don't check a surface twice
 792                         if (surf->colframe != colframecount)
 793                         {
 794                                 surf->colframe = colframecount;
 795                                 if (surf->flags & SURF_SOLIDCLIP)
 796                                 {
 797                                         Collision_TraceBrushPolygonFloat(info->trace, info->thisbrush_start, info->thisbrush_end, surf->poly_numverts, surf->poly_verts);
 798                                         //Collision_TraceBrushPolygonTransformFloat(info->trace, info->thisbrush_start, info->thisbrush_end, surf->poly_numverts, surf->poly_verts, info->modelmatrixstart, info->modelmatrixend);
 799                                 }
 800                         }
 801                 }
 802         }
 803         else
 804         {
 805                 // recurse down node sides
 806                 int i;
 807                 float dist;
 808                 colpointf_t *ps, *pe;
 809                 // FIXME? if TraceBrushPolygonTransform were to be made usable, the
 810                 // node planes would need to be transformed too
 811                 dist = node->plane->dist - (1.0f / 8.0f);
 812                 for (i = 0, ps = info->thisbrush_start->points, pe = info->thisbrush_end->points;i < info->thisbrush_start->numpoints;i++, ps++, pe++)
 813                 {
 814                         if (DotProduct(ps->v, node->plane->normal) > dist || DotProduct(pe->v, node->plane->normal) > dist)
 815                         {
 816                                 Collision_RecursiveTraceBrushNode(info, node->children[0]);
 817                                 break;
 818                         }
 819                 }
 820                 dist = node->plane->dist + (1.0f / 8.0f);
 821                 for (i = 0, ps = info->thisbrush_start->points, pe = info->thisbrush_end->points;i < info->thisbrush_start->numpoints;i++, ps++, pe++)
 822                 {
 823                         if (DotProduct(ps->v, node->plane->normal) < dist || DotProduct(pe->v, node->plane->normal) < dist)
 824                         {
 825                                 Collision_RecursiveTraceBrushNode(info, node->children[1]);
 826                                 break;
 827                         }
 828                 }
 829         }
 830 }
 831
 832 void Collision_TraceBrushBModel(trace_t *trace, const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, model_t *model)
 833 {
 834         colbrushbmodelinfo_t info;
 835         colframecount++;
 836         memset(trace, 0, sizeof(*trace));
 837         trace->fraction = 1;
 838         info.trace = trace;
 839         info.model = model;
 840         info.thisbrush_start = thisbrush_start;
 841         info.thisbrush_end = thisbrush_end;
 842         Collision_RecursiveTraceBrushNode(&info, model->brushq1.nodes + model->brushq1.hulls[0].firstclipnode);
 843 }
 844
 845 void Collision_TraceBrushBModelTransform(trace_t *trace, const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, model_t *model, const matrix4x4_t *modelmatrixstart, const matrix4x4_t *modelmatrixend)
 846 {
 847         colbrushbmodelinfo_t info;
 848         colframecount++;
 849         memset(trace, 0, sizeof(*trace));
 850         trace->fraction = 1;
 851         info.trace = trace;
 852         info.model = model;
 853         info.modelmatrixstart = modelmatrixstart;
 854         info.modelmatrixend = modelmatrixend;
 855         info.thisbrush_start = thisbrush_start;
 856         info.thisbrush_end = thisbrush_end;
 857         Collision_RecursiveTraceBrushNode(&info, model->brushq1.nodes + model->brushq1.hulls[0].firstclipnode);
 858 }
 859
 860
 861
 862 #define MAX_BRUSHFORBOX 16
 863 static int brushforbox_index = 0;
 864 static colpointf_t brushforbox_point[MAX_BRUSHFORBOX*8];
 865 static colplanef_t brushforbox_plane[MAX_BRUSHFORBOX*6];
 866 static colbrushf_t brushforbox_brush[MAX_BRUSHFORBOX];
 867
 868 void Collision_InitBrushForBox(void)
 869 {
 870         int i;
 871         for (i = 0;i < MAX_BRUSHFORBOX;i++)
 872         {
 873                 brushforbox_brush[i].numpoints = 8;
 874                 brushforbox_brush[i].numplanes = 6;
 875                 brushforbox_brush[i].points = brushforbox_point + i * 8;
 876                 brushforbox_brush[i].planes = brushforbox_plane + i * 6;
 877         }
 878 }
 879
 880 colbrushf_t *Collision_BrushForBox(const matrix4x4_t *matrix, const vec3_t mins, const vec3_t maxs)
 881 {
 882         int i;
 883         vec3_t v;
 884         colbrushf_t *brush;
 885         if (brushforbox_brush[0].numpoints == 0)
 886                 Collision_InitBrushForBox();
 887         brush = brushforbox_brush + ((brushforbox_index++) % MAX_BRUSHFORBOX);
 888         // FIXME: optimize
 889         for (i = 0;i < 8;i++)
 890         {
 891                 v[0] = i & 1 ? maxs[0] : mins[0];
 892                 v[1] = i & 2 ? maxs[1] : mins[1];
 893                 v[2] = i & 4 ? maxs[2] : mins[2];
 894                 Matrix4x4_Transform(matrix, v, brush->points[i].v);
 895         }
 896         // FIXME: optimize!
 897         for (i = 0;i < 6;i++)
 898         {
 899                 VectorClear(v);
 900                 v[i >> 1] = i & 1 ? 1 : -1;
 901                 Matrix4x4_Transform3x3(matrix, v, brush->planes[i].normal);
 902                 VectorNormalize(brush->planes[i].normal);
 903                 brush->planes[i].dist = furthestplanedist_float(brush->planes[i].normal, brush->points, brush->numpoints);
 904         }
 905         return brush;
 906 }
 907
 908 void Collision_PolygonClipTrace (trace_t *trace, const void *cent, model_t *cmodel, const vec3_t corigin, const vec3_t cangles, const vec3_t cmins, const vec3_t cmaxs, const vec3_t start, const vec3_t mins, const vec3_t maxs, const vec3_t end)
 909 {
 910         vec3_t impactnormal;
 911         //vec3_t mins2, maxs2;
 912         matrix4x4_t cmatrix, cimatrix, startmatrix, endmatrix;
 913         matrix4x4_t mstartmatrix, mendmatrix, identitymatrix;
 914         colbrushf_t *thisbrush_start, *thisbrush_end, *cbrush;
 915
 916         Matrix4x4_CreateFromQuakeEntity(&cmatrix, corigin[0], corigin[1], corigin[2], cangles[0], cangles[1], cangles[2], 1);
 917         Matrix4x4_Invert_Simple(&cimatrix, &cmatrix);
 918         Matrix4x4_CreateTranslate(&startmatrix, start[0], start[1], start[2]);
 919         Matrix4x4_CreateTranslate(&endmatrix, end[0], end[1], end[2]);
 920
 921         Matrix4x4_CreateIdentity(&identitymatrix);
 922         Matrix4x4_Concat(&mstartmatrix, &cimatrix, &startmatrix);
 923         Matrix4x4_Concat(&mendmatrix, &cimatrix, &endmatrix);
 924         thisbrush_start = Collision_BrushForBox(&mstartmatrix, mins, maxs);
 925         //mins2[0] = mins[0] - 0.0625;mins2[1] = mins[1] - 0.0625;mins2[2] = mins[2] - 0.0625;
 926         //maxs2[0] = maxs[0] + 0.0625;maxs2[1] = maxs[1] + 0.0625;maxs2[2] = maxs[2] + 0.0625;
 927         thisbrush_end = Collision_BrushForBox(&mendmatrix, mins, maxs);
 928
 929         //Collision_PrintBrushAsQHull(thisbrush_start, "thisbrush_start");
 930         //Collision_PrintBrushAsQHull(thisbrush_end, "thisbrush_end");
 931         memset (trace, 0, sizeof(trace_t));
 932         if (cmodel && cmodel->type == mod_brush)
 933         {
 934                 // brush model
 935                 Collision_TraceBrushBModel(trace, thisbrush_start, thisbrush_end, cmodel);
 936                 //Collision_TraceBrushBModelTransform(trace, thisbrush_start, thisbrush_end, cmodel, &cmatrix, &cmatrix);
 937         }
 938         else
 939         {
 940                 // bounding box
 941                 cbrush = Collision_BrushForBox(&identitymatrix, cmins, cmaxs);
 942                 Collision_TraceBrushBrushFloat(trace, thisbrush_start, thisbrush_end, cbrush, cbrush);
 943                 //cbrush = Collision_BrushForBox(&cmatrix, cmins, cmaxs);
 944                 //trace->fraction = Collision_TraceBrushBrushFloat(trace, thisbrush_start, thisbrush_end, cbrush, cbrush);
 945         }
 946
 947         if (trace->fraction < 0 || trace->fraction > 1)
 948                 Con_Printf("fraction out of bounds %f %s:%d\n", trace->fraction, __FILE__, __LINE__);
 949
 950         VectorBlend(start, end, trace->fraction, trace->endpos);
 951         if (trace->fraction < 1)
 952         {
 953                 trace->ent = (void *) cent;
 954                 VectorCopy(trace->plane.normal, impactnormal);
 955                 Matrix4x4_Transform(&cmatrix, impactnormal, trace->plane.normal);
 956                 VectorNormalize(trace->plane.normal);
 957                 //Con_Printf("fraction %f normal %f %f %f\n", trace->fraction, trace->plane.normal[0], trace->plane.normal[1], trace->plane.normal[2]);
 958         }
 959 }
 960
 961
 962 // LordHavoc: currently unused and not yet tested
 963 // note: this can be used for tracing a moving sphere vs a stationary sphere,
 964 // by simply adding the moving sphere's radius to the sphereradius parameter,
 965 // all the results are correct (impactpoint, impactnormal, and fraction)
 966 float Collision_ClipTrace_Line_Sphere(double *linestart, double *lineend, double *sphereorigin, double sphereradius, double *impactpoint, double *impactnormal)
 967 {
 968         double dir[3], scale, v[3], deviationdist, impactdist, linelength;
 969         // make sure the impactpoint and impactnormal are valid even if there is
 970         // no collision
 971         impactpoint[0] = lineend[0];
 972         impactpoint[1] = lineend[1];
 973         impactpoint[2] = lineend[2];
 974         impactnormal[0] = 0;
 975         impactnormal[1] = 0;
 976         impactnormal[2] = 0;
 977         // calculate line direction
 978         dir[0] = lineend[0] - linestart[0];
 979         dir[1] = lineend[1] - linestart[1];
 980         dir[2] = lineend[2] - linestart[2];
 981         // normalize direction
 982         linelength = sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
 983         if (linelength)
 984         {
 985                 scale = 1.0 / linelength;
 986                 dir[0] *= scale;
 987                 dir[1] *= scale;
 988                 dir[2] *= scale;
 989         }
 990         // this dotproduct calculates the distance along the line at which the
 991         // sphere origin is (nearest point to the sphere origin on the line)
 992         impactdist = dir[0] * (sphereorigin[0] - linestart[0]) + dir[1] * (sphereorigin[1] - linestart[1]) + dir[2] * (sphereorigin[2] - linestart[2]);
 993         // calculate point on line at that distance, and subtract the
 994         // sphereorigin from it, so we have a vector to measure for the distance
 995         // of the line from the sphereorigin (deviation, how off-center it is)
 996         v[0] = linestart[0] + impactdist * dir[0] - sphereorigin[0];
 997         v[1] = linestart[1] + impactdist * dir[1] - sphereorigin[1];
 998         v[2] = linestart[2] + impactdist * dir[2] - sphereorigin[2];
 999         deviationdist = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
1000         // if outside the radius, it's a miss for sure
1001         // (we do this comparison using squared radius to avoid a sqrt)
1002         if (deviationdist > sphereradius*sphereradius)
1003                 return 1; // miss (off to the side)
1004         // nudge back to find the correct impact distance
1005         impactdist += (sqrt(deviationdist) - sphereradius);
1006         if (impactdist >= linelength)
1007                 return 1; // miss (not close enough)
1008         if (impactdist < 0)
1009                 return 1; // miss (linestart is past or inside sphere)
1010         // calculate new impactpoint
1011         impactpoint[0] = linestart[0] + impactdist * dir[0];
1012         impactpoint[1] = linestart[1] + impactdist * dir[1];
1013         impactpoint[2] = linestart[2] + impactdist * dir[2];
1014         // calculate impactnormal (surface normal at point of impact)
1015         impactnormal[0] = impactpoint[0] - sphereorigin[0];
1016         impactnormal[1] = impactpoint[1] - sphereorigin[1];
1017         impactnormal[2] = impactpoint[2] - sphereorigin[2];
1018         // normalize impactnormal
1019         scale = impactnormal[0] * impactnormal[0] + impactnormal[1] * impactnormal[1] + impactnormal[2] * impactnormal[2];
1020         if (scale)
1021         {
1022                 scale = 1.0 / sqrt(scale);
1023                 impactnormal[0] *= scale;
1024                 impactnormal[1] *= scale;
1025                 impactnormal[2] *= scale;
1026         }
1027         // return fraction of movement distance
1028         return impactdist / linelength;
1029 }
1030