Sphere.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 Sphere.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Sphere geometry object. */
18 #include "Sphere.h"
19 #ifdef MATH_ENABLE_STL_SUPPORT
20 #include <utility>
21 #include <vector>
22 #include <iostream>
23 #include <algorithm>
24 #else
25 #include "Container/Array.h"
26 #endif
27
28 #include "../Math/MathLog.h"
29 #include "../Math/MathFunc.h"
30 #include "OBB.h"
31 #include "AABB.h"
32 #include "Capsule.h"
33 #include "Frustum.h"
34 #include "../Algorithm/Random/LCG.h"
35 #include "LineSegment.h"
36 #include "Line.h"
37 #include "Ray.h"
38 #include "Polygon.h"
39 #include "Polyhedron.h"
40 #include "Plane.h"
41 #include "../Math/float2.h"
42 #include "../Math/float3x3.h"
43 #include "../Math/float3x4.h"
44 #include "../Math/float4.h"
45 #include "../Math/float4x4.h"
46 #include "../Math/Quat.h"
47 #include "Triangle.h"
48
49 #ifdef MATH_GRAPHICSENGINE_INTEROP
50 #include "VertexBuffer.h"
51 #endif
52
53 MATH_BEGIN_NAMESPACE
54
55 Sphere::Sphere(const vec &center, float radius)
56 :pos(center), r(radius)
57 {
58 }
59
60 Sphere::Sphere(const vec &a, const vec &b)
61 {
62         *this = FitThroughPoints(a, b);
63 }
64
65 Sphere::Sphere(const vec &a, const vec &b, const vec &c)
66 {
67         *this = FitThroughPoints(a, b, c);
68 }
69
70 Sphere::Sphere(const vec &a, const vec &b, const vec &c, const vec &d)
71 {
72         *this = FitThroughPoints(a, b, c, d);
73 }
74
75 void Sphere::Translate(const vec &offset)
76 {
77         pos += offset;
78 }
79
80 void Sphere::Transform(const float3x3 &transform)
81 {
82         assume(transform.HasUniformScale());
83         pos = transform * pos;
84         r *= transform.Col(0).Length();
85 }
86
87 void Sphere::Transform(const float3x4 &transform)
88 {
89         assume(transform.HasUniformScale());
90         pos = transform.MulPos(pos);
91         r *= transform.Col(0).Length();
92 }
93
94 void Sphere::Transform(const float4x4 &transform)
95 {
96         assume(transform.HasUniformScale());
97         assume(!transform.ContainsProjection());
98         pos = transform.MulPos(pos);
99         r *= transform.Col3(0).Length();
100 }
101
102 void Sphere::Transform(const Quat &transform)
103 {
104         pos = transform * pos;
105 }
106
107 AABB Sphere::MinimalEnclosingAABB() const
108 {
109         AABB aabb;
110         aabb.SetFrom(*this);
111         return aabb;
112 }
113
114 AABB Sphere::MaximalContainedAABB() const
115 {
116         AABB aabb;
117         static const float recipSqrt3 = RSqrt(3);
118         float halfSideLength = r * recipSqrt3;
119         aabb.SetFromCenterAndSize(pos, DIR_VEC(halfSideLength,halfSideLength,halfSideLength));
120         return aabb;
121 }
122
123 void Sphere::SetNegativeInfinity()
124 {
125         pos = POINT_VEC(0,0,0);
126         r = -FLOAT_INF;
127 }
128
129 float Sphere::Volume() const
130 {
131         return 4.f * pi * r*r*r / 3.f;
132 }
133
134 float Sphere::SurfaceArea() const
135 {
136         return 4.f * pi * r*r;
137 }
138
139 vec Sphere::ExtremePoint(const vec &direction) const
140 {
141         float len = direction.Length();
142         assume(len > 0.f);
143         return pos + direction * (r / len);
144 }
145
146 vec Sphere::ExtremePoint(const vec &direction, float &projectionDistance) const
147 {
148         vec extremePoint = ExtremePoint(direction);
149         projectionDistance = extremePoint.Dot(direction);
150         return extremePoint;
151 }
152
153 void Sphere::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
154 {
155         float d = Dot(direction, pos);
156         outMin = d - r;
157         outMax = d + r;
158 }
159
160 bool Sphere::IsFinite() const
161 {
162         return pos.IsFinite() && MATH_NS::IsFinite(r);
163 }
164
165 bool Sphere::IsDegenerate() const
166 {
167         return !(r > 0.f) || !pos.IsFinite(); // Peculiar order of testing so that NaNs end up being degenerate.
168 }
169
170 void Sphere::SetDegenerate()
171 {
172         pos = vec::nan;
173         r = nan;
174 }
175
176 bool Sphere::Contains(const vec &point) const
177 {
178         return pos.DistanceSq(point) <= r*r;
179 }
180
181 bool Sphere::Contains(const vec &point, float epsilon) const
182 {
183         return pos.DistanceSq(point) <= r*r + epsilon;
184 }
185
186 bool Sphere::Contains(const LineSegment &lineSegment) const
187 {
188         return Contains(lineSegment.a) && Contains(lineSegment.b);
189 }
190
191 bool Sphere::Contains(const Triangle &triangle) const
192 {
193         return Contains(triangle.a) && Contains(triangle.b) && Contains(triangle.c);
194 }
195
196 bool Sphere::Contains(const Polygon &polygon) const
197 {
198         for(int i = 0; i < polygon.NumVertices(); ++i)
199                 if (!Contains(polygon.Vertex(i)))
200                         return false;
201         return true;
202 }
203
204 bool Sphere::Contains(const AABB &aabb) const
205 {
206         for(int i = 0; i < 8; ++i)
207                 if (!Contains(aabb.CornerPoint(i)))
208                         return false;
209
210         return true;
211 }
212
213 bool Sphere::Contains(const OBB &obb) const
214 {
215         for(int i = 0; i < 8; ++i)
216                 if (!Contains(obb.CornerPoint(i)))
217                         return false;
218
219         return true;
220 }
221
222 bool Sphere::Contains(const Frustum &frustum) const
223 {
224         for(int i = 0; i < 8; ++i)
225                 if (!Contains(frustum.CornerPoint(i)))
226                         return false;
227
228         return true;
229 }
230
231 bool Sphere::Contains(const Polyhedron &polyhedron) const
232 {
233         assume(polyhedron.IsClosed());
234         for(int i = 0; i < polyhedron.NumVertices(); ++i)
235                 if (!Contains(polyhedron.Vertex(i)))
236                         return false;
237
238         return true;
239 }
240
241 bool Sphere::Contains(const Sphere &sphere) const
242 {
243         return pos.Distance(sphere.pos) + sphere.r <= r;
244 }
245
246 bool Sphere::Contains(const Sphere &sphere, float epsilon) const
247 {
248         return pos.Distance(sphere.pos) + sphere.r -r <= epsilon;
249 }
250
251 bool Sphere::Contains(const Capsule &capsule) const
252 {
253         return pos.Distance(capsule.l.a) + capsule.r <= r &&
254                 pos.Distance(capsule.l.b) + capsule.r <= r;
255 }
256
257 Sphere Sphere::FastEnclosingSphere(const vec *pts, int numPoints)
258 {
259         Sphere s;
260         if (numPoints == 0)
261         {
262                 s.SetNegativeInfinity();
263                 return s;
264         }
265         assume(pts || numPoints == 0);
266 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
267         if (!pts)
268                 return Sphere();
269 #endif
270
271         // First pass: Pick the cardinal axis (X,Y or Z) which has the two most distant points.
272         int minx, maxx, miny, maxy, minz, maxz;
273         AABB::ExtremePointsAlongAABB(pts, numPoints, minx, maxx, miny, maxy, minz, maxz);
274         float dist2x = pts[minx].DistanceSq(pts[maxx]);
275         float dist2y = pts[miny].DistanceSq(pts[maxy]);
276         float dist2z = pts[minz].DistanceSq(pts[maxz]);
277
278         int min = minx;
279         int max = maxx;
280         if (dist2y > dist2x && dist2y > dist2z)
281         {
282                 min = miny;
283                 max = maxy;
284         }
285         else if (dist2z > dist2x && dist2z > dist2y)
286         {
287                 min = minz;
288                 max = maxz;
289         }
290
291         // The two points on the longest axis define the initial sphere.
292         s.pos = (pts[min] + pts[max]) * 0.5f;
293         s.r = pts[min].Distance(s.pos);
294
295         // Second pass: Make sure each point lies inside this sphere, expand if necessary.
296         for(int i = 0; i < numPoints; ++i)
297                 s.Enclose(pts[i]);
298         return s;
299 }
300
301 /* This implementation was adapted from Christer Ericson's Real-time Collision Detection, pp. 99-100.
302         @bug This implementation is broken! */
303 /*
304 Sphere WelzlSphere(const vec *pts, int numPoints, vec *support, int numSupports)
305 {
306         if (numPoints == 0)
307         {
308                 switch(numSupports)
309                 {
310                 default: assert(false);
311                 case 0: return Sphere();
312                 case 1: return Sphere(support[0], 0.f);
313                 case 2: return Sphere(support[0], support[1]);
314                 case 3: return Sphere(support[0], support[1], support[2]);
315                 case 4: return Sphere(support[0], support[1], support[2], support[3]);
316                 }
317         }
318
319         // todo The following recursion can easily crash the stack for large inputs.  Convert this to proper form.
320         Sphere smallestSphere = WelzlSphere(pts, numPoints - 1, support, numSupports);
321         if (smallestSphere.Contains(pts[numPoints-1]))
322                 return smallestSphere;
323         support[numSupports] = pts[numPoints-1];
324         return WelzlSphere(pts, numPoints - 1,  support, numSupports + 1);
325 }
326 */
327
328 // The epsilon value used for enclosing sphere computations.
329 static const float epsilon = 1e-4f;
330
331 Sphere Sphere::OptimalEnclosingSphere(const vec *pts, int numPoints)
332 {
333         // If we have only a small number of points, can solve with a specialized function.
334         switch(numPoints)
335         {
336         case 0: return Sphere();
337         case 1: return Sphere(pts[0], 0.f); // Sphere around a single point will be degenerate with r = 0.
338         case 2: return OptimalEnclosingSphere(pts[0], pts[1]);
339         case 3: return OptimalEnclosingSphere(pts[0], pts[1], pts[2]);
340         case 4: return OptimalEnclosingSphere(pts[0], pts[1], pts[2], pts[3]);
341         default: break;
342         }
343
344         // The set of supporting points for the minimal sphere. Even though the minimal enclosing
345         // sphere might have 2, 3 or 4 points in its support (sphere surface), always store here
346         // indices to exactly four points.
347         int sp[4] = { 0, 1, 2, 3 };
348         // Due to numerical issues, it can happen that the minimal sphere for four points {a,b,c,d} does not
349         // accommodate a fifth point e, but replacing any of the points a-d from the support with the point e
350         // does not accommodate the all the five points either.
351         // Therefore, keep a set of flags for each support point to avoid going in cycles, where the same
352         // set of points are again and again added and removed from the support, causing an infinite loop.
353         bool expendable[4] = { true, true, true, true };
354         // The so-far constructed minimal sphere.
355         Sphere s = OptimalEnclosingSphere(pts[sp[0]], pts[sp[1]], pts[sp[2]], pts[sp[3]]);
356         float rSq = s.r * s.r + epsilon;
357         for(int i = 4; i < numPoints; ++i)
358         {
359                 if (i == sp[0] || i == sp[1] || i == sp[2] || i == sp[3])
360                         continue; // Take care not to add the same point twice to the support set.
361                 // If the next point (pts[i]) does not fit inside the currently computed minimal sphere, compute
362                 // a new minimal sphere that also contains pts[i].
363                 if (pts[i].DistanceSq(s.pos) > rSq)
364                 {
365                         int redundant;
366                         s = OptimalEnclosingSphere(pts[sp[0]], pts[sp[1]], pts[sp[2]], pts[sp[3]], pts[i], redundant);
367                         rSq = s.r*s.r + epsilon;
368                         // A sphere is uniquely defined by four points, so one of the five points passed in above is
369                         // now redundant, and can be removed from the support set.
370                         if (redundant != 4 && (sp[redundant] < i || expendable[redundant]))
371                         {
372                                 sp[redundant] = i; // Replace the old point with the new one.
373                                 expendable[redundant] = false; // This new one cannot be evicted (until we proceed past it in the input list later)
374                                 // Mark all points in the array before this index "expendable", meaning that they can be removed from the support set.
375                                 if (sp[0] < i) expendable[0] = true;
376                                 if (sp[1] < i) expendable[1] = true;
377                                 if (sp[2] < i) expendable[2] = true;
378                                 if (sp[3] < i) expendable[3] = true;
379
380                                 // Have to start all over and make sure all old points also lie inside this new sphere,
381                                 // since our guess for the minimal enclosing sphere changed.
382                                 i = 0;
383                         }
384                 }
385         }
386
387         return s;
388 }
389
390 float Sphere::Distance(const vec &point) const
391 {
392         return Max(0.f, pos.Distance(point) - r);
393 }
394
395 float Sphere::Distance(const Sphere &sphere) const
396 {
397         return Max(0.f, pos.Distance(sphere.pos) - r - sphere.r);
398 }
399
400 float Sphere::Distance(const Capsule &capsule) const
401 {
402         return capsule.Distance(*this);
403 }
404
405 float Sphere::Distance(const AABB &aabb) const
406 {
407         return aabb.Distance(*this);
408 }
409
410 float Sphere::Distance(const OBB &obb) const
411 {
412         return obb.Distance(*this);
413 }
414
415 float Sphere::Distance(const Plane &plane) const
416 {
417         return plane.Distance(*this);
418 }
419
420 float Sphere::Distance(const Triangle &triangle) const
421 {
422         return triangle.Distance(*this);
423 }
424
425 float Sphere::Distance(const Ray &ray) const
426 {
427         return ray.Distance(*this);
428 }
429
430 float Sphere::Distance(const LineSegment &lineSegment) const
431 {
432         return lineSegment.Distance(*this);
433 }
434
435 float Sphere::Distance(const Line &line) const
436 {
437         return line.Distance(*this);
438 }
439
440 float Sphere::MaxDistance(const vec &point) const
441 {
442         return point.Distance(pos) + r;
443 }
444
445 vec Sphere::ClosestPoint(const vec &point) const
446 {
447         float d = pos.Distance(point);
448         float t = (d >= r ? r : d);
449         return pos + (point - pos) * (t / d);
450 }
451
452 bool Sphere::Intersects(const Sphere &sphere) const
453 {
454         return (pos - sphere.pos).LengthSq() <= (r + sphere.r) * (r + sphere.r);
455 }
456
457 bool Sphere::Intersects(const Capsule &capsule) const
458 {
459         return capsule.Intersects(*this);
460 }
461
462 int Sphere::IntersectLine(const vec &linePos, const vec &lineDir, const vec &sphereCenter,
463                           float sphereRadius, float &t1, float &t2)
464 {
465         assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq());
466         assume1(sphereRadius >= 0.f, sphereRadius);
467
468         /* A line is represented explicitly by the set { linePos + t * lineDir }, where t is an arbitrary float.
469           A sphere is represented implictly by the set of vectors that satisfy ||v - sphereCenter|| == sphereRadius.
470           To solve which points on the line are also points on the sphere, substitute v <- linePos + t * lineDir
471           to obtain:
472
473             || linePos + t * lineDir - sphereCenter || == sphereRadius, and squaring both sides we get
474             || linePos + t * lineDir - sphereCenter ||^2 == sphereRadius^2, or rearranging:
475             || (linePos - sphereCenter) + t * lineDir ||^2 == sphereRadius^2. */
476
477         // This equation represents the set of points which lie both on the line and the sphere. There is only one
478         // unknown variable, t, for which we solve to get the actual points of intersection.
479
480         // Compute variables from the above equation:
481         const vec a = linePos - sphereCenter;
482         const float radSq = sphereRadius * sphereRadius;
483
484         /* so now the equation looks like
485
486             || a + t * lineDir ||^2 == radSq.
487
488           Since ||x||^2 == <x,x> (i.e. the square of a vector norm equals the dot product with itself), we get
489
490             <a + t * lineDir, a + t * lineDir> == radSq,
491
492           and using the identity <a+b, a+b> == <a,a> + 2*<a,b> + <b,b> (which holds for dot product when a and b are reals),
493           we have
494
495             <a,a> + 2 * <a, t * lineDir> + <t * lineDir, t * lineDir> == radSq, or
496             <a,a> - radSq + 2 * <a, lineDir> * t + <lineDir, lineDir> * t^2 == 0, or
497
498             C + Bt + At^2 == 0, where
499
500             C = <a,a> - radSq,
501             B = 2 * <a, lineDir>, and
502             A = <lineDir, lineDir> == 1, since we assumed lineDir is normalized. */
503
504         // Warning! If Dot(a,a) is large (distance between line pos and sphere center) and sphere radius very small,
505         // catastrophic cancellation can occur here!
506         const float C = Dot(a,a) - radSq;
507         const float B = 2.f * Dot(a, lineDir);
508
509         /* The equation A + Bt + Ct^2 == 0 is a second degree equation on t, which is easily solvable using the
510           known formula, and we obtain
511
512             t = [-B +/- Sqrt(B^2 - 4AC)] / 2A. */
513
514         float D = B*B - 4.f * C; // D = B^2 - 4AC.
515         if (D < 0.f) // There is no solution to the square root, so the ray doesn't intersect the sphere.
516         {
517                 // Output a degenerate enter-exit range so that batch processing code may use min of t1's and max of t2's to
518                 // compute the nearest enter and farthest exit without requiring branching on the return value of this function.
519                 t1 = FLOAT_INF;
520                 t2 = -FLOAT_INF;
521                 return 0;
522         }
523
524         if (D < 1e-4f) // The expression inside Sqrt is ~ 0. The line is tangent to the sphere, and we have one solution.
525         {
526                 t1 = t2 = -B * 0.5f;
527                 return 1;
528         }
529
530         // The Sqrt expression is strictly positive, so we get two different solutions for t.
531         D = Sqrt(D);
532         t1 = (-B - D) * 0.5f;
533         t2 = (-B + D) * 0.5f;
534         return 2;
535 }
536
537 int Sphere::Intersects(const Ray &ray, vec *intersectionPoint, vec *intersectionNormal, float *d, float *d2) const
538 {
539         float t1, t2;
540         int numIntersections = IntersectLine(ray.pos, ray.dir, pos, r, t1, t2);
541
542         // If the line of this ray intersected in two places, but the first intersection was "behind" this ray,
543         // handle the second point of intersection instead. This case occurs when the origin of the ray is inside
544         // the Sphere.
545         if (t1 < 0.f && numIntersections == 2)
546                 t1 = t2;
547
548         if (t1 < 0.f)
549                 return 0; // The intersection position is on the negative direction of the ray.
550
551         vec hitPoint = ray.pos + t1 * ray.dir;
552         if (intersectionPoint)
553                 *intersectionPoint = hitPoint;
554         if (intersectionNormal)
555                 *intersectionNormal = (hitPoint - pos).Normalized();
556         if (d)
557                 *d = t1;
558         if (d2)
559                 *d2 = t2;
560
561         return numIntersections;
562 }
563
564 int Sphere::Intersects(const Line &line, vec *intersectionPoint, vec *intersectionNormal, float *d, float *d2) const
565 {
566         float t1, t2;
567         int numIntersections = IntersectLine(line.pos, line.dir, pos, r, t1, t2);
568         if (numIntersections == 0)
569                 return 0;
570
571         vec hitPoint = line.pos + t1 * line.dir;
572         if (intersectionPoint)
573                 *intersectionPoint = hitPoint;
574         if (intersectionNormal)
575                 *intersectionNormal = (hitPoint - pos).Normalized();
576         if (d)
577                 *d = t1;
578         if (d2)
579                 *d2 = t2;
580
581         return numIntersections;
582 }
583
584 int Sphere::Intersects(const LineSegment &l, vec *intersectionPoint, vec *intersectionNormal, float *d, float *d2) const
585 {
586         float t1, t2;
587         int numIntersections = IntersectLine(l.a, l.Dir(), pos, r, t1, t2);
588
589         if (numIntersections == 0)
590                 return 0;
591
592         float lineLength = l.Length();
593         if (t2 < 0.f || t1 > lineLength)
594                 return 0;
595         vec hitPoint = l.GetPoint(t1 / lineLength);
596         if (intersectionPoint)
597                 *intersectionPoint = hitPoint;
598         if (intersectionNormal)
599                 *intersectionNormal = (hitPoint - pos).Normalized();
600         if (d)
601                 *d = t1 / lineLength;
602         if (d2)
603                 *d2 = t2 / lineLength;
604
605         return true;
606 }
607
608 bool Sphere::Intersects(const Plane &plane) const
609 {
610         return plane.Intersects(*this);
611 }
612
613 bool Sphere::Intersects(const AABB &aabb, vec *closestPointOnAABB) const
614 {
615         return aabb.Intersects(*this, closestPointOnAABB);
616 }
617
618 bool Sphere::Intersects(const OBB &obb, vec *closestPointOnOBB) const
619 {
620         return obb.Intersects(*this, closestPointOnOBB);
621 }
622
623 bool Sphere::Intersects(const Triangle &triangle, vec *closestPointOnTriangle) const
624 {
625         return triangle.Intersects(*this, closestPointOnTriangle);
626 }
627
628 bool Sphere::Intersects(const Polygon &polygon) const
629 {
630         return polygon.Intersects(*this);
631 }
632
633 bool Sphere::Intersects(const Frustum &frustum) const
634 {
635         return frustum.Intersects(*this);
636 }
637
638 bool Sphere::Intersects(const Polyhedron &polyhedron) const
639 {
640         return polyhedron.Intersects(*this);
641 }
642
643 void Sphere::Enclose(const vec &point, float epsilon)
644 {
645         vec d = point - pos;
646         float dist2 = d.LengthSq();
647         if (dist2 + epsilon > r*r)
648         {
649 #ifdef MATH_ASSERT_CORRECTNESS
650                 Sphere copy = *this;
651 #endif
652                 float dist = Sqrt(dist2);
653                 float halfDist = (dist - r) * 0.5f;
654                 // Nudge this Sphere towards the target point. Add half the missing distance to radius,
655                 // and the other half to position. This gives a tighter enclosure, instead of if
656                 // the whole missing distance were just added to radius.
657                 pos += d * halfDist / dist;
658                 r += halfDist + 1e-4f; // Use a fixed epsilon deliberately, the param is a squared epsilon, so different order of magnitude.
659 #ifdef MATH_ASSERT_CORRECTNESS
660                 mathassert(this->Contains(copy, epsilon));
661 #endif
662         }
663
664         assume(this->Contains(point));
665 }
666
667 struct PointWithDistance
668 {
669         vec pt;
670         float d;
671
672         bool operator <(const PointWithDistance &rhs) const { return d < rhs.d; }
673 };
674
675 // A quick and dirty RAII container for allocated AlignedNew-AlignedFree array to avoid leaking
676 // memory if a test throws an exception.
677 template<typename T>
678 class AutoArrayPtr
679 {
680 public:
681         T *ptr;
682         AutoArrayPtr(T *ptr):ptr(ptr) {}
683         ~AutoArrayPtr() { AlignedFree(ptr); }
684 private:
685         AutoArrayPtr(const AutoArrayPtr&);
686         void operator=(const AutoArrayPtr&);
687 };
688
689 // Encloses n points into the Sphere s, in the order of farthest first, in order to
690 // generate the tightest resulting enclosure.
691 void Sphere_Enclose_pts(Sphere &s, const vec *pts, int n)
692 {
693         AutoArrayPtr<PointWithDistance> cornersPtr(AlignedNew<PointWithDistance>(n, 16));
694         PointWithDistance *corners = cornersPtr.ptr;
695
696         for(int i = 0; i < n; ++i)
697         {
698                 corners[i].pt = pts[i];
699                 corners[i].d = s.pos.DistanceSq(corners[i].pt);
700         }
701         std::sort(corners, corners+n);
702
703         for(int i = n-1; i >= 0; --i)
704                 s.Enclose(corners[i].pt);
705 }
706
707 // Optimize/reimplement the above function in the case when enclosing geometric objects where the number of
708 // points to enclose is fixed at compile-time. This avoids dynamic memory allocation.
709 template<typename T, int n>
710 void Sphere_Enclose(Sphere &s, const T &obj)
711 {
712         PointWithDistance corners[n];
713         for(int i = 0; i < n; ++i)
714         {
715                 corners[i].pt = obj.CornerPoint(i);
716                 corners[i].d = s.pos.DistanceSq(corners[i].pt);
717         }
718         std::sort(corners, corners+n);
719
720         for(int i = n-1; i >= 0; --i)
721                 s.Enclose(corners[i].pt);
722 }
723
724 void Sphere::Enclose(const AABB &aabb)
725 {
726         Sphere_Enclose<AABB, 8>(*this, aabb);
727
728         assume(this->Contains(aabb));
729 }
730
731 void Sphere::Enclose(const OBB &obb)
732 {
733         Sphere_Enclose<OBB, 8>(*this, obb);
734
735         assume(this->Contains(obb));
736 }
737
738 void Sphere::Enclose(const Sphere &sphere)
739 {
740         // To enclose another sphere into this sphere, we only need to enclose two points:
741         // 1) Enclose the farthest point on the other sphere into this sphere.
742         // 2) Enclose the opposite point of the farthest point into this sphere.
743         vec toFarthestPoint = (sphere.pos - pos).ScaledToLength(sphere.r);
744         Enclose(sphere.pos + toFarthestPoint);
745         Enclose(sphere.pos - toFarthestPoint);
746
747         assume(this->Contains(sphere));
748 }
749
750 void Sphere::Enclose(const LineSegment &lineSegment)
751 {
752         if (pos.DistanceSq(lineSegment.a) > pos.DistanceSq(lineSegment.b))
753         {
754                 Enclose(lineSegment.a);
755                 Enclose(lineSegment.b);
756         }
757         else
758         {
759                 Enclose(lineSegment.b);
760                 Enclose(lineSegment.a);
761         }
762 }
763
764 void Sphere::Enclose(const vec *pointArray, int numPoints)
765 {
766         assume(pointArray || numPoints == 0);
767 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
768         if (!pointArray)
769                 return;
770 #endif
771         Sphere_Enclose_pts(*this, pointArray, numPoints);
772 }
773
774 void Sphere::Enclose(const Triangle &triangle)
775 {
776         Sphere_Enclose<Triangle, 3>(*this, triangle);
777 }
778
779 void Sphere::Enclose(const Polygon &polygon)
780 {
781         Enclose(polygon.VertexArrayPtr(), polygon.NumVertices());
782 }
783
784 void Sphere::Enclose(const Polyhedron &polyhedron)
785 {
786         Enclose(polyhedron.VertexArrayPtr(), polyhedron.NumVertices());
787 }
788
789 void Sphere::Enclose(const Frustum &frustum)
790 {
791         Sphere_Enclose<Frustum, 8>(*this, frustum);
792 }
793
794 void Sphere::Enclose(const Capsule &capsule)
795 {
796         // Capsule is a convex object spanned by the endpoint spheres - enclosing
797         // the endpoint spheres will also cause this Sphere to enclose the middle
798         // section since Sphere is convex as well.
799         float da = pos.DistanceSq(capsule.l.a);
800         float db = pos.DistanceSq(capsule.l.b);
801
802         // Enclose the farther Sphere of the Capsule first, and the closer one second to retain the tightest fit.
803         if (da > db)
804         {
805                 Enclose(capsule.SphereA());
806                 Enclose(capsule.SphereB());
807         }
808         else
809         {
810                 Enclose(capsule.SphereB());
811                 Enclose(capsule.SphereA());
812         }
813 }
814
815 void Sphere::ExtendRadiusToContain(const vec &point, float epsilon)
816 {
817         float requiredRadius = pos.Distance(point) + epsilon;
818         r = Max(r, requiredRadius);
819 }
820
821 void Sphere::ExtendRadiusToContain(const Sphere &sphere, float epsilon)
822 {
823         float requiredRadius = pos.Distance(sphere.pos) + sphere.r + epsilon;
824         r = Max(r, requiredRadius);
825 }
826
827 int Sphere::Triangulate(vec *outPos, vec *outNormal, float2 *outUV, int numVertices, bool ccwIsFrontFacing) const
828 {
829         assume(outPos);
830         assume(numVertices >= 24 && "At minimum, sphere triangulation will contain at least 8 triangles, which is 24 vertices, but fewer were specified!");
831         assume(numVertices % 3 == 0 && "Warning:: The size of output should be divisible by 3 (each triangle takes up 3 vertices!)");
832
833 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
834         if (!outPos)
835                 return 0;
836 #endif
837         assume(this->r > 0.f);
838
839         if (numVertices < 24)
840                 return 0;
841
842 #ifdef MATH_ENABLE_STL_SUPPORT
843         TriangleArray temp;
844 #else
845         Array<Triangle> temp;
846 #endif
847         // Start subdividing from a diamond shape.
848         vec xp = POINT_VEC(r,0,0);
849         vec xn = POINT_VEC(-r, 0, 0);
850         vec yp = POINT_VEC(0, r, 0);
851         vec yn = POINT_VEC(0, -r, 0);
852         vec zp = POINT_VEC(0, 0, r);
853         vec zn = POINT_VEC(0, 0, -r);
854
855         if (ccwIsFrontFacing)
856         {
857                 temp.push_back(Triangle(yp,xp,zp));
858                 temp.push_back(Triangle(xp,yp,zn));
859                 temp.push_back(Triangle(yn,zp,xp));
860                 temp.push_back(Triangle(yn,xp,zn));
861                 temp.push_back(Triangle(zp,xn,yp));
862                 temp.push_back(Triangle(yp,xn,zn));
863                 temp.push_back(Triangle(yn,xn,zp));
864                 temp.push_back(Triangle(xn,yn,zn));
865         }
866         else
867         {
868                 temp.push_back(Triangle(yp,zp,xp));
869                 temp.push_back(Triangle(xp,zn,yp));
870                 temp.push_back(Triangle(yn,xp,zp));
871                 temp.push_back(Triangle(yn,zn,xp));
872                 temp.push_back(Triangle(zp,yp,xn));
873                 temp.push_back(Triangle(yp,zn,xn));
874                 temp.push_back(Triangle(yn,zp,xn));
875                 temp.push_back(Triangle(xn,zn,yn));
876         }
877
878         int oldEnd = 0;
879         while(((int)temp.size()-oldEnd+3)*3 <= numVertices)
880         {
881                 Triangle cur = temp[oldEnd];
882                 vec a = ((cur.a + cur.b) * 0.5f).ScaledToLength(this->r);
883                 vec b = ((cur.a + cur.c) * 0.5f).ScaledToLength(this->r);
884                 vec c = ((cur.b + cur.c) * 0.5f).ScaledToLength(this->r);
885
886                 temp.push_back(Triangle(cur.a, a, b));
887                 temp.push_back(Triangle(cur.b, c, a));
888                 temp.push_back(Triangle(cur.c, b, c));
889                 temp.push_back(Triangle(a, c, b));
890
891                 ++oldEnd;
892         }
893         // Check that we really did tessellate as many new triangles as possible.
894         assert(((int)temp.size()-oldEnd)*3 <= numVertices && ((int)temp.size()-oldEnd)*3 + 9 > numVertices);
895
896         for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
897         {
898                 outPos[3*j] = this->pos + TRIANGLE(temp[i]).a;
899                 outPos[3*j+1] = this->pos + TRIANGLE(temp[i]).b;
900                 outPos[3*j+2] = this->pos + TRIANGLE(temp[i]).c;
901         }
902
903         if (outNormal)
904                 for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
905                 {
906                         outNormal[3*j] = TRIANGLE(temp[i]).a.Normalized();
907                         outNormal[3*j+1] = TRIANGLE(temp[i]).b.Normalized();
908                         outNormal[3*j+2] = TRIANGLE(temp[i]).c.Normalized();
909                 }
910
911         if (outUV)
912                 for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
913                 {
914                         outUV[3*j] = float2(atan2(TRIANGLE(temp[i]).a.y, TRIANGLE(temp[i]).a.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).a.z + r) / (2.f * r));
915                         outUV[3*j+1] = float2(atan2(TRIANGLE(temp[i]).b.y, TRIANGLE(temp[i]).b.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).b.z + r) / (2.f * r));
916                         outUV[3*j+2] = float2(atan2(TRIANGLE(temp[i]).c.y, TRIANGLE(temp[i]).c.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).c.z + r) / (2.f * r));
917                 }
918
919         return ((int)temp.size() - oldEnd) * 3;
920 }
921
922 vec Sphere::RandomPointInside(LCG &lcg)
923 {
924         assume(r > 1e-3f);
925         vec v = vec::zero;
926         // Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box, so this
927         // loop is expected to take only very few iterations before succeeding.
928         for(int i = 0; i < 1000; ++i)
929         {
930                 v.x = lcg.Float(-r, r);
931                 v.y = lcg.Float(-r, r);
932                 v.z = lcg.Float(-r, r);
933                 if (v.LengthSq() <= r*r)
934                         return pos + v;
935         }
936         assume(false && "Sphere::RandomPointInside failed!");
937
938         // Failed to generate a point inside this sphere. Return the sphere center as fallback.
939         return pos;
940 }
941
942 vec Sphere::RandomPointOnSurface(LCG &lcg)
943 {
944         assume(r > 1e-3f);
945         vec v = vec::zero;
946         // Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box, so this
947         // loop is expected to take only very few iterations before succeeding.
948         for(int i = 0; i < 1000; ++i)
949         {
950                 v.x = lcg.FloatNeg1_1();
951                 v.y = lcg.FloatNeg1_1();
952                 v.z = lcg.FloatNeg1_1();
953                 float lenSq = v.LengthSq();
954                 if (lenSq >= 1e-6f && lenSq <= r*r)
955                         return pos + (r / Sqrt(lenSq)) * v;
956         }
957         // Astronomically small probability to reach here, and if we do so, the provided random number generator must have been in a bad state.
958         assume(false && "Sphere::RandomPointOnSurface failed!");
959
960         // Failed to generate a point inside this sphere. Return an arbitrary point on the surface as fallback.
961         return pos + DIR_VEC(r, 0, 0);
962 }
963
964 vec Sphere::RandomPointInside(LCG &lcg, const vec &center, float radius)
965 {
966         return Sphere(center, radius).RandomPointInside(lcg);
967 }
968
969 vec Sphere::RandomPointOnSurface(LCG &lcg, const vec &center, float radius)
970 {
971         return Sphere(center, radius).RandomPointOnSurface(lcg);
972 }
973
974 Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b)
975 {
976         Sphere s;
977         s.pos = (a + b) * 0.5f;
978         s.r = (b - s.pos).Length();
979         assume(s.pos.IsFinite());
980         assume(s.r >= 0.f);
981
982         // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests
983         // really contain the points (note that the points must be sufficiently near enough to the origin)
984         s.r += epsilon;
985
986         mathassert(s.Contains(a));
987         mathassert(s.Contains(b));
988         return s;
989 }
990
991 /** Computes the (s,t) coordinates of the smallest sphere that passes through three points (0,0,0), ab and ac.
992         @param ab The first point to fit the sphere through.
993         @param ac The second point to fit the sphere through. The third point is hardcoded to (0,0,0). When fitting a sphere
994                 through three points a, b and c, pass in b-a as the parameter ab, and c-a as the parameter ac (i.e. translate
995                 the coordinate system center to lie at a).
996         @param s [out] Outputs the s-coordinate of the sphere center (in the 2D barycentric UV convention)
997         @param t [out] Outputs the t-coordinate of the sphere center (in the 2D barycentric UV convention) To
998                 compute the actual point, calculate the expression origin + s*ab + t*ac.
999         @note The returned sphere is one that passes through the three points (0,0,0), ab and ac. It is NOT necessarily the
1000                 smallest sphere that encloses these three points!
1001         @return True if the function succeeded. False on failure. This function fails if the points (0,0,0), ab and ac
1002                 are collinear, in which case there does not exist a sphere that passes through the three given points. */
1003 bool FitSphereThroughPoints(const vec &ab, const vec &ac, float &s, float &t)
1004 {
1005         /* The task is to compute the minimal radius sphere through the three points
1006            a, b and c. (Note that this is not necessarily the minimal radius sphere enclosing
1007            the points a, b and c!)
1008
1009            Denote by p the sphere center position, and r the sphere radius. If the sphere
1010            is to run through the points a, b and c, then the center point of the sphere
1011            must be equidistant to these points, i.e.
1012
1013               || p - a || == || p - b || == || p - c ||,
1014
1015            or
1016
1017               a^2 - 2ap + p^2 == b^2 - 2bp + p^2 == c^2 - 2cp + p^2.
1018
1019            Subtracting pairwise, we get
1020
1021               (b-a)p == (b^2 - a^2)/2 and       (1)
1022               (c-a)p == (c^2 - a^2)/2.          (2)
1023
1024            Additionally, the center point of the sphere must lie on the same plane as the triangle
1025            defined by the points a, b and c. Therefore, the point p can be represented as a 2D
1026            barycentric coordinates (s,t) as follows:
1027
1028               p == a + s*(b-a) + t*(c-a).        (3)
1029
1030            Now, without loss of generality, assume that the point a lies at origin (translate the origin
1031            of the coordinate system to be centered at the point a), i.e. make the substitutions
1032            A = (0,0,0), B = b-a, C = c-a, and we have:and we have:
1033
1034               BP == B^2/2,            (1')
1035               CP == C^2/2 and         (2')
1036                P == s*B + t*C.        (3') */
1037
1038         const float BB = Dot(ab,ab);
1039         const float CC = Dot(ac,ac);
1040         const float BC = Dot(ab,ac);
1041
1042         /* Substitute (3') into (1') and (2'), to obtain a matrix equation
1043
1044            ( B^2  BC  ) * (s) = (B^2 / 2)
1045            ( BC   C^2 )   (t)   (C^2 / 2)
1046
1047            which equals
1048
1049            (s) = ( B^2  BC  )^-1  *  (B^2 / 2)
1050            (t)   ( BC   C^2 )        (C^2 / 2)
1051
1052                 Use the formula for inverting a 2x2 matrix, and we have
1053
1054            (s) = 1 / (2 * B^2 * C^2 - (BC)^2) * ( C^2   -BC ) *  (B^2)
1055            (t)                                  ( -BC   B^2 )    (C^2)
1056         */
1057
1058         float denom = BB*CC - BC*BC;
1059
1060         if (EqualAbs(denom, 0.f, 5e-3f))
1061                 return false;
1062
1063         denom = 0.5f / denom; // == 1 / (2 * B^2 * C^2 - (BC)^2)
1064
1065         s = (CC * BB - BC * CC) * denom;
1066         t = (CC * BB - BC * BB) * denom;
1067
1068         return true;
1069 }
1070
1071 /** Computes the center point coordinates of the sphere that passes through four points (0,0,0), ab, ac and ad.
1072         @param ab The first point to fit the sphere through.
1073         @param ac The second point to fit the sphere through.
1074         @param ad The third point to fit the sphere through. The fourth point is hardcoded to (0,0,0). When fitting a sphere
1075                 through four points a, b c and d, pass in b-a as the parameter ab, and c-a as the parameter ac and d-a as
1076                 the parameter ad (i.e. translate
1077                 the coordinate system center to lie at a).
1078         @param s [out] Outputs the s-coordinate of the sphere center.
1079         @param t [out] Outputs the t-coordinate of the sphere center.
1080         @param u [out] Outputs the u-coordinate of the sphere center. To
1081                 compute the actual point, calculate the expression a + s*ab + t*ac + u*ad.
1082         @note The returned sphere is one that passes through the four points (0,0,0), ab, ac and ad. It is NOT necessarily
1083                 the smallest sphere that encloses these four points!
1084         @return True if the function succeeded. False on failure. This function fails if the points (0,0,0), ab, ac and ad
1085                 are coplanar, in which case there does not exist a sphere that passes through the four given points. */
1086 bool FitSphereThroughPoints(const vec &ab, const vec &ac, const vec &ad, float &s, float &t, float &u)
1087 {
1088         /* The task is to compute the (unique) sphere through the four points
1089            a, b c and d. (Note that this is not necessarily the minimal radius sphere enclosing
1090            the points a, b, c and d!)
1091
1092            Denote by p the sphere center position, and r the sphere radius. If the sphere
1093            is to run through the points a, b, c and d, then the center point of the sphere
1094            must be equidistant to these points, i.e.
1095
1096               || p - a || == || p - b || == || p - c || == || p - d ||,
1097
1098            or
1099
1100               a^2 - 2ap + p^2 == b^2 - 2bp + p^2 == c^2 - 2cp + p^2 == d^2 - 2dp + p ^2.
1101
1102            Subtracting pairwise, we get
1103
1104               (b-a)p == (b^2 - a^2)/2,          (1)
1105               (c-a)p == (c^2 - a^2)/2 and       (2)
1106               (d-a)p == (d^2 - a^2)/2.          (3)
1107
1108            Additionally, the center point of the sphere can be represented as a linear combination
1109            of the four points a, b, c and d, as follows:
1110
1111               p == a + s*(b-a) + t*(c-a) + u*(d-a).        (4)
1112
1113            Now, without loss of generality, assume that the point a lies at origin (translate the origin
1114            of the coordinate system to be centered at the point a, i.e. make the substitutions
1115            A = (0,0,0), B = b-a, C = c-a, D = d-a, and we have:
1116
1117               BP == B^2/2,            (1')
1118               CP == C^2/2 and         (2')
1119               DP == D^2/2 and         (3')
1120                P == s*B + t*C + u*D.  (4')
1121
1122            Substitute (4') into (1'), (2') and (3'), to obtain a matrix equation
1123
1124               ( B^2  BC  BD )   (s)   (B^2 / 2)
1125               ( BC   C^2 CD ) * (t) = (C^2 / 2)
1126               ( BD   CD  D^2)   (u)   (D^2 / 2)
1127
1128            which equals
1129
1130               (s)   ( B^2  BC  BD )^-1   (B^2 / 2)
1131               (t) = ( BC   C^2 CD )    * (C^2 / 2)
1132               (u)   ( BD   CD  D^2)      (D^2 / 2)
1133
1134                 Then we simply invert the 3x3 matrix and compute the vector (s, t, u). */
1135
1136         const float BB = Dot(ab, ab);
1137         const float BC = Dot(ab, ac);
1138         const float BD = Dot(ab, ad);
1139         const float CC = Dot(ac, ac);
1140         const float CD = Dot(ac, ad);
1141         const float DD = Dot(ad, ad);
1142
1143         float3x3 m;
1144         m[0][0] = BB; m[0][1] = BC; m[0][2] = BD;
1145         m[1][0] = BC; m[1][1] = CC; m[1][2] = CD;
1146         m[2][0] = BD; m[2][1] = CD; m[2][2] = DD;
1147         bool success = m.InverseSymmetric();
1148         if (!success)
1149                 return false;
1150         float3 v = m * float3(BB * 0.5f, CC * 0.5f, DD * 0.5f);
1151         s = v.x;
1152         t = v.y;
1153         u = v.z;
1154
1155         return true;
1156 }
1157
1158 /** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */
1159 Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c)
1160 {
1161         Sphere sphere;
1162
1163         vec ab = b-a;
1164         vec ac = c-a;
1165
1166         float s, t;
1167         bool areCollinear = ab.Cross(ac).LengthSq() < 1e-4f; // Manually test that we don't try to fit sphere to three collinear points.
1168         bool success = !areCollinear && FitSphereThroughPoints(ab, ac, s, t);
1169         if (!success || Abs(s) > 10000.f || Abs(t) > 10000.f) // If s and t are very far from the triangle, do a manual box fitting for numerical stability.
1170         {
1171                 vec minPt = Min(a, b, c);
1172                 vec maxPt = Max(a, b, c);
1173                 sphere.pos = (minPt + maxPt) * 0.5f;
1174                 sphere.r = sphere.pos.Distance(minPt);
1175         }
1176         else if (s < 0.f)
1177         {
1178                 sphere.pos = (a + c) * 0.5f;
1179                 sphere.r = a.Distance(c) * 0.5f;
1180                 sphere.r = Max(sphere.r, b.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
1181         }
1182         else if (t < 0.f)
1183         {
1184                 sphere.pos = (a + b) * 0.5f;
1185                 sphere.r = a.Distance(b) * 0.5f;
1186                 sphere.r = Max(sphere.r, c.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
1187         }
1188         else if (s+t > 1.f)
1189         {
1190                 sphere.pos = (b + c) * 0.5f;
1191                 sphere.r = b.Distance(c) * 0.5f;
1192                 sphere.r = Max(sphere.r, a.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
1193         }
1194         else
1195         {
1196                 const vec center = s*ab + t*ac;
1197                 sphere.pos = a + center;
1198                 // Mathematically, the following would be correct, but it suffers from floating point inaccuracies,
1199                 // since it only tests distance against one point.
1200                 //sphere.r = center.Length();
1201
1202                 // For robustness, take the radius to be the distance to the farthest point (though the distance are all
1203                 // equal).
1204                 sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c)));
1205         }
1206
1207         // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests
1208         // really contain the points (note that the points must be sufficiently near enough to the origin)
1209         sphere.r += 2.f * epsilon; // We test against one epsilon, so expand by two epsilons.
1210
1211 #ifdef MATH_ASSERT_CORRECTNESS
1212         if (!sphere.Contains(a, epsilon) || !sphere.Contains(b, epsilon) || !sphere.Contains(c, epsilon))
1213         {
1214                 LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r);
1215                 LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos));
1216                 LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos));
1217                 LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos));
1218                 mathassert(false);
1219         }
1220 #endif
1221         return sphere;
1222 }
1223
1224 /** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */
1225 Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c, const vec &d)
1226 {
1227         Sphere sphere;
1228
1229         float s,t,u;
1230         const vec ab = b-a;
1231         const vec ac = c-a;
1232         const vec ad = d-a;
1233         bool success = FitSphereThroughPoints(ab, ac, ad, s, t, u);
1234         if (!success || s < 0.f || t < 0.f || u < 0.f || s+t+u > 1.f)
1235         {
1236                 sphere = OptimalEnclosingSphere(a,b,c);
1237                 if (!sphere.Contains(d))
1238                 {
1239                         sphere = OptimalEnclosingSphere(a,b,d);
1240                         if (!sphere.Contains(c))
1241                         {
1242                                 sphere = OptimalEnclosingSphere(a,c,d);
1243                                 if (!sphere.Contains(b))
1244                                 {
1245                                         sphere = OptimalEnclosingSphere(b,c,d);
1246                                         sphere.r = Max(sphere.r, a.Distance(sphere.pos) + 1e-3f); // For numerical stability, expand the radius of the sphere so it certainly contains the fourth point.
1247                                         assume(sphere.Contains(a));
1248                                 }
1249                         }
1250                 }
1251         }
1252         /* // Note: Trying to approach the problem like this, like was in the triangle case, is flawed:
1253         if (s < 0.f)
1254                 sphere = OptimalEnclosingSphere(a, c, d);
1255         else if (t < 0.f)
1256                 sphere = OptimalEnclosingSphere(a, b, d);
1257         else if (u < 0.f)
1258                 sphere = OptimalEnclosingSphere(a, b, c);
1259         else if (s + t + u > 1.f)
1260                 sphere = OptimalEnclosingSphere(b, c, d); */
1261         else // The fitted sphere is inside the convex hull of the vertices (a,b,c,d), so it must be optimal.
1262         {
1263                 const vec center = s*ab + t*ac + u*ad;
1264
1265                 sphere.pos = a + center;
1266                 // Mathematically, the following would be correct, but it suffers from floating point inaccuracies,
1267                 // since it only tests distance against one point.
1268                 //sphere.r = center.Length();
1269
1270                 // For robustness, take the radius to be the distance to the farthest point (though the distance are all
1271                 // equal).
1272                 sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c), sphere.pos.DistanceSq(d)));
1273         }
1274
1275                 // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests
1276                 // really contain the points (note that the points must be sufficiently near enough to the origin)
1277                 sphere.r += 2.f*epsilon; // We test against one epsilon, so expand using 2 epsilons.
1278
1279 #ifdef MATH_ASSERT_CORRECTNESS
1280         if (!sphere.Contains(a, epsilon) || !sphere.Contains(b, epsilon) || !sphere.Contains(c, epsilon) || !sphere.Contains(d, epsilon))
1281         {
1282                 LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r);
1283                 LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos));
1284                 LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos));
1285                 LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos));
1286                 LOGE("D: %s, dist: %f", d.ToString().c_str(), d.Distance(sphere.pos));
1287                 mathassert(false);
1288         }
1289 #endif
1290
1291         return sphere;
1292 }
1293
1294 Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c, const vec &d, const vec &e,
1295                                       int &redundantPoint)
1296 {
1297         Sphere s = OptimalEnclosingSphere(b,c,d,e);
1298         if (s.Contains(a, epsilon))
1299         {
1300                 redundantPoint = 0;
1301                 return s;
1302         }
1303         s = OptimalEnclosingSphere(a,c,d,e);
1304         if (s.Contains(b, epsilon))
1305         {
1306                 redundantPoint = 1;
1307                 return s;
1308         }
1309         s = OptimalEnclosingSphere(a,b,d,e);
1310         if (s.Contains(c, epsilon))
1311         {
1312                 redundantPoint = 2;
1313                 return s;
1314         }
1315         s = OptimalEnclosingSphere(a,b,c,e);
1316         if (s.Contains(d, epsilon))
1317         {
1318                 redundantPoint = 3;
1319                 return s;
1320         }
1321         s = OptimalEnclosingSphere(a,b,c,d);
1322         mathassert(s.Contains(e, epsilon));
1323         redundantPoint = 4;
1324         return s;
1325 }
1326
1327 /** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */
1328 Sphere Sphere::FitThroughPoints(const vec &a, const vec &b, const vec &c)
1329 {
1330         Sphere sphere;
1331
1332         vec ab = b-a;
1333         vec ac = c-a;
1334
1335         float s, t;
1336         bool success = FitSphereThroughPoints(ab, ac, s, t);
1337         if (!success)
1338         {
1339                 LOGW("Sphere::FitThroughPoints(a,b,c) failed! The three input points are collinear!");
1340                 sphere.SetDegenerate();
1341                 return sphere;
1342         }
1343
1344         const vec p = s*ab + t*ac;
1345
1346         // In our translated coordinate space, the origin lies on the sphere, so the distance of p from origin
1347         // gives the radius of the sphere.
1348         sphere.r = p.Length();
1349
1350         // Translate back to original coordinate space.
1351         sphere.pos = a + p;
1352
1353         return sphere;
1354 }
1355
1356 /** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */
1357 Sphere Sphere::FitThroughPoints(const vec &a, const vec &b, const vec &c, const vec &d)
1358 {
1359         Sphere sphere;
1360
1361         float s,t,u;
1362         const vec ab = b-a;
1363         const vec ac = c-a;
1364         const vec ad = d-a;
1365         bool success = FitSphereThroughPoints(ab, ac, ad, s, t, u);
1366         if (success)
1367         {
1368                 const vec center = s*ab + t*ac + u*ad;
1369                 sphere.r = center.Length();
1370                 sphere.pos = a + center;
1371         }
1372         else
1373         {
1374                 LOGW("Sphere::FitThroughPoints through four points failed! The points lie on the same plane!");
1375                 sphere.SetDegenerate();
1376         }
1377
1378         return sphere;
1379 }
1380
1381 #ifdef MATH_ENABLE_STL_SUPPORT
1382 std::string Sphere::ToString() const
1383 {
1384         char str[256];
1385         sprintf(str, "Sphere(pos:(%.2f, %.2f, %.2f) r:%.2f)",
1386                 pos.x, pos.y, pos.z, r);
1387         return str;
1388 }
1389
1390 std::string Sphere::SerializeToString() const
1391 {
1392         char str[256];
1393         char *s = SerializeFloat(pos.x, str); *s = ','; ++s;
1394         s = SerializeFloat(pos.y, s); *s = ','; ++s;
1395         s = SerializeFloat(pos.z, s); *s = ','; ++s;
1396         s = SerializeFloat(r, s);
1397         assert(s+1 - str < 256);
1398         MARK_UNUSED(s);
1399         return str;
1400 }
1401
1402 std::string Sphere::SerializeToCodeString() const
1403 {
1404         char str[256];
1405         sprintf(str, "%.9g", r);
1406         return "Sphere(" + pos.SerializeToCodeString() + "," + str + ")";
1407 }
1408
1409 std::ostream &operator <<(std::ostream &o, const Sphere &sphere)
1410 {
1411         o << sphere.ToString();
1412         return o;
1413 }
1414
1415 #endif
1416
1417 Sphere Sphere::FromString(const char *str, const char **outEndStr)
1418 {
1419         assume(str);
1420         if (!str)
1421                 return Sphere(vec::nan, FLOAT_NAN);
1422         Sphere s;
1423         MATH_SKIP_WORD(str, "Sphere(");
1424         MATH_SKIP_WORD(str, "pos:(");
1425         s.pos = PointVecFromString(str, &str);
1426         MATH_SKIP_WORD(str, " r:");
1427         s.r = DeserializeFloat(str, &str);
1428         if (outEndStr)
1429                 *outEndStr = str;
1430         return s;
1431 }
1432
1433 bool Sphere::BitEquals(const Sphere &other) const
1434 {
1435         return pos.BitEquals(other.pos) && ReinterpretAsU32(r) == ReinterpretAsU32(other.r);
1436 }
1437
1438 Sphere operator *(const float3x3 &transform, const Sphere &sphere)
1439 {
1440         Sphere s(sphere);
1441         s.Transform(transform);
1442         return s;
1443 }
1444
1445 Sphere operator *(const float3x4 &transform, const Sphere &sphere)
1446 {
1447         Sphere s(sphere);
1448         s.Transform(transform);
1449         return s;
1450 }
1451
1452 Sphere operator *(const float4x4 &transform, const Sphere &sphere)
1453 {
1454         Sphere s(sphere);
1455         s.Transform(transform);
1456         return s;
1457 }
1458
1459 Sphere operator *(const Quat &transform, const Sphere &sphere)
1460 {
1461         Sphere s(sphere);
1462         s.Transform(transform);
1463         return s;
1464 }
1465
1466 #ifdef MATH_GRAPHICSENGINE_INTEROP
1467 void Sphere::Triangulate(VertexBuffer &vb, int numVertices, bool ccwIsFrontFacing) const
1468 {
1469         Array<vec> pos;
1470         Array<vec> normal;
1471         Array<float2> uv;
1472         pos.Resize_pod(numVertices);
1473         normal.Resize_pod(numVertices);
1474         uv.Resize_pod(numVertices);
1475         Triangulate(pos.beginptr(), normal.beginptr(), uv.beginptr(), numVertices, ccwIsFrontFacing);
1476         int startIndex = vb.AppendVertices(numVertices);
1477         for(int i = 0; i < (int)pos.size(); ++i)
1478         {
1479                 vb.Set(startIndex+i, VDPosition, POINT_TO_FLOAT4(pos[i]));
1480                 if (vb.Declaration()->TypeOffset(VDNormal) >= 0)
1481                         vb.Set(startIndex+i, VDNormal, DIR_TO_FLOAT4(normal[i]));
1482                 if (vb.Declaration()->TypeOffset(VDUV) >= 0)
1483                         vb.SetFloat2(startIndex+i, VDUV, 0, uv[i]);
1484         }
1485 }
1486 #endif
1487
1488 MATH_END_NAMESPACE