1 /* bzflag
2 * Copyright (c) 1993-2021 Tim Riker
3 *
4 * This package is free software; you can redistribute it and/or
5 * modify it under the terms of the license found in the file
6 * named COPYING that should have accompanied this file.
7 *
8 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
9 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
10 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
11 */
12
13 #include "common.h"
14 #include <math.h>
15 #include "Intersect.h"
16 #include "Extents.h"
17
18
19 // get angle of normal vector to axis aligned rect centered at origin by point p
getNormalOrigRect(const float * p,float dx,float dy)20 static float getNormalOrigRect(const float* p, float dx, float dy)
21 {
22 if (p[0] > dx) // east of box
23 {
24 if (p[1] > dy) // ne corner
25 return atan2f(p[1] - dy, p[0] - dx);
26 else if (p[1] < -dy) // se corner
27 return atan2f(p[1] + dy, p[0] - dx);
28 else // east side
29 return 0.0f;
30 }
31
32 if (p[0] < -dx) // west of box
33 {
34 if (p[1] > dy) // nw corner
35 return atan2f(p[1] - dy, p[0] + dx);
36 else if (p[1] < -dy) // sw corner
37 return atan2f(p[1] + dy, p[0] + dx);
38 else // west side
39 return (float)M_PI;
40 }
41
42 if (p[1] > dy) // north of box
43 return (float)(0.5 * M_PI);
44
45 if (p[1] < -dy) // south of box
46 return (float)(1.5 * M_PI);
47
48 // inside box
49 if (p[0] > 0.0f) // inside east
50 if (p[1] > 0.0f) // inside ne quadrant
51 if (dy * p[0] > dx * p[1]) // east wall
52 return 0.0f;
53 else // north wall
54 return (float)(0.5 * M_PI);
55 else // inside se quadrant
56 if (dy * p[0] > -dx * p[1]) // east wall
57 return 0.0f;
58 else // south wall
59 return (float)(1.5 * M_PI);
60
61 else // inside west
62 if (p[1] > 0.0f) // inside nw quadrant
63 if (dy * p[0] < -dx * p[1]) // west wall
64 return (float)M_PI;
65 else // north wall
66 return (float)(0.5 * M_PI);
67 else // inside sw quadrant
68 if (dy * p[0] < dx * p[1]) // west wall
69 return (float)M_PI;
70 else // south wall
71 return (float)(1.5 * M_PI);
72 }
73
74
getNormalRect(const float * p1,const float * p2,float angle,float dx,float dy,float * n)75 void getNormalRect(const float* p1, const float* p2,
76 float angle, float dx, float dy, float* n)
77 {
78 // translate origin
79 float pa[2];
80 pa[0] = p1[0] - p2[0];
81 pa[1] = p1[1] - p2[1];
82
83 // rotate
84 float pb[2];
85 const float c = cosf(-angle), s = sinf(-angle);
86 pb[0] = c * pa[0] - s * pa[1];
87 pb[1] = c * pa[1] + s * pa[0];
88
89 // get angle
90 const float normAngle = getNormalOrigRect(pb, dx, dy) + angle;
91
92 // make normal
93 n[0] = cosf(normAngle);
94 n[1] = sinf(normAngle);
95 n[2] = 0.0f;
96 }
97
98
99 // true iff axis aligned rect centered at origin intersects circle
testOrigRectCircle(float dx,float dy,const float * p,float r)100 bool testOrigRectCircle(float dx, float dy, const float* p, float r)
101 {
102 // Algorithm from Graphics Gems, pp51-53.
103 const float rr = r * r, rx = -p[0], ry = -p[1];
104 if (rx + dx < 0.0) // west of rect
105 if (ry + dy < 0.0) // sw corner
106 return (rx + dx) * (rx + dx) + (ry + dy) * (ry + dy) < rr;
107 else if (ry - dy > 0.0) // nw corner
108 return (rx + dx) * (rx + dx) + (ry - dy) * (ry - dy) < rr;
109 else // due west
110 return rx + dx > -r;
111
112 else if (rx - dx > 0.0) // east of rect
113 if (ry + dy < 0.0) // se corner
114 return (rx - dx) * (rx - dx) + (ry + dy) * (ry + dy) < rr;
115 else if (ry - dy > 0.0) // ne corner
116 return (rx - dx) * (rx - dx) + (ry - dy) * (ry - dy) < rr;
117 else // due east
118 return rx - dx < r;
119
120 else if (ry + dy < 0.0) // due south
121 return ry + dy > -r;
122
123 else if (ry - dy > 0.0) // due north
124 return ry - dy < r;
125
126 return true; // circle origin in rect
127 }
128
129
testRectCircle(const float * p1,float angle,float dx,float dy,const float * p2,float r)130 bool testRectCircle(const float* p1, float angle,
131 float dx, float dy, const float* p2, float r)
132 {
133 // translate origin
134 float pa[2];
135 pa[0] = p2[0] - p1[0];
136 pa[1] = p2[1] - p1[1];
137
138 // rotate
139 float pb[2];
140 const float c = cosf(-angle), s = sinf(-angle);
141 pb[0] = c * pa[0] - s * pa[1];
142 pb[1] = c * pa[1] + s * pa[0];
143
144 // do test
145 return testOrigRectCircle(dx, dy, pb, r);
146 }
147
148
149 // ray r1 started at time t1 minus ray r2 started at time t2
rayMinusRay(const Ray & r1,float t1,const Ray & r2,float t2)150 Ray rayMinusRay(const Ray& r1, float t1, const Ray& r2, float t2)
151 {
152 // get points at respective times
153 float p1[3], p2[3];
154 r1.getPoint(t1, p1);
155 r2.getPoint(t2, p2);
156
157 // construct new ray
158 float p[3], d[3];
159 p[0] = p1[0] - p2[0];
160 p[1] = p1[1] - p2[1];
161 p[2] = p1[2] - p2[2];
162 d[0] = r1.getDirection()[0] - r2.getDirection()[0];
163 d[1] = r1.getDirection()[1] - r2.getDirection()[1];
164 d[2] = r1.getDirection()[2] - r2.getDirection()[2];
165 return Ray(p, d);
166 }
167
168
rayAtDistanceFromOrigin(const Ray & r,float radius)169 float rayAtDistanceFromOrigin(const Ray& r, float radius)
170 {
171 const float* d = r.getDirection();
172 if (d[0] == 0.0 && d[1] == 0.0 && d[2] == 0.0) return 0.0f;
173
174 const float* p = r.getOrigin();
175 const float a = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
176 const float b = -(p[0] * d[0] + p[1] * d[1] + p[2] * d[2]);
177 const float c = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] - radius * radius;
178 const float disc = b * b - a * c;
179 if (disc < 0.0f) return -1.0f; // misses sphere
180 const float d1_2 = sqrtf(disc);
181 const float t0 = b + d1_2;
182 const float t1 = b - d1_2;
183 if (t0 < t1)
184 if (t0 < 0.0f) return t1 / a;
185 else return t0 / a;
186 else if (t1 < 0.0) return t0 / a;
187 else return t1 / a;
188 }
189
190
191 // block covers interval x=[-dx,dx], y=[-dy,dy], z=[0.0,dz]
timeRayHitsOrigBox(const float * p,const float * v,float dx,float dy,float dz)192 static float timeRayHitsOrigBox(const float* p, const float* v,
193 float dx, float dy, float dz)
194 {
195 float tx, ty, tz;
196
197 // there's gotta be a better way to do this whole thing
198
199 if (fabsf(p[0]) <= dx && fabsf(p[1]) <= dy && p[2] >= 0.0 && p[2] <= dz)
200 return 0.0; // inside
201
202 if (p[0] > dx) // to east
203 if (v[0] >= 0.0) return -1.0f; // going east
204 else tx = (dx - p[0]) / v[0]; // get east wall hit
205 else if (p[0] < -dx) // to west
206 if (v[0] <= 0.0) return -1.0f; // going west
207 else tx = -(dx + p[0]) / v[0]; // get west wall hit
208 else tx = -1.0f; // doesn't matter
209
210 if (p[1] > dy) // to north
211 if (v[1] >= 0.0) return -1.0f; // going north
212 else ty = (dy - p[1]) / v[1]; // get north wall hit
213 else if (p[1] < -dy) // to south
214 if (v[1] <= 0.0) return -1.0f; // going south
215 else ty = -(dy + p[1]) / v[1]; // get north wall hit
216 else ty = -1.0f; // doesn't matter
217
218 if (p[2] > dz) // above
219 if (v[2] >= 0.0) return -1.0f; // going up
220 else tz = (dz - p[2]) / v[2]; // get ceiling hit
221 else if (p[2] < 0.0) // below
222 if (v[2] <= 0.0) return -1.0f; // going down
223 else tz = -p[2] / v[2]; // get floor hit
224 else tz = -1.0f; // doesn't matter
225
226 // throw out solutions < 0.0 or that intersect outside box
227 if (tx < 0.0 ||
228 fabsf(p[1] + tx * v[1]) > dy ||
229 p[2] + tx * v[2] < 0.0 ||
230 p[2] + tx * v[2] > dz)
231 tx = -1.0f;
232 if (ty < 0.0 ||
233 fabsf(p[0] + ty * v[0]) > dx ||
234 p[2] + ty * v[2] < 0.0 ||
235 p[2] + ty * v[2] > dz)
236 ty = -1.0f;
237 if (tz < 0.0 ||
238 fabsf(p[0] + tz * v[0]) > dx ||
239 fabsf(p[1] + tz * v[1]) > dy)
240 tz = -1.0f;
241
242 if (tx < 0.0 && ty < 0.0 && tz < 0.0) return -1.0f; // no hits
243
244 // pick closest valid solution
245 if (tx < 0.0f)
246 {
247 if (ty < 0.0f) return tz;
248 if (tz < 0.0f || ty < tz) return ty;
249 return tz;
250 }
251 else if (ty < 0.0f)
252 {
253 if (tz < 0.0 || tx < tz) return tx;
254 return tz;
255 }
256 else if (tz < 0.0f)
257 {
258 if (tx < ty) return tx;
259 return ty;
260 }
261 else
262 {
263 if (tx < ty && tx < tz) return tx;
264 if (ty < tz) return ty;
265 return tz;
266 }
267 }
268
269
timeRayHitsBlock(const Ray & r,const float * p1,float angle,float dx,float dy,float dz)270 float timeRayHitsBlock(const Ray& r, const float* p1,
271 float angle, float dx, float dy, float dz)
272 {
273 // get names for ray info
274 const float* p2 = r.getOrigin();
275 const float* d = r.getDirection();
276
277 // translate origin
278 float pa[2];
279 pa[0] = p2[0] - p1[0];
280 pa[1] = p2[1] - p1[1];
281
282 // rotate
283 float pb[3], db[3];
284 const float c = cosf(-angle), s = sinf(-angle);
285 pb[0] = c * pa[0] - s * pa[1];
286 pb[1] = c * pa[1] + s * pa[0];
287 pb[2] = p2[2] - p1[2];
288 db[0] = c * d[0] - s * d[1];
289 db[1] = c * d[1] + s * d[0];
290 db[2] = d[2];
291
292 // find t
293 return timeRayHitsOrigBox(pb, db, dx, dy, dz);
294 }
295
296
297 /** Computing ray travel time to the plane described by 3 points
298 */
timeRayHitsPlane(const float pb[3],const float db[3],const float x1[3],const float x2[3],const float x3[3])299 static float timeRayHitsPlane(const float pb[3], const float db[3],
300 const float x1[3], const float x2[3],
301 const float x3[3])
302 {
303 float u[3], v[3], d[3];
304 int i;
305
306 // Compute the 2 vectors describing the plane
307 for (i = 0; i < 3; i++)
308 u[i] = x2[i] - x1[i];
309 for (i = 0; i < 3; i++)
310 v[i] = x3[i] - x1[i];
311 // Thats a distance vector: a vector from the plane to the ray beginning
312 for (i = 0; i < 3; i++)
313 d[i] = pb[i] - x1[i];
314
315 // plane versor unnormalized
316 float n[3];
317 n[0] = u[1] * v[2] - u[2] * v[1];
318 n[1] = u[2] * v[0] - u[0] * v[2];
319 n[2] = u[0] * v[1] - u[1] * v[0];
320
321 // computing unnormalized distance projecting the distance on versor
322 float distance = 0.0;
323 for (i = 0; i < 3; i++)
324 distance += n[i] * d[i];
325
326 // if distance is negative, plane is already passed
327 if (distance <= 0.0f)
328 return 0.0f;
329
330 // project velocity vector on the plan versor unnormalized
331 float velocity = 0.0f;
332 for (i = 0; i < 3; i++)
333 velocity += n[i] * db[i];
334
335 // if velocity is greater than 0 no way to trespass the plane
336 if (velocity > 0.0f)
337 return -1.0f;
338
339 // time is ... that is normalized
340 return - distance / velocity;
341 }
342
343
timeRayHitsPyramids(const Ray & r,const float * p1,float angle,float dx,float dy,float dz,bool flipZ)344 float timeRayHitsPyramids(const Ray& r, const float* p1, float angle,
345 float dx, float dy, float dz, bool flipZ)
346 {
347
348 const float epsilon = 1.0e-3f;
349 // get names for ray info
350 int i;
351 const float* p2 = r.getOrigin();
352 const float* d = r.getDirection();
353
354 // translate origin
355 float pa[2];
356 pa[0] = p2[0] - p1[0];
357 pa[1] = p2[1] - p1[1];
358
359 // rotate
360 float pb[3], db[3];
361 const float c = cosf(-angle), s = sinf(-angle);
362 pb[0] = c * pa[0] - s * pa[1];
363 pb[1] = c * pa[1] + s * pa[0];
364 pb[2] = p2[2] - p1[2];
365 db[0] = c * d[0] - s * d[1];
366 db[1] = c * d[1] + s * d[0];
367 db[2] = d[2];
368
369 if (dx < 0)
370 dx = - dx;
371 if (dy < 0)
372 dy = - dy;
373 if (dz < 0)
374 dz = - dz;
375 if (flipZ)
376 {
377 pb[2] = dz - pb[2];
378 db[2] = - db[2];
379 }
380
381 float residualTime = 0.0f;
382
383 float x1[3], x2[3], x3[3];
384 float residualTemp;
385
386 x1[2] = 0.0f;
387 x2[2] = 0.0f;
388 x3[0] = 0.0f;
389 x3[1] = 0.0f;
390 x3[2] = dz;
391
392 // trying to get to the pyramid removing half space at time
393 // start with the 4 faces, and end with the base
394 x1[0] = - dx;
395 x1[1] = - dy;
396 x2[0] = dx;
397 x2[1] = - dy;
398 residualTemp = timeRayHitsPlane(pb, db, x1, x2, x3);
399 if (residualTemp < -0.5f)
400 return residualTemp;
401 for (i = 0; i < 3; i++)
402 pb[i] += residualTemp * db[i];
403 residualTime += residualTemp;
404
405 x1[0] = - x1[0];
406 x1[1] = - x1[1];
407 residualTemp = timeRayHitsPlane(pb, db, x2, x1, x3);
408 if (residualTemp < -0.5f)
409 return residualTemp;
410 for (i = 0; i < 3; i++)
411 pb[i] += residualTemp * db[i];
412 residualTime += residualTemp;
413
414 x2[0] = - x2[0];
415 x2[1] = - x2[1];
416 residualTemp = timeRayHitsPlane(pb, db, x1, x2, x3);
417 if (residualTemp < -0.5f)
418 return residualTemp;
419 for (i = 0; i < 3; i++)
420 pb[i] += residualTemp * db[i];
421 residualTime += residualTemp;
422
423 x1[0] = - x1[0];
424 x1[1] = - x1[1];
425 residualTemp = timeRayHitsPlane(pb, db, x2, x1, x3);
426 if (residualTemp < -0.5f)
427 return residualTemp;
428 for (i = 0; i < 3; i++)
429 pb[i] += residualTemp * db[i];
430 residualTime += residualTemp;
431
432 x3[0] = dx;
433 x3[1] = dy;
434 x3[2] = 0.0f;
435 residualTemp = timeRayHitsPlane(pb, db, x1, x2, x3);
436 if (residualTemp < -0.5f)
437 return residualTemp;
438 for (i = 0; i < 3; i++)
439 pb[i] += residualTemp * db[i];
440 residualTime += residualTemp;
441
442 // No way to move further. See if inside
443 // first bounding box
444 pb[0] = fabsf(pb[0]);
445 pb[1] = fabsf(pb[1]);
446 if (pb[0] > dx + epsilon * dx)
447 return -1.0f;
448 if (pb[1] > dy + epsilon * dy)
449 return -1.0f;
450 if (pb[2] < 0.0f - epsilon * dz)
451 return -1.0f;
452 // now shrink
453 float scaledDistance;
454 if (pb[0] * dy > pb[1] * dx)
455 scaledDistance = dz * (dx - pb[0]) - pb[2] * dx;
456 else
457 scaledDistance = dz * (dy - pb[1]) - pb[2] * dy;
458 if (scaledDistance < - epsilon * dz)
459 residualTime = -1.0f;
460
461 return residualTime;
462 }
463
464
465 // rect covers interval x=[-dx,dx], y=[-dy,dy]
timeAndSideRayHitsOrigRect(const float * p,const float * v,float dx,float dy,int & side)466 float timeAndSideRayHitsOrigRect(const float* p, const float* v,
467 float dx, float dy, int& side)
468 {
469 // check if inside
470 if (fabsf(p[0]) <= dx && fabsf(p[1]) <= dy)
471 {
472 side = -2;
473 return 0.0f;
474 }
475
476 // assume it won't hit
477 side = -1;
478
479 float tx, ty;
480 if (p[0] > dx) // to east
481 if (v[0] >= 0.0f) return -1.0f; // going east
482 else tx = (dx - p[0]) / v[0]; // get east wall hit
483 else if (p[0] < -dx) // to west
484 if (v[0] <= 0.0f) return -1.0f; // going west
485 else tx = -(dx + p[0]) / v[0]; // get west wall hit
486 else tx = -1.0f; // doesn't matter
487
488 if (p[1] > dy) // to north
489 if (v[1] >= 0.0f) return -1.0f; // going north
490 else ty = (dy - p[1]) / v[1]; // get north wall hit
491 else if (p[1] < -dy) // to south
492 if (v[1] <= 0.0f) return -1.0f; // going south
493 else ty = -(dy + p[1]) / v[1]; // get north wall hit
494 else ty = -1.0f; // doesn't matter
495
496 // throw out solutions < 0.0 or that intersect outside box
497 if (fabsf(p[1] + tx * v[1]) > dy) tx = -1.0f;
498 if (fabsf(p[0] + ty * v[0]) > dx) ty = -1.0f;
499 if (tx < 0.0f && ty < 0.0f) return -1.0f; // no hits
500
501 // pick closest valid solution
502 if (tx < 0.0f || (ty >= 0.0f && ty < tx))
503 {
504 side = (p[1] > dy) ? 1 : 3;
505 return ty;
506 }
507 side = (p[0] > dx) ? 0 : 2;
508 return tx;
509 }
510
511
timeAndSideRayHitsRect(const Ray & r,const float * p1,float angle,float dx,float dy,int & side)512 float timeAndSideRayHitsRect(const Ray& r, const float* p1, float angle,
513 float dx, float dy, int& side)
514 {
515 // get names for ray info
516 const float* p2 = r.getOrigin();
517 const float* d = r.getDirection();
518
519 // translate origin
520 float pa[2];
521 pa[0] = p2[0] - p1[0];
522 pa[1] = p2[1] - p1[1];
523
524 // rotate
525 float pb[3], db[3];
526 const float c = cosf(-angle), s = sinf(-angle);
527 pb[0] = c * pa[0] - s * pa[1];
528 pb[1] = c * pa[1] + s * pa[0];
529 pb[2] = p2[2] - p1[2];
530 db[0] = c * d[0] - s * d[1];
531 db[1] = c * d[1] + s * d[0];
532 db[2] = d[2];
533
534 // find t and side
535 return timeAndSideRayHitsOrigRect(pb, db, dx, dy, side);
536 }
537
538
testOrigRectRect(const float * p,float angle,float dx1,float dy1,float dx2,float dy2)539 static bool testOrigRectRect(const float* p, float angle,
540 float dx1, float dy1, float dx2, float dy2)
541 {
542 static const float box[4][2] = { { 1.0, 1.0 }, { 1.0, -1.0 },
543 { -1.0, -1.0 }, { -1.0, 1.0 }
544 };
545 const float c = cosf(angle), s = sinf(angle);
546 float corner1[4][2];
547 int i, region[4][2];
548
549 // return true if the second rectangle is within the first
550 // hint: cos(+a) = cos(-a) and -sin(a) = sin(-a)
551 float sx = (c * p[0]) + (s * p[1]);
552 float sy = (c * p[1]) - (s * p[0]);
553 if ((fabsf(sx) < dx1) && (fabsf(sy) < dy1))
554 return true;
555
556 // get corners of first rect and classify according to position with
557 // respect to second rect, return true iff any lies inside second.
558 for (i = 0; i < 4; i++)
559 {
560 corner1[i][0] = p[0] + c * dx1 * box[i][0] - s * dy1 * box[i][1];
561 corner1[i][1] = p[1] + s * dx1 * box[i][0] + c * dy1 * box[i][1];
562 region[i][0] = corner1[i][0] < -dx2 ? -1 : (corner1[i][0] > dx2 ? 1 : 0);
563 region[i][1] = corner1[i][1] < -dy2 ? -1 : (corner1[i][1] > dy2 ? 1 : 0);
564 if (!region[i][0] && !region[i][1]) return true;
565 }
566
567 // check each edge of rect1
568 for (i = 0; i < 4; i++)
569 {
570 float corner2[2], e[2];
571 int j = (i + 1) % 4;
572
573 // if the edge lies completely to one side of rect2 then continue
574 // if it crosses the center then return true
575 if (region[i][0] == region[j][0])
576 {
577 if (region[i][0] == 0 && region[i][1] != region[j][1])
578 return true;
579 else
580 continue;
581 }
582 else if (region[i][1] == region[j][1])
583 {
584 if (region[i][1] == 0)
585 return true;
586 else
587 continue;
588 }
589
590 // determine corners of rect2 the edge might pass between
591 if (region[i][0] == 0)
592 {
593 corner2[0] = region[j][0] * dx2;
594 corner2[1] = region[i][1] * dy2;
595 }
596 else if (region[j][0] == 0)
597 {
598 corner2[0] = region[i][0] * dx2;
599 corner2[1] = region[j][1] * dy2;
600 }
601 else if (region[i][1] == 0)
602 {
603 corner2[0] = region[i][0] * dx2;
604 corner2[1] = region[j][1] * dy2;
605 }
606 else
607 {
608 corner2[0] = region[j][0] * dx2;
609 corner2[1] = region[i][1] * dy2;
610 }
611
612 // see if edge crosses the rectangle
613 e[0] = corner1[j][0] - corner1[i][0];
614 e[1] = corner1[j][1] - corner1[i][1];
615 if ((e[1]*(corner2[0]-corner1[i][0])-e[0]*(corner2[1]-corner1[i][1])) *
616 (e[1]*(corner2[0]+corner1[i][0])-e[0]*(corner2[1]+corner1[i][1])) > 0.0)
617 return true;
618 }
619 return false;
620 }
621
622
testRectRect(const float * p1,float angle1,float dx1,float dy1,const float * p2,float angle2,float dx2,float dy2)623 bool testRectRect(const float* p1, float angle1, float dx1, float dy1,
624 const float* p2, float angle2, float dx2, float dy2)
625 {
626 // translate origin
627 float pa[2];
628 pa[0] = p2[0] - p1[0];
629 pa[1] = p2[1] - p1[1];
630
631 // rotate
632 float pb[2];
633 const float c = cosf(-angle1), s = sinf(-angle1);
634 pb[0] = c * pa[0] - s * pa[1];
635 pb[1] = c * pa[1] + s * pa[0];
636
637 // do test
638 return testOrigRectRect(pb, angle2 - angle1, dx2, dy2, dx1, dy1);
639 }
640
641
testRectInRect(const float * p1,float angle1,float dx1,float dy1,const float * p2,float angle2,float dx2,float dy2)642 bool testRectInRect(const float* p1, float angle1, float dx1, float dy1,
643 const float* p2, float angle2, float dx2, float dy2)
644 {
645 static const float box[4][2] = { { 1.0, 1.0 }, { 1.0, -1.0 },
646 { -1.0, -1.0 }, { -1.0, 1.0 }
647 };
648
649 // translate origin
650 float pa[2];
651 pa[0] = p2[0] - p1[0];
652 pa[1] = p2[1] - p1[1];
653
654 // rotate
655 float pb[2];
656 const float c = cosf(-angle1), s = sinf(-angle1);
657 pb[0] = c * pa[0] - s * pa[1];
658 pb[1] = c * pa[1] + s * pa[0];
659
660 // see if corners of second rectangle are inside first
661 const float c2 = cosf(angle2 - angle1), s2 = sinf(angle2 - angle1);
662 for (int i = 0; i < 4; i++)
663 {
664 const float x = pb[0] + c2 * dx2 * box[i][0] - s2 * dy2 * box[i][1];
665 const float y = pb[1] + s2 * dx2 * box[i][0] + c2 * dy2 * box[i][1];
666 if (fabsf(x) > dx1 || fabsf(y) > dy1) return false;
667 }
668 return true;
669 }
670
671
projectAxisBox(const float * dir,const Extents & extents,float * minDist,float * maxDist)672 static inline void projectAxisBox(const float* dir, const Extents& extents,
673 float* minDist, float* maxDist)
674 {
675 static float i[3];
676 static float o[3];
677
678 // find the extreme corners
679 for (int t = 0; t < 3; t++)
680 {
681 if (dir[t] > 0.0f)
682 {
683 i[t] = extents.maxs[t];
684 o[t] = extents.mins[t];
685 }
686 else
687 {
688 i[t] = extents.mins[t];
689 o[t] = extents.maxs[t];
690 }
691 }
692
693 float idist = (i[0] * dir[0]) + (i[1] * dir[1]) + (i[2] * dir[2]);
694 float odist = (o[0] * dir[0]) + (o[1] * dir[1]) + (o[2] * dir[2]);
695
696 if (idist < odist)
697 {
698 *minDist = idist;
699 *maxDist = odist;
700 }
701 else
702 {
703 *minDist = odist;
704 *maxDist = idist;
705 }
706
707 return;
708 }
709
710
projectPolygon(const float * dir,int count,const float (* points)[3],float * minDist,float * maxDist)711 static inline void projectPolygon(const float* dir,
712 int count, const float (*points)[3],
713 float* minDist, float* maxDist)
714 {
715 float mind = MAXFLOAT;
716 float maxd = -MAXFLOAT;
717
718 for (int i = 0; i < count; i++)
719 {
720 const float* p = points[i];
721 float dist = (p[0] * dir[0]) + (p[1] * dir[1]) + (p[2] * dir[2]);
722 if (dist < mind)
723 mind = dist;
724 if (dist > maxd)
725 maxd = dist;
726 }
727
728 *minDist = mind;
729 *maxDist = maxd;
730
731 return;
732 }
733
734
735 // return true if polygon touches the axis aligned box
736 // *** assumes that an extents test has already been done ***
testPolygonInAxisBox(int pointCount,const float (* points)[3],const float * plane,const Extents & extents)737 bool testPolygonInAxisBox(int pointCount, const float (*points)[3],
738 const float* plane, const Extents& extents)
739 {
740 int t;
741 static float i[3]; // inside point (assuming partial)
742 static float o[3]; // outside point (assuming partial)
743
744 // test the plane
745 for (t = 0; t < 3; t++)
746 {
747 if (plane[t] > 0.0f)
748 {
749 i[t] = extents.maxs[t];
750 o[t] = extents.mins[t];
751 }
752 else
753 {
754 i[t] = extents.mins[t];
755 o[t] = extents.maxs[t];
756 }
757 }
758 const float icross = (plane[0] * i[0]) +
759 (plane[1] * i[1]) +
760 (plane[2] * i[2]) + plane[3];
761 const float ocross = (plane[0] * o[0]) +
762 (plane[1] * o[1]) +
763 (plane[2] * o[2]) + plane[3];
764 if ((icross * ocross) > 0.0f)
765 {
766 // same polarity means that the plane doesn't cut the box
767 return false;
768 }
769
770 // test the edges
771 const float axisNormals[3][3] =
772 {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}};
773 for (t = 0; t < pointCount; t++)
774 {
775 int next = (t + 1) % pointCount;
776 float edge[3];
777 edge[0] = points[next][0] - points[t][0];
778 edge[1] = points[next][1] - points[t][1];
779 edge[2] = points[next][2] - points[t][2];
780 for (int a = 0; a < 3; a++)
781 {
782 float cross[3];
783 const float* axis = axisNormals[a];
784 cross[0] = (edge[1] * axis[2]) - (edge[2] * axis[1]);
785 cross[1] = (edge[2] * axis[0]) - (edge[0] * axis[2]);
786 cross[2] = (edge[0] * axis[1]) - (edge[1] * axis[0]);
787 const float length =
788 (cross[0] * cross[0]) + (cross[1] * cross[1]) + (cross[2] * cross[2]);
789 if (length < 0.001f)
790 continue;
791 // find the projected distances
792 float boxMinDist, boxMaxDist;
793 float polyMinDist, polyMaxDist;
794 projectAxisBox(cross, extents, &boxMinDist, &boxMaxDist);
795 projectPolygon(cross, pointCount, points, &polyMinDist, &polyMaxDist);
796 // check if this is a separation axis
797 if ((boxMinDist > polyMaxDist) || (boxMaxDist < polyMinDist))
798 return false;
799 }
800 }
801
802 return true;
803 }
804
805
806 // return level of axis box intersection with Frumstum
807 // possible values are Outside, Partial, and Contained.
808 // the frustum plane normals point inwards
testAxisBoxInFrustum(const Extents & extents,const Frustum * frustum)809 IntersectLevel testAxisBoxInFrustum(const Extents& extents,
810 const Frustum* frustum)
811 {
812 // FIXME - use a sphere vs. cone test first?
813
814 static int s, t;
815 static float i[3]; // inside point (assuming partial)
816 static float o[3]; // outside point (assuming partial)
817 static float len;
818 static const float* p; // the plane
819 IntersectLevel result = Contained;
820
821 // FIXME - 0 is the near clip plane, not that useful really?
822 // OpenGL should easily clip the few items sneak in
823
824 const int planeCount = frustum->getPlaneCount();
825
826 for (s = 1 /* NOTE: not 0 */; s < planeCount; s++)
827 {
828
829 p = frustum->getSide(s);
830
831 // setup the inside/outside corners
832 // this can be determined easily based
833 // on the normal vector for the plane
834 for (t = 0; t < 3; t++)
835 {
836 if (p[t] > 0.0f)
837 {
838 i[t] = extents.maxs[t];
839 o[t] = extents.mins[t];
840 }
841 else
842 {
843 i[t] = extents.mins[t];
844 o[t] = extents.maxs[t];
845 }
846 }
847 // check the inside length
848 len = (p[0] * i[0]) + (p[1] * i[1]) + (p[2] * i[2]) + p[3];
849 if (len < -1.0f)
850 {
851 return Outside; // box is fully outside the frustum
852 }
853
854 // check the outside length
855 len = (p[0] * o[0]) + (p[1] * o[1]) + (p[2] * o[2]) + p[3];
856 if (len < -1.0f)
857 {
858 result = Partial; // partial containment at best
859 }
860 }
861
862 return result;
863 }
864
865
866 // return true if the axis aligned bounding box
867 // is contained within all of the planes.
868 // the occluder plane normals point inwards
testAxisBoxOcclusion(const Extents & extents,const float (* planes)[4],int planeCount)869 IntersectLevel testAxisBoxOcclusion(const Extents& extents,
870 const float (*planes)[4], int planeCount)
871 {
872 static int s, t;
873 static float i[3]; // inside point (assuming partial)
874 static float o[3]; // outside point (assuming partial)
875 static float len;
876 static const float* p; // the plane
877 IntersectLevel result = Contained;
878
879 for (s = 0; s < planeCount; s++)
880 {
881
882 p = planes[s];
883
884 // setup the inside/outside corners
885 // this can be determined easily based
886 // on the normal vector for the plane
887 for (t = 0; t < 3; t++)
888 {
889 if (p[t] > 0.0f)
890 {
891 i[t] = extents.maxs[t];
892 o[t] = extents.mins[t];
893 }
894 else
895 {
896 i[t] = extents.mins[t];
897 o[t] = extents.maxs[t];
898 }
899 }
900
901 // check the inside length
902 len = (p[0] * i[0]) + (p[1] * i[1]) + (p[2] * i[2]) + p[3];
903 if (len < +0.1f)
904 {
905 return Outside; // box is fully outside the occluder
906 }
907
908 // FIXME - if we don't do occlusion by SceneNode,
909 // then ditch the partial test. This will
910 // save an extra dot product, and reduce
911 // the likely number of loops
912
913 // check the outside length
914 len = (p[0] * o[0]) + (p[1] * o[1]) + (p[2] * o[2]) + p[3];
915 if (len < +0.1f)
916 {
917 result = Partial; // partial containment at best
918 }
919 }
920
921 return result;
922 }
923
924 // return true if the ray hits the box
925 // if it does hit, set the inTime value
testRayHitsAxisBox(const Ray * ray,const Extents & exts,float * inTime)926 bool testRayHitsAxisBox(const Ray* ray, const Extents& exts,
927 float* inTime)
928 {
929 int a;
930 const float* const o = ray->getOrigin();
931 const float* const v = ray->getDirection();
932 const float* extents[2] = { exts.mins, exts.maxs };
933 int zone[3];
934 bool inside = true;
935
936 // setup the zones
937 for (a = 0; a < 3; a++)
938 {
939 if (o[a] < exts.mins[a])
940 {
941 if (v[a] <= 0.0f)
942 return false;
943 zone[a] = 0;
944 inside = false;
945 }
946 else if (o[a] > exts.maxs[a])
947 {
948 if (v[a] >= 0.0f)
949 return false;
950 zone[a] = 1;
951 inside = false;
952 }
953 else
954 zone[a] = -1;
955 }
956
957 float hitTime[3];
958
959 if (inside)
960 *inTime = 0.0f;
961 else
962 {
963 int hitPlane;
964 // calculate the hitTimes
965 for (a = 0; a < 3; a++)
966 {
967 if (zone[a] < 0)
968 hitTime[a] = -1.0f;
969 else
970 hitTime[a] = (extents[zone[a]][a] - o[a]) / v[a];
971 }
972
973 // use the largest hitTime
974 hitPlane = 0;
975 if (hitTime[1] > hitTime[0])
976 hitPlane = 1;
977 if (hitTime[2] > hitTime[hitPlane])
978 hitPlane = 2;
979
980 // check the hitPlane
981 const float useTime = hitTime[hitPlane];
982 if (useTime < 0.0f)
983 return false;
984 for (a = 0; a < 3; a++)
985 {
986 if (a != hitPlane)
987 {
988 const float hitDist = o[a] + (useTime * v[a]);
989 if ((hitDist < exts.mins[a]) || (hitDist > exts.maxs[a]))
990 return false;
991 }
992 }
993 *inTime = useTime;
994 }
995
996 return true;
997 }
998
999
1000 // return true if the ray hits the box
1001 // if it does hit, set the inTime and outTime values
testRayHitsAxisBox(const Ray * ray,const Extents & extents,float * inTime,float * outTime)1002 bool testRayHitsAxisBox(const Ray* ray, const Extents& extents,
1003 float* inTime, float* outTime)
1004 {
1005 if (!testRayHitsAxisBox(ray, extents, inTime))
1006 return false;
1007
1008 int a;
1009 const float* const o = ray->getOrigin();
1010 const float* const v = ray->getDirection();
1011
1012 // calculate the hitTimes for the outTime
1013 float hitTime[3];
1014 for (a = 0; a < 3; a++)
1015 {
1016 if (v[a] == 0.0f)
1017 hitTime[a] = MAXFLOAT;
1018 else if (v[a] < 0.0f)
1019 hitTime[a] = (extents.mins[a] - o[a]) / v[a];
1020 else
1021 hitTime[a] = (extents.maxs[a] - o[a]) / v[a];
1022 }
1023
1024 // use the smallest hitTime
1025 int hitPlane = 0;
1026 if (hitTime[1] < hitTime[0])
1027 hitPlane = 1;
1028 if (hitTime[2] < hitTime[hitPlane])
1029 hitPlane = 2;
1030 *outTime = hitTime[hitPlane];
1031
1032 return true;
1033 }
1034
1035
1036 // Local Variables: ***
1037 // mode: C++ ***
1038 // tab-width: 4 ***
1039 // c-basic-offset: 4 ***
1040 // indent-tabs-mode: nil ***
1041 // End: ***
1042 // ex: shiftwidth=4 tabstop=4
1043