AABB.cpp

1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file AABB.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Axis-Aligned Bounding Box (AABB) geometry object. */
18 #include "AABB.h"
19 #include "../Math/MathFunc.h"
20 #ifdef MATH_ENABLE_STL_SUPPORT
21 #include <iostream>
22 #include <utility>
23 #endif
24 #include "Frustum.h"
25 #include "LineSegment.h"
26 #include "Line.h"
27 #include "Ray.h"
28 #include "../Algorithm/Random/LCG.h"
29 #include "OBB.h"
30 #include "Plane.h"
31 #include "Polygon.h"
32 #include "Polyhedron.h"
33 #include "Sphere.h"
34 #include "PBVolume.h"
35 #include "../Math/float2.h"
36 #include "../Math/float3x3.h"
37 #include "../Math/float3x4.h"
38 #include "../Math/float4.h"
39 #include "../Math/float4x4.h"
40 #include "../Math/Quat.h"
41 #include "Triangle.h"
42 #include "Capsule.h"
43
44 #include "../Math/float4x4_neon.h"
45
46 #ifdef MATH_GRAPHICSENGINE_INTEROP
47 #include "VertexBuffer.h"
48 #endif
49
50 MATH_BEGIN_NAMESPACE
51
52 AABB::AABB(const vec &minPoint_, const vec &maxPoint_)
53 :minPoint(minPoint_), maxPoint(maxPoint_)
54 {
55 }
56
57 AABB::AABB(const OBB &obb)
58 {
59         SetFrom(obb);
60 }
61
62 AABB::AABB(const Sphere &s)
63 {
64         SetFrom(s);
65 }
66
67 void AABB::SetNegativeInfinity()
68 {
69         minPoint.SetFromScalar(FLOAT_INF);
70         maxPoint.SetFromScalar(-FLOAT_INF);
71 }
72
73 void AABB::SetFromCenterAndSize(const vec &center, const vec &size)
74 {
75         vec halfSize = 0.5f * size;
76         minPoint = center - halfSize;
77         maxPoint = center + halfSize;
78 }
79
80 void AABB::SetFrom(const OBB &obb)
81 {
82         vec halfSize = Abs(obb.axis[0]*obb.r[0]) + Abs(obb.axis[1]*obb.r[1]) + Abs(obb.axis[2]*obb.r[2]);
83         SetFromCenterAndSize(obb.pos, 2.f*halfSize);
84 }
85
86 void AABB::SetFrom(const Sphere &s)
87 {
88         vec d = DIR_VEC_SCALAR(s.r);
89         minPoint = s.pos - d;
90         maxPoint = s.pos + d;
91 }
92
93 void AABB::SetFrom(const vec *pointArray, int numPoints)
94 {
95         assume(pointArray || numPoints == 0);
96         SetNegativeInfinity();
97         if (!pointArray)
98                 return;
99         for(int i = 0; i < numPoints; ++i)
100                 Enclose(pointArray[i]);
101 }
102
103 PBVolume<6> AABB::ToPBVolume() const
104 {
105         PBVolume<6> pbVolume;
106         for(int i = 0; i < 6; ++i)
107                 pbVolume.p[i] = FacePlane(i);
108
109         return pbVolume;
110 }
111
112 Polyhedron AABB::ToPolyhedron() const
113 {
114         // Note to maintainer: This function is an exact copy of OBB:ToPolyhedron() and Frustum::ToPolyhedron().
115         Polyhedron p;
116         // Populate the corners of this AABB.
117         // The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
118         for(int i = 0; i < 8; ++i)
119                 p.v.push_back(CornerPoint(i));
120
121         // Generate the 6 faces of this AABB.
122         const int faces[6][4] =
123         {
124                 { 0, 1, 3, 2 }, // X-
125                 { 4, 6, 7, 5 }, // X+
126                 { 0, 4, 5, 1 }, // Y-
127                 { 7, 6, 2, 3 }, // Y+
128                 { 0, 2, 6, 4 }, // Z-
129                 { 1, 5, 7, 3 }, // Z+
130         };
131
132         for(int f = 0; f < 6; ++f)
133         {
134                 Polyhedron::Face face;
135                 for(int v = 0; v < 4; ++v)
136                         face.v.push_back(faces[f][v]);
137                 p.f.push_back(face);
138         }
139
140         assume(p.IsClosed());
141         assume(p.IsConvex());
142         assume(p.EulerFormulaHolds());
143         assume(p.FaceIndicesValid());
144         assume(p.FacesAreNondegeneratePlanar());
145         assume(p.Contains(this->CenterPoint()));
146         return p;
147 }
148
149 OBB AABB::ToOBB() const
150 {
151         return OBB(*this);
152 }
153
154 Sphere AABB::MinimalEnclosingSphere() const
155 {
156         return Sphere(CenterPoint(), Size().Length() * 0.5f);
157 }
158
159 Sphere AABB::MaximalContainedSphere() const
160 {
161         vec halfSize = HalfSize();
162         return Sphere(CenterPoint(), Min(halfSize.x, halfSize.y, halfSize.z));
163 }
164
165 bool AABB::IsFinite() const
166 {
167         return minPoint.IsFinite() && maxPoint.IsFinite();
168 }
169
170 bool AABB::IsDegenerate() const
171 {
172 #ifdef _MSC_VER
173         // MSVC generates code that assumes nans can't be present - add an explicit check for that case.
174         return IsNan(minPoint.x) || IsNan(minPoint.y) || IsNan(minPoint.z) ||
175                 IsNan(maxPoint.x) || IsNan(maxPoint.y) || IsNan(maxPoint.z) ||
176                 !(minPoint.x < maxPoint.x && minPoint.y < maxPoint.y && minPoint.z < maxPoint.z);
177 #else
178         return !(minPoint.x < maxPoint.x && minPoint.y < maxPoint.y && minPoint.z < maxPoint.z);
179 #endif
180 }
181
182 vec AABB::CenterPoint() const
183 {
184         return (minPoint + maxPoint) * 0.5f;
185 }
186
187 vec AABB::PointInside(float x, float y, float z) const
188 {
189         assume(0.f <= x && x <= 1.f);
190         assume(0.f <= y && y <= 1.f);
191         assume(0.f <= z && z <= 1.f);
192
193         vec d = maxPoint - minPoint;
194         return minPoint + d.Mul(POINT_VEC(x, y, z));
195 }
196
197 LineSegment AABB::Edge(int edgeIndex) const
198 {
199         assume(0 <= edgeIndex && edgeIndex <= 11);
200         switch(edgeIndex)
201         {
202                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
203                 /* For documentation, here's the segments that are returned:
204                 case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
205                 case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
206                 case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
207                 case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
208                 case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
209                 case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
210                 case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
211                 case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
212                 case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
213                 case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
214                 case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
215                 case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
216                 */
217                 // Force-optimize to avoid calling to CornerPoint for another switch-case statement.
218                 case 0: return LineSegment(minPoint, POINT_VEC(minPoint.x, minPoint.y, maxPoint.z));
219                 case 1: return LineSegment(minPoint, POINT_VEC(minPoint.x, maxPoint.y, minPoint.z));
220                 case 2: return LineSegment(minPoint, POINT_VEC(maxPoint.x, minPoint.y, minPoint.z));
221                 case 3: return LineSegment(POINT_VEC(minPoint.x, minPoint.y, maxPoint.z), POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z));
222                 case 4: return LineSegment(POINT_VEC(minPoint.x, minPoint.y, maxPoint.z), POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z));
223                 case 5: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, minPoint.z), POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z));
224                 case 6: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, minPoint.z), POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z));
225                 case 7: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z), maxPoint);
226                 case 8: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, minPoint.z), POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z));
227                 case 9: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, minPoint.z), POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z));
228                 case 10: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z), maxPoint);
229                 case 11: return LineSegment(POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z), maxPoint);
230         }
231 }
232
233 vec AABB::CornerPoint(int cornerIndex) const
234 {
235         assume(0 <= cornerIndex && cornerIndex <= 7);
236         switch(cornerIndex)
237         {
238                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
239                 case 0: return minPoint;
240                 case 1: return POINT_VEC(minPoint.x, minPoint.y, maxPoint.z);
241                 case 2: return POINT_VEC(minPoint.x, maxPoint.y, minPoint.z);
242                 case 3: return POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z);
243                 case 4: return POINT_VEC(maxPoint.x, minPoint.y, minPoint.z);
244                 case 5: return POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z);
245                 case 6: return POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z);
246                 case 7: return maxPoint;
247         }
248 }
249
250 vec AABB::ExtremePoint(const vec &direction) const
251 {
252         return POINT_VEC((direction.x >= 0.f ? maxPoint.x : minPoint.x),
253                          (direction.y >= 0.f ? maxPoint.y : minPoint.y),
254                          (direction.z >= 0.f ? maxPoint.z : minPoint.z));
255 }
256
257 vec AABB::ExtremePoint(const vec &direction, float &projectionDistance) const
258 {
259         vec extremePoint = ExtremePoint(direction);
260         projectionDistance = extremePoint.Dot(direction);
261         return extremePoint;
262 }
263
264 vec AABB::PointOnEdge(int edgeIndex, float u) const
265 {
266         assume(0 <= edgeIndex && edgeIndex <= 11);
267         assume(0 <= u && u <= 1.f);
268
269         vec d = maxPoint - minPoint;
270         switch(edgeIndex)
271         {
272         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
273         case 0: return POINT_VEC(minPoint.x, minPoint.y, minPoint.z + u * d.z);
274         case 1: return POINT_VEC(minPoint.x, maxPoint.y, minPoint.z + u * d.z);
275         case 2: return POINT_VEC(maxPoint.x, minPoint.y, minPoint.z + u * d.z);
276         case 3: return POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z + u * d.z);
277
278         case 4: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, minPoint.z);
279         case 5: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, minPoint.z);
280         case 6: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, maxPoint.z);
281         case 7: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, maxPoint.z);
282
283         case 8: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, minPoint.z);
284         case 9: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, maxPoint.z);
285         case 10: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, minPoint.z);
286         case 11: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, maxPoint.z);
287         }
288 }
289
290 vec AABB::FaceCenterPoint(int faceIndex) const
291 {
292         assume(0 <= faceIndex && faceIndex <= 5);
293
294         vec center = (minPoint + maxPoint) * 0.5f;
295         switch(faceIndex)
296         {
297         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
298         case 0: return POINT_VEC(minPoint.x, center.y, center.z);
299         case 1: return POINT_VEC(maxPoint.x, center.y, center.z);
300         case 2: return POINT_VEC(center.x, minPoint.y, center.z);
301         case 3: return POINT_VEC(center.x, maxPoint.y, center.z);
302         case 4: return POINT_VEC(center.x, center.y, minPoint.z);
303         case 5: return POINT_VEC(center.x, center.y, maxPoint.z);
304         }
305 }
306
307 vec AABB::FacePoint(int faceIndex, float u, float v) const
308 {
309         assume(0 <= faceIndex && faceIndex <= 5);
310         assume(0 <= u && u <= 1.f);
311         assume(0 <= v && v <= 1.f);
312
313         vec d = maxPoint - minPoint;
314         switch(faceIndex)
315         {
316         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
317         case 0: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, minPoint.z + v * d.z);
318         case 1: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, minPoint.z + v * d.z);
319         case 2: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, minPoint.z + v * d.z);
320         case 3: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, minPoint.z + v * d.z);
321         case 4: return POINT_VEC(minPoint.x + u * d.x, minPoint.y + v * d.y, minPoint.z);
322         case 5: return POINT_VEC(minPoint.x + u * d.x, minPoint.y + v * d.y, maxPoint.z);
323         }
324 }
325
326 vec AABB::FaceNormal(int faceIndex) const
327 {
328         assume(0 <= faceIndex && faceIndex <= 5);
329         switch(faceIndex)
330         {
331         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
332         case 0: return DIR_VEC(-1,  0,  0);
333         case 1: return DIR_VEC( 1,  0,  0);
334         case 2: return DIR_VEC( 0, -1,  0);
335         case 3: return DIR_VEC( 0,  1,  0);
336         case 4: return DIR_VEC( 0,  0, -1);
337         case 5: return DIR_VEC( 0,  0,  1);
338         }
339 }
340
341 Plane AABB::FacePlane(int faceIndex) const
342 {
343         assume(0 <= faceIndex && faceIndex <= 5);
344         return Plane(FaceCenterPoint(faceIndex), FaceNormal(faceIndex));
345 }
346
347 void AABB::GetCornerPoints(vec *outPointArray) const
348 {
349         assume(outPointArray);
350 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
351         if (!outPointArray)
352                 return;
353 #endif
354         for(int i = 0; i < 8; ++i)
355                 outPointArray[i] = CornerPoint(i);
356 }
357
358 void AABB::GetFacePlanes(Plane *outPlaneArray) const
359 {
360         assume(outPlaneArray);
361 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
362         if (!outPlaneArray)
363                 return;
364 #endif
365         for(int i = 0; i < 6; ++i)
366                 outPlaneArray[i] = FacePlane(i);
367 }
368
369 AABB AABB::MinimalEnclosingAABB(const vec *pointArray, int numPoints)
370 {
371         AABB aabb;
372         aabb.SetFrom(pointArray, numPoints);
373         return aabb;
374 }
375
376 void AABB::ExtremePointsAlongAABB(const vec *pts, int numPoints, int &minx, int &maxx, int &miny, int &maxy, int &minz, int &maxz)
377 {
378         assume(pts || numPoints == 0);
379         if (!pts)
380                 return;
381         minx = maxx = miny = maxy = minz = maxz = 0;
382         for(int i = 1; i < numPoints; ++i)
383         {
384                 if (pts[i].x < pts[minx].x) minx = i;
385                 if (pts[i].x > pts[maxx].x) maxx = i;
386                 if (pts[i].y < pts[miny].y) miny = i;
387                 if (pts[i].y > pts[maxy].y) maxy = i;
388                 if (pts[i].z < pts[minz].z) minz = i;
389                 if (pts[i].z > pts[maxz].z) maxz = i;
390         }
391 }
392
393 AABB AABB::FromCenterAndSize(const vec &aabbCenterPos, const vec &aabbSize)
394 {
395         vec halfSize = aabbSize * 0.5f;
396         return AABB(aabbCenterPos - halfSize, aabbCenterPos + halfSize);
397 }
398
399 vec AABB::Size() const
400 {
401         return maxPoint - minPoint;
402 }
403
404 vec AABB::HalfSize() const
405 {
406         return Size() * 0.5f;
407 }
408
409 float AABB::Volume() const
410 {
411         return Size().ProductOfElements();
412 }
413
414 float AABB::SurfaceArea() const
415 {
416         vec size = Size();
417         return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
418 }
419
420 vec AABB::RandomPointInside(LCG &rng) const
421 {
422         float f1 = rng.Float();
423         float f2 = rng.Float();
424         float f3 = rng.Float();
425         return PointInside(f1, f2, f3);
426 }
427
428 vec AABB::RandomPointOnSurface(LCG &rng) const
429 {
430         int i = rng.Int(0, 5);
431         float f1 = rng.Float();
432         float f2 = rng.Float();
433         return FacePoint(i, f1, f2);
434 }
435
436 vec AABB::RandomPointOnEdge(LCG &rng) const
437 {
438         int i = rng.Int(0, 11);
439         float f = rng.Float();
440         return PointOnEdge(i, f);
441 }
442
443 vec AABB::RandomCornerPoint(LCG &rng) const
444 {
445         return CornerPoint(rng.Int(0, 7));
446 }
447
448 void AABB::Translate(const vec &offset)
449 {
450         minPoint += offset;
451         maxPoint += offset;
452 }
453
454 void AABB::Scale(const vec &centerPoint, float scaleFactor)
455 {
456         minPoint = (minPoint - centerPoint) * scaleFactor + centerPoint;
457         maxPoint = (maxPoint - centerPoint) * scaleFactor + centerPoint;
458 }
459
460 void AABB::Scale(const vec &centerPoint, const vec &scaleFactor)
461 {
462         float3x4 transform = float3x4::Scale(DIR_TO_FLOAT3(scaleFactor), POINT_TO_FLOAT3(centerPoint)); ///\todo mat
463         minPoint = POINT_VEC(transform.MulPos(POINT_TO_FLOAT3(minPoint))); ///\todo mat
464         maxPoint = POINT_VEC(transform.MulPos(POINT_TO_FLOAT3(maxPoint))); ///\todo mat
465 }
466
467 /// See Christer Ericson's Real-time Collision Detection, p. 87, or
468 /// James Arvo's "Transforming Axis-aligned Bounding Boxes" in Graphics Gems 1, pp. 548-550.
469 /// http://www.graphicsgems.org/
470 template<typename Matrix>
471 void AABBTransformAsAABB(AABB &aabb, Matrix &m)
472 {
473         const vec centerPoint = (aabb.minPoint + aabb.maxPoint) * 0.5f;
474         const vec halfSize = centerPoint - aabb.minPoint;
475         vec newCenter = m.MulPos(centerPoint);
476
477         // The following is equal to taking the absolute value of the whole matrix m.
478         vec newDir = DIR_VEC(ABSDOT3(m[0], halfSize), ABSDOT3(m[1], halfSize), ABSDOT3(m[2], halfSize));
479         aabb.minPoint = newCenter - newDir;
480         aabb.maxPoint = newCenter + newDir;
481 }
482
483 #ifdef MATH_SIMD
484 void AABBTransformAsAABB_SIMD(AABB &aabb, const float4x4 &m)
485 {
486         simd4f minPt = aabb.minPoint;
487         simd4f maxPt = aabb.maxPoint;
488         simd4f centerPoint = mul_ps(add_ps(minPt, maxPt), set1_ps(0.5f));
489         simd4f halfSize = sub_ps(centerPoint, minPt);
490         simd4f newCenter = mat4x4_mul_vec4(m.row, centerPoint);
491
492         simd4f x = abs_ps(mul_ps(m.row[0], halfSize));
493         simd4f y = abs_ps(mul_ps(m.row[1], halfSize));
494         simd4f z = abs_ps(mul_ps(m.row[2], halfSize));
495         simd4f w = zero_ps();
496         _MM_TRANSPOSE4_PS(x, y, z, w); // Contains 2x unpacklo's, 2x unpackhi's, 2x movelh's and 2x movehl's. (or 8 shuffles, depending on the compiler)
497
498         simd4f newDir = add_ps(add_ps(x, y), add_ps(z, w));
499
500         aabb.minPoint = sub_ps(newCenter, newDir);
501         aabb.maxPoint = add_ps(newCenter, newDir);
502 }
503 #endif
504
505 void AABB::TransformAsAABB(const float3x3 &transform)
506 {
507         assume(transform.IsColOrthogonal());
508         assume(transform.HasUniformScale());
509
510         AABBTransformAsAABB(*this, transform);
511 }
512
513 void AABB::TransformAsAABB(const float3x4 &transform)
514 {
515         assume(transform.IsColOrthogonal());
516         assume(transform.HasUniformScale());
517
518         AABBTransformAsAABB(*this, transform);
519 }
520
521 void AABB::TransformAsAABB(const float4x4 &transform)
522 {
523         assume(transform.IsColOrthogonal3());
524         assume(transform.HasUniformScale());
525         assume(transform.Row(3).Equals(0,0,0,1));
526
527 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
528         AABBTransformAsAABB_SIMD(*this, transform);
529 #else
530         AABBTransformAsAABB(*this, transform);
531 #endif
532 }
533
534 void AABB::TransformAsAABB(const Quat &transform)
535 {
536         vec newCenter = transform.Transform(CenterPoint());
537         vec newDir = Abs((transform.Transform(Size()) * 0.5f));
538         minPoint = newCenter - newDir;
539         maxPoint = newCenter + newDir;
540 }
541
542 OBB AABB::Transform(const float3x3 &transform) const
543 {
544         OBB obb;
545         obb.SetFrom(*this, transform);
546         return obb;
547 }
548
549 OBB AABB::Transform(const float3x4 &transform) const
550 {
551         OBB obb;
552         obb.SetFrom(*this, transform);
553         return obb;
554 }
555
556 OBB AABB::Transform(const float4x4 &transform) const
557 {
558         OBB obb;
559         obb.SetFrom(*this, transform);
560         return obb;
561 }
562
563 OBB AABB::Transform(const Quat &transform) const
564 {
565         OBB obb;
566         obb.SetFrom(*this, transform);
567         return obb;
568 }
569
570 vec AABB::ClosestPoint(const vec &targetPoint) const
571 {
572 #ifdef MATH_VEC_IS_FLOAT4
573         assume(EqualAbs(minPoint.w, 1.f));
574         assume(EqualAbs(maxPoint.w, 1.f));
575         assume(EqualAbs(targetPoint.w, 1.f));
576 #endif
577         return targetPoint.Clamp(minPoint, maxPoint);
578 }
579
580 float AABB::Distance(const vec &point) const
581 {
582         ///@todo This function could be slightly optimized. See Christer Ericson's
583         /// Real-Time Collision Detection, p.131.
584         return ClosestPoint(point).Distance(point);
585 }
586
587 float AABB::Distance(const Sphere &sphere) const
588 {
589         return Max(0.f, Distance(sphere.pos) - sphere.r);
590 }
591
592 bool AABB::Contains(const vec &point) const
593 {
594 // Benchmarking this code is very difficult, since branch prediction makes the scalar version
595 // look very good. In isolation the scalar version might be better, however when joined with
596 // other SSE computation, the SIMD variants are probably more efficient because the data is
597 // already "hot" in the registers. Therefore favoring the SSE version over the scalar version
598 // when possible.
599
600 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
601         // Benchmark 'AABBContains_positive': AABB::Contains(point) positive
602         //    Best: 2.048 nsecs / 3.5128 ticks, Avg: 2.241 nsecs, Worst: 4.277 nsecs
603         // Benchmark 'AABBContains_negative': AABB::Contains(point) negative
604         //    Best: 2.048 nsecs / 3.467 ticks, Avg: 2.115 nsecs, Worst: 4.156 nsecs
605         // Benchmark 'AABBContains_unpredictable': AABB::Contains(point) unpredictable
606         //    Best: 2.590 nsecs / 4.4106 ticks, Avg: 2.978 nsecs, Worst: 6.084 nsecs
607         simd4f a = cmplt_ps(point, minPoint);
608         simd4f b = cmpgt_ps(point, maxPoint);
609         a = or_ps(a, b);
610         return allzero_ps(a) != 0;
611 #else
612         // Benchmark 'AABBContains_positive': AABB::Contains(point) positive
613         //    Best: 2.108 nsecs / 3.6022 ticks, Avg: 2.232 nsecs, Worst: 4.638 nsecs
614         // Benchmark 'AABBContains_negative': AABB::Contains(point) negative
615         //    Best: 1.988 nsecs / 3.361 ticks, Avg: 2.148 nsecs, Worst: 4.457 nsecs
616         // Benchmark 'AABBContains_unpredictable': AABB::Contains(point) unpredictable
617         //    Best: 3.554 nsecs / 6.0764 ticks, Avg: 3.803 nsecs, Worst: 6.264 nsecs
618         return minPoint.x <= point.x && point.x <= maxPoint.x &&
619                minPoint.y <= point.y && point.y <= maxPoint.y &&
620                minPoint.z <= point.z && point.z <= maxPoint.z;
621 #endif
622 }
623
624 bool AABB::Contains(const LineSegment &lineSegment) const
625 {
626         return Contains(Min(lineSegment.a, lineSegment.b), Max(lineSegment.a, lineSegment.b));
627 }
628
629 bool AABB::Contains(const vec &aabbMinPoint, const vec &aabbMaxPoint) const
630 {
631 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
632         simd4f a = cmplt_ps(aabbMinPoint, minPoint);
633         simd4f b = cmpgt_ps(aabbMaxPoint, maxPoint);
634         a = or_ps(a, b);
635         return allzero_ps(a) != 0;
636 #else
637         return minPoint.x <= aabbMinPoint.x && maxPoint.x >= aabbMaxPoint.x &&
638                minPoint.y <= aabbMinPoint.y && maxPoint.y >= aabbMaxPoint.y &&
639                minPoint.z <= aabbMinPoint.z && maxPoint.z >= aabbMaxPoint.z;
640 #endif
641 }
642
643 bool AABB::Contains(const OBB &obb) const
644 {
645         return Contains(obb.MinimalEnclosingAABB());
646 }
647
648 bool AABB::Contains(const Sphere &sphere) const
649 {
650         return Contains(sphere.pos - DIR_VEC_SCALAR(sphere.r), sphere.pos + DIR_VEC_SCALAR(sphere.r));
651 }
652
653 bool AABB::Contains(const Capsule &capsule) const
654 {
655         return Contains(capsule.MinimalEnclosingAABB());
656 }
657
658 bool AABB::Contains(const Triangle &triangle) const
659 {
660         return Contains(triangle.BoundingAABB());
661 }
662
663 bool AABB::Contains(const Polygon &polygon) const
664 {
665         return Contains(polygon.MinimalEnclosingAABB());
666 }
667
668 bool AABB::Contains(const Frustum &frustum) const
669 {
670         return Contains(frustum.MinimalEnclosingAABB());
671 }
672
673 bool AABB::Contains(const Polyhedron &polyhedron) const
674 {
675         return Contains(polyhedron.MinimalEnclosingAABB());
676 }
677
678 bool AABB::IntersectLineAABB(const vec &linePos, const vec &lineDir, float &tNear, float &tFar) const
679 {
680         // Never call the SSE version here. The SSE version does not output tNear and tFar, because
681         // the memory stores have been profiled to make it slower than the CPP version. Therefore the SSE
682         // version does not output tNear and tFar (profile shows it to be about 10x faster than the CPP version).
683         return IntersectLineAABB_CPP(linePos, lineDir, tNear, tFar);
684 }
685
686 bool AABB::Intersects(const Line &line) const
687 {
688         float tNear = -FLOAT_INF;
689         float tFar = FLOAT_INF;
690
691 #ifdef MATH_SSE
692         return IntersectLineAABB_SSE(line.pos, line.dir, tNear, tFar);
693 #else
694         return IntersectLineAABB_CPP(line.pos, line.dir, tNear, tFar);
695 #endif
696 }
697
698 bool AABB::Intersects(const Ray &ray) const
699 {
700         float tNear = 0;
701         float tFar = FLOAT_INF;
702
703 #ifdef MATH_SSE
704         return IntersectLineAABB_SSE(ray.pos, ray.dir, tNear, tFar);
705 #else
706         return IntersectLineAABB_CPP(ray.pos, ray.dir, tNear, tFar);
707 #endif
708 }
709
710 bool AABB::Intersects(const LineSegment &lineSegment) const
711 {
712         vec dir = lineSegment.b - lineSegment.a;
713         float len = dir.Length();
714         if (len <= 1e-4f) // Degenerate line segment? Fall back to point-in-AABB test.
715                 return Contains(lineSegment.a);
716
717         float invLen = 1.f / len;
718         dir *= invLen;
719         float tNear = 0.f, tFar = len;
720 #ifdef MATH_SSE
721         return IntersectLineAABB_SSE(lineSegment.a, dir, tNear, tFar);
722 #else
723         return IntersectLineAABB_CPP(lineSegment.a, dir, tNear, tFar);
724 #endif
725 }
726
727 bool AABB::IntersectLineAABB_CPP(const vec &linePos, const vec &lineDir, float &tNear, float &tFar) const
728 {
729         assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq());
730         assume2(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!", tNear, tFar);
731         // The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
732         // for this intersection test.
733         // For a Line-AABB test, pass in
734         //    tNear = -FLOAT_INF;
735         //    tFar = FLOAT_INF;
736         // For a Ray-AABB test, pass in
737         //    tNear = 0.f;
738         //    tFar = FLOAT_INF;
739         // For a LineSegment-AABB test, pass in
740         //    tNear = 0.f;
741         //    tFar = LineSegment.Length();
742
743         // Test each cardinal plane (X, Y and Z) in turn.
744         if (!EqualAbs(lineDir.x, 0.f))
745         {
746                 float recipDir = RecipFast(lineDir.x);
747                 float t1 = (minPoint.x - linePos.x) * recipDir;
748                 float t2 = (maxPoint.x - linePos.x) * recipDir;
749
750                 // tNear tracks distance to intersect (enter) the AABB.
751                 // tFar tracks the distance to exit the AABB.
752                 if (t1 < t2)
753                         tNear = Max(t1, tNear), tFar = Min(t2, tFar);
754                 else // Swap t1 and t2.
755                         tNear = Max(t2, tNear), tFar = Min(t1, tFar);
756
757                 if (tNear > tFar)
758                         return false; // Box is missed since we "exit" before entering it.
759         }
760         else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
761                 return false; // The ray can't possibly enter the box, abort.
762
763         if (!EqualAbs(lineDir.y, 0.f))
764         {
765                 float recipDir = RecipFast(lineDir.y);
766                 float t1 = (minPoint.y - linePos.y) * recipDir;
767                 float t2 = (maxPoint.y - linePos.y) * recipDir;
768
769                 if (t1 < t2)
770                         tNear = Max(t1, tNear), tFar = Min(t2, tFar);
771                 else // Swap t1 and t2.
772                         tNear = Max(t2, tNear), tFar = Min(t1, tFar);
773
774                 if (tNear > tFar)
775                         return false; // Box is missed since we "exit" before entering it.
776         }
777         else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
778                 return false; // The ray can't possibly enter the box, abort.
779
780         if (!EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
781         {
782                 float recipDir = RecipFast(lineDir.z);
783                 float t1 = (minPoint.z - linePos.z) * recipDir;
784                 float t2 = (maxPoint.z - linePos.z) * recipDir;
785
786                 if (t1 < t2)
787                         tNear = Max(t1, tNear), tFar = Min(t2, tFar);
788                 else // Swap t1 and t2.
789                         tNear = Max(t2, tNear), tFar = Min(t1, tFar);
790         }
791         else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
792                 return false; // The ray can't possibly enter the box, abort.
793
794         return tNear <= tFar;
795 }
796
797 #ifdef MATH_SSE
798 bool AABB::IntersectLineAABB_SSE(const float4 &rayPos, const float4 &rayDir, float tNear, float tFar) const
799 {
800         assume(rayDir.IsNormalized4());
801         assume(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!");
802         /* For reference, this is the C++ form of the vectorized SSE code below.
803
804         float4 recipDir = rayDir.RecipFast4();
805         float4 t1 = (aabbMinPoint - rayPos).Mul(recipDir);
806         float4 t2 = (aabbMaxPoint - rayPos).Mul(recipDir);
807         float4 near = t1.Min(t2);
808         float4 far = t1.Max(t2);
809         float4 rayDirAbs = rayDir.Abs();
810
811         if (rayDirAbs.x > 1e-4f) // ray is parallel to plane in question
812         {
813                 tNear = Max(near.x, tNear); // tNear tracks distance to intersect (enter) the AABB.
814                 tFar = Min(far.x, tFar); // tFar tracks the distance to exit the AABB.
815         }
816         else if (rayPos.x < aabbMinPoint.x || rayPos.x > aabbMaxPoint.x) // early-out if the ray can't possibly enter the box.
817                 return false;
818
819         if (rayDirAbs.y > 1e-4f) // ray is parallel to plane in question
820         {
821                 tNear = Max(near.y, tNear); // tNear tracks distance to intersect (enter) the AABB.
822                 tFar = Min(far.y, tFar); // tFar tracks the distance to exit the AABB.
823         }
824         else if (rayPos.y < aabbMinPoint.y || rayPos.y > aabbMaxPoint.y) // early-out if the ray can't possibly enter the box.
825                 return false;
826
827         if (rayDirAbs.z > 1e-4f) // ray is parallel to plane in question
828         {
829                 tNear = Max(near.z, tNear); // tNear tracks distance to intersect (enter) the AABB.
830                 tFar = Min(far.z, tFar); // tFar tracks the distance to exit the AABB.
831         }
832         else if (rayPos.z < aabbMinPoint.z || rayPos.z > aabbMaxPoint.z) // early-out if the ray can't possibly enter the box.
833                 return false;
834
835         return tNear < tFar;
836         */
837
838         simd4f recipDir = rcp_ps(rayDir.v);
839         // Note: The above performs an approximate reciprocal (11 bits of precision).
840         // For a full precision reciprocal, perform a div:
841 //      simd4f recipDir = div_ps(set1_ps(1.f), rayDir.v);
842
843         simd4f t1 = mul_ps(sub_ps(minPoint, rayPos.v), recipDir);
844         simd4f t2 = mul_ps(sub_ps(maxPoint, rayPos.v), recipDir);
845
846         simd4f nearD = min_ps(t1, t2); // [0 n3 n2 n1]
847         simd4f farD = max_ps(t1, t2);  // [0 f3 f2 f1]
848
849         // Check if the ray direction is parallel to any of the cardinal axes, and if so,
850         // mask those [near, far] ranges away from the hit test computations.
851         simd4f rayDirAbs = abs_ps(rayDir.v);
852
853         const simd4f epsilon = set1_ps(1e-4f);
854         // zeroDirections[i] will be nonzero for each axis i the ray is parallel to.
855         simd4f zeroDirections = cmple_ps(rayDirAbs, epsilon);
856
857         const simd4f floatInf = set1_ps(FLOAT_INF);
858         const simd4f floatNegInf = set1_ps(-FLOAT_INF);
859
860         // If the ray is parallel to one of the axes, replace the slab range for that axis
861         // with [-inf, inf] range instead. (which is a no-op in the comparisons below)
862         nearD = cmov_ps(nearD, floatNegInf, zeroDirections);
863         farD = cmov_ps(farD , floatInf, zeroDirections);
864
865         // Next, we need to compute horizontally max(nearD[0], nearD[1], nearD[2]) and min(farD[0], farD[1], farD[2])
866         // to see if there is an overlap in the hit ranges.
867         simd4f v1 = _mm_shuffle_ps(nearD, farD, _MM_SHUFFLE(0, 0, 0, 0)); // [f1 f1 n1 n1]
868         simd4f v2 = _mm_shuffle_ps(nearD, farD, _MM_SHUFFLE(1, 1, 1, 1)); // [f2 f2 n2 n2]
869         simd4f v3 = _mm_shuffle_ps(nearD, farD, _MM_SHUFFLE(2, 2, 2, 2)); // [f3 f3 n3 n3]
870         nearD = max_ps(v1, max_ps(v2, v3));
871         farD = min_ps(v1, min_ps(v2, v3));
872         farD = wwww_ps(farD); // Unpack the result from high offset in the register.
873         nearD = max_ps(nearD, setx_ps(tNear));
874         farD = min_ps(farD, setx_ps(tFar));
875
876         // Finally, test if the ranges overlap.
877         simd4f rangeIntersects = _mm_cmple_ss(nearD, farD);
878
879         // To store out out the interval of intersection, uncomment the following:
880         // These are disabled, since without these, the whole function runs without a single memory store,
881         // which has been profiled to be very fast! Uncommenting these causes an order-of-magnitude slowdown.
882         // For now, using the SSE version only where the tNear and tFar ranges are not interesting.
883 //      _mm_store_ss(&tNear, nearD);
884 //      _mm_store_ss(&tFar, farD);
885
886         // To avoid false positives, need to have an additional rejection test for each cardinal axis the ray direction
887         // is parallel to.
888         simd4f out2 = cmplt_ps(rayPos.v, minPoint);
889         simd4f out3 = cmpgt_ps(rayPos.v, maxPoint);
890         out2 = or_ps(out2, out3);
891         zeroDirections = and_ps(zeroDirections, out2);
892
893         simd4f yOut = yyyy_ps(zeroDirections);
894         simd4f zOut = zzzz_ps(zeroDirections);
895
896         zeroDirections = or_ps(or_ps(zeroDirections, yOut), zOut);
897         // Intersection occurs if the slab ranges had positive overlap and if the test was not rejected by the ray being
898         // parallel to some cardinal axis.
899         simd4f intersects = andnot_ps(zeroDirections, rangeIntersects);
900         simd4f epsilonMasked = and_ps(epsilon, intersects);
901         return _mm_comieq_ss(epsilon, epsilonMasked) != 0;
902 }
903 #endif
904
905 bool AABB::Intersects(const Ray &ray, float &dNear, float &dFar) const
906 {
907         dNear = 0.f;
908         dFar = FLOAT_INF;
909         return IntersectLineAABB(ray.pos, ray.dir, dNear, dFar);
910 }
911
912 bool AABB::Intersects(const Line &line, float &dNear, float &dFar) const
913 {
914         dNear = -FLOAT_INF;
915         dFar = FLOAT_INF;
916         return IntersectLineAABB(line.pos, line.dir, dNear, dFar);
917 }
918
919 bool AABB::Intersects(const LineSegment &lineSegment, float &dNear, float &dFar) const
920 {
921         vec dir = lineSegment.b - lineSegment.a;
922         float len = dir.Length();
923         if (len <= 1e-4f) // Degenerate line segment? Fall back to point-in-AABB test.
924         {
925                 dNear = 0.f;
926                 dFar = 1.f;
927                 return Contains(lineSegment.a);
928         }
929         float invLen = 1.f / len;
930         dir *= invLen;
931         dNear = 0.f;
932         dFar = len;
933         bool hit = IntersectLineAABB(lineSegment.a, dir, dNear, dFar);
934         dNear *= invLen;
935         dFar *= invLen;
936         return hit;
937 }
938
939 bool AABB::Intersects(const Plane &plane) const
940 {
941         return plane.Intersects(*this);
942 }
943
944 bool AABB::Intersects(const AABB &aabb) const
945 {
946 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE41)
947         // Benchmark 'AABBIntersectsAABB_positive': AABB::Intersects(AABB) positive
948         //    Best: 2.229 nsecs / 3.848 ticks, Avg: 2.409 nsecs, Worst: 4.457 nsecs
949         // Benchmark 'AABBIntersectsAABB_random': AABB::Intersects(AABB) random
950         //    Best: 3.072 nsecs / 5.2904 ticks, Avg: 3.262 nsecs, Worst: 5.301 nsecs
951
952         simd4f a = cmpge_ps(minPoint.v, aabb.maxPoint.v);
953         simd4f b = cmpge_ps(aabb.minPoint.v, maxPoint.v);
954         a = or_ps(a, b);
955         a = and_ps(a, set_ps_hex(0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)); // Mask off results from the W channel.
956         return _mm_testz_si128(_mm_castps_si128(a), _mm_castps_si128(a)) != 0;
957 #else
958         // Benchmark 'AABBIntersectsAABB_positive': AABB::Intersects(AABB) positive
959         //    Best: 2.108 nsecs / 3.588 ticks, Avg: 2.310 nsecs, Worst: 5.481 nsecs
960         // Benchmark 'AABBIntersectsAABB_random': AABB::Intersects(AABB) random
961         //    Best: 7.529 nsecs / 12.8282 ticks, Avg: 8.892 nsecs, Worst: 16.323 nsecs
962
963         // If any of the cardinal X,Y,Z axes is a separating axis, then
964         // there is no intersection.
965         return minPoint.x < aabb.maxPoint.x &&
966                minPoint.y < aabb.maxPoint.y &&
967                minPoint.z < aabb.maxPoint.z &&
968                aabb.minPoint.x < maxPoint.x &&
969                aabb.minPoint.y < maxPoint.y &&
970                aabb.minPoint.z < maxPoint.z;
971 #endif
972 }
973
974 bool AABB::Intersects(const OBB &obb) const
975 {
976         return obb.Intersects(*this);
977 }
978
979 bool AABB::Intersects(const Sphere &sphere, vec *closestPointOnAABB) const
980 {
981         // Find the point on this AABB closest to the sphere center.
982         vec pt = ClosestPoint(sphere.pos);
983
984         // If that point is inside sphere, the AABB and sphere intersect.
985         if (closestPointOnAABB)
986                 *closestPointOnAABB = pt;
987
988         return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
989 }
990
991 bool AABB::Intersects(const Capsule &capsule) const
992 {
993         return capsule.Intersects(*this);
994 }
995
996 bool AABB::Intersects(const Triangle &triangle) const
997 {
998         return triangle.Intersects(*this);
999 }
1000
1001 bool AABB::Intersects(const Polygon &polygon) const
1002 {
1003         return polygon.Intersects(*this);
1004 }
1005
1006 bool AABB::Intersects(const Frustum &frustum) const
1007 {
1008         return frustum.Intersects(*this);
1009 }
1010
1011 bool AABB::Intersects(const Polyhedron &polyhedron) const
1012 {
1013         return polyhedron.Intersects(*this);
1014 }
1015
1016 void AABB::ProjectToAxis(const vec &axis, float &dMin, float &dMax) const
1017 {
1018         vec c = (minPoint + maxPoint) * 0.5f;
1019         vec e = maxPoint - c;
1020
1021 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
1022         vec absAxis = axis.Abs();
1023         float r = Abs(e.Dot(absAxis));
1024 #else
1025         // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
1026         float r = Abs(e[0]*Abs(axis[0]) + e[1]*Abs(axis[1]) + e[2]*Abs(axis[2]));
1027 #endif
1028         // Compute the distance of the box center from plane.
1029         float s = axis.Dot(c);
1030         dMin = s - r;
1031         dMax = s + r;
1032 }
1033
1034 int AABB::UniqueFaceNormals(vec *out) const
1035 {
1036         out[0] = DIR_VEC(1,0,0);
1037         out[1] = DIR_VEC(0,1,0);
1038         out[2] = DIR_VEC(0,0,1);
1039         return 3;
1040 }
1041
1042 int AABB::UniqueEdgeDirections(vec *out) const
1043 {
1044         out[0] = DIR_VEC(1,0,0);
1045         out[1] = DIR_VEC(0,1,0);
1046         out[2] = DIR_VEC(0,0,1);
1047         return 3;
1048 }
1049
1050 void AABB::Enclose(const vec &point)
1051 {
1052         minPoint = Min(minPoint, point);
1053         maxPoint = Max(maxPoint, point);
1054 }
1055
1056 void AABB::Enclose(const LineSegment &lineSegment)
1057 {
1058         Enclose(Min(lineSegment.a, lineSegment.b), Max(lineSegment.a, lineSegment.b));
1059 }
1060
1061 void AABB::Enclose(const vec &aabbMinPoint, const vec &aabbMaxPoint)
1062 {
1063         minPoint = Min(minPoint, aabbMinPoint);
1064         maxPoint = Max(maxPoint, aabbMaxPoint);
1065 }
1066
1067 void AABB::Enclose(const OBB &obb)
1068 {
1069         vec absAxis0 = obb.axis[0].Abs();
1070         vec absAxis1 = obb.axis[1].Abs();
1071         vec absAxis2 = obb.axis[2].Abs();
1072         vec d = obb.r.x * absAxis0 + obb.r.y * absAxis1 + obb.r.z * absAxis2;
1073         Enclose(obb.pos - d, obb.pos + d);
1074 }
1075
1076 void AABB::Enclose(const Sphere &sphere)
1077 {
1078         vec d = DIR_VEC_SCALAR(sphere.r);
1079         Enclose(sphere.pos - d, sphere.pos + d);
1080 }
1081
1082 void AABB::Enclose(const Triangle &triangle)
1083 {
1084         Enclose(Min(triangle.a, triangle.b, triangle.c), Max(triangle.a, triangle.b, triangle.c));
1085 }
1086
1087 void AABB::Enclose(const Capsule &capsule)
1088 {
1089         vec d = DIR_VEC_SCALAR(capsule.r);
1090         minPoint = Min(minPoint, Min(capsule.l.a, capsule.l.b) - d);
1091         maxPoint = Max(maxPoint, Max(capsule.l.a, capsule.l.b) + d);
1092 }
1093
1094 void AABB::Enclose(const Frustum &frustum)
1095 {
1096         Enclose(frustum.MinimalEnclosingAABB());
1097 }
1098
1099 void AABB::Enclose(const Polygon &polygon)
1100 {
1101         Enclose(polygon.MinimalEnclosingAABB());
1102 }
1103
1104 void AABB::Enclose(const Polyhedron &polyhedron)
1105 {
1106         Enclose(polyhedron.MinimalEnclosingAABB());
1107 }
1108
1109 void AABB::Enclose(const vec *pointArray, int numPoints)
1110 {
1111         assume(pointArray || numPoints == 0);
1112         if (!pointArray)
1113                 return;
1114         for(int i = 0; i < numPoints; ++i)
1115                 Enclose(pointArray[i]);
1116 }
1117
1118 void AABB::Triangulate(int numFacesX, int numFacesY, int numFacesZ,
1119                        vec *outPos, vec *outNormal, float2 *outUV,
1120                        bool ccwIsFrontFacing) const
1121 {
1122         assume(numFacesX >= 1);
1123         assume(numFacesY >= 1);
1124         assume(numFacesZ >= 1);
1125
1126         assume(outPos);
1127         if (!outPos)
1128                 return;
1129
1130         // Generate both X-Y planes.
1131         int i = 0;
1132         for(int face = 0; face < 6; ++face) // Faces run in the order -X, +X, -Y, +Y, -Z, +Z.
1133         {
1134                 int numFacesU;
1135                 int numFacesV;
1136                 bool flip = (face == 1 || face == 2 || face == 5);
1137                 if (ccwIsFrontFacing)
1138                         flip = !flip;
1139                 if (face == 0 || face == 1)
1140                 {
1141                         numFacesU = numFacesY;
1142                         numFacesV = numFacesZ;
1143                 }
1144                 else if (face == 2 || face == 3)
1145                 {
1146                         numFacesU = numFacesX;
1147                         numFacesV = numFacesZ;
1148                 }
1149                 else// if (face == 4 || face == 5)
1150                 {
1151                         numFacesU = numFacesX;
1152                         numFacesV = numFacesY;
1153                 }
1154                 for(int x = 0; x < numFacesU; ++x)
1155                         for(int y = 0; y < numFacesV; ++y)
1156                         {
1157                                 float u = (float)x / (numFacesU);
1158                                 float v = (float)y / (numFacesV);
1159                                 float u2 = (float)(x+1) / (numFacesU);
1160                                 float v2 = (float)(y+1) / (numFacesV);
1161
1162                                 outPos[i]   = FacePoint(face, u, v);
1163                                 outPos[i+1] = FacePoint(face, u, v2);
1164                                 outPos[i+2] = FacePoint(face, u2, v);
1165                                 if (flip)
1166                                         Swap(outPos[i+1], outPos[i+2]);
1167                                 outPos[i+3] = outPos[i+2];
1168                                 outPos[i+4] = outPos[i+1];
1169                                 outPos[i+5] = FacePoint(face, u2, v2);
1170
1171                                 if (outUV)
1172                                 {
1173                                         outUV[i]   = float2(u,v);
1174                                         outUV[i+1] = float2(u,v2);
1175                                         outUV[i+2] = float2(u2,v);
1176                                         if (flip)
1177                                                 Swap(outUV[i+1], outUV[i+2]);
1178                                         outUV[i+3] = outUV[i+2];
1179                                         outUV[i+4] = outUV[i+1];
1180                                         outUV[i+5] = float2(u2,v2);
1181                                 }
1182
1183                                 if (outNormal)
1184                                         for(int j = 0; j < 6; ++j)
1185                                                 outNormal[i+j] = FaceNormal(face);
1186
1187                                 i += 6;
1188                         }
1189         }
1190         assert(i == NumVerticesInTriangulation(numFacesX, numFacesY, numFacesZ));
1191 }
1192
1193 void AABB::ToEdgeList(vec *outPos) const
1194 {
1195         assume(outPos);
1196         if (!outPos)
1197                 return;
1198         for(int i = 0; i < 12; ++i)
1199         {
1200                 LineSegment edge = Edge(i);
1201                 outPos[i*2] = edge.a;
1202                 outPos[i*2+1] = edge.b;
1203         }
1204 }
1205
1206 #ifdef MATH_ENABLE_STL_SUPPORT
1207 std::string AABB::ToString() const
1208 {
1209         char str[256];
1210         sprintf(str, "AABB(Min:(%.2f, %.2f, %.2f) Max:(%.2f, %.2f, %.2f))", minPoint.x, minPoint.y, minPoint.z, maxPoint.x, maxPoint.y, maxPoint.z);
1211         return str;
1212 }
1213
1214 std::string AABB::SerializeToString() const
1215 {
1216         char str[256];
1217         char *s = SerializeFloat(minPoint.x, str); *s = ','; ++s;
1218         s = SerializeFloat(minPoint.y, s); *s = ','; ++s;
1219         s = SerializeFloat(minPoint.z, s); *s = ','; ++s;
1220         s = SerializeFloat(maxPoint.x, s); *s = ','; ++s;
1221         s = SerializeFloat(maxPoint.y, s); *s = ','; ++s;
1222         s = SerializeFloat(maxPoint.z, s);
1223         assert(s+1 - str < 256);
1224         MARK_UNUSED(s);
1225         return str;
1226 }
1227
1228 std::string AABB::SerializeToCodeString() const
1229 {
1230         return "AABB(" + minPoint.SerializeToCodeString() + "," + maxPoint.SerializeToCodeString() + ")";
1231 }
1232
1233 AABB AABB::FromString(const char *str, const char **outEndStr)
1234 {
1235         assume(str);
1236         if (!str)
1237                 return AABB(vec::nan, vec::nan);
1238         AABB a;
1239         MATH_SKIP_WORD(str, "AABB(");
1240         MATH_SKIP_WORD(str, "Min:(");
1241         a.minPoint = PointVecFromString(str, &str);
1242         MATH_SKIP_WORD(str, " Max:(");
1243         a.maxPoint = PointVecFromString(str, &str);
1244         if (outEndStr)
1245                 *outEndStr = str;
1246         return a;
1247 }
1248
1249 std::ostream &operator <<(std::ostream &o, const AABB &aabb)
1250 {
1251         o << aabb.ToString();
1252         return o;
1253 }
1254
1255 #endif
1256
1257 AABB AABB::Intersection(const AABB &aabb) const
1258 {
1259         return AABB(Max(minPoint, aabb.minPoint), Min(maxPoint, aabb.maxPoint));
1260 }
1261
1262 #ifdef MATH_GRAPHICSENGINE_INTEROP
1263 void AABB::Triangulate(VertexBuffer &vb, int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const
1264 {
1265         Array<vec> pos;
1266         Array<vec> normal;
1267         Array<float2> uv;
1268         int numVertices = (numFacesX*numFacesY + numFacesY*numFacesZ + numFacesX*numFacesZ)*2*6;
1269         pos.Resize_pod(numVertices);
1270         normal.Resize_pod(numVertices);
1271         uv.Resize_pod(numVertices);
1272         Triangulate(numFacesX, numFacesY, numFacesZ, &pos[0], &normal[0], &uv[0], ccwIsFrontFacing);
1273         int startIndex = vb.AppendVertices(numVertices);
1274         for(int i = 0; i < (int)pos.size(); ++i)
1275         {
1276                 vb.Set(startIndex+i, VDPosition, POINT_TO_FLOAT4(pos[i]));
1277                 if (vb.Declaration()->TypeOffset(VDNormal) >= 0)
1278                         vb.Set(startIndex+i, VDNormal, DIR_TO_FLOAT4(normal[i]));
1279                 if (vb.Declaration()->TypeOffset(VDUV) >= 0)
1280                         vb.SetFloat2(startIndex+i, VDUV, 0, uv[i]);
1281         }
1282 }
1283
1284 void AABB::ToLineList(VertexBuffer &vb) const
1285 {
1286         Array<vec> pos;
1287         pos.Resize_pod(NumVerticesInEdgeList());
1288         ToEdgeList(&pos[0]);
1289         int startIndex = vb.AppendVertices((int)pos.size());
1290         for(int i = 0; i < (int)pos.size(); ++i)
1291                 vb.Set(startIndex+i, VDPosition, POINT_TO_FLOAT4(pos[i]));
1292 }
1293
1294 #endif
1295
1296 OBB operator *(const float3x3 &transform, const AABB &aabb)
1297 {
1298         return aabb.Transform(transform);
1299 }
1300
1301 OBB operator *(const float3x4 &transform, const AABB &aabb)
1302 {
1303         return aabb.Transform(transform);
1304 }
1305
1306 OBB operator *(const float4x4 &transform, const AABB &aabb)
1307 {
1308         return aabb.Transform(transform);
1309 }
1310
1311 OBB operator *(const Quat &transform, const AABB &aabb)
1312 {
1313         return aabb.Transform(transform);
1314 }
1315
1316 MATH_END_NAMESPACE