Q3BSP collisions implemented (although not for patches, which aren't supported anyway...
[divverent/darkplaces.git] / 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