1 /*---------------------------------------------------------------------------*
2   Project:  Horizon
3   File:     math_Geometry.cpp
4 
5   Copyright (C)2009 Nintendo Co., Ltd.  All rights reserved.
6 
7   These coded instructions, statements, and computer programs contain
8   proprietary information of Nintendo of America Inc. and/or Nintendo
9   Company Ltd., and are protected by Federal copyright law.  They may
10   not be disclosed to third parties or copied or duplicated in any form,
11   in whole or in part, without the prior written consent of Nintendo.
12 
13   $Rev: 24051 $
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 // 3Dジオメトリのメンバ関数
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は上から見て時計まわり
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     // 新しいX軸における最小値と最大値を求める
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     // 新しいY軸における最小値と最大値を求める
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     // 新しいZ軸における最小値と最大値を求める
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     // 逆行列のアサートが入る
170 
171     VEC3 P0(0.f, 0.f, 0.f);
172     VEC3 P[8];
173 
174     f32 f_n = f / n;
175 
176     // 右手系
177     // P[0]はnearの左上
178     P[0].x = left; P[0].y = top; P[0].z = -n;
179 
180     // P[1]はnearの右上
181     P[1].x = right; P[1].y = top; P[1].z = -n;
182 
183     // P[2]はnearの右下
184     P[2].x = right; P[2].y = bottom; P[2].z = -n;
185 
186     // P[3]はnearの左下
187     P[3].x = left; P[3].y = bottom; P[3].z = -n;
188 
189     // P[4]はfarの左上
190     P[4].x = f_n * left; P[4].y = f_n * top; P[4].z = -f;
191 
192     // P[5]はfarの右上
193     P[5].x = f_n * right; P[5].y = f_n * top; P[5].z = -f;
194 
195     // P[6]はfarの右下
196     P[6].x = f_n * right; P[6].y = f_n * bottom; P[6].z = -f;
197 
198     // P[7]は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     // 右手系である
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     // P[0-7]をワールド座標系に変換して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     // 平面の法線ベクトルは正規化されていることが前提になっています。
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     // nVerticesは2以上
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     // 最初のセグメント(線)との距離を計算する。
375     SEGMENT3 S(vertices[0], vertices[1]);
376     dSq = DistSqPoint3ToSegment3(P, &S, NULL);
377 
378     // 続くセグメントを出来る限りリジェクトする。
379     // リジェクトできなければ距離を計算
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             // リジェクト
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 // 線・レイ・線分の間の距離の計算(合計6種類)
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     // 線の方向ベクトルは正規化されていることが前提になっています。
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     // 平行かどうかをチェック
503     if (det < internal::epsilon)
504     {
505         // 平行な場合はL0の起点から算出
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     // 線分を線とした場合の最近点を求める
552     if (D < internal::epsilon) { // 2つの線分は平行
553         sN = 0.0;              // S1の始点とS2の距離をとる
554         sD = 1.0;              // 後のコードの除算対策
555         tN = e;
556         tD = c;
557     }
558     else {                     // 線分でなく線とした場合最近点を計算
559         sN = (b*e - c*d);
560         tN = (a*e - b*d);
561 
562         if (sN < 0.0f) {       // sc < 0ならば、s=0の辺が「見える」
563             sN = 0.0f;
564             tN = e;
565             tD = c;
566         }
567         else if (sN > sD) {    // sc > 1ならば、s=1の辺が「見える」
568             sN = sD;
569             tN = e + b;
570             tD = c;
571         }
572     }
573 
574     if (tN < 0.0f) {            // tc < 0ならば、t=0の辺が「見える」
575         tN = 0.0f;
576 
577         // tc = 0としてscの最近点を再計算
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) {        // tc > 1ならば、t=1の辺が「見える」
588         tN = tD;
589         // tc = 1としてscの最近点を再計算
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     // 最近点同士の差をとる
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         // 適当に決める
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     // t(Rayのパラメータ)が0以下の場合をチェック
646     if (tNum < 0)
647     {
648         // Rayの起点とそれに一番近い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     // 平行かどうかをチェック
691     if (det < internal::epsilon)
692     {
693         // 適当に決める
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     // t(Segnemtのパラメータ)が0未満、1より大きい場合のチェック
707     if (tNum < 0.f)
708     {
709         // Segmentの始点
710         tNum = 0.f;
711         sNum = -d;
712         sDenom = 1.f;
713     }
714     else if (tNum > tDenom)
715     {
716         // 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     // 平行かどうかをチェック
756     if (det < internal::epsilon)
757     {
758         // 適当に決める
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         // Ray0を起点にする。
774         sNum = 0.f;
775         tNum = e;
776         tDenom = 1.f;
777     }
778 
779     if (tNum < 0.f)
780     {
781         // Ray1の起点
782         tNum = 0.f;
783         if (-d < 0)
784         {
785             // Ray0, Ray1の起点同士が最も近い
786             sNum = 0.f;
787         }
788         else
789         {
790             // Ray0上のRay0に垂直な線分がRay1の起点に到達する場合
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     // 平行かどうかをチェック
832     if (det < internal::epsilon)
833     {
834         // 適当に決める
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     // Rayをチェック
848     if (sNum < 0.f)
849     {
850         // Rayの起点を使う。
851         sNum = 0.f;
852         tNum = e;
853         tDenom = c;
854     }
855 
856     // Segmentをチェック
857     if (tNum < 0.f)
858     {
859         // Segmentの始点
860         tNum = 0.f;
861         if (-d < 0.f)
862         {
863             sNum = 0.f;
864         }
865         else
866         {
867             // Rayの上にSegmentの始点への最近点がある場合
868             sNum = -d;
869             sDenom = 1.f;
870         }
871     }
872     else if (tNum > tDenom)
873     {
874         // Segmentの終点
875         tNum = tDenom;
876         if ((-d + b) < 0.f)
877         {
878             sNum = 0.f;
879         }
880         else
881         {
882             // Rayの上にSegmentの終点への最近点がある場合
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                 // 交点の計算
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         // 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         // 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)は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             // ここで、t0_ < t1_が保証されている
1083             if (t0_ >= 0.f)
1084             {
1085                 // Rayは2点でSphereと交わる
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は1点でSphereと交わる
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その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             // ここで、t0_ < t1_が保証されている
1164             if (t0_ >= 0.f)
1165             {
1166                 if (t1_ <= P0P1)
1167                 {
1168                     f32 tmp = 1.f / P0P1;
1169                     // 2点が交わる
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                     // t0_のみ交わる
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                 // t1_のみ交わる
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         // 面に対して平行
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(&centerDiff, &s0->C, &s1->C);
1317     f32 radiusSum = s0->r + s1->r;
1318 
1319     if (VEC3SquareLen(&centerDiff) <= 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); // ここでEの要素は全て正である。
1332 
1333     // AABBの中心からAABBの各点へのベクトルとNとの内積で最大のものはrで最小のものは-rである。
1334     // つまり法線Nを持ち、AABBの中心を通る平面からr以内は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     // 平面とAABBの中心の距離
1340     f32 s = VEC3Dot(&J->N, &C) + J->d;
1341 
1342     // 平面とAABBの中心の距離が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     // 球はワールド座標系
1394 
1395     f32 Dist;
1396     VEC3 viewPos;
1397 
1398     // 球の中心のZ座標をビュー座標系に変換する
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     // ニアプレーンの前かどうかを判定
1405     if (viewPos.z - S->r > znear)
1406         return false;
1407 
1408     // ファープレーンの後ろかどうかを判定
1409     if (viewPos.z + S->r < zfar)
1410         return false;
1411 
1412     // 球の中心のX座標をビュー座標系に変換する
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     // 左クリップ平面とテスト
1419     // 左クリップ平面は原点を通るのでN.d = 0で、y軸に平行なので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     // 右クリップ平面とテスト
1426     // 右クリップ平面は原点を通るのでN.d = 0で、y軸に平行なので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     // 球の中心のY座標をビュー座標系に変換する
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     // 上クリップ平面とテスト
1439     // 上クリップ平面は原点を通るのでN.d = 0で、x軸に平行なので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     // 下クリップ平面とテスト
1446     // 下クリップ平面は原点を通るのでN.d = 0で、x軸に平行なので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     // 球の一部または全部がフラスタム内にある
1453     return true;
1454 }
1455 
1456 bool
IntersectAABB(const AABB * B) const1457 FRUSTUM::IntersectAABB(const AABB* B) const
1458 {
1459     // フラスタムを包むAABBでフィルタリング
1460     if (!IntersectionAABB(B, &box))
1461         return false;
1462 
1463     int i;
1464     VEC3 p;
1465     for (i = 0; i < 6; ++i)
1466     {
1467         // フラスタムの各面に対して
1468         // Test: ax + by + cz + dを最小にする点を決める。
1469         // これはa,b,cの符号を見ることでで決めることができる。
1470         // Testが正ならAABBの全ての点はフラスタムの外ということになる。
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
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     // フラスタムを包むAABBでフィルタリング
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
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 // 領域のマージ
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でも問題ない
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