1 /*---------------------------------------------------------------------------*
2 Project: Horizon
3 File: math_Geometry.cpp
4 Copyright (C)2009 Nintendo Co., Ltd. All rights reserved.
5 These coded instructions, statements, and computer programs contain
6 proprietary information of Nintendo of America Inc. and/or Nintendo
7 Company Ltd., and are protected by Federal copyright law. They may
8 not be disclosed to third parties or copied or duplicated in any form,
9 in whole or in part, without the prior written consent of Nintendo.
10 $Rev: 24051 $
11 *---------------------------------------------------------------------------
12
13
14 */
15
16 #include <nn/math.h>
17
18 #include <nn/math/math_Geometry.h>
19 #include <algorithm>
20
21 namespace nn { namespace math {
22
23 namespace internal
24 {
25 f32 epsilon = 0.0001F;
26 f32 infinity = 100000000.F;
27 }
28
29 //
30 // Member function of the 3D geometry
31 //
32 void
Set(const VEC3 * P0,const VEC3 * P1,const VEC3 * P2)33 PLANE::Set(const VEC3* P0, const VEC3* P1, const VEC3* P2)
34 {
35 // P0, P1, P2 is clockwise looking from above.
36 VEC3 v0, v1, v2;
37 VEC3Sub(&v0, P2, P0);
38 VEC3Sub(&v1, P1, P0);
39
40 VEC3Normalize(&N, VEC3Cross(&v2, &v0, &v1));
41 d = -VEC3Dot(&N, P0);
42 }
43
44 void
Normalize()45 AABB::Normalize()
46 {
47 if (Pmin.x > Pmax.x)
48 ::std::swap(Pmin.x, Pmax.x);
49 if (Pmin.y > Pmax.y)
50 ::std::swap(Pmin.y, Pmax.y);
51 if (Pmin.z > Pmax.z)
52 ::std::swap(Pmin.z, Pmax.z);
53 }
54
55 void
Set(const VEC3 * arrayPoint,unsigned int numPoints)56 AABB::Set(const VEC3* arrayPoint, unsigned int numPoints)
57 {
58 Pmin = arrayPoint[0];
59 Pmax = arrayPoint[0];
60
61 for (unsigned int i = 1; i < numPoints; ++i)
62 {
63 if (arrayPoint[i].x < Pmin.x)
64 Pmin.x = arrayPoint[i].x;
65 else if (arrayPoint[i].x > Pmax.x)
66 Pmax.x = arrayPoint[i].x;
67
68 if (arrayPoint[i].y < Pmin.y)
69 Pmin.y = arrayPoint[i].y;
70 else if (arrayPoint[i].y > Pmax.y)
71 Pmax.y = arrayPoint[i].y;
72
73 if (arrayPoint[i].z < Pmin.z)
74 Pmin.z = arrayPoint[i].z;
75 else if (arrayPoint[i].z > Pmax.z)
76 Pmax.z = arrayPoint[i].z;
77 }
78 }
79
80 void
Set(const AABB * box,const MTX34 * M)81 AABB::Set(const AABB* box, const MTX34* M)
82 {
83 f32 x0, y0, z0;
84 f32 x1, y1, z1;
85
86 f32 a0, a1;
87 f32 b0, b1;
88
89 // Obtain the minimum and maximum values for the new x-axis.
90 x0 = M->f._00 * box->Pmin.x + M->f._03;
91 x1 = M->f._00 * box->Pmax.x + M->f._03;
92 a0 = M->f._01 * box->Pmin.y;
93 a1 = M->f._01 * box->Pmax.y;
94 b0 = M->f._02 * box->Pmin.z;
95 b1 = M->f._02 * box->Pmax.z;
96
97 if (x0 > x1)
98 ::std::swap(x0, x1);
99 if (a0 < a1) { x0 += a0; x1 += a1; }
100 else { x0 += a1; x1 += a0; }
101 if (b0 < b1) { x0 += b0; x1 += b1; }
102 else { x0 += b1; x1 += b0; }
103
104 // Obtain the minimum and maximum values for the new y-axis.
105 y0 = M->f._10 * box->Pmin.x + M->f._13;
106 y1 = M->f._10 * box->Pmax.x + M->f._13;
107 a0 = M->f._11 * box->Pmin.y;
108 a1 = M->f._11 * box->Pmax.y;
109 b0 = M->f._12 * box->Pmin.z;
110 b1 = M->f._12 * box->Pmax.z;
111
112 if (y0 > y1)
113 ::std::swap(y0, y1);
114 if (a0 < a1) { y0 += a0; y1 += a1; }
115 else { y0 += a1; y1 += a0; }
116 if (b0 < b1) { y0 += b0; y1 += b1; }
117 else { y0 += b1; y1 += b0; }
118
119 // Obtain the minimum and maximum values for the new z-axis.
120 z0 = M->f._20 * box->Pmin.x + M->f._23;
121 z1 = M->f._20 * box->Pmax.x + M->f._23;
122 a0 = M->f._21 * box->Pmin.y;
123 a1 = M->f._21 * box->Pmax.y;
124 b0 = M->f._22 * box->Pmin.z;
125 b1 = M->f._22 * box->Pmax.z;
126
127 if (z0 > z1)
128 ::std::swap(z0, z1);
129 if (a0 < a1) { z0 += a0; z1 += a1; }
130 else { z0 += a1; z1 += a0; }
131 if (b0 < b1) { z0 += b0; z1 += b1; }
132 else { z0 += b1; z1 += b0; }
133
134 Pmin.x = x0;
135 Pmin.y = y0;
136 Pmin.z = z0;
137
138 Pmax.x = x1;
139 Pmax.y = y1;
140 Pmax.z = z1;
141 }
142
143
144
145
146 void
Set(const VEC3 * arrayPoint,unsigned int numPoints)147 SPHERE::Set(const VEC3* arrayPoint, unsigned int numPoints)
148 {
149 AABB tmp;
150 tmp.Set(arrayPoint, numPoints);
151 VEC3Lerp(&C, &tmp.Pmin, &tmp.Pmax, 0.5f);
152
153 f32 maxDistance = VEC3SquareDist(&C, &arrayPoint[0]);
154 for (unsigned int i = 1; i < numPoints; ++i)
155 {
156 f32 dist = VEC3SquareDist(&C, &arrayPoint[i]);
157 if (dist > maxDistance)
158 maxDistance = dist;
159 }
160 r = FSqrt(maxDistance);
161 }
162
163 void
Set(f32 top,f32 bottom,f32 left,f32 right,f32 n,f32 f,const MTX34 & camera)164 FRUSTUM::Set(f32 top, f32 bottom, f32 left, f32 right, f32 n, f32 f, const MTX34& camera)
165 {
166 MTX34 invCamera;
167 MTX34Inverse(&invCamera, &camera);
168 MTX34Copy(&cam, &camera);
169 // An assert of inverse matrix will be inserted.
170
171 VEC3 P0(0.f, 0.f, 0.f);
172 VEC3 P[8];
173
174 f32 f_n = f / n;
175
176 // right hand type
177 // P[0] is at upper left of 'near'
178 P[0].x = left; P[0].y = top; P[0].z = -n;
179
180 // P[1] is at upper right of 'near'
181 P[1].x = right; P[1].y = top; P[1].z = -n;
182
183 // P[2] is at lower right of 'near'
184 P[2].x = right; P[2].y = bottom; P[2].z = -n;
185
186 // P[3] is at lower left of 'near'
187 P[3].x = left; P[3].y = bottom; P[3].z = -n;
188
189 // P[4] is at upper left of 'far'
190 P[4].x = f_n * left; P[4].y = f_n * top; P[4].z = -f;
191
192 // P[5] is at upper right of 'far'
193 P[5].x = f_n * right; P[5].y = f_n * top; P[5].z = -f;
194
195 // P[6] is at lower right of 'far'
196 P[6].x = f_n * right; P[6].y = f_n * bottom; P[6].z = -f;
197
198 // P[7] is at lower left of 'far'
199 P[7].x = f_n * left; P[7].y = f_n * bottom; P[7].z = -f;
200
201 // near, far
202 znear = -n;
203 zfar = -f;
204
205 // right hand type
206 leftPlane.Set(&P0, &P[3], &P[0]);
207 rightPlane.Set(&P0, &P[1], &P[2]);
208 topPlane.Set(&P0, &P[0], &P[1]);
209 bottomPlane.Set(&P0, &P[2], &P[3]);
210
211 // Convert P[0-7] to world coordinate system and set AABB.
212 int i;
213 for (i = 0; i < 8; ++i)
214 {
215 VEC3Transform(&P[i], &invCamera, &P[i]);
216 }
217
218 VEC3Transform(&P0, &invCamera, &P0);
219 box.Set(&P[0], 8);
220
221 planes[0].Set(&P0, &P[3], &P[0]); // Left
222 planes[1].Set(&P0, &P[1], &P[2]); // Right
223 planes[2].Set(&P[0], &P[1], &P[2]); // Near
224 planes[3].Set(&P[4], &P[7], &P[6]); // Far
225 planes[4].Set(&P0, &P[0], &P[1]); // Up
226 planes[5].Set(&P0, &P[2], &P[3]); // down
227 }
228
229
230
231
232
233
234 f32
DistSqPoint3ToLine3(const VEC3 * P,const LINE3 * L,f32 * t)235 DistSqPoint3ToLine3(const VEC3* P, const LINE3* L, f32* t)
236 {
237 f32 distSq;
238
239 // P, L
240 VEC3 LP;
241 f32 t_ = VEC3Dot(&L->d, VEC3Sub(&LP, P, &L->P));
242
243 VEC3 PP;
244 VEC3Add(&PP, &L->P, VEC3Scale(&PP, &L->d, t_));
245
246 VEC3 PP_P;
247 VEC3Sub(&PP_P, P, &PP);
248 distSq = VEC3SquareLen(&PP_P);
249
250 if (t)
251 *t = t_;
252 return distSq;
253 }
254
255
256 f32
DistSqPoint3ToRay3(const VEC3 * P,const RAY3 * R,f32 * t)257 DistSqPoint3ToRay3(const VEC3* P, const RAY3* R, f32* t)
258 {
259 f32 distSq;
260 f32 t_;
261
262 distSq = DistSqPoint3ToLine3(P, (const LINE3*)R, &t_);
263
264 if (t_ < 0)
265 {
266 t_ = 0;
267
268 VEC3 vec;
269 VEC3Sub(&vec, P, &R->P);
270 distSq = VEC3SquareLen(&vec);
271 }
272
273
274 if (t)
275 *t = t_;
276 return distSq;
277 }
278
279 f32
DistSqPoint3ToSegment3(const VEC3 * P,const SEGMENT3 * S,f32 * t)280 DistSqPoint3ToSegment3(const VEC3* P, const SEGMENT3* S, f32* t)
281 {
282 f32 distSq;
283 f32 t_;
284 LINE3 L;
285 L.Set(S);
286
287 f32 P0P1 = FSqrt((S->P0 - S->P1).LenSq());
288 distSq = DistSqPoint3ToLine3(P, &L, &t_);
289
290 if (t_ < 0.f)
291 {
292 if (t)
293 *t = 0.f;
294
295 VEC3 vec;
296 VEC3Sub(&vec, P, &S->P0);
297 distSq = VEC3SquareLen(&vec);
298 }
299 else if (t_ > P0P1)
300 {
301 if (t)
302 *t = 1.f;
303
304 VEC3 vec;
305 VEC3Sub(&vec, P, &S->P1);
306 distSq = VEC3SquareLen(&vec);
307 }
308 else
309 {
310 if (t)
311 *t = t_ / P0P1;
312 }
313
314 return distSq;
315 }
316
317
318 f32
DistSqPoint3ToPlane(const VEC3 * P,const PLANE * J,VEC3 * Q)319 DistSqPoint3ToPlane(const VEC3* P, const PLANE* J, VEC3* Q)
320 {
321 // The normal vector of the plane is assumed to be normalized.
322 f32 k = J->Test(*P);
323
324 if (Q)
325 {
326 VEC3 tmp;
327 VEC3Sub(Q, P, VEC3Scale(&tmp, &J->N, k));
328 }
329
330 return k * k;
331 }
332
333
334 f32
DistSqSphereToPlane(const SPHERE * S,const PLANE * J)335 DistSqSphereToPlane(const SPHERE* S, const PLANE* J)
336 {
337 f32 distance = J->Test(S->C);
338 if (distance > S->r)
339 return (distance - S->r) * (distance - S->r);
340 else if (distance < -S->r)
341 return (distance + S->r) * (distance + S->r);
342 else
343 return 0.f;
344 }
345
346 f32
DistSqPoint3ToPolyline3(const VEC3 * P,const VEC3 * vertices,unsigned int nVertices)347 DistSqPoint3ToPolyline3(const VEC3* P, const VEC3* vertices, unsigned int nVertices)
348 {
349 // There must be two or more nVertices
350 unsigned int nSegments = nVertices - 1;
351
352 f32 dSq = internal::infinity;
353 f32 xMinusA, yMinusB, zMinusC;
354 f32 xNextMinusA, yNextMinusB, zNextMinusC;
355 f32 xMinusASq, yMinusBSq, zMinusCSq;
356 f32 xNextMinusASq, yNextMinusBSq, zNextMinusCSq;
357
358 xMinusA = vertices[0].x - P->x;
359 yMinusB = vertices[0].y - P->y;
360 zMinusC = vertices[0].z - P->z;
361
362 xMinusASq = xMinusA * xMinusA;
363 yMinusBSq = yMinusB * yMinusB;
364 zMinusCSq = zMinusC * zMinusC;
365
366 xNextMinusA = vertices[1].x - P->x;
367 yNextMinusB = vertices[1].y - P->y;
368 zNextMinusC = vertices[1].z - P->z;
369
370 xNextMinusASq = xNextMinusA * xNextMinusA;
371 yNextMinusBSq = yNextMinusB * yNextMinusB;
372 zNextMinusCSq = zNextMinusC * zNextMinusC;
373
374 // Calculate the distance with the first segment (line).
375 SEGMENT3 S(vertices[0], vertices[1]);
376 dSq = DistSqPoint3ToSegment3(P, &S, NULL);
377
378 // Reject the subsequent segments as much as possible.
379 // Calculate distance if cannot be rejected
380 for (unsigned int i = 1; i < nSegments - 1; ++i)
381 {
382 // Rejection Test
383 if (((FAbs(xMinusASq) >= dSq) &&
384 (FAbs(xNextMinusASq) >= dSq) &&
385 (xMinusA * xNextMinusA > 0)) ||
386 ((FAbs(yMinusBSq) >= dSq) &&
387 (FAbs(yNextMinusBSq) >= dSq) &&
388 (yMinusB * yNextMinusB > 0)) ||
389 ((FAbs(zMinusCSq) >= dSq) &&
390 (FAbs(zNextMinusCSq) >= dSq) &&
391 (zMinusC * zNextMinusC > 0)))
392 {
393 // Reject
394 ;
395 }
396 else
397 {
398 SEGMENT3 S2(vertices[i], vertices[i + 1]);
399 f32 dSq_ = DistSqPoint3ToSegment3(P, &S2, NULL);
400
401 if (dSq_ < dSq)
402 dSq = dSq_;
403 }
404
405 if (i != nSegments - 2)
406 {
407 xMinusA = xNextMinusA;
408 yMinusB = yNextMinusB;
409 zMinusC = zNextMinusC;
410
411 xMinusASq = xMinusA * xMinusA;
412 yMinusBSq = yMinusB * yMinusB;
413 zMinusCSq = zMinusC * zMinusC;
414
415 xNextMinusA = vertices[i + 2].x - P->x;
416 yNextMinusB = vertices[i + 2].y - P->y;
417 zNextMinusC = vertices[i + 2].z - P->z;
418
419 xNextMinusASq = xNextMinusA * xNextMinusA;
420 yNextMinusBSq = yNextMinusB * yNextMinusB;
421 zNextMinusCSq = zNextMinusC * zNextMinusC;
422 }
423 }
424
425 return dSq;
426 }
427
428
429 f32
DistSqPoint3ToAABB(const VEC3 * P,const AABB * B,VEC3 * q)430 DistSqPoint3ToAABB(const VEC3* P, const AABB* B, VEC3* q)
431 {
432 f32 sqDist = 0.f;
433 f32 v;
434 f32 vv;
435
436 vv = v = P->x;
437 if (v < B->Pmin.x)
438 {
439 sqDist += (B->Pmin.x - v) * (B->Pmin.x - v);
440 vv = B->Pmin.x;
441 }
442 else if (v > B->Pmax.x)
443 {
444 sqDist += (B->Pmax.x - v) * (B->Pmax.x - v);
445 vv = B->Pmax.x;
446 }
447 if (q)
448 q->x = vv;
449
450 vv = v = P->y;
451 if (v < B->Pmin.y)
452 {
453 sqDist += (B->Pmin.y - v) * (B->Pmin.y - v);
454 vv = B->Pmin.y;
455 }
456 else if (v > B->Pmax.y)
457 {
458 sqDist += (B->Pmax.y - v) * (B->Pmax.y - v);
459 vv = B->Pmax.y;
460 }
461 if (q)
462 q->y = vv;
463
464 vv = v = P->z;
465 if (v < B->Pmin.z)
466 {
467 sqDist += (B->Pmin.z - v) * (B->Pmin.z - v);
468 vv = B->Pmin.z;
469 }
470 else if (v > B->Pmax.z)
471 {
472 sqDist += (B->Pmax.z - v) * (B->Pmax.z - v);
473 vv = B->Pmax.z;
474 }
475 if (q)
476 q->z = vv;
477
478 return sqDist;
479 }
480
481
482
483 //
484 // Distance calculation between line/ray/line segment (6 total)
485 //
486 f32
DistSqLine3ToLine3(const LINE3 * L0,const LINE3 * L1,f32 * s,f32 * t)487 DistSqLine3ToLine3(const LINE3* L0, const LINE3* L1, f32* s, f32* t)
488 {
489 // The direction vector of the line is assumed to be normalized.
490 VEC3 u;
491 f32 b, d, e, f, det;
492
493 VEC3Sub(&u, &L0->P, &L1->P);
494 //a = 1.f; // a = VEC3SquareLen(&L0->d);
495 b = VEC3Dot(&L0->d, &L1->d);
496 //c = 1.f; // c = VEC3SquareLen(&L1->d);
497 d = VEC3Dot(&L0->d, &u);
498 e = VEC3Dot(&L1->d, &u);
499 f = VEC3SquareLen(&u);
500 det = 1.f - b * b; // det = a * c - b * b;
501
502 // Check whether parallel
503 if (det < internal::epsilon)
504 {
505 // Calculate from starting point of L0 if parallel
506 if (s)
507 *s = 0.f;
508 if (t)
509 *t = e;
510
511 return f;
512 }
513 else
514 {
515 f32 invDet = 1.f / det;
516 f32 s_, t_;
517
518 s_ = (b * e - d) * invDet;
519 t_ = (e - b * d) * invDet;
520
521 if (s)
522 *s = s_;
523 if (t)
524 *t = t_;
525
526 VEC3 tmp0, tmp1, tmp2;
527 VEC3Sub(&tmp2,
528 VEC3Scale(&tmp0, &L0->d, s_),
529 VEC3Scale(&tmp1, &L1->d, t_));
530 VEC3Add(&tmp2, &tmp2, &u);
531 return VEC3SquareLen(&tmp2);
532 }
533 }
534
535
536 f32
DistSqSegment3ToSegment3(const SEGMENT3 * S1,const SEGMENT3 * S2,f32 * s,f32 * t)537 DistSqSegment3ToSegment3(const SEGMENT3* S1, const SEGMENT3* S2, f32* s, f32* t)
538 {
539 VEC3 u(S1->P1 - S1->P0);
540 VEC3 v(S2->P1 - S2->P0);
541 VEC3 w(S1->P0 - S2->P0);
542 f32 a = VEC3SquareLen(&u); // always >= 0
543 f32 b = VEC3Dot(&u, &v);
544 f32 c = VEC3SquareLen(&v); // always >= 0
545 f32 d = VEC3Dot(&u, &w);
546 f32 e = VEC3Dot(&v, &w);
547 f32 D = a*c - b*b; // always >= 0
548 f32 sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
549 f32 tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
550
551 // Obtain the nearest point when the line segment is taken as line
552 if (D < internal::epsilon) { // The two line segments are parallel
553 sN = 0.0; // Take the distance between S1 starting point and S2.
554 sD = 1.0; // Measure against division operation in the code to follow
555 tN = e;
556 tD = c;
557 }
558 else { // Calculate the nearest point when taken as line, not line segment
559 sN = (b*e - c*d);
560 tN = (a*e - b*d);
561
562 if (sN < 0.0f) { // The side (s = 0) is "visible" if sc < 0.
563 sN = 0.0f;
564 tN = e;
565 tD = c;
566 }
567 else if (sN > sD) { // The side (s = 1) is "visible" if sc > 1.
568 sN = sD;
569 tN = e + b;
570 tD = c;
571 }
572 }
573
574 if (tN < 0.0f) { // The side (t = 0) is "visible" if tc < 0.
575 tN = 0.0f;
576
577 // Recalculate the nearest point of sc with tc = 0.
578 if (-d < 0.0f)
579 sN = 0.0f;
580 else if (-d > a)
581 sN = sD;
582 else {
583 sN = -d;
584 sD = a;
585 }
586 }
587 else if (tN > tD) { // The side (t = 1) is "visible" if tc > 1.
588 tN = tD;
589 // Recalculate the nearest point of sc with tc = 1.
590 if ((-d + b) < 0.0f)
591 sN = 0.0f;
592 else if ((-d + b) > a)
593 sN = sD;
594 else {
595 sN = (-d + b);
596 sD = a;
597 }
598 }
599 // finally do the division to get sc and tc
600 sc = (FAbs(sN) < internal::epsilon ? 0.0f : sN / sD);
601 tc = (FAbs(tN) < internal::epsilon ? 0.0f : tN / tD);
602
603 if (s)
604 *s = sc;
605 if (t)
606 *t = tc;
607
608 // Take the difference between the nearest points.
609 VEC3 dP = w + (u * sc) - (v * tc); // = S1(sc) - S2(tc)
610
611 return VEC3SquareLen(&dP);
612 }
613
614
615 f32
DistSqLine3ToRay3(const LINE3 * L,const RAY3 * R,f32 * s,f32 * t)616 DistSqLine3ToRay3(const LINE3* L, const RAY3* R, f32* s, f32* t)
617 {
618 VEC3 u;
619 f32 b, d, e, det;
620 f32 sNum, tNum, tDenom, sDenom;
621
622 VEC3Sub(&u, &L->P, &R->P);
623 //a = 1.f; // a = VEC3SquareLen(&L0->d);
624 b = VEC3Dot(&L->d, &R->d);
625 //c = 1.f; // c = VEC3SquareLen(&L1->d);
626 d = VEC3Dot(&L->d, &u);
627 e = VEC3Dot(&R->d, &u);
628 det = 1.f - b * b; // det = a * c - b * b;
629
630 if (det < internal::epsilon)
631 {
632 // Randomly choose
633 sNum = 0.f;
634 tNum = e;
635 tDenom = 1.f;
636 sDenom = det;
637 }
638 else
639 {
640 sNum = b * e - d;
641 tNum = e - b * d;
642 sDenom = tDenom = det;
643 }
644
645 // Check the case where t (parameter of Ray) is 0 or smaller.
646 if (tNum < 0)
647 {
648 // Combination of the starting point of Ray and its closest position on the Line.
649 tNum = 0;
650 sNum = -d;
651 sDenom = 1.f;
652 }
653
654 f32 s_ = sNum / sDenom;
655 f32 t_ = tNum / tDenom;
656
657 if (s)
658 *s = s_;
659 if (t)
660 *t = t_;
661
662 // v = (L.P + (s * L.d)) - (R.P + (t * R.d))
663 VEC3 v;
664 VEC3 tmp;
665 VEC3Add(&v, &L->P, VEC3Scale(&tmp, &L->d, s_));
666 VEC3Sub(&v, &v, &R->P);
667 VEC3Sub(&v, &v, VEC3Scale(&tmp, &R->d, t_));
668
669 return VEC3SquareLen(&v);
670 }
671
672 f32
DistSqLine3ToSegment3(const LINE3 * L0,const SEGMENT3 * S,f32 * s,f32 * t)673 DistSqLine3ToSegment3(const LINE3* L0, const SEGMENT3* S, f32* s, f32* t)
674 {
675 VEC3 segDir;
676 VEC3Sub(&segDir, &S->P1, &S->P0);
677
678 VEC3 u;
679 f32 b, c, d, e, det;
680 f32 sNum, tNum, tDenom, sDenom;
681
682 VEC3Sub(&u, &L0->P, &S->P0);
683 // const f32 a = 1.f; // a = VEC3SquareLen(&L0->d);
684 b = VEC3Dot(&L0->d, &segDir);
685 c = VEC3SquareLen(&segDir);
686 d = VEC3Dot(&L0->d, &u);
687 e = VEC3Dot(&segDir, &u);
688 det = c - b * b; // det = a * c - b * b;
689
690 // Check whether parallel
691 if (det < internal::epsilon)
692 {
693 // Randomly choose
694 sNum = 0.f;
695 tNum = e;
696 sDenom = det;
697 tDenom = c;
698 }
699 else
700 {
701 sNum = b * e - c * d;
702 tNum = e - b * d;
703 sDenom = tDenom = det;
704 }
705
706 // Check when t (parameter of Segment) is smaller than 0 or larger than 1.
707 if (tNum < 0.f)
708 {
709 // Starting point of Segment
710 tNum = 0.f;
711 sNum = -d;
712 sDenom = 1.f;
713 }
714 else if (tNum > tDenom)
715 {
716 // Ending point of Segment.
717 tNum = tDenom;
718 sNum = -d + b;
719 sDenom = 1.f;
720 }
721
722 f32 s_ = sNum / sDenom;
723 f32 t_ = tNum / tDenom;
724
725 if (s)
726 *s = s_;
727 if (t)
728 *t = t_;
729
730 VEC3 v;
731 VEC3 tmp;
732 VEC3Add(&v, &L0->P, VEC3Scale(&tmp, &L0->d, s_));
733 VEC3Sub(&v, &v, &S->P0);
734 VEC3Sub(&v, &v, VEC3Scale(&tmp, &segDir, t_));
735
736 return VEC3SquareLen(&v);
737 }
738
739
740 f32
DistSqRay3ToRay3(const RAY3 * R0,const RAY3 * R1,f32 * s,f32 * t)741 DistSqRay3ToRay3(const RAY3* R0, const RAY3* R1, f32* s, f32* t)
742 {
743 VEC3 u;
744 f32 b, d, e, det;
745 f32 sNum, tNum, tDenom, sDenom;
746
747 VEC3Sub(&u, &R0->P, &R1->P);
748 // const f32 a = 1.f; // a = VEC3SquareLen(&R0->d);
749 b = VEC3Dot(&R0->d, &R1->d);
750 // const f32 c = 1.f; // c = VEC3SquareLen(&R1->d);
751 d = VEC3Dot(&R0->d, &u);
752 e = VEC3Dot(&R1->d, &u);
753 det = 1.f - b * b; // det = a * c - b * b;
754
755 // Check whether parallel
756 if (det < internal::epsilon)
757 {
758 // Randomly choose
759 sNum = 0.f;
760 tNum = e;
761 tDenom = 1.f;
762 sDenom = det;
763 }
764 else
765 {
766 sNum = b * e - d;
767 tNum = e - b * d;
768 sDenom = tDenom = det;
769 }
770
771 if (sNum < 0.f)
772 {
773 // Take Ray0 as the starting point.
774 sNum = 0.f;
775 tNum = e;
776 tDenom = 1.f;
777 }
778
779 if (tNum < 0.f)
780 {
781 // Starting point of Ray1
782 tNum = 0.f;
783 if (-d < 0)
784 {
785 // The distance between the starting points of Ray0 and Ray1 will be the closest
786 sNum = 0.f;
787 }
788 else
789 {
790 // When the line segment on Ray0 vertical to the Ray0 reaches the Ray1 starting point.
791 sNum = -d;
792 sDenom = 1.f;
793 }
794 }
795
796 f32 s_ = sNum / sDenom;
797 f32 t_ = tNum / tDenom;
798
799 if (s)
800 *s = s_;
801 if (t)
802 *t = t_;
803
804 // v = (L0.P + (s * L0.d)) - (L1.P + (t * L1.d))
805 VEC3 v;
806 VEC3 tmp;
807 VEC3Add(&v, &R0->P, VEC3Scale(&tmp, &R0->d, s_));
808 VEC3Sub(&v, &v, &R1->P);
809 VEC3Sub(&v, &v, VEC3Scale(&tmp, &R1->d, t_));
810
811 return VEC3SquareLen(&v);
812 }
813
814 f32
DistSqRay3ToSegment3(const RAY3 * R0,const SEGMENT3 * S,f32 * s,f32 * t)815 DistSqRay3ToSegment3(const RAY3* R0, const SEGMENT3* S, f32* s, f32* t)
816 {
817 VEC3 segDir;
818 VEC3 u;
819 f32 b, c, d, e, det;
820 f32 sNum, tNum, tDenom, sDenom;
821
822 VEC3Sub(&segDir, &S->P1, &S->P0);
823 VEC3Sub(&u, &R0->P, &S->P0);
824 // const f32 a = 1.f; // a = VEC3SquareLen(&R0->d);
825 b = VEC3Dot(&R0->d, &segDir);
826 c = VEC3SquareLen(&segDir);
827 d = VEC3Dot(&R0->d, &u);
828 e = VEC3Dot(&segDir, &u);
829 det = c - b * b; // det = a * c - b * b;
830
831 // Check whether parallel
832 if (det < internal::epsilon)
833 {
834 // Randomly choose
835 sNum = 0.f;
836 tNum = e;
837 tDenom = c;
838 sDenom = det;
839 }
840 else
841 {
842 sNum = b * e - c * d;
843 tNum = e - b * d;
844 sDenom = tDenom = det;
845 }
846
847 // Check Ray
848 if (sNum < 0.f)
849 {
850 // Use the starting position of Ray.
851 sNum = 0.f;
852 tNum = e;
853 tDenom = c;
854 }
855
856 // Check Segment
857 if (tNum < 0.f)
858 {
859 // Starting point of Segment
860 tNum = 0.f;
861 if (-d < 0.f)
862 {
863 sNum = 0.f;
864 }
865 else
866 {
867 // If the nearest point to the Segment starting point is on Ray
868 sNum = -d;
869 sDenom = 1.f;
870 }
871 }
872 else if (tNum > tDenom)
873 {
874 // Ending point of Segment.
875 tNum = tDenom;
876 if ((-d + b) < 0.f)
877 {
878 sNum = 0.f;
879 }
880 else
881 {
882 // If the nearest point to the Segment ending point is on Ray
883 sNum = -d + b;
884 sDenom = 1.f;
885 }
886 }
887
888 f32 s_ = sNum / sDenom;
889 f32 t_ = tNum / tDenom;
890
891 if (s)
892 *s = s_;
893 if (t)
894 *t = t_;
895
896 // v = (L0.P + (s * L0.d)) - (S.P + (t * S.d))
897 VEC3 v;
898 VEC3 tmp;
899 VEC3Add(&v, &R0->P, VEC3Scale(&tmp, &R0->d, s_));
900 VEC3Sub(&v, &v, &S->P0);
901 VEC3Sub(&v, &v, VEC3Scale(&tmp, &segDir, t_));
902
903 return VEC3SquareLen(&v);
904 }
905
906
907
908 //
909 // Intersection
910 //
911
912 // Line, Plane
913 IntersectionResult
IntersectionLine3Plane(const LINE3 * L,const PLANE * J,f32 * t,VEC3 * I)914 IntersectionLine3Plane(const LINE3* L, const PLANE* J, f32* t, VEC3* I)
915 {
916 f32 denom;
917 denom = VEC3Dot(&L->d, &J->N);
918
919 if (FAbs(denom) < internal::epsilon)
920 {
921 if (FAbs(VEC3Dot(&J->N, &L->P) + J->d) < internal::epsilon)
922 return INTERSECTION_LINE3_ON_PLANE;
923 else
924 return INTERSECTION_NONE;
925 }
926 else
927 {
928 if (I != NULL || t != NULL)
929 {
930 f32 t_ = -(VEC3Dot(&J->N, &L->P) + J->d) / denom;
931
932 if (I)
933 {
934 VEC3 tmp;
935 // Intersect calculation
936 VEC3Add(I, &L->P, VEC3Scale(&tmp, &L->d, t_));
937 }
938
939 if (t)
940 *t = t_;
941 }
942
943 return INTERSECTION_1;
944 }
945 }
946
947 // Ray, Plane
948 IntersectionResult
IntersectionRay3Plane(const RAY3 * R,const PLANE * J,f32 * t,VEC3 * I)949 IntersectionRay3Plane(const RAY3* R, const PLANE* J, f32* t, VEC3* I)
950 {
951 f32 t_;
952 VEC3 I_;
953 IntersectionResult result;
954 result = IntersectionLine3Plane((const LINE3*)R, J, &t_, (I != NULL ? &I_ : NULL));
955
956 if (result == INTERSECTION_1)
957 {
958 // Decide based on whether t_ >= 0.
959 if (t_ >= 0)
960 {
961 if (t)
962 *t = t_;
963 if (I)
964 *I = I_;
965 return result;
966 }
967 else
968 {
969 return INTERSECTION_NONE;
970 }
971 }
972 else
973 {
974 return result;
975 }
976 }
977
978 // Segment, Plane
979 IntersectionResult
IntersectionSegment3Plane(const SEGMENT3 * S,const PLANE * J,f32 * t,VEC3 * I)980 IntersectionSegment3Plane(const SEGMENT3* S, const PLANE* J, f32* t, VEC3* I)
981 {
982 f32 t_;
983 VEC3 I_;
984 VEC3 dir;
985 VEC3Sub(&dir, &S->P1, &S->P0);
986 f32 P0P1 = FSqrt(dir.LenSq());
987 LINE3 L(S->P0, dir);
988
989 IntersectionResult result;
990 result = IntersectionLine3Plane(&L, J, &t_, (I != NULL ? &I_ : NULL));
991
992 if (result == INTERSECTION_1)
993 {
994 // Decide using 0 <= t_ <= P0P1
995 if (t_ >= 0 && t_ <= P0P1)
996 {
997 if (t)
998 *t = t_ / P0P1;
999 if (I)
1000 *I = I_;
1001 return result;
1002 }
1003 else
1004 {
1005 return INTERSECTION_NONE;
1006 }
1007 }
1008 else
1009 {
1010 return result;
1011 }
1012 }
1013
1014 // Line, Sphere
1015 IntersectionResult
IntersectionLine3Sphere(const LINE3 * L,const SPHERE * sphere,f32 * t0,f32 * t1)1016 IntersectionLine3Sphere(const LINE3* L, const SPHERE* sphere, f32* t0, f32* t1)
1017 {
1018 f32 b, c, discrm;
1019 VEC3 PmC;
1020 VEC3Sub(&PmC, &L->P, &sphere->C);
1021 // a = 1.f; // dot(L->d, L->d) is 1.f
1022 b = 2.f * VEC3Dot(&L->d, &PmC);
1023 c = VEC3SquareLen(&PmC) - sphere->r * sphere->r;
1024 discrm = b * b - 4.f * c /* * a*/;
1025
1026 if (discrm > 0.f)
1027 {
1028 f32 sq = FSqrt(discrm);
1029
1030 if (t0)
1031 {
1032 *t0 = (-b - sq) * 0.5f/* inv(2 * a) */;
1033 }
1034
1035 if (t1)
1036 {
1037 *t1 = (-b + sq) * 0.5f/* inv(2 * a) */;
1038 }
1039
1040 return INTERSECTION_2;
1041 }
1042 else if (discrm == 0.f)
1043 {
1044 if (t0)
1045 {
1046 *t0 = -0.5f * b;
1047 //*t0 = -b / (2.f * a);
1048 }
1049 return INTERSECTION_1;
1050 }
1051 else
1052 {
1053 return INTERSECTION_NONE;
1054 }
1055 }
1056
1057 // Ray, Sphere
1058 IntersectionResult
IntersectionRay3Sphere(const RAY3 * R,const SPHERE * sphere,f32 * t0,f32 * t1)1059 IntersectionRay3Sphere(const RAY3* R, const SPHERE* sphere, f32* t0, f32* t1)
1060 {
1061 IntersectionResult result;
1062 f32 t0_, t1_;
1063 result = IntersectionLine3Sphere((const LINE3*)R, sphere, &t0_, &t1_);
1064
1065 if (result != INTERSECTION_NONE)
1066 {
1067 if (result == INTERSECTION_1)
1068 {
1069 if (t0_ >= 0.f)
1070 {
1071 if (t0)
1072 *t0 = t0_;
1073 return result;
1074 }
1075 else
1076 {
1077 return INTERSECTION_NONE;
1078 }
1079 }
1080 else
1081 {
1082 // Here (t0_ < t1_) is guaranteed
1083 if (t0_ >= 0.f)
1084 {
1085 // Ray will intersect with Sphere at two points
1086 if (t0)
1087 *t0 = t0_;
1088 if (t1)
1089 *t1 = t1_;
1090 return result;
1091 }
1092 else if (t1_ >= 0.f)
1093 {
1094 // Ray will intersect with Sphere at one point
1095 if (t0)
1096 *t0 = t1_;
1097 return INTERSECTION_1;
1098 }
1099 else
1100 {
1101 return INTERSECTION_NONE;
1102 }
1103 }
1104 }
1105 else
1106 {
1107 return result;
1108 }
1109 }
1110
1111
1112
1113 // Ray, Sphere - number 2
1114 bool
IntersectionRay3Sphere(const RAY3 * R,const SPHERE * sphere)1115 IntersectionRay3Sphere(const RAY3* R, const SPHERE* sphere)
1116 {
1117 VEC3 w;
1118 VEC3Sub(&w, &sphere->C, &R->P);
1119 f32 wsq = VEC3SquareLen(&w);
1120 f32 proj = VEC3Dot(&w, &R->d);
1121 f32 rsq = sphere->r * sphere->r;
1122
1123 if (proj < 0.f && wsq > rsq)
1124 return false;
1125
1126 f32 vsq = VEC3SquareLen(&R->d);
1127
1128 if (vsq * wsq - proj * proj <= vsq * rsq)
1129 return true;
1130 else
1131 return false;
1132 }
1133
1134 // Segment, Sphere
1135 IntersectionResult
IntersectionSegment3Sphere(const SEGMENT3 * S,const SPHERE * sphere,f32 * t0,f32 * t1)1136 IntersectionSegment3Sphere(const SEGMENT3* S, const SPHERE* sphere, f32* t0, f32* t1)
1137 {
1138 VEC3 dir;
1139 VEC3Sub(&dir, &S->P1, &S->P0);
1140 f32 P0P1 = FSqrt(dir.LenSq());
1141 LINE3 L(S->P0, dir);
1142 IntersectionResult result;
1143 f32 t0_, t1_;
1144 result = IntersectionLine3Sphere(&L, sphere, &t0_, &t1_);
1145
1146 if (result != INTERSECTION_NONE)
1147 {
1148 if (result == INTERSECTION_1)
1149 {
1150 if (t0_ >= 0.f && t0_ <= P0P1)
1151 {
1152 if (t0)
1153 *t0 = t0_ / P0P1;
1154 return result;
1155 }
1156 else
1157 {
1158 return INTERSECTION_NONE;
1159 }
1160 }
1161 else
1162 {
1163 // Here (t0_ < t1_) is guaranteed
1164 if (t0_ >= 0.f)
1165 {
1166 if (t1_ <= P0P1)
1167 {
1168 f32 tmp = 1.f / P0P1;
1169 // Two points intersect
1170 if (t0)
1171 *t0 = t0_ * tmp;
1172 if (t1)
1173 *t1 = t1_ * tmp;
1174
1175 return result;
1176 }
1177 else if (t0_ <= P0P1)
1178 {
1179 // Only t0_ intersects
1180 if (t0)
1181 *t0 = t0_ / P0P1;
1182 return INTERSECTION_1;
1183 }
1184 else
1185 {
1186 return INTERSECTION_NONE;
1187 }
1188 }
1189 else if (t1_ >= 0.f && t1_ <= P0P1)
1190 {
1191 // Only t1_ intersects
1192 if (t0)
1193 *t0 = t1_ / P0P1;
1194 return INTERSECTION_1;
1195 }
1196 else
1197 {
1198 return INTERSECTION_NONE;
1199 }
1200
1201 }
1202 }
1203 else
1204 {
1205 return result;
1206 }
1207 }
1208
1209 static bool
IntersectionRay3AABB_(f32 min,f32 max,f32 o,f32 dir,f32 & tNear,f32 & tFar)1210 IntersectionRay3AABB_(f32 min, f32 max, f32 o, f32 dir, f32& tNear, f32& tFar)
1211 {
1212 if (FAbs(dir) < internal::epsilon)
1213 {
1214 // Parallel to plane
1215 if (o < min || o > max)
1216 return false;
1217 else
1218 return true;
1219 }
1220
1221 f32 inv_dir = 1.f / dir;
1222 f32 t0 = (min - o) * inv_dir;
1223 f32 t1 = (max - o) * inv_dir;
1224
1225 if (t0 > t1)
1226 ::std::swap(t0, t1);
1227
1228 if (t0 > tNear)
1229 tNear = t0;
1230 if (t1 < tFar)
1231 tFar = t1;
1232
1233 if (tNear > tFar)
1234 return false;
1235 if (tFar < 0.f)
1236 return false;
1237 return true;
1238 }
1239
1240
1241 // Ray, AABB
1242 bool
IntersectionRay3AABB(const RAY3 * R,const AABB * box,f32 * t)1243 IntersectionRay3AABB(const RAY3* R, const AABB* box, f32* t)
1244 {
1245 f32 tNear = -internal::infinity;
1246 f32 tFar = internal::infinity;
1247 bool result;
1248
1249 result = IntersectionRay3AABB_(box->Pmin.x, box->Pmax.x, R->P.x, R->d.x, tNear, tFar);
1250 if (!result)
1251 return false;
1252
1253 result = IntersectionRay3AABB_(box->Pmin.y, box->Pmax.y, R->P.y, R->d.y, tNear, tFar);
1254 if (!result)
1255 return false;
1256
1257 result = IntersectionRay3AABB_(box->Pmin.z, box->Pmax.z, R->P.z, R->d.z, tNear, tFar);
1258 if (!result)
1259 return false;
1260
1261 if (t)
1262 {
1263 if (tNear > 0.f)
1264 *t = tNear;
1265 else
1266 *t = tFar;
1267 }
1268
1269 return true;
1270 }
1271
1272 // AABB, AABB
1273 bool
IntersectionAABB(const AABB * a,const AABB * b)1274 IntersectionAABB(const AABB* a, const AABB* b)
1275 {
1276 if (a->Pmin.x > b->Pmax.x || b->Pmin.x > a->Pmax.x ||
1277 a->Pmin.y > b->Pmax.y || b->Pmin.y > a->Pmax.y ||
1278 a->Pmin.z > b->Pmax.z || b->Pmin.z > a->Pmax.z)
1279 return false;
1280 else
1281 return true;
1282 }
1283
1284
1285 // Sphere, AABB
1286 bool
IntersectionSphereAABB(const SPHERE * sphere,const AABB * aabb)1287 IntersectionSphereAABB(const SPHERE* sphere, const AABB* aabb)
1288 {
1289 f32 distSq = 0.f;
1290
1291 if (sphere->C.x < aabb->Pmin.x)
1292 distSq += (sphere->C.x - aabb->Pmin.x) * (sphere->C.x - aabb->Pmin.x);
1293 else if (sphere->C.x > aabb->Pmax.x)
1294 distSq += (sphere->C.x - aabb->Pmax.x) * (sphere->C.x - aabb->Pmax.x);
1295
1296 if (sphere->C.y < aabb->Pmin.y)
1297 distSq += (sphere->C.y - aabb->Pmin.y) * (sphere->C.y - aabb->Pmin.y);
1298 else if (sphere->C.y > aabb->Pmax.y)
1299 distSq += (sphere->C.y - aabb->Pmax.y) * (sphere->C.y - aabb->Pmax.y);
1300
1301 if (sphere->C.z < aabb->Pmin.z)
1302 distSq += (sphere->C.z - aabb->Pmin.z) * (sphere->C.z - aabb->Pmin.z);
1303 else if (sphere->C.z > aabb->Pmax.z)
1304 distSq += (sphere->C.z - aabb->Pmax.z) * (sphere->C.z - aabb->Pmax.z);
1305
1306 if (distSq <= sphere->r * sphere->r)
1307 return true;
1308 else
1309 return false;
1310 }
1311
1312 bool
IntersectionSphere(const SPHERE * s0,const SPHERE * s1)1313 IntersectionSphere(const SPHERE* s0, const SPHERE* s1)
1314 {
1315 VEC3 centerDiff;
1316 VEC3Sub(¢erDiff, &s0->C, &s1->C);
1317 f32 radiusSum = s0->r + s1->r;
1318
1319 if (VEC3SquareLen(¢erDiff) <= radiusSum * radiusSum)
1320 return true;
1321 else
1322 return false;
1323 }
1324
1325 bool
IntersectionPlaneAABB(const PLANE * J,const AABB * B)1326 IntersectionPlaneAABB(const PLANE* J, const AABB* B)
1327 {
1328 VEC3 C;
1329 VEC3 E;
1330 VEC3Lerp(&C, &B->Pmin, &B->Pmax, .5f);
1331 VEC3Sub(&E, &B->Pmax, &C); // Here, the elements of E will all be positive.
1332
1333 // The largest of the dot product between N and the vector from the center of AABB to each point of AABB will be r, and the smallest -r.
1334 // That is, a location within distance r of the plane that passes through center of AABB and a normal of N will be inside AABB.
1335 f32 r = E.x * FAbs(J->N.x) +
1336 E.y * FAbs(J->N.y) +
1337 E.z * FAbs(J->N.z);
1338
1339 // Distance between the plane and the center of AABB
1340 f32 s = VEC3Dot(&J->N, &C) + J->d;
1341
1342 // Intersects if the distance between the plane and the center of AABB is within r.
1343 return (FAbs(s) <= r);
1344 }
1345
1346 bool
IntersectionCapsule(const CAPSULE * C0,const CAPSULE * C1)1347 IntersectionCapsule(const CAPSULE* C0, const CAPSULE* C1)
1348 {
1349 f32 radiusSum = C0->r + C1->r;
1350 if (DistSqSegment3ToSegment3(&C0->S, &C1->S, NULL, NULL)
1351 <= radiusSum * radiusSum)
1352 return true;
1353 else
1354 return false;
1355 }
1356
1357
1358 bool
IntersectionRay3Capsule(const RAY3 * R,const CAPSULE * C)1359 IntersectionRay3Capsule(const RAY3* R, const CAPSULE* C)
1360 {
1361 if (DistSqRay3ToSegment3(R, &C->S, NULL, NULL) <= C->r * C->r)
1362 return true;
1363 else
1364 return false;
1365 }
1366
1367 bool
IntersectionLine3Capsule(const LINE3 * L,const CAPSULE * C)1368 IntersectionLine3Capsule(const LINE3* L, const CAPSULE* C)
1369 {
1370 if (DistSqLine3ToSegment3(L, &C->S, NULL, NULL) <= C->r * C->r)
1371 return true;
1372 else
1373 return false;
1374 }
1375
1376 bool
IntersectionPlaneCapsule(const PLANE * J,const CAPSULE * C)1377 IntersectionPlaneCapsule(const PLANE* J, const CAPSULE* C)
1378 {
1379 f32 s0 = J->Test(C->S.P0);
1380 f32 s1 = J->Test(C->S.P1);
1381
1382 if (s0 * s1 < 0.f ||
1383 FAbs(s0) <= C->r ||
1384 FAbs(s1) <= C->r)
1385 return true;
1386 else
1387 return false;
1388 }
1389
1390 bool
IntersectSphere(const SPHERE * S) const1391 FRUSTUM::IntersectSphere(const SPHERE* S) const
1392 {
1393 // Sphere is in world coordinate system
1394
1395 f32 Dist;
1396 VEC3 viewPos;
1397
1398 // Convert the z-coordinate of the center of sphere to view coordinate system coordinate
1399 viewPos.z = cam.f._20 * S->C.x +
1400 cam.f._21 * S->C.y +
1401 cam.f._22 * S->C.z +
1402 cam.f._23;
1403
1404 // Determine whether it is in front of the near plane
1405 if (viewPos.z - S->r > znear)
1406 return false;
1407
1408 // Determine whether it is behind the far plane
1409 if (viewPos.z + S->r < zfar)
1410 return false;
1411
1412 // Convert the x-coordinate of the center of sphere to view coordinate system coordinate
1413 viewPos.x = cam.f._00 * S->C.x +
1414 cam.f._01 * S->C.y +
1415 cam.f._02 * S->C.z +
1416 cam.f._03;
1417
1418 // Test with left clip plane
1419 // Because the left clip plane passes through the origin, N.d = 0. It is also parallel to the y-axis, so N.y = 0.
1420 Dist = viewPos.x * leftPlane.N.x +
1421 viewPos.z * leftPlane.N.z;
1422 if (Dist > S->r)
1423 return false;
1424
1425 // Test with right clip plane
1426 // Because the right clip plane passes through the origin, N.d = 0. It is also parallel to the y-axis, so N.y = 0.
1427 Dist = viewPos.x * rightPlane.N.x +
1428 viewPos.z * rightPlane.N.z;
1429 if (Dist > S->r)
1430 return false;
1431
1432 // Convert the y-coordinate of the center of sphere to view coordinate system
1433 viewPos.y = cam.f._10 * S->C.x +
1434 cam.f._11 * S->C.y +
1435 cam.f._12 * S->C.z +
1436 cam.f._13;
1437
1438 // Test with upper clip plane
1439 // Because the upper clip plane passes through the origin, N.d = 0. It is also parallel to the x-axis, so N.x = 0.
1440 Dist = viewPos.y * topPlane.N.y +
1441 viewPos.z * topPlane.N.z;
1442 if (Dist > S->r)
1443 return false;
1444
1445 // Test with lower clip plane
1446 // Because the lower clip plane passes through the origin, N.d = 0. It is also parallel to the x-axis, so N.x = 0.
1447 Dist = viewPos.y * bottomPlane.N.y +
1448 viewPos.z * bottomPlane.N.z;
1449 if (Dist > S->r)
1450 return false;
1451
1452 // Part or whole of the sphere is inside frustum.
1453 return true;
1454 }
1455
1456 bool
IntersectAABB(const AABB * B) const1457 FRUSTUM::IntersectAABB(const AABB* B) const
1458 {
1459 // Filter using AABB surrounding the frustum
1460 if (!IntersectionAABB(B, &box))
1461 return false;
1462
1463 int i;
1464 VEC3 p;
1465 for (i = 0; i < 6; ++i)
1466 {
1467 // For each sides of the frustum
1468 // Test: determine the point that will minimize ax + by + cz + d.
1469 // This can be determined by looking at the sign of a, b, c.
1470 // If Test is positive, all points of AABB will be outside the frustum.
1471 p.x = (planes[i].N.x >= 0) ? B->Pmin.x : B->Pmax.x;
1472 p.y = (planes[i].N.y >= 0) ? B->Pmin.y : B->Pmax.y;
1473 p.z = (planes[i].N.z >= 0) ? B->Pmin.z : B->Pmax.z;
1474
1475 // FALSE if the nearest point to the frustum is outside.
1476 if (planes[i].Test(p) > 0)
1477 return false;
1478 }
1479
1480 return true;
1481 }
1482
1483 IntersectionResult
IntersectAABB_Ex(const AABB * B) const1484 FRUSTUM::IntersectAABB_Ex(const AABB* B) const
1485 {
1486 // Filter using AABB surrounding the frustum
1487 if (!IntersectionAABB(B, &box))
1488 return INTERSECTION_OUTSIDE;
1489
1490 IntersectionResult result = INTERSECTION_INSIDE;
1491
1492 int i;
1493 VEC3 p, n;
1494 for (i = 0; i < 6; ++i)
1495 {
1496 if (planes[i].N.x >= 0)
1497 {
1498 p.x = B->Pmin.x; n.x = B->Pmax.x;
1499 }
1500 else
1501 {
1502 p.x = B->Pmax.x; n.x = B->Pmin.x;
1503 }
1504
1505 if (planes[i].N.y >= 0)
1506 {
1507 p.y = B->Pmin.y; n.y = B->Pmax.y;
1508 }
1509 else
1510 {
1511 p.y = B->Pmax.y; n.y = B->Pmin.y;
1512 }
1513
1514 if (planes[i].N.z >= 0)
1515 {
1516 p.z = B->Pmin.z; n.z = B->Pmax.z;
1517 }
1518 else
1519 {
1520 p.z = B->Pmax.z; n.z = B->Pmin.z;
1521 }
1522
1523 // 'outside' if the nearest point to the frustum is outside.
1524 if (planes[i].Test(p) > 0)
1525 return INTERSECTION_OUTSIDE;
1526 if (planes[i].Test(n) > 0)
1527 result = INTERSECTION_INTERSECT;
1528 }
1529
1530 return result;
1531 }
1532
1533
1534
1535
1536 //
1537 // Merge area
1538 //
1539
1540
1541 SPHERE*
MergeSphere(SPHERE * s2,const SPHERE * s0,const SPHERE * s1)1542 MergeSphere(SPHERE* s2, const SPHERE* s0, const SPHERE* s1)
1543 {
1544 // s2 == s0,s1 will still be fine.
1545
1546 VEC3 diff;
1547 VEC3Sub(&diff, &s1->C, &s0->C);
1548 f32 distsq = VEC3SquareLen(&diff);
1549 f32 radiusDiff = s1->r - s0->r;
1550
1551 if (distsq <= radiusDiff * radiusDiff)
1552 {
1553 if (s0->r > s1->r)
1554 *s2 = *s0;
1555 else
1556 *s2 = *s1;
1557 }
1558 else
1559 {
1560 f32 dist = FSqrt(distsq);
1561 f32 newRadius = 0.5f * (s0->r + s1->r + dist);
1562
1563 if (FAbs(dist) > internal::epsilon)
1564 {
1565 VEC3Add(&s2->C, &s0->C, VEC3Scale(&diff, &diff, ((newRadius - s0->r)/dist)));
1566 }
1567 else
1568 {
1569 s2->C = s0->C;
1570 }
1571
1572 s2->r = newRadius;
1573 }
1574 return s2;
1575 }
1576
1577 AABB*
MergeAABB(AABB * a2,const AABB * a0,const AABB * a1)1578 MergeAABB(AABB* a2, const AABB* a0, const AABB* a1)
1579 {
1580 a2->Pmin.x = ::std::min(a0->Pmin.x, a1->Pmin.x);
1581 a2->Pmin.y = ::std::min(a0->Pmin.y, a1->Pmin.y);
1582 a2->Pmin.z = ::std::min(a0->Pmin.z, a1->Pmin.z);
1583
1584 a2->Pmax.x = ::std::max(a0->Pmax.x, a1->Pmax.x);
1585 a2->Pmax.y = ::std::max(a0->Pmax.y, a1->Pmax.y);
1586 a2->Pmax.z = ::std::max(a0->Pmax.z, a1->Pmax.z);
1587
1588 return a2;
1589 }
1590
1591
1592 }} // nw::math
1593