optimized polygon collision code a bit (optimized node side comparison)
[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 float Collision_TraceBrushBrushFloat(const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, const colbrushf_t *thatbrush_start, const colbrushf_t *thatbrush_end, float *impactnormal, int *startsolid, int *allsolid)
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 1;
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 1;
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                 if (startsolid)
498                         *startsolid = true;
499                 if (fendsolid && allsolid)
500                         *allsolid = true;
501         }
502
503         // LordHavoc: we need an epsilon nudge here because for a point trace the
504         // penetrating line segment is normally zero length if this brush was
505         // generated from a polygon (infinitely thin), and could even be slightly
506         // positive or negative due to rounding errors in that case.
507         if (enterfrac > -1 && enterfrac < 1 && enterfrac - (1.0f / 1024.0f) <= leavefrac)
508         {
509                 //if (enterfrac < (1.0f / 1024.0f))
510                 //      enterfrac = 0;
511                 enterfrac = bound(0, enterfrac, 1);
512                 VectorCopy(newimpactnormal, impactnormal);
513                 return enterfrac;
514         }
515         return 1;
516 }
517
518 static colplanef_t polyf_planes[256 + 2];
519 static colbrushf_t polyf_brush;
520
521 float Collision_TraceBrushPolygonFloat(const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, int numpoints, const float *points, float *impactnormal, int *startsolid, int *allsolid)
522 {
523         if (numpoints > 256)
524         {
525                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
526                 return 1;
527         }
528         polyf_brush.numpoints = numpoints;
529         polyf_brush.numplanes = numpoints + 2;
530         polyf_brush.points = (colpointf_t *)points;
531         polyf_brush.planes = polyf_planes;
532         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brush);
533         //Collision_PrintBrushAsQHull(&polyf_brush, "polyf_brush");
534         return Collision_TraceBrushBrushFloat(thisbrush_start, thisbrush_end, &polyf_brush, &polyf_brush, impactnormal, startsolid, allsolid);
535 }
536
537 static colpointf_t polyf_pointsstart[256], polyf_pointsend[256];
538 static colplanef_t polyf_planesstart[256 + 2], polyf_planesend[256 + 2];
539 static colbrushf_t polyf_brushstart, polyf_brushend;
540
541 float Collision_TraceBrushPolygonTransformFloat(const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, int numpoints, const float *points, float *impactnormal, const matrix4x4_t *polygonmatrixstart, const matrix4x4_t *polygonmatrixend, int *startsolid, int *allsolid)
542 {
543         int i;
544         if (numpoints > 256)
545         {
546                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
547                 return 1;
548         }
549         polyf_brushstart.numpoints = numpoints;
550         polyf_brushstart.numplanes = numpoints + 2;
551         polyf_brushstart.points = polyf_pointsstart;//(colpointf_t *)points;
552         polyf_brushstart.planes = polyf_planesstart;
553         for (i = 0;i < numpoints;i++)
554                 Matrix4x4_Transform(polygonmatrixstart, points + i * 3, polyf_brushstart.points[i].v);
555         polyf_brushend.numpoints = numpoints;
556         polyf_brushend.numplanes = numpoints + 2;
557         polyf_brushend.points = polyf_pointsend;//(colpointf_t *)points;
558         polyf_brushend.planes = polyf_planesend;
559         for (i = 0;i < numpoints;i++)
560                 Matrix4x4_Transform(polygonmatrixend, points + i * 3, polyf_brushend.points[i].v);
561         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brushstart);
562         Collision_CalcPlanesForPolygonBrushFloat(&polyf_brushend);
563
564         //Collision_PrintBrushAsQHull(&polyf_brushstart, "polyf_brushstart");
565         //Collision_PrintBrushAsQHull(&polyf_brushend, "polyf_brushend");
566
567         return Collision_TraceBrushBrushFloat(thisbrush_start, thisbrush_end, &polyf_brushstart, &polyf_brushend, impactnormal, startsolid, allsolid);
568 }
569
570 colbrushd_t *Collision_AllocBrushDouble(mempool_t *mempool, int numpoints, int numplanes)
571 {
572         colbrushd_t *brush;
573         brush = Mem_Alloc(mempool, sizeof(colbrushd_t) + sizeof(colpointd_t) * numpoints + sizeof(colplaned_t) * numplanes);
574         brush->numpoints = numpoints;
575         brush->numplanes = numplanes;
576         brush->planes = (void *)(brush + 1);
577         brush->points = (void *)(brush->planes + brush->numplanes);
578         return brush;
579 }
580
581 void Collision_CalcPlanesForPolygonBrushDouble(colbrushd_t *brush)
582 {
583         int i;
584         double edge0[3], edge1[3], normal[3], dist, bestdist;
585         colpointd_t *p, *p2;
586
587         // choose best surface normal for polygon's plane
588         bestdist = 0;
589         for (i = 2, p = brush->points + 2;i < brush->numpoints;i++, p++)
590         {
591                 VectorSubtract(p[-1].v, p[0].v, edge0);
592                 VectorSubtract(p[1].v, p[0].v, edge1);
593                 CrossProduct(edge0, edge1, normal);
594                 dist = DotProduct(normal, normal);
595                 if (i == 2 || bestdist < dist)
596                 {
597                         bestdist = dist;
598                         VectorCopy(normal, brush->planes->normal);
599                 }
600         }
601
602         VectorNormalize(brush->planes->normal);
603         brush->planes->dist = DotProduct(brush->points->v, brush->planes->normal);
604
605         // negate plane to create other side
606         VectorNegate(brush->planes[0].normal, brush->planes[1].normal);
607         brush->planes[1].dist = -brush->planes[0].dist;
608         for (i = 0, p = brush->points + (brush->numpoints - 1), p2 = brush->points + 2;i < brush->numpoints;i++, p = p2, p2++)
609         {
610                 VectorSubtract(p->v, p2->v, edge0);
611                 CrossProduct(edge0, brush->planes->normal, brush->planes[i].normal);
612                 VectorNormalize(brush->planes[i].normal);
613                 brush->planes[i].dist = DotProduct(p->v, brush->planes[i].normal);
614         }
615
616 #if 1
617         // validity check - will be disabled later
618         for (i = 0;i < brush->numplanes;i++)
619         {
620                 int j;
621                 for (j = 0, p = brush->points;j < brush->numpoints;j++, p++)
622                         if (DotProduct(p->v, brush->planes[i].normal) > brush->planes[i].dist + (1.0 / 32.0))
623                                 Con_Printf("Error in brush plane generation, plane %i\n");
624         }
625 #endif
626 }
627
628 colbrushd_t *Collision_AllocBrushFromPermanentPolygonDouble(mempool_t *mempool, int numpoints, double *points)
629 {
630         colbrushd_t *brush;
631         brush = Mem_Alloc(mempool, sizeof(colbrushd_t) + sizeof(colplaned_t) * (numpoints + 2));
632         brush->numpoints = numpoints;
633         brush->numplanes = numpoints + 2;
634         brush->planes = (void *)(brush + 1);
635         brush->points = (colpointd_t *)points;
636         return brush;
637 }
638
639
640 double nearestplanedist_double(const double *normal, const colpointd_t *points, int numpoints)
641 {
642         double dist, bestdist;
643         bestdist = DotProduct(points->v, normal);
644         points++;
645         while(--numpoints)
646         {
647                 dist = DotProduct(points->v, normal);
648                 if (bestdist > dist)
649                         bestdist = dist;
650                 points++;
651         }
652         return bestdist;
653 }
654
655 double furthestplanedist_double(const double *normal, const colpointd_t *points, int numpoints)
656 {
657         double dist, bestdist;
658         bestdist = DotProduct(points->v, normal);
659         points++;
660         while(--numpoints)
661         {
662                 dist = DotProduct(points->v, normal);
663                 if (bestdist < dist)
664                         bestdist = dist;
665                 points++;
666         }
667         return bestdist;
668 }
669
670 // NOTE: start and end of each brush pair must have same numplanes/numpoints
671 double Collision_TraceBrushBrushDouble(const colbrushd_t *thisbrush_start, const colbrushd_t *thisbrush_end, const colbrushd_t *thatbrush_start, const colbrushd_t *thatbrush_end, double *impactnormal)
672 {
673         int nplane;
674         double enterfrac, leavefrac, d1, d2, f, newimpactnormal[3];
675         const colplaned_t *startplane, *endplane;
676
677         enterfrac = -1;
678         leavefrac = 1;
679
680         for (nplane = 0;nplane < thatbrush_start->numplanes;nplane++)
681         {
682                 startplane = thatbrush_start->planes + nplane;
683                 endplane = thatbrush_end->planes + nplane;
684                 d1 = nearestplanedist_double(startplane->normal, thisbrush_start->points, thisbrush_start->numpoints) - furthestplanedist_double(startplane->normal, thatbrush_start->points, thatbrush_start->numpoints);
685                 d2 = nearestplanedist_double(endplane->normal, thisbrush_end->points, thisbrush_start->numpoints) - furthestplanedist_double(endplane->normal, thatbrush_end->points, thatbrush_start->numpoints) - (1.0 / 32.0);
686
687                 f = d1 - d2;
688                 if (f >= 0)
689                 {
690                         // moving into brush
691                         if (d2 > 0)
692                                 return 1;
693                         if (d1 < 0)
694                                 continue;
695                         // enter
696                         f = d1 / f;
697                         if (enterfrac < f)
698                         {
699                                 enterfrac = f;
700                                 VectorSubtract(endplane->normal, startplane->normal, newimpactnormal);
701                                 VectorMA(startplane->normal, enterfrac, impactnormal, newimpactnormal);
702                         }
703                 }
704                 else
705                 {
706                         // moving out of brush
707                         if (d1 > 0)
708                                 return 1;
709                         if (d2 < 0)
710                                 continue;
711                         // leave
712                         f = d1 / f;
713                         if (leavefrac > f)
714                                 leavefrac = f;
715                 }
716         }
717
718         for (nplane = 0;nplane < thisbrush_start->numplanes;nplane++)
719         {
720                 startplane = thisbrush_start->planes + nplane;
721                 endplane = thisbrush_end->planes + nplane;
722                 d1 = nearestplanedist_double(startplane->normal, thisbrush_start->points, thisbrush_start->numpoints) - furthestplanedist_double(startplane->normal, thatbrush_start->points, thatbrush_start->numpoints);
723                 d2 = nearestplanedist_double(endplane->normal, thisbrush_end->points, thisbrush_start->numpoints) - furthestplanedist_double(endplane->normal, thatbrush_end->points, thatbrush_start->numpoints) - (1.0 / 32.0);
724
725                 f = d1 - d2;
726                 if (f >= 0)
727                 {
728                         // moving into brush
729                         if (d2 > 0)
730                                 return 1;
731                         if (d1 < 0)
732                                 continue;
733                         // enter
734                         f = d1 / f;
735                         if (enterfrac < f)
736                         {
737                                 enterfrac = f;
738                                 VectorSubtract(endplane->normal, startplane->normal, newimpactnormal);
739                                 VectorMA(startplane->normal, enterfrac, impactnormal, newimpactnormal);
740                         }
741                 }
742                 else
743                 {
744                         // moving out of brush
745                         if (d1 > 0)
746                                 return 1;
747                         if (d2 < 0)
748                                 continue;
749                         // leave
750                         f = d1 / f;
751                         if (leavefrac > f)
752                                 leavefrac = f;
753                 }
754         }
755
756         // LordHavoc: we need an epsilon nudge here because for a point trace the
757         // penetrating line segment is normally zero length if this brush was
758         // generated from a polygon (infinitely thin), and could even be slightly
759         // positive or negative due to rounding errors in that case.
760         enterfrac -= (1.0 / 16384.0);
761         if (leavefrac - enterfrac >= 0 && enterfrac > -1)
762         {
763                 VectorCopy(newimpactnormal, impactnormal);
764                 enterfrac = bound(0, enterfrac, 1);
765                 return enterfrac;
766         }
767         return 1;
768 }
769
770 static colplaned_t polyd_planes[256 + 2];
771 static colbrushd_t polyd_brush;
772 double Collision_TraceBrushPolygonDouble(const colbrushd_t *thisbrush_start, const colbrushd_t *thisbrush_end, int numpoints, const double *points, double *impactnormal)
773 {
774         if (numpoints > 256)
775         {
776                 Con_Printf("Polygon with more than 256 points not supported yet (fixme!)\n");
777                 return 1;
778         }
779         polyd_brush.numpoints = numpoints;
780         polyd_brush.numplanes = numpoints + 2;
781         polyd_brush.points = (colpointd_t *)points;
782         polyd_brush.planes = polyd_planes;
783         Collision_CalcPlanesForPolygonBrushDouble(&polyd_brush);
784         return Collision_TraceBrushBrushDouble(thisbrush_start, thisbrush_end, &polyd_brush, &polyd_brush, impactnormal);
785 }
786
787
788
789
790 typedef struct colbrushbmodelinfo_s
791 {
792         model_t *model;
793         const matrix4x4_t *modelmatrixstart;
794         const matrix4x4_t *modelmatrixend;
795         const colbrushf_t *thisbrush_start;
796         const colbrushf_t *thisbrush_end;
797         float impactnormal[3];
798         float tempimpactnormal[3];
799         float fraction;
800         int startsolid;
801         int allsolid;
802 }
803 colbrushbmodelinfo_t;
804
805 static int colframecount = 1;
806
807 void Collision_RecursiveTraceBrushNode(colbrushbmodelinfo_t *info, mnode_t *node)
808 {
809         if (node->contents)
810         {
811                 // collide with surfaces marked by this leaf
812                 int i, *mark;
813                 float result;
814                 mleaf_t *leaf = (mleaf_t *)node;
815                 msurface_t *surf;
816                 for (i = 0, mark = leaf->firstmarksurface;i < leaf->nummarksurfaces;i++, mark++)
817                 {
818                         surf = info->model->brushq1.surfaces + *mark;
819                         // don't check a surface twice
820                         if (surf->colframe != colframecount)
821                         {
822                                 surf->colframe = colframecount;
823                                 if (surf->flags & SURF_SOLIDCLIP)
824                                 {
825                                         result = Collision_TraceBrushPolygonFloat(info->thisbrush_start, info->thisbrush_end, surf->poly_numverts, surf->poly_verts, info->tempimpactnormal, &info->startsolid, &info->allsolid);
826                                         //result = Collision_TraceBrushPolygonTransformFloat(info->thisbrush_start, info->thisbrush_end, surf->poly_numverts, surf->poly_verts, info->tempimpactnormal, info->modelmatrixstart, info->modelmatrixend, &info->startsolid, &info->allsolid);
827                                         if (info->fraction > result)
828                                         {
829                                                 info->fraction = result;
830                                                 // use the surface's plane instead of the actual
831                                                 // collision plane because the actual collision plane
832                                                 // might be to the side (on a seam between polygons)
833                                                 // or something, we want objects to bounce off the
834                                                 // front...
835                                                 //if (surf->flags & SURF_PLANEBACK)
836                                                 //      VectorNegate(surf->plane->normal, info->impactnormal);
837                                                 //else
838                                                 //      VectorCopy(surf->plane->normal, info->impactnormal);
839                                                 VectorCopy(info->tempimpactnormal, info->impactnormal);
840                                         }
841                                 }
842                         }
843                 }
844         }
845         else
846         {
847                 // recurse down node sides
848                 int i;
849                 float dist;
850                 colpointf_t *ps, *pe;
851                 // FIXME? if TraceBrushPolygonTransform were to be made usable, the
852                 // node planes would need to be transformed too
853                 dist = node->plane->dist - (1.0f / 8.0f);
854                 for (i = 0, ps = info->thisbrush_start->points, pe = info->thisbrush_end->points;i < info->thisbrush_start->numpoints;i++, ps++, pe++)
855                 {
856                         if (DotProduct(ps->v, node->plane->normal) > dist || DotProduct(pe->v, node->plane->normal) > dist)
857                         {
858                                 Collision_RecursiveTraceBrushNode(info, node->children[0]);
859                                 break;
860                         }
861                 }
862                 dist = node->plane->dist + (1.0f / 8.0f);
863                 for (i = 0, ps = info->thisbrush_start->points, pe = info->thisbrush_end->points;i < info->thisbrush_start->numpoints;i++, ps++, pe++)
864                 {
865                         if (DotProduct(ps->v, node->plane->normal) < dist || DotProduct(pe->v, node->plane->normal) < dist)
866                         {
867                                 Collision_RecursiveTraceBrushNode(info, node->children[1]);
868                                 break;
869                         }
870                 }
871         }
872 }
873
874 float Collision_TraceBrushBModel(const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, model_t *model, float *impactnormal, int *startsolid, int *allsolid)
875 {
876         colbrushbmodelinfo_t info;
877         colframecount++;
878         info.model = model;
879         info.thisbrush_start = thisbrush_start;
880         info.thisbrush_end = thisbrush_end;
881         info.fraction = 1;
882         info.startsolid = false;
883         info.allsolid = false;
884         Collision_RecursiveTraceBrushNode(&info, model->brushq1.nodes + model->brushq1.hulls[0].firstclipnode);
885         if (info.fraction < 1)
886                 VectorCopy(info.impactnormal, impactnormal);
887         if (startsolid)
888                 *startsolid = info.startsolid;
889         if (allsolid)
890                 *allsolid = info.allsolid;
891         return info.fraction;
892 }
893
894 float Collision_TraceBrushBModelTransform(const colbrushf_t *thisbrush_start, const colbrushf_t *thisbrush_end, model_t *model, float *impactnormal, const matrix4x4_t *modelmatrixstart, const matrix4x4_t *modelmatrixend, int *startsolid, int *allsolid)
895 {
896         colbrushbmodelinfo_t info;
897         colframecount++;
898         info.model = model;
899         info.modelmatrixstart = modelmatrixstart;
900         info.modelmatrixend = modelmatrixend;
901         info.thisbrush_start = thisbrush_start;
902         info.thisbrush_end = thisbrush_end;
903         info.fraction = 1;
904         info.startsolid = false;
905         info.allsolid = false;
906         Collision_RecursiveTraceBrushNode(&info, model->brushq1.nodes);
907         if (info.fraction < 1)
908                 VectorCopy(info.impactnormal, impactnormal);
909         if (startsolid)
910                 *startsolid = info.startsolid;
911         if (allsolid)
912                 *allsolid = info.allsolid;
913         return info.fraction;
914 }
915
916
917
918 #define MAX_BRUSHFORBOX 16
919 static int brushforbox_index = 0;
920 static colpointf_t brushforbox_point[MAX_BRUSHFORBOX*8];
921 static colplanef_t brushforbox_plane[MAX_BRUSHFORBOX*6];
922 static colbrushf_t brushforbox_brush[MAX_BRUSHFORBOX];
923
924 void Collision_InitBrushForBox(void)
925 {
926         int i;
927         for (i = 0;i < MAX_BRUSHFORBOX;i++)
928         {
929                 brushforbox_brush[i].numpoints = 8;
930                 brushforbox_brush[i].numplanes = 6;
931                 brushforbox_brush[i].points = brushforbox_point + i * 8;
932                 brushforbox_brush[i].planes = brushforbox_plane + i * 6;
933         }
934 }
935
936 colbrushf_t *Collision_BrushForBox(const matrix4x4_t *matrix, const vec3_t mins, const vec3_t maxs)
937 {
938         int i;
939         vec3_t v;
940         colbrushf_t *brush;
941         if (brushforbox_brush[0].numpoints == 0)
942                 Collision_InitBrushForBox();
943         brush = brushforbox_brush + ((brushforbox_index++) % MAX_BRUSHFORBOX);
944         // FIXME: optimize
945         for (i = 0;i < 8;i++)
946         {
947                 v[0] = i & 1 ? maxs[0] : mins[0];
948                 v[1] = i & 2 ? maxs[1] : mins[1];
949                 v[2] = i & 4 ? maxs[2] : mins[2];
950                 Matrix4x4_Transform(matrix, v, brush->points[i].v);
951         }
952         // FIXME: optimize!
953         for (i = 0;i < 6;i++)
954         {
955                 VectorClear(v);
956                 v[i >> 1] = i & 1 ? 1 : -1;
957                 Matrix4x4_Transform3x3(matrix, v, brush->planes[i].normal);
958                 VectorNormalize(brush->planes[i].normal);
959                 brush->planes[i].dist = furthestplanedist_float(brush->planes[i].normal, brush->points, brush->numpoints);
960         }
961         return brush;
962 }
963
964 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)
965 {
966         vec3_t impactnormal;
967         //vec3_t mins2, maxs2;
968         matrix4x4_t cmatrix, cimatrix, startmatrix, endmatrix;
969         matrix4x4_t mstartmatrix, mendmatrix, identitymatrix;
970         colbrushf_t *thisbrush_start, *thisbrush_end, *cbrush;
971
972         Matrix4x4_CreateFromQuakeEntity(&cmatrix, corigin[0], corigin[1], corigin[2], cangles[0], cangles[1], cangles[2], 1);
973         Matrix4x4_Invert_Simple(&cimatrix, &cmatrix);
974         Matrix4x4_CreateTranslate(&startmatrix, start[0], start[1], start[2]);
975         Matrix4x4_CreateTranslate(&endmatrix, end[0], end[1], end[2]);
976
977         Matrix4x4_CreateIdentity(&identitymatrix);
978         Matrix4x4_Concat(&mstartmatrix, &cimatrix, &startmatrix);
979         Matrix4x4_Concat(&mendmatrix, &cimatrix, &endmatrix);
980         thisbrush_start = Collision_BrushForBox(&mstartmatrix, mins, maxs);
981         //mins2[0] = mins[0] - 0.0625;mins2[1] = mins[1] - 0.0625;mins2[2] = mins[2] - 0.0625;
982         //maxs2[0] = maxs[0] + 0.0625;maxs2[1] = maxs[1] + 0.0625;maxs2[2] = maxs[2] + 0.0625;
983         thisbrush_end = Collision_BrushForBox(&mendmatrix, mins, maxs);
984
985         //Collision_PrintBrushAsQHull(thisbrush_start, "thisbrush_start");
986         //Collision_PrintBrushAsQHull(thisbrush_end, "thisbrush_end");
987         memset (trace, 0, sizeof(trace_t));
988         if (cmodel && cmodel->type == mod_brush)
989         {
990                 // brush model
991                 trace->fraction = Collision_TraceBrushBModel(thisbrush_start, thisbrush_end, cmodel, impactnormal, &trace->startsolid, &trace->allsolid);
992                 //trace->fraction = Collision_TraceBrushBModelTransform(thisbrush_start, thisbrush_end, cmodel, trace->plane.normal, &cmatrix, &cmatrix, &trace->startsolid, &trace->allsolid);
993         }
994         else
995         {
996                 // bounding box
997                 cbrush = Collision_BrushForBox(&identitymatrix, cmins, cmaxs);
998                 trace->fraction = Collision_TraceBrushBrushFloat(thisbrush_start, thisbrush_end, cbrush, cbrush, impactnormal, &trace->startsolid, &trace->allsolid);
999                 //cbrush = Collision_BrushForBox(&cmatrix, cmins, cmaxs);
1000                 //trace->fraction = Collision_TraceBrushBrushFloat(thisbrush_start, thisbrush_end, cbrush, cbrush, trace->plane.normal, &trace->startsolid, &trace->allsolid);
1001         }
1002
1003         if (trace->fraction < 0 || trace->fraction > 1)
1004                 Con_Printf("fraction out of bounds %f %s:%d\n", trace->fraction, __FILE__, __LINE__);
1005
1006         if (trace->fraction < 1)
1007         {
1008                 trace->ent = (void *) cent;
1009                 VectorBlend(start, end, trace->fraction, trace->endpos);
1010                 Matrix4x4_Transform(&cmatrix, impactnormal, trace->plane.normal);
1011                 VectorNormalize(trace->plane.normal);
1012                 //Con_Printf("fraction %f normal %f %f %f\n", trace->fraction, trace->plane.normal[0], trace->plane.normal[1], trace->plane.normal[2]);
1013         }
1014         else
1015                 VectorCopy(end, trace->endpos);
1016 }
1017
1018
1019 // LordHavoc: currently unused and not yet tested
1020 // note: this can be used for tracing a moving sphere vs a stationary sphere,
1021 // by simply adding the moving sphere's radius to the sphereradius parameter,
1022 // all the results are correct (impactpoint, impactnormal, and fraction)
1023 float Collision_ClipTrace_Line_Sphere(double *linestart, double *lineend, double *sphereorigin, double sphereradius, double *impactpoint, double *impactnormal)
1024 {
1025         double dir[3], scale, v[3], deviationdist, impactdist, linelength;
1026         // make sure the impactpoint and impactnormal are valid even if there is
1027         // no collision
1028         impactpoint[0] = lineend[0];
1029         impactpoint[1] = lineend[1];
1030         impactpoint[2] = lineend[2];
1031         impactnormal[0] = 0;
1032         impactnormal[1] = 0;
1033         impactnormal[2] = 0;
1034         // calculate line direction
1035         dir[0] = lineend[0] - linestart[0];
1036         dir[1] = lineend[1] - linestart[1];
1037         dir[2] = lineend[2] - linestart[2];
1038         // normalize direction
1039         linelength = sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
1040         if (linelength)
1041         {
1042                 scale = 1.0 / linelength;
1043                 dir[0] *= scale;
1044                 dir[1] *= scale;
1045                 dir[2] *= scale;
1046         }
1047         // this dotproduct calculates the distance along the line at which the
1048         // sphere origin is (nearest point to the sphere origin on the line)
1049         impactdist = dir[0] * (sphereorigin[0] - linestart[0]) + dir[1] * (sphereorigin[1] - linestart[1]) + dir[2] * (sphereorigin[2] - linestart[2]);
1050         // calculate point on line at that distance, and subtract the
1051         // sphereorigin from it, so we have a vector to measure for the distance
1052         // of the line from the sphereorigin (deviation, how off-center it is)
1053         v[0] = linestart[0] + impactdist * dir[0] - sphereorigin[0];
1054         v[1] = linestart[1] + impactdist * dir[1] - sphereorigin[1];
1055         v[2] = linestart[2] + impactdist * dir[2] - sphereorigin[2];
1056         deviationdist = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
1057         // if outside the radius, it's a miss for sure
1058         // (we do this comparison using squared radius to avoid a sqrt)
1059         if (deviationdist > sphereradius*sphereradius)
1060                 return 1; // miss (off to the side)
1061         // nudge back to find the correct impact distance
1062         impactdist += (sqrt(deviationdist) - sphereradius);
1063         if (impactdist >= linelength)
1064                 return 1; // miss (not close enough)
1065         if (impactdist < 0)
1066                 return 1; // miss (linestart is past or inside sphere)
1067         // calculate new impactpoint
1068         impactpoint[0] = linestart[0] + impactdist * dir[0];
1069         impactpoint[1] = linestart[1] + impactdist * dir[1];
1070         impactpoint[2] = linestart[2] + impactdist * dir[2];
1071         // calculate impactnormal (surface normal at point of impact)
1072         impactnormal[0] = impactpoint[0] - sphereorigin[0];
1073         impactnormal[1] = impactpoint[1] - sphereorigin[1];
1074         impactnormal[2] = impactpoint[2] - sphereorigin[2];
1075         // normalize impactnormal
1076         scale = impactnormal[0] * impactnormal[0] + impactnormal[1] * impactnormal[1] + impactnormal[2] * impactnormal[2];
1077         if (scale)
1078         {
1079                 scale = 1.0 / sqrt(scale);
1080                 impactnormal[0] *= scale;
1081                 impactnormal[1] *= scale;
1082                 impactnormal[2] *= scale;
1083         }
1084         // return fraction of movement distance
1085         return impactdist / linelength;
1086 }
1087