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 Triangle.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Triangle geometry object. */
18 #include "Triangle.h"
19 #include "../Math/MathFunc.h"
20 #include "../Math/float2.h"
21 #include "../Math/float3.h"
22 #include "../Math/float3x3.h"
23 #include "../Math/float3x4.h"
24 #include "../Math/float4x4.h"
25 #include "../Math/Quat.h"
26 #include "Capsule.h"
27 #include "Frustum.h"
28 #include "Plane.h"
29 #include "Polygon.h"
30 #include "Polyhedron.h"
31 #include "Line.h"
32 #include "LineSegment.h"
33 #include "Ray.h"
34 #include "Sphere.h"
35 #include "AABB.h"
36 #include "OBB.h"
37 #include "../Algorithm/Random/LCG.h"
38
39 #ifdef MATH_ENABLE_STL_SUPPORT
40 #include <iostream>
41 #endif
42
43 #if defined(MATH_SSE) && defined(MATH_AUTOMATIC_SSE)
44 #include "../Math/float4_sse.h"
45 #endif
46
47 MATH_BEGIN_NAMESPACE
48
49 Triangle::Triangle(const vec &a_, const vec &b_, const vec &c_)
50 :a(a_), b(b_), c(c_)
51 {
52 }
53
54 void Triangle::Translate(const vec &offset)
55 {
56         a += offset;
57         b += offset;
58         c += offset;
59 }
60
61 void Triangle::Transform(const float3x3 &transform)
62 {
63         transform.BatchTransform(&a, 3);
64 }
65
66 void Triangle::Transform(const float3x4 &transform)
67 {
68         transform.BatchTransformPos(&a, 3);
69 }
70
71 void Triangle::Transform(const float4x4 &transform)
72 {
73         a = transform.MulPos(a);
74         b = transform.MulPos(b);
75         c = transform.MulPos(c);
76 }
77
78 void Triangle::Transform(const Quat &transform)
79 {
80         a = transform * a;
81         b = transform * b;
82         c = transform * c;
83 }
84
85 /// Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
86 inline float TriArea2D(float x1, float y1, float x2, float y2, float x3, float y3)
87 {
88         return (x1-x2)*(y2-y3) - (x2-x3)*(y1-y2);
89 }
90
91 float3 Triangle::BarycentricUVW(const vec &point) const
92 {
93         // Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
94
95         // Unnormalized triangle normal.
96         vec m = Cross(b-a, c-a);
97
98         // Nominators and one-over-denominator for u and v ratios.
99         float nu, nv, ood;
100
101         // Absolute components for determining projection plane.
102         float x = Abs(m.x);
103         float y = Abs(m.y);
104         float z = Abs(m.z);
105
106         if (x >= y && x >= z)
107         {
108                 // Project to the yz plane.
109                 nu = TriArea2D(point.y, point.z, b.y, b.z, c.y, c.z); // Area of PBC in yz-plane.
110                 nv = TriArea2D(point.y, point.z, c.y, c.z, a.y, a.z); // Area OF PCA in yz-plane.
111                 ood = 1.f / m.x; // 1 / (2*area of ABC in yz plane)
112         }
113         else if (y >= z) // Note: The book has a redundant 'if (y >= x)' comparison
114         {
115                 // y is largest, project to the xz-plane.
116                 nu = TriArea2D(point.x, point.z, b.x, b.z, c.x, c.z);
117                 nv = TriArea2D(point.x, point.z, c.x, c.z, a.x, a.z);
118                 ood = 1.f / -m.y;
119         }
120         else // z is largest, project to the xy-plane.
121         {
122                 nu = TriArea2D(point.x, point.y, b.x, b.y, c.x, c.y);
123                 nv = TriArea2D(point.x, point.y, c.x, c.y, a.x, a.y);
124                 ood = 1.f / m.z;
125         }
126         float u = nu * ood;
127         float v = nv * ood;
128         float w = 1.f - u - v;
129         return float3(u,v,w);
130 #if 0 // TODO: This version should be more SIMD-friendly, but for some reason, it doesn't return good values for all points inside the triangle.
131         vec v0 = b - a;
132         vec v1 = c - a;
133         vec v2 = point - a;
134         float d00 = Dot(v0, v0);
135         float d01 = Dot(v0, v1);
136         float d02 = Dot(v0, v2);
137         float d11 = Dot(v1, v1);
138         float d12 = Dot(v1, v2);
139         float denom = 1.f / (d00 * d11 - d01 * d01);
140         float v = (d11 * d02 - d01 * d12) * denom;
141         float w = (d00 * d12 - d01 * d02) * denom;
142         float u = 1.0f - v - w;
143         return vec(u, v, w);
144 #endif
145 }
146
147 float2 Triangle::BarycentricUV(const vec &point) const
148 {
149         float3 uvw = BarycentricUVW(point);
150         return float2(uvw.y, uvw.z);
151 }
152
153 bool Triangle::BarycentricInsideTriangle(const float3 &barycentric)
154 {
155         return barycentric.x >= 0.f && barycentric.y >= 0.f && barycentric.z >= 0.f &&
156                 EqualAbs(barycentric.x + barycentric.y + barycentric.z, 1.f);
157 }
158
159 vec Triangle::Point(float u, float v) const
160 {
161         // In case the triangle is far away from the origin but is small in size, the elements of 'a' will have large magnitudes,
162         // and the elements of (b-a) and (c-a) will be much smaller quantities. Therefore be extra careful with the
163         // parentheses and first sum the small floats together before adding it to the large one.
164         return a + ((b-a) * u + (c-a) * v);
165 }
166
167 vec Triangle::Point(float u, float v, float w) const
168 {
169         return u * a + v * b + w * c;
170 }
171
172 vec Triangle::Point(const float3 &b) const
173 {
174         return Point(b.x, b.y, b.z);
175 }
176
177 vec Triangle::Point(const float2 &b) const
178 {
179         return Point(b.x, b.y);
180 }
181
182 vec Triangle::Centroid() const
183 {
184         return (a + b + c) * (1.f/3.f);
185 }
186
187 float Triangle::Area() const
188 {
189         return 0.5f * Cross(b-a, c-a).Length();
190 }
191
192 float Triangle::Perimeter() const
193 {
194         return a.Distance(b) + b.Distance(c) + c.Distance(a);
195 }
196
197 LineSegment Triangle::Edge(int i) const
198 {
199         assume(0 <= i);
200         assume(i <= 2);
201         if (i == 0)
202                 return LineSegment(a, b);
203         else if (i == 1)
204                 return LineSegment(b, c);
205         else if (i == 2)
206                 return LineSegment(c, a);
207         else
208                 return LineSegment(vec::nan, vec::nan);
209 }
210
211 vec Triangle::Vertex(int i) const
212 {
213         assume(0 <= i);
214         assume(i <= 2);
215         if (i == 0)
216                 return a;
217         else if (i == 1)
218                 return b;
219         else if (i == 2)
220                 return c;
221         else
222                 return vec::nan;
223 }
224
225 Plane Triangle::PlaneCCW() const
226 {
227         return Plane(a, b, c);
228 }
229
230 Plane Triangle::PlaneCW() const
231 {
232         return Plane(a, c, b);
233 }
234
235 vec Triangle::NormalCCW() const
236 {
237         return UnnormalizedNormalCCW().Normalized();
238 }
239
240 vec Triangle::NormalCW() const
241 {
242         return UnnormalizedNormalCW().Normalized();
243 }
244
245 vec Triangle::UnnormalizedNormalCCW() const
246 {
247         return Cross(b-a, c-a);
248 }
249
250 vec Triangle::UnnormalizedNormalCW() const
251 {
252         return Cross(c-a, b-a);
253 }
254
255 vec Triangle::ExtremePoint(const vec &direction) const
256 {
257         vec mostExtreme = vec::nan;
258         float mostExtremeDist = -FLT_MAX;
259         for(int i = 0; i < 3; ++i)
260         {
261                 vec pt = Vertex(i);
262                 float d = Dot(direction, pt);
263                 if (d > mostExtremeDist)
264                 {
265                         mostExtremeDist = d;
266                         mostExtreme = pt;
267                 }
268         }
269         return mostExtreme;
270 }
271
272 vec Triangle::ExtremePoint(const vec &direction, float &projectionDistance) const
273 {
274         vec extremePoint = ExtremePoint(direction);
275         projectionDistance = extremePoint.Dot(direction);
276         return extremePoint;
277 }
278
279 Polygon Triangle::ToPolygon() const
280 {
281         Polygon p;
282         p.p.push_back(a);
283         p.p.push_back(b);
284         p.p.push_back(c);
285         return p;
286 }
287
288 Polyhedron Triangle::ToPolyhedron() const
289 {
290         return ToPolygon().ToPolyhedron();
291 }
292
293 AABB Triangle::BoundingAABB() const
294 {
295         AABB aabb;
296         aabb.minPoint = Min(a, b, c);
297         aabb.maxPoint = Max(a, b, c);
298         return aabb;
299 }
300
301 float Triangle::Area2D(const float2 &p1, const float2 &p2, const float2 &p3)
302 {
303         return (p1.x - p2.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p2.y);
304 }
305
306 float Triangle::SignedArea(const vec &pt, const vec &a, const vec &b, const vec &c)
307 {
308         return Dot(Cross(b-pt, c-pt), Cross(b-a, c-a).Normalized());
309 }
310
311 bool Triangle::IsFinite() const
312 {
313         return a.IsFinite() && b.IsFinite() && c.IsFinite();
314 }
315
316 bool Triangle::IsDegenerate(float epsilon) const
317 {
318         return IsDegenerate(a, b, c, epsilon);
319 }
320
321 bool Triangle::IsDegenerate(const vec &a, const vec &b, const vec &c, float epsilon)
322 {
323         return a.Equals(b, epsilon) || a.Equals(c, epsilon) || b.Equals(c, epsilon);
324 }
325
326 bool Triangle::Contains(const vec &point, float triangleThicknessSq) const
327 {
328         vec normal = (b-a).Cross(c-a);
329         float lenSq = normal.LengthSq();
330         float d = normal.Dot(b - point);
331         if (d*d > triangleThicknessSq * lenSq)
332                 return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
333
334         float3 br = BarycentricUVW(point);
335         return br.x >= -1e-3f && br.y >= -1e-3f && br.z >= -1e-3f; // Allow for a small epsilon to properly account for points very near the edges of the triangle.
336 }
337
338 bool Triangle::Contains(const LineSegment &lineSegment, float triangleThickness) const
339 {
340         return Contains(lineSegment.a, triangleThickness) && Contains(lineSegment.b, triangleThickness);
341 }
342
343 bool Triangle::Contains(const Triangle &triangle, float triangleThickness) const
344 {
345         return Contains(triangle.a, triangleThickness) && Contains(triangle.b, triangleThickness)
346           && Contains(triangle.c, triangleThickness);
347 }
348
349 /*
350 bool Triangle::Contains(const Polygon &polygon, float triangleThickness) const
351 {
352         if (polygon.points.size() == 0)
353                 return false;
354         for(int i = 0; i < polygon.points.size(); ++i)
355                 if (!Contains(polygon.points[i], triangleThickness))
356                         return false;
357         return true;
358 }
359 */
360 float Triangle::Distance(const vec &point) const
361 {
362         return ClosestPoint(point).Distance(point);
363 }
364
365 float Triangle::DistanceSq(const vec &point) const
366 {
367         return ClosestPoint(point).DistanceSq(point);
368 }
369
370 float Triangle::Distance(const Sphere &sphere) const
371 {
372         return Max(0.f, Distance(sphere.pos) - sphere.r);
373 }
374
375 float Triangle::Distance(const Capsule &capsule) const
376 {
377         vec otherPt;
378         vec thisPt = ClosestPoint(capsule.l, &otherPt);
379         return Max(0.f, thisPt.Distance(otherPt) - capsule.r);
380 }
381
382 /** Calculates the intersection between a line and a triangle. The facing is not accounted for, so
383         rays are reported to intersect triangles that are both front and backfacing.
384         According to "T. M&ouml;ller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
385         http://jgt.akpeters.com/papers/MollerTrumbore97/
386         @param linePos The starting point of the line.
387         @param lineDir The direction vector of the line. This does not need to be normalized.
388         @param v0 Vertex 0 of the triangle.
389         @param v1 Vertex 1 of the triangle.
390         @param v2 Vertex 2 of the triangle.
391         @param u [out] The barycentric u coordinate is returned here if an intersection occurred.
392         @param v [out] The barycentric v coordinate is returned here if an intersection occurred.
393         @return The distance along the ray to the point of intersection, or +inf if no intersection occurred.
394                 If no intersection, then u and v and t will contain undefined values. If lineDir was not normalized, then to get the
395                 real world-space distance, one must scale the returned value with lineDir.Length(). If the returned value is negative,
396                 then the intersection occurs 'behind' the line starting position, with respect to the direction vector lineDir. */
397 float Triangle::IntersectLineTri(const vec &linePos, const vec &lineDir,
398                 const vec &v0, const vec &v1, const vec &v2,
399                 float &u, float &v)
400 {
401         const float epsilon = 1e-4f;
402
403         // Edge vectors
404         vec vE1 = v1 - v0;
405         vec vE2 = v2 - v0;
406
407         // begin calculating determinant - also used to calculate U parameter
408         vec vP = lineDir.Cross(vE2);
409
410         // If det < 0, intersecting backfacing tri, > 0, intersecting frontfacing tri, 0, parallel to plane.
411         const float det = vE1.Dot(vP);
412
413         // If determinant is near zero, ray lies in plane of triangle.
414         if (Abs(det) <= epsilon)
415                 return FLOAT_INF;
416         const float recipDet = 1.f / det;
417
418         // Calculate distance from v0 to ray origin
419         vec vT = linePos - v0;
420
421         // Output barycentric u
422         u = vT.Dot(vP) * recipDet;
423         if (u < -epsilon || u > 1.f + epsilon)
424                 return FLOAT_INF; // Barycentric U is outside the triangle - early out.
425
426         // Prepare to test V parameter
427         vec vQ = vT.Cross(vE1);
428
429         // Output barycentric v
430         v = lineDir.Dot(vQ) * recipDet;
431         if (v < -epsilon || u + v > 1.f + epsilon) // Barycentric V or the combination of U and V are outside the triangle - no intersection.
432                 return FLOAT_INF;
433
434         // Barycentric u and v are in limits, the ray intersects the triangle.
435         
436         // Output signed distance from ray to triangle.
437         return vE2.Dot(vQ) * recipDet;
438 //      return (det < 0.f) ? IntersectBackface : IntersectFrontface;
439 }
440
441 /// [groupSyntax]
442 bool Triangle::Intersects(const LineSegment &l, float *d, vec *intersectionPoint) const
443 {
444         /** The Triangle-Line/LineSegment/Ray intersection tests are based on M&ouml;ller-Trumbore method:
445                 "T. M&ouml;ller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
446                 http://jgt.akpeters.com/papers/MollerTrumbore97/. */
447         float u, v;
448         float t = IntersectLineTri(l.a, l.b - l.a, a, b, c, u, v);
449         bool success = (t >= 0.0f && t <= 1.0f);
450         if (!success)
451                 return false;
452         if (d)
453                 *d = t;
454         if (intersectionPoint)
455                 *intersectionPoint = l.GetPoint(t);
456         return true;
457 }
458
459 bool Triangle::Intersects(const Line &l, float *d, vec *intersectionPoint) const
460 {
461         float u, v;
462         float t = IntersectLineTri(l.pos, l.dir, a, b, c, u, v);
463         bool success = (t != FLOAT_INF);
464         if (!success)
465                 return false;
466         if (d)
467                 *d = t;
468         if (intersectionPoint)
469                 *intersectionPoint = l.GetPoint(t);
470         return success;
471 }
472
473 bool Triangle::Intersects(const Ray &r, float *d, vec *intersectionPoint) const
474 {
475         float u, v;
476         float t = IntersectLineTri(r.pos, r.dir, a, b, c, u, v);
477         bool success = (t >= 0 && t != FLOAT_INF);
478         if (!success)
479                 return false;
480         if (d)
481                 *d = t;
482         if (intersectionPoint)
483                 *intersectionPoint = r.GetPoint(t);
484         return success;
485 }
486
487 bool Triangle::Intersects(const Plane &plane) const
488 {
489         return plane.Intersects(*this);
490 }
491
492 /// [groupSyntax]
493 /** For Triangle-Sphere intersection code, see Christer Ericson's Real-Time Collision Detection, p.167. */
494 bool Triangle::Intersects(const Sphere &sphere, vec *closestPointOnTriangle) const
495 {
496         vec pt = ClosestPoint(sphere.pos);
497
498         if (closestPointOnTriangle)
499                 *closestPointOnTriangle = pt;
500
501         return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
502 }
503
504 bool Triangle::Intersects(const Sphere &sphere) const
505 {
506         return Intersects(sphere, 0);
507 }
508
509 static void FindIntersectingLineSegments(const Triangle &t, float da, float db, float dc, LineSegment &l1, LineSegment &l2)
510 {
511         if (da*db > 0.f)
512         {
513                 l1 = LineSegment(t.a, t.c);
514                 l2 = LineSegment(t.b, t.c);
515         }
516         else if (db*dc > 0.f)
517         {
518                 l1 = LineSegment(t.a, t.b);
519                 l2 = LineSegment(t.a, t.c);
520         }
521         else
522         {
523                 l1 = LineSegment(t.a, t.b);
524                 l2 = LineSegment(t.b, t.c);
525         }
526 }
527
528 int IntervalIntersection(float u0, float u1, float v0, float v1, float &s, float &t)
529 {
530         if (u1 < v0 || u0 > v1)
531         {
532                 s = t = FLOAT_NAN;
533                 return 0;
534         }
535
536         if (u1 > v0)
537         {
538                 if (u0 < v1)
539                 {
540                         s = Max(u0, v0);
541                         t = Min(u1, v1);
542                         return 2;
543                 }
544                 else // u0 == v1
545                 {
546                         s = t = u0;
547                         return 1;
548                 }
549         }
550         else
551         {
552                 // u1 == v0
553                 s = t = u1;
554                 return 1;
555         }
556 }
557
558 /// 2D line segment-line segment intersection from Geometric Tools for Computer Graphics, pp. 807-813, by Schneider and Eberly.
559 bool LineSegment2DLineSegment2DIntersect(const float2 &p0, const float2 &dir0, const float2 &p1, const float2 &dir1, float &s, float &t)
560 {
561         float2 e = p1 - p0;
562         float cross = dir0.PerpDot(dir1);
563         float sqrCross = cross*cross;
564         float sqrLen0 = dir0.LengthSq();
565         float sqrLen1 = dir1.LengthSq();
566         const float sqrEpsilon = 1e-8f;
567         if (sqrCross > sqrEpsilon * sqrLen0 * sqrLen1)
568         {
569                 // The lines are not parallel
570                 s = e.PerpDot(dir1) / cross;
571                 t = e.PerpDot(dir0) / cross;
572                 // The intersection point is p0 + s * dir0;
573                 return s >= 0.f && s <= 1.f && t >= 0.f && t <= 1.f;
574         }
575
576         // The line segments are parallel
577         float sqrLenE = e.LengthSq();
578         cross = e.PerpDot(dir0);
579         sqrCross = cross*cross;
580         if (sqrCross > sqrEpsilon * sqrLen0 * sqrLenE)
581         {
582                 // The lines are at a positive distance, no intersection.
583                 s = t = FLOAT_NAN;
584                 return false;
585         }
586
587         // The line segments are along the same line. Do an overlap test.
588         float s0 = Dot(dir0, e) / sqrLen0;
589         float s1 = s0 + Dot(dir0, dir1) / sqrLen0;
590         float smin = Min(s0, s1);
591         float smax = Max(s0, s1);
592
593         int nIntersections = IntervalIntersection(0.f, 1.f, smin, smax, s, t);
594         return nIntersections > 0.f;
595 }
596
597 /// [groupSyntax]
598 /** The Triangle-Triangle test implementation is based on pseudo-code from Tomas M&ouml;ller's
599         "A Fast Triangle-Triangle Intersection Test": http://jgt.akpeters.com/papers/Moller97/.
600         See also Christer Ericson's Real-Time Collision Detection, p. 172. */
601 bool Triangle::Intersects(const Triangle &t2, LineSegment *outLine) const
602 {
603         const float triangleThickness = 1e-4f;
604         // Is the triangle t2 completely on one side of the plane of this triangle?
605         Plane p1 = this->PlaneCCW();
606         Plane p2 = t2.PlaneCCW();
607         if (!p1.normal.Cross(p2.normal).IsZero())
608         {
609                 float t2da = p1.SignedDistance(t2.a);
610                 float t2db = p1.SignedDistance(t2.b);
611                 float t2dc = p1.SignedDistance(t2.c);
612                 float t2min = Min(Min(t2da, t2db), t2dc);
613                 float t2max = Max(Max(t2da, t2db), t2dc);
614                 if (t2max < -triangleThickness || t2min > triangleThickness)
615                         return false;
616                 // Is this triangle completely on one side of the plane of the triangle t2?
617                 float t1da = p2.SignedDistance(this->a);
618                 float t1db = p2.SignedDistance(this->b);
619                 float t1dc = p2.SignedDistance(this->c);
620                 float t1min = Min(Min(t1da, t1db), t1dc);
621                 float t1max = Max(Max(t1da, t1db), t1dc);
622                 if (t1max < -triangleThickness || t1min > triangleThickness)
623                         return false;
624
625                 // Find the intersection line of the two planes.
626                 Line l;
627                 bool success = p1.Intersects(p2, &l);
628                 assume(success); // We already determined the two triangles have intersecting planes, so this should always succeed.
629                 if (!success)
630                         return false;
631
632                 // Find the two line segments of both triangles which straddle the intersection line.
633                 LineSegment l1a, l1b;
634                 LineSegment l2a, l2b;
635                 FindIntersectingLineSegments(*this, t1da, t1db, t1dc, l1a, l1b);
636                 FindIntersectingLineSegments(t2, t2da, t2db, t2dc, l2a, l2b);
637
638                 // Find the projection intervals on the intersection line.
639                 float d1a, d1b, d2a, d2b;
640                 l.Distance(l1a, d1a);
641                 l.Distance(l1b, d1b);
642                 l.Distance(l2a, d2a);
643                 l.Distance(l2b, d2b);
644                 if (d1a > d1b)
645                         Swap(d1a, d1b);
646                 if (d2a > d2b)
647                         Swap(d2a, d2b);
648                 float rStart = Max(d1a, d2a);
649                 float rEnd = Min(d1b, d2b);
650                 if (rStart <= rEnd)
651                 {
652                         if (outLine)
653                                 *outLine = LineSegment(l.GetPoint(rStart), l.GetPoint(rEnd));
654                         return true;
655                 }
656                 return false;
657         }
658         else // The two triangles lie in the same plane. Perform the intersection test in 2D.
659         {
660                 float tri0Pos = p1.normal.Dot(a);
661                 float tri1Pos = p1.normal.Dot(t2.a);
662                 if (Abs(tri0Pos-tri1Pos) > triangleThickness)
663                         return false;
664
665                 if (t2.Contains(a, FLOAT_INF) || this->Contains(t2.a, FLOAT_INF))
666                         return true;
667
668                 vec basisU, basisV;
669                 p1.normal.PerpendicularBasis(basisU, basisV);
670                 float2 a1 = float2::zero;
671                 float2 a2 = float2(basisU.Dot(b-a), basisV.Dot(b-a));
672                 float2 a3 = float2(basisU.Dot(c-a), basisV.Dot(c-a));
673                 float2 b1 = float2(basisU.Dot(t2.a-a), basisV.Dot(t2.a-a));
674                 float2 b2 = float2(basisU.Dot(t2.b-a), basisV.Dot(t2.b-a));
675                 float2 b3 = float2(basisU.Dot(t2.c-a), basisV.Dot(t2.c-a));
676                 float s, t;
677                 if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b1, b2-b1, s, t)) { if (outLine) { float2 pt = s*(a2-a1); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
678                 if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b2, b3-b2, s, t)) { if (outLine) { float2 pt = s*(a2-a1); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
679                 if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b3, b1-b3, s, t)) { if (outLine) { float2 pt = s*(a2-a1); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
680                 if (LineSegment2DLineSegment2DIntersect(a2, a3-a2, b1, b2-b1, s, t)) { if (outLine) { float2 pt = s*(a3-a2); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
681                 if (LineSegment2DLineSegment2DIntersect(a2, a3-a2, b2, b3-b2, s, t)) { if (outLine) { float2 pt = s*(a3-a2); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
682                 if (LineSegment2DLineSegment2DIntersect(a2, a3-a2, b3, b1-b3, s, t)) { if (outLine) { float2 pt = s*(a3-a2); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
683                 if (LineSegment2DLineSegment2DIntersect(a3, a1-a3, b1, b2-b1, s, t)) { if (outLine) { float2 pt = s*(a1-a3); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
684                 if (LineSegment2DLineSegment2DIntersect(a3, a1-a3, b2, b3-b2, s, t)) { if (outLine) { float2 pt = s*(a1-a3); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
685                 if (LineSegment2DLineSegment2DIntersect(a3, a1-a3, b3, b1-b3, s, t)) { if (outLine) { float2 pt = s*(a1-a3); vec pt3d = a+pt.x*basisU+pt.y*basisV; *outLine = LineSegment(pt3d, pt3d); } return true; }
686                 return false;
687         }
688 }
689
690 bool RangesOverlap(float start1, float end1, float start2, float end2)
691 {
692         return end1 >= start2 && end2 >= start1;
693 }
694
695 /// [groupSyntax]
696 bool Triangle::Intersects(const AABB &aabb) const
697 {
698 /** The AABB-Triangle test implementation is based on the pseudo-code in
699         Christer Ericson's Real-Time Collision Detection, pp. 169-172. It is
700         practically a standard SAT test. */
701 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
702         // Benchmark 'Triangle_intersects_AABB': Triangle::Intersects(AABB)
703         //    Best: 8.065 nsecs / 21.664 ticks, Avg: 8.207 nsecs, Worst: 9.985 nsecs
704         simd4f tMin = min_ps(a, min_ps(b, c));
705         simd4f tMax = max_ps(a, max_ps(b, c));
706
707         simd4f cmp = cmpge_ps(tMin, aabb.maxPoint.v);
708         cmp = or_ps(cmp, cmple_ps(tMax, aabb.minPoint.v));
709         cmp = and_ps(cmp, set_ps_hex(0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)); // Mask off results from the W channel.
710         if (!allzero_ps(cmp)) return false;
711
712         simd4f center = mul_ps(add_ps(aabb.minPoint.v, aabb.maxPoint.v), set1_ps(0.5f));
713         simd4f h = sub_ps(aabb.maxPoint.v, center);
714
715         simd4f t0 = sub_ps(b, a);
716         simd4f t1 = sub_ps(c, a);
717         simd4f ac = sub_ps(a, center);
718         simd4f n = cross_ps(t0, t1);
719         if (_mm_ucomige_ss(abs_ps(dot4_ps(n, ac)), abs_ps(dot4_ps(h, abs_ps(n))))) return false;
720
721         // {eX, eY, eZ} cross t1
722         simd4f ac_wyxz  = shuffle1_ps(ac,  _MM_SHUFFLE(3, 1, 0, 2));
723         simd4f h_wyxz   = shuffle1_ps(h,   _MM_SHUFFLE(3, 1, 0, 2));
724         simd4f ac_wxzy  = shuffle1_ps(ac,  _MM_SHUFFLE(3, 0, 2, 1));
725         simd4f h_wxzy   = shuffle1_ps(h,   _MM_SHUFFLE(3, 0, 2, 1));
726         simd4f bc = sub_ps(b, center);
727         simd4f bc_wyxz  = shuffle1_ps(bc,  _MM_SHUFFLE(3, 1, 0, 2));
728         simd4f at1 = abs_ps(t1);
729         simd4f t1_wyxz  = shuffle1_ps(t1,  _MM_SHUFFLE(3, 1, 0, 2));
730         simd4f at1_wyxz = shuffle1_ps(at1, _MM_SHUFFLE(3, 1, 0, 2));
731         simd4f bc_wxzy  = shuffle1_ps(bc,  _MM_SHUFFLE(3, 0, 2, 1));
732         simd4f t1_wxzy  = shuffle1_ps(t1,  _MM_SHUFFLE(3, 0, 2, 1));
733         simd4f at1_wxzy = shuffle1_ps(at1, _MM_SHUFFLE(3, 0, 2, 1));
734
735         simd4f d1 = sub_ps(mul_ps(t1_wxzy, ac_wyxz), mul_ps(t1_wyxz, ac_wxzy));
736         simd4f d2 = sub_ps(mul_ps(t1_wxzy, bc_wyxz), mul_ps(t1_wyxz, bc_wxzy));
737         simd4f tc = mul_ps(add_ps(d1, d2), set1_ps(0.5f));
738         simd4f r = abs_ps(add_ps(mul_ps(h_wyxz, at1_wxzy), mul_ps(h_wxzy, at1_wyxz)));
739         cmp = cmple_ps(add_ps(r, abs_ps(sub_ps(tc, d1))), abs_ps(tc));
740         // Note: The three masks of W channel could be omitted if cmplt_ps was used instead of cmple_ps, but
741         // want to be strict here and define that AABB and Triangle which touch at a vertex should not intersect.
742         cmp = and_ps(cmp, set_ps_hex(0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)); // Mask off results from the W channel.
743         if (!allzero_ps(cmp)) return false;
744
745         // {eX, eY, eZ} cross t2
746         simd4f t2 = sub_ps(c, b);
747         simd4f at2 = abs_ps(t2);
748         simd4f t2_wyxz  = shuffle1_ps(t2,  _MM_SHUFFLE(3, 1, 0, 2));
749         simd4f at2_wyxz = shuffle1_ps(at2, _MM_SHUFFLE(3, 1, 0, 2));
750         simd4f t2_wxzy  = shuffle1_ps(t2,  _MM_SHUFFLE(3, 0, 2, 1));
751         simd4f at2_wxzy = shuffle1_ps(at2, _MM_SHUFFLE(3, 0, 2, 1));
752
753         d1 = sub_ps(mul_ps(t2_wxzy, ac_wyxz), mul_ps(t2_wyxz, ac_wxzy));
754         d2 = sub_ps(mul_ps(t2_wxzy, bc_wyxz), mul_ps(t2_wyxz, bc_wxzy));
755         tc = mul_ps(add_ps(d1, d2), set1_ps(0.5f));
756         r = abs_ps(add_ps(mul_ps(h_wyxz, at2_wxzy), mul_ps(h_wxzy, at2_wyxz)));
757         cmp = cmple_ps(add_ps(r, abs_ps(sub_ps(tc, d1))), abs_ps(tc));
758         cmp = and_ps(cmp, set_ps_hex(0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)); // Mask off results from the W channel.
759         if (!allzero_ps(cmp)) return false;
760
761         // {eX, eY, eZ} cross t0
762         simd4f cc = sub_ps(c, center);
763         simd4f cc_wyxz  = shuffle1_ps(cc,  _MM_SHUFFLE(3, 1, 0, 2));
764         simd4f t0_wyxz  = shuffle1_ps(t0,  _MM_SHUFFLE(3, 1, 0, 2));
765         simd4f at0 = abs_ps(t0);
766         simd4f at0_wyxz = shuffle1_ps(at0, _MM_SHUFFLE(3, 1, 0, 2));
767         simd4f at0_wxzy = shuffle1_ps(at0, _MM_SHUFFLE(3, 0, 2, 1));
768         simd4f t0_wxzy  = shuffle1_ps(t0,  _MM_SHUFFLE(3, 0, 2, 1));
769         simd4f cc_wxzy  = shuffle1_ps(cc,  _MM_SHUFFLE(3, 0, 2, 1));
770
771         d1 = sub_ps(mul_ps(t0_wxzy, ac_wyxz), mul_ps(t0_wyxz, ac_wxzy));
772         d2 = sub_ps(mul_ps(t0_wxzy, cc_wyxz), mul_ps(t0_wyxz, cc_wxzy));
773         tc = mul_ps(add_ps(d1, d2), set1_ps(0.5f));
774         r = abs_ps(add_ps(mul_ps(h_wyxz, at0_wxzy), mul_ps(h_wxzy, at0_wyxz)));
775         cmp = cmple_ps(add_ps(r, abs_ps(sub_ps(tc, d1))), abs_ps(tc));
776         cmp = and_ps(cmp, set_ps_hex(0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)); // Mask off results from the W channel.
777         return allzero_ps(cmp) != 0;
778 #else
779         // Benchmark 'Triangle_intersects_AABB': Triangle::Intersects(AABB)
780         //    Best: 17.282 nsecs / 46.496 ticks, Avg: 17.804 nsecs, Worst: 18.434 nsecs
781         vec tMin = a.Min(b.Min(c));
782         vec tMax = a.Max(b.Max(c));
783
784         if (tMin.x >= aabb.maxPoint.x || tMax.x <= aabb.minPoint.x
785                 || tMin.y >= aabb.maxPoint.y || tMax.y <= aabb.minPoint.y
786                 || tMin.z >= aabb.maxPoint.z || tMax.z <= aabb.minPoint.z)
787                 return false;
788
789         vec center = (aabb.minPoint + aabb.maxPoint) * 0.5f;
790         vec h = aabb.maxPoint - center;
791
792         const vec t[3] = { b-a, c-a, c-b };
793
794         vec ac = a-center;
795
796         vec n = Cross(t[0], t[1]);
797         float s = n.Dot(ac);
798         float r = Abs(h.Dot(n.Abs()));
799         if (Abs(s) >= r)
800                 return false;
801
802         const vec at[3] = { Abs(t[0]), Abs(t[1]), Abs(t[2]) };
803
804         vec bc = b-center;
805         vec cc = c-center;
806
807         // SAT test all cross-axes.
808         // The following is a fully unrolled loop of this code, stored here for reference:
809         /*
810         float d1, d2, a1, a2;
811         const vec e[3] = { DIR_VEC(1, 0, 0), DIR_VEC(0, 1, 0), DIR_VEC(0, 0, 1) };
812         for(int i = 0; i < 3; ++i)
813                 for(int j = 0; j < 3; ++j)
814                 {
815                         vec axis = Cross(e[i], t[j]);
816                         ProjectToAxis(axis, d1, d2);
817                         aabb.ProjectToAxis(axis, a1, a2);
818                         if (d2 <= a1 || d1 >= a2) return false;
819                 }
820         */
821
822         // eX <cross> t[0]
823         float d1 = t[0].y * ac.z - t[0].z * ac.y;
824         float d2 = t[0].y * cc.z - t[0].z * cc.y;
825         float tc = (d1 + d2) * 0.5f;
826         r = Abs(h.y * at[0].z + h.z * at[0].y);
827         if (r + Abs(tc - d1) <= Abs(tc))
828                 return false;
829
830         // eX <cross> t[1]
831         d1 = t[1].y * ac.z - t[1].z * ac.y;
832         d2 = t[1].y * bc.z - t[1].z * bc.y;
833         tc = (d1 + d2) * 0.5f;
834         r = Abs(h.y * at[1].z + h.z * at[1].y);
835         if (r + Abs(tc - d1) <= Abs(tc))
836                 return false;
837
838         // eX <cross> t[2]
839         d1 = t[2].y * ac.z - t[2].z * ac.y;
840         d2 = t[2].y * bc.z - t[2].z * bc.y;
841         tc = (d1 + d2) * 0.5f;
842         r = Abs(h.y * at[2].z + h.z * at[2].y);
843         if (r + Abs(tc - d1) <= Abs(tc))
844                 return false;
845
846         // eY <cross> t[0]
847         d1 = t[0].z * ac.x - t[0].x * ac.z;
848         d2 = t[0].z * cc.x - t[0].x * cc.z;
849         tc = (d1 + d2) * 0.5f;
850         r = Abs(h.x * at[0].z + h.z * at[0].x);
851         if (r + Abs(tc - d1) <= Abs(tc))
852                 return false;
853
854         // eY <cross> t[1]
855         d1 = t[1].z * ac.x - t[1].x * ac.z;
856         d2 = t[1].z * bc.x - t[1].x * bc.z;
857         tc = (d1 + d2) * 0.5f;
858         r = Abs(h.x * at[1].z + h.z * at[1].x);
859         if (r + Abs(tc - d1) <= Abs(tc))
860                 return false;
861
862         // eY <cross> t[2]
863         d1 = t[2].z * ac.x - t[2].x * ac.z;
864         d2 = t[2].z * bc.x - t[2].x * bc.z;
865         tc = (d1 + d2) * 0.5f;
866         r = Abs(h.x * at[2].z + h.z * at[2].x);
867         if (r + Abs(tc - d1) <= Abs(tc))
868                 return false;
869
870         // eZ <cross> t[0]
871         d1 = t[0].x * ac.y - t[0].y * ac.x;
872         d2 = t[0].x * cc.y - t[0].y * cc.x;
873         tc = (d1 + d2) * 0.5f;
874         r = Abs(h.y * at[0].x + h.x * at[0].y);
875         if (r + Abs(tc - d1) <= Abs(tc))
876                 return false;
877
878         // eZ <cross> t[1]
879         d1 = t[1].x * ac.y - t[1].y * ac.x;
880         d2 = t[1].x * bc.y - t[1].y * bc.x;
881         tc = (d1 + d2) * 0.5f;
882         r = Abs(h.y * at[1].x + h.x * at[1].y);
883         if (r + Abs(tc - d1) <= Abs(tc))
884                 return false;
885
886         // eZ <cross> t[2]
887         d1 = t[2].x * ac.y - t[2].y * ac.x;
888         d2 = t[2].x * bc.y - t[2].y * bc.x;
889         tc = (d1 + d2) * 0.5f;
890         r = Abs(h.y * at[2].x + h.x * at[2].y);
891         if (r + Abs(tc - d1) <= Abs(tc))
892                 return false;
893
894         // No separating axis exists, the AABB and triangle intersect.
895         return true;
896 #endif
897 }
898
899 bool Triangle::Intersects(const OBB &obb) const
900 {
901         return obb.Intersects(*this);
902 }
903
904 bool Triangle::Intersects(const Polygon &polygon) const
905 {
906         return polygon.Intersects(*this);
907 }
908
909 bool Triangle::Intersects(const Frustum &frustum) const
910 {
911         return frustum.Intersects(*this);
912 }
913
914 bool Triangle::Intersects(const Polyhedron &polyhedron) const
915 {
916         return polyhedron.Intersects(*this);
917 }
918
919 bool Triangle::Intersects(const Capsule &capsule) const
920 {
921         return capsule.Intersects(*this);
922 }
923
924 void Triangle::ProjectToAxis(const vec &axis, float &dMin, float &dMax) const
925 {
926         dMin = dMax = Dot(axis, a);
927         float t = Dot(axis, b);
928         dMin = Min(t, dMin);
929         dMax = Max(t, dMax);
930         t = Dot(axis, c);
931         dMin = Min(t, dMin);
932         dMax = Max(t, dMax);
933 }
934
935 int Triangle::UniqueFaceNormals(vec *out) const
936 {
937         out[0] = Cross(b-a, c-a);
938         // If testing a pair of coplanar triangles for SAT intersection,
939         // the common face normal will not be a separating axis, and neither will
940         // the cross products of the pairwise edges, since those all coincide with the face normals.
941         // Therefore to make SAT test work properly in that case, also report all the edge normals
942         // as possible separation test directions, which fixes the coplanar case.
943         // For more info, see Geometric Tools for Computer Graphics, 11.11.1, p. 612.
944         out[1] = Cross(out[0], b-a);
945         out[2] = Cross(out[0], c-a);
946         out[3] = Cross(out[0], c-b);
947         return 4;
948 }
949
950 int Triangle::UniqueEdgeDirections(vec *out) const
951 {
952         out[0] = b-a;
953         out[1] = c-a;
954         out[2] = c-b;
955         return 3;
956 }
957
958 /// [groupSyntax]
959 vec Triangle::ClosestPoint(const vec &p) const
960 {
961         /** The code for Triangle-float3 test is from Christer Ericson's Real-Time Collision Detection, pp. 141-142. */
962
963         // Check if P is in vertex region outside A.
964         vec ab = b - a;
965         vec ac = c - a;
966         vec ap = p - a;
967         float d1 = Dot(ab, ap);
968         float d2 = Dot(ac, ap);
969         if (d1 <= 0.f && d2 <= 0.f)
970                 return a; // Barycentric coordinates are (1,0,0).
971
972         // Check if P is in vertex region outside B.
973         vec bp = p - b;
974         float d3 = Dot(ab, bp);
975         float d4 = Dot(ac, bp);
976         if (d3 >= 0.f && d4 <= d3)
977                 return b; // Barycentric coordinates are (0,1,0).
978
979         // Check if P is in edge region of AB, and if so, return the projection of P onto AB.
980         float vc = d1*d4 - d3*d2;
981         if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
982         {
983                 float v = d1 / (d1 - d3);
984                 return a + v * ab; // The barycentric coordinates are (1-v, v, 0).
985         }
986
987         // Check if P is in vertex region outside C.
988         vec cp = p - c;
989         float d5 = Dot(ab, cp);
990         float d6 = Dot(ac, cp);
991         if (d6 >= 0.f && d5 <= d6)
992                 return c; // The barycentric coordinates are (0,0,1).
993
994         // Check if P is in edge region of AC, and if so, return the projection of P onto AC.
995         float vb = d5*d2 - d1*d6;
996         if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
997         {
998                 float w = d2 / (d2 - d6);
999                 return a + w * ac; // The barycentric coordinates are (1-w, 0, w).
1000         }
1001
1002         // Check if P is in edge region of BC, and if so, return the projection of P onto BC.
1003         float va = d3*d6 - d5*d4;
1004         if (va <= 0.f && d4 - d3 >= 0.f && d5 - d6 >= 0.f)
1005         {
1006                 float w = (d4 - d3) / (d4 - d3 + d5 - d6);
1007                 return b + w * (c - b); // The barycentric coordinates are (0, 1-w, w).
1008         }
1009
1010         // P must be inside the face region. Compute the closest point through its barycentric coordinates (u,v,w).
1011         float denom = 1.f / (va + vb + vc);
1012         float v = vb * denom;
1013         float w = vc * denom;
1014         return a + ab * v + ac * w;
1015 }
1016
1017 vec Triangle::ClosestPoint(const LineSegment &lineSegment, vec *otherPt) const
1018 {
1019         ///\todo Optimize.
1020         float u, v;
1021         float t = IntersectLineTri(lineSegment.a, lineSegment.b - lineSegment.a, a, b, c, u, v);
1022         bool intersects = (t >= 0.0f && t <= 1.0f);
1023         if (intersects)
1024         {
1025 //              assume3(lineSegment.GetPoint(t).Equals(this->Point(u, v)), lineSegment.GetPoint(t).SerializeToCodeString(), this->Point(u, v).SerializeToCodeString(), lineSegment.GetPoint(t).Distance(this->Point(u, v)));
1026                 if (otherPt)
1027                         *otherPt = lineSegment.GetPoint(t);
1028                 return this->Point(u, v);
1029         }
1030
1031         float u1,v1,d1;
1032         vec pt1 = ClosestPointToTriangleEdge(lineSegment, &u1, &v1, &d1);
1033
1034         vec pt2 = ClosestPoint(lineSegment.a);
1035         vec pt3 = ClosestPoint(lineSegment.b);
1036         
1037         float D1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
1038         float D2 = pt2.DistanceSq(lineSegment.a);
1039         float D3 = pt3.DistanceSq(lineSegment.b);
1040
1041         if (D1 <= D2 && D1 <= D3)
1042         {
1043                 if (otherPt)
1044                         *otherPt = lineSegment.GetPoint(d1);
1045                 return pt1;
1046         }
1047         else if (D2 <= D3)
1048         {
1049                 if (otherPt)
1050                         *otherPt = lineSegment.a;
1051                 return pt2;
1052         }
1053         else
1054         {
1055                 if (otherPt)
1056                         *otherPt = lineSegment.b;
1057                 return pt3;
1058         }
1059 }
1060
1061 #if 0
1062 ///\todo Enable this codepath. This if rom Geometric Tools for Computer Graphics,
1063 /// but the algorithm in the book is broken and does not take into account the
1064 /// direction of the gradient to determine the proper region of intersection.
1065 /// Instead using a slower code path above.
1066 /// [groupSyntax]
1067 vec Triangle::ClosestPoint(const LineSegment &lineSegment, vec *otherPt) const
1068 {
1069         vec e0 = b - a;
1070         vec e1 = c - a;
1071         vec v_p = a - lineSegment.a;
1072         vec d = lineSegment.b - lineSegment.a;
1073
1074         // Q(u,v) = a + u*e0 + v*e1
1075         // L(t)   = ls.a + t*d
1076         // Minimize the distance |Q(u,v) - L(t)|^2 under u >= 0, v >= 0, u+v <= 1, t >= 0, t <= 1.
1077
1078         float v_p_dot_e0 = Dot(v_p, e0);
1079         float v_p_dot_e1 = Dot(v_p, e1);
1080         float v_p_dot_d = Dot(v_p, d);
1081
1082         float3x3 m;
1083         m[0][0] = Dot(e0, e0); m[0][1] = Dot(e0, e1); m[0][2] = -Dot(e0, d);
1084         m[1][0] =     m[0][1]; m[1][1] = Dot(e1, e1); m[1][2] = -Dot(e1, d);
1085         m[2][0] =     m[0][2]; m[2][1] =     m[1][2]; m[2][2] =  Dot(d, d);
1086
1087         float3 B(-v_p_dot_e0, -v_p_dot_e1, v_p_dot_d);
1088
1089         float3 uvt;
1090         bool success = m.SolveAxb(B, uvt);
1091         if (!success)
1092         {
1093                 float t1, t2, t3;
1094                 float s1, s2, s3;
1095                 LineSegment e1 = Edge(0);
1096                 LineSegment e2 = Edge(1);
1097                 LineSegment e3 = Edge(2);
1098                 float d1 = e1.Distance(lineSegment, &t1, &s1);
1099                 float d2 = e2.Distance(lineSegment, &t2, &s2);
1100                 float d3 = e3.Distance(lineSegment, &t3, &s3);
1101                 if (d1 < d2 && d1 < d3)
1102                 {
1103                         if (otherPt)
1104                                 *otherPt = lineSegment.GetPoint(s1);
1105                         return e1.GetPoint(t1);
1106                 }
1107                 else if (d2 < d3)
1108                 {
1109                         if (otherPt)
1110                                 *otherPt = lineSegment.GetPoint(s2);
1111                         return e2.GetPoint(t2);
1112                 }
1113                 else
1114                 {
1115                         if (otherPt)
1116                                 *otherPt = lineSegment.GetPoint(s3);
1117                         return e3.GetPoint(t3);
1118                 }
1119         }
1120
1121         if (uvt.x < 0.f)
1122         {
1123                 // Clamp to u == 0 and solve again.
1124                 float m_00 = m[2][2];
1125                 float m_01 = -m[1][2];
1126                 float m_10 = -m[2][1];
1127                 float m_11 = m[1][1];
1128                 float det = m_00 * m_11 - m_01 * m_10;
1129                 float v = m_00 * B[1] + m_01 * B[2];
1130                 float t = m_10 * B[1] + m_11 * B[2];
1131                 v /= det;
1132                 t /= det;
1133                 if (v < 0.f)
1134                 {
1135                         // Clamp to v == 0 and solve for t.
1136                         t = B[2] / m[2][2];
1137                         t = Clamp01(t); // The solution for t must also be in the range [0,1].
1138                         // The solution is (u,v,t)=(0,0,t).
1139                         if (otherPt)
1140                                 *otherPt = lineSegment.GetPoint(t);
1141                         return a;
1142                 }
1143                 else if (v > 1.f)
1144                 {
1145                         // Clamp to v == 1 and solve for t.
1146                         t = (B[2] - m[2][1]) / m[2][2];
1147                         t = Clamp01(t);
1148                         // The solution is (u,v,t)=(0,1,t).
1149                         if (otherPt)
1150                                 *otherPt = lineSegment.GetPoint(t);
1151                         return c; // == a + v*e1
1152                 }
1153                 else if (t < 0.f)
1154                 {
1155                         // Clamp to t == 0 and solve for v.
1156                         v = B[1] / m[1][1];
1157 //                      mathassert(EqualAbs(v, Clamp01(v)));
1158                         v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
1159                         // The solution is (u,v,t)=(0,v,0).
1160                         if (otherPt)
1161                                 *otherPt = lineSegment.a;
1162                         return a + v * e1;
1163                 }
1164                 else if (t > 1.f)
1165                 {
1166                         // Clamp to t == 1 and solve for v.
1167                         v = (B[1] - m[1][2]) / m[1][1];
1168 //                      mathassert(EqualAbs(v, Clamp01(v)));
1169                         v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
1170                         // The solution is (u,v,t)=(0,v,1).
1171                         if (otherPt)
1172                                 *otherPt = lineSegment.b;
1173                         return a + v * e1;
1174                 }
1175                 else
1176                 {
1177                         // The solution is (u,v,t)=(0,v,t).
1178                         if (otherPt)
1179                                 *otherPt = lineSegment.GetPoint(t);
1180                         return a + v * e1;
1181                 }
1182         }
1183         else if (uvt.y < 0.f)
1184         {
1185                 // Clamp to v == 0 and solve again.
1186                 float m_00 = m[2][2];
1187                 float m_01 = -m[0][2];
1188                 float m_10 = -m[2][0];
1189                 float m_11 = m[0][0];
1190                 float det = m_00 * m_11 - m_01 * m_10;
1191                 float u = m_00 * B[0] + m_01 * B[2];
1192                 float t = m_10 * B[0] + m_11 * B[2];
1193                 u /= det;
1194                 t /= det;
1195
1196                 if (u < 0.f)
1197                 {
1198                         // Clamp to u == 0 and solve for t.
1199                         t = B[2] / m[2][2];
1200                         t = Clamp01(t); // The solution for t must also be in the range [0,1].
1201                         // The solution is (u,v,t)=(0,0,t).
1202                         if (otherPt)
1203                                 *otherPt = lineSegment.GetPoint(t);
1204                         return a;
1205                 }
1206                 else if (u > 1.f)
1207                 {
1208                         // Clamp to u == 1 and solve for t.
1209                         t = (B[2] - m[2][0]) / m[2][2];
1210                         t = Clamp01(t); // The solution for t must also be in the range [0,1].
1211                         // The solution is (u,v,t)=(1,0,t).
1212                         if (otherPt)
1213                                 *otherPt = lineSegment.GetPoint(t);
1214                         return b;
1215                 }
1216                 else if (t < 0.f)
1217                 {
1218                         // Clamp to t == 0 and solve for u.
1219                         u = B[0] / m[0][0];
1220 //                      mathassert(EqualAbs(u, Clamp01(u)));
1221                         u = Clamp01(u); // The solution for u must also be in the range [0,1].
1222                         if (otherPt)
1223                                 *otherPt = lineSegment.a;
1224                         return a + u * e0;
1225                 }
1226                 else if (t > 1.f)
1227                 {
1228                         // Clamp to t == 1 and solve for u.
1229                         u = (B[0] - m[0][2]) / m[0][0];
1230 //                      mathassert(EqualAbs(u, Clamp01(u)));
1231                         u = Clamp01(u); // The solution for u must also be in the range [0,1].
1232                         if (otherPt)
1233                                 *otherPt = lineSegment.b;
1234                         return a + u * e0;
1235                 }
1236                 else
1237                 {
1238                         // The solution is (u, 0, t).
1239                         if (otherPt)
1240                                 *otherPt = lineSegment.GetPoint(t);
1241                         return a + u * e0;
1242                 }
1243         }
1244         else if (uvt.z < 0.f)
1245         {
1246                 if (otherPt)
1247                         *otherPt = lineSegment.a;
1248                 // Clamp to t == 0 and solve again.
1249                 float m_00 = m[1][1];
1250                 float m_01 = -m[0][1];
1251                 float m_10 = -m[1][0];
1252                 float m_11 = m[0][0];
1253                 float det = m_00 * m_11 - m_01 * m_10;
1254                 float u = m_00 * B[0] + m_01 * B[1];
1255                 float v = m_10 * B[0] + m_11 * B[1];
1256                 u /= det;
1257                 v /= det;
1258                 if (u < 0.f)
1259                 {
1260                         // Clamp to u == 0 and solve for v.
1261                         v = B[1] / m[1][1];
1262                         v = Clamp01(v);
1263                         return a + v*e1;
1264                 }
1265                 else if (v < 0.f)
1266                 {
1267                         // Clamp to v == 0 and solve for u.
1268                         u = B[0] / m[0][0];
1269                         u = Clamp01(u);
1270                         return a + u*e0;
1271                 }
1272                 else if (u+v > 1.f)
1273                 {
1274                         // Set v = 1-u and solve again.
1275 //                      u = (B[0] - m[0][0]) / (m[0][0] - m[0][1]);
1276 //                      mathassert(EqualAbs(u, Clamp01(u)));
1277 //                      u = Clamp01(u); // The solution for u must also be in the range [0,1].
1278 //                      return a + u*e0;
1279
1280                         // Clamp to v = 1-u and solve again.
1281                         float m_00 = m[2][2];
1282                         float m_01 = m[1][2] - m[0][2];
1283                         float m_10 = m_01;
1284                         float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
1285                         float det = m_00 * m_11 - m_01 * m_10;
1286                         float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
1287                         float b1 = -m[1][2] + v_p_dot_d;
1288                         float u = m_00 * b0 + m_01 * b1;
1289                         u /= det;
1290                         u = Clamp01(u);
1291
1292                         float t = m_10 * b0 + m_11 * b1;
1293                         t /= det;
1294                         t = Clamp01(t);
1295                         if (otherPt)
1296                                 *otherPt = lineSegment.GetPoint(t);
1297                         return a + u*e0 + (1.f-u)*e1;
1298                 }
1299                 else
1300                 {
1301                         // The solution is (u, v, 0)
1302                         return a + u * e0 + v * e1;
1303                 }
1304         }
1305         else if (uvt.z > 1.f)
1306         {
1307                 if (otherPt)
1308                         *otherPt = lineSegment.b;
1309                 // Clamp to t == 1 and solve again.
1310                 float m_00 = m[1][1];
1311                 float m_01 = -m[0][1];
1312                 float m_10 = -m[1][0];
1313                 float m_11 = m[0][0];
1314                 float det = m_00 * m_11 - m_01 * m_10;
1315                 float u = m_00 * (B[0]-m[0][2]) + m_01 * (B[1]-m[1][2]);
1316                 float v = m_10 * (B[0]-m[0][2]) + m_11 * (B[1]-m[1][2]);
1317                 u /= det;
1318                 v /= det;
1319                 if (u < 0.f)
1320                 {
1321                         // Clamp to u == 0 and solve again.
1322                         v = (B[1] - m[1][2]) / m[1][1];
1323                         v = Clamp01(v);
1324                         return a + v*e1;
1325                 }
1326                 else if (u > 1.f)
1327                 {
1328                         // Clamp to u == 1 and solve again.
1329                         v = (B[1] - m[1][0] - m[1][2]) / m[1][1];
1330                         v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
1331                         // The solution is (u,v,t)=(1,v,1).
1332                         return a + e0 + v*e1;
1333                 }
1334                 else if (u+v > 1.f)
1335                 {
1336                         // Set v = 1-u and solve again.
1337
1338                         // Q(u,1-u) = a + u*e0 + e1 - u*e1 = a+e1 + u*(e0-e1)
1339                         // L(1)   = ls.a + t*d = ls.b
1340                         // Minimize the distance |Q(u,1-u) - L(1)| = |a+e1+ls.b + u*(e0-e1)|
1341
1342                         // |K + u*(e0-e1)|^2 = (K,K) + 2*u(K,e0-e1) + u^2 * (e0-e1,e0-e1)
1343
1344                         // grad = 2*(K,e0-e1) + 2*u*(e0-e1,e0-e1) == 0
1345                         //                                      u == (K,e1-e0) / (e0-e1,e0-e1)
1346
1347                         u = (B[0] - m[0][1] - m[0][2]) / (m[0][0] - m[0][1]);
1348 //                      u = Dot(a + e1 + lineSegment.b, e1 - e0) / Dot(e0-e1, e0-e1);
1349
1350 //                      mathassert(EqualAbs(u, Clamp01(u)));
1351                         u = Clamp01(u);
1352                         return a + u*e0 + (1-u)*e1;
1353                 }
1354                 else
1355                 {
1356                         // The solution is (u, v, 1)
1357                         return a + u*e0 + v*e1;
1358                 }
1359         }
1360         else if (uvt.x + uvt.y > 1.f)
1361         {
1362                 // Clamp to v = 1-u and solve again.
1363                 float m_00 = m[2][2];
1364                 float m_01 = m[1][2] - m[0][2];
1365                 float m_10 = m_01;
1366                 float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
1367                 float det = m_00 * m_11 - m_01 * m_10;
1368                 float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
1369                 float b1 = -m[1][2] + v_p_dot_d;
1370                 float u = m_00 * b0 + m_01 * b1;
1371                 float t = m_10 * b0 + m_11 * b1;
1372                 u /= det;
1373                 t /= det;
1374
1375                 t = Clamp01(t);
1376                 if (otherPt)
1377                         *otherPt = lineSegment.GetPoint(t);
1378
1379                 if (u < 0.f)
1380                 {
1381                         // The solution is (u,v,t)=(0,1,t)
1382                         return c;
1383                 }
1384                 if (u > 1.f)
1385                 {
1386                         // The solution is (u,v,t)=(1,0,t)
1387                         return b;
1388                 }
1389                 mathassert(t >= 0.f);
1390                 mathassert(t <= 1.f);
1391                 return a + u*e0 + (1.f-u)*e1;
1392         }
1393         else // All parameters are within range, so the triangle and the line segment intersect, and the intersection point is the closest point.
1394         {
1395                 if (otherPt)
1396                         *otherPt = lineSegment.GetPoint(uvt.z);
1397                 return a + uvt.x * e0 + uvt.y * e1;
1398         }
1399 }
1400 #endif
1401 vec Triangle::ClosestPointToTriangleEdge(const Line &other, float *outU, float *outV, float *outD) const
1402 {
1403         ///@todo Optimize!
1404         // The line is parallel to the triangle.
1405         float unused1, unused2, unused3;
1406         float d1, d2, d3;
1407         vec pt1 = Edge(0).ClosestPoint(other, unused1, d1);
1408         vec pt2 = Edge(1).ClosestPoint(other, unused2, d2);
1409         vec pt3 = Edge(2).ClosestPoint(other, unused3, d3);
1410         float dist1 = pt1.DistanceSq(other.GetPoint(d1));
1411         float dist2 = pt2.DistanceSq(other.GetPoint(d2));
1412         float dist3 = pt3.DistanceSq(other.GetPoint(d3));
1413         if (dist1 <= dist2 && dist1 <= dist3)
1414         {
1415                 if (outU) *outU = BarycentricUV(pt1).x;
1416                 if (outV) *outV = BarycentricUV(pt1).y;
1417                 if (outD) *outD = d1;
1418                 return pt1;
1419         }
1420         else if (dist2 <= dist3)
1421         {
1422                 if (outU) *outU = BarycentricUV(pt2).x;
1423                 if (outV) *outV = BarycentricUV(pt2).y;
1424                 if (outD) *outD = d2;
1425                 return pt2;
1426         }
1427         else
1428         {
1429                 if (outU) *outU = BarycentricUV(pt3).x;
1430                 if (outV) *outV = BarycentricUV(pt3).y;
1431                 if (outD) *outD = d3;
1432                 return pt3;
1433         }
1434 }
1435
1436 vec Triangle::ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const
1437 {
1438         ///@todo Optimize!
1439         // The line is parallel to the triangle.
1440         float unused1, unused2, unused3;
1441         float d1, d2, d3;
1442         vec pt1 = Edge(0).ClosestPoint(lineSegment, unused1, d1);
1443         vec pt2 = Edge(1).ClosestPoint(lineSegment, unused2, d2);
1444         vec pt3 = Edge(2).ClosestPoint(lineSegment, unused3, d3);
1445         float dist1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
1446         float dist2 = pt2.DistanceSq(lineSegment.GetPoint(d2));
1447         float dist3 = pt3.DistanceSq(lineSegment.GetPoint(d3));
1448         if (dist1 <= dist2 && dist1 <= dist3)
1449         {
1450                 if (outU) *outU = BarycentricUV(pt1).x;
1451                 if (outV) *outV = BarycentricUV(pt1).y;
1452                 if (outD) *outD = d1;
1453                 return pt1;
1454         }
1455         else if (dist2 <= dist3)
1456         {
1457                 if (outU) *outU = BarycentricUV(pt2).x;
1458                 if (outV) *outV = BarycentricUV(pt2).y;
1459                 if (outD) *outD = d2;
1460                 return pt2;
1461         }
1462         else
1463         {
1464                 if (outU) *outU = BarycentricUV(pt3).x;
1465                 if (outV) *outV = BarycentricUV(pt3).y;
1466                 if (outD) *outD = d3;
1467                 return pt3;
1468         }
1469 }
1470
1471 vec Triangle::ClosestPoint(const Line &line, vec *otherPt) const
1472 {
1473         ///\todo Optimize this function.
1474         vec intersectionPoint;
1475         if (Intersects(line, 0, &intersectionPoint))
1476         {
1477                 if (otherPt)
1478                         *otherPt = intersectionPoint;
1479                 return intersectionPoint;
1480         }
1481
1482         float u1,v1,d1;
1483         vec pt1 = ClosestPointToTriangleEdge(line, &u1, &v1, &d1);
1484         if (otherPt)
1485                 *otherPt = line.GetPoint(d1);
1486         return pt1;
1487 }
1488
1489 #if 0
1490 ///\todo Enable this codepath. This if rom Geometric Tools for Computer Graphics,
1491 /// but the algorithm in the book is broken and does not take into account the
1492 /// direction of the gradient to determine the proper region of intersection.
1493 /// Instead using a slower code path above.
1494 vec Triangle::ClosestPoint(const Line &line, vec *otherPt) const
1495 {
1496         vec e0 = b - a;
1497         vec e1 = c - a;
1498         vec v_p = a - line.pos;
1499         vec d = line.dir;
1500
1501         float v_p_dot_e0 = Dot(v_p, e0);
1502         float v_p_dot_e1 = Dot(v_p, e1);
1503         float v_p_dot_d = Dot(v_p, d);
1504
1505         float3x3 m;
1506         m[0][0] = Dot(e0, e0); m[0][1] = Dot(e0, e1); m[0][2] = -Dot(e0, d);
1507         m[1][0] =     m[0][1]; m[1][1] = Dot(e1, e1); m[1][2] = -Dot(e1, d);
1508         m[2][0] =     m[0][2]; m[2][1] =     m[1][2]; m[2][2] =  Dot(d, d);
1509
1510         float3 B(-v_p_dot_e0, -v_p_dot_e1, v_p_dot_d);
1511
1512         float3 uvt;
1513         bool success = m.SolveAxb(B, uvt);
1514         if (!success)
1515         {
1516                 float t1, t2, t3;
1517                 float s1, s2, s3;
1518                 LineSegment e1 = Edge(0);
1519                 LineSegment e2 = Edge(1);
1520                 LineSegment e3 = Edge(2);
1521                 float d1 = e1.Distance(line, &t1, &s1);
1522                 float d2 = e2.Distance(line, &t2, &s2);
1523                 float d3 = e3.Distance(line, &t3, &s3);
1524                 if (d1 < d2 && d1 < d3)
1525                 {
1526                         if (otherPt)
1527                                 *otherPt = line.GetPoint(s1);
1528                         return e1.GetPoint(t1);
1529                 }
1530                 else if (d2 < d3)
1531                 {
1532                         if (otherPt)
1533                                 *otherPt = line.GetPoint(s2);
1534                         return e2.GetPoint(t2);
1535                 }
1536                 else
1537                 {
1538                         if (otherPt)
1539                                 *otherPt = line.GetPoint(s3);
1540                         return e3.GetPoint(t3);
1541                 }
1542         }
1543
1544         if (uvt.x < 0.f)
1545         {
1546                 // Clamp to u == 0 and solve again.
1547                 float m_00 = m[2][2];
1548                 float m_01 = -m[1][2];
1549                 float m_10 = -m[2][1];
1550                 float m_11 = m[1][1];
1551                 float det = m_00 * m_11 - m_01 * m_10;
1552                 float v = m_00 * B[1] + m_01 * B[2];
1553                 float t = m_10 * B[1] + m_11 * B[2];
1554                 v /= det;
1555                 t /= det;
1556                 if (v < 0.f)
1557                 {
1558                         // Clamp to v == 0 and solve for t.
1559                         t = B[2] / m[2][2];
1560                         // The solution is (u,v,t)=(0,0,t).
1561                         if (otherPt)
1562                                 *otherPt = line.GetPoint(t);
1563                         return a;
1564                 }
1565                 else if (v > 1.f)
1566                 {
1567                         // Clamp to v == 1 and solve for t.
1568                         t = (B[2] - m[2][1]) / m[2][2];
1569                         // The solution is (u,v,t)=(0,1,t).
1570                         if (otherPt)
1571                                 *otherPt = line.GetPoint(t);
1572                         return c; // == a + v*e1
1573                 }
1574                 else
1575                 {
1576                         // The solution is (u,v,t)=(0,v,t).
1577                         if (otherPt)
1578                                 *otherPt = line.GetPoint(t);
1579                         return a + v * e1;
1580                 }
1581         }
1582         else if (uvt.y < 0.f)
1583         {
1584                 // Clamp to v == 0 and solve again.
1585                 float m_00 = m[2][2];
1586                 float m_01 = -m[0][2];
1587                 float m_10 = -m[2][0];
1588                 float m_11 = m[0][0];
1589                 float det = m_00 * m_11 - m_01 * m_10;
1590                 float u = m_00 * B[0] + m_01 * B[2];
1591                 float t = m_10 * B[0] + m_11 * B[2];
1592                 u /= det;
1593                 t /= det;
1594
1595                 if (u < 0.f)
1596                 {
1597                         // Clamp to u == 0 and solve for t.
1598                         t = B[2] / m[2][2];
1599                         // The solution is (u,v,t)=(0,0,t).
1600                         if (otherPt)
1601                                 *otherPt = line.GetPoint(t);
1602                         return a;
1603                 }
1604                 else if (u > 1.f)
1605                 {
1606                         // Clamp to u == 1 and solve for t.
1607                         t = (B[2] - m[2][0]) / m[2][2];
1608                         // The solution is (u,v,t)=(1,0,t).
1609                         if (otherPt)
1610                                 *otherPt = line.GetPoint(t);
1611                         return b;
1612                 }
1613                 else
1614                 {
1615                         // The solution is (u, 0, t).
1616                         if (otherPt)
1617                                 *otherPt = line.GetPoint(t);
1618                         return a + u * e0;
1619                 }
1620         }
1621         else if (uvt.x + uvt.y > 1.f)
1622         {
1623                 // Clamp to v = 1-u and solve again.
1624                 float m_00 = m[2][2];
1625                 float m_01 = m[1][2] - m[0][2];
1626                 float m_10 = m_01;
1627                 float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
1628                 float det = m_00 * m_11 - m_01 * m_10;
1629                 float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
1630                 float b1 = -m[1][2] + v_p_dot_d;
1631                 float u = m_00 * b0 + m_01 * b1;
1632                 float t = m_10 * b0 + m_11 * b1;
1633                 u /= det;
1634                 t /= det;
1635
1636                 if (otherPt)
1637                         *otherPt = line.GetPoint(t);
1638
1639                 if (u < 0.f)
1640                 {
1641                         // The solution is (u,v,t)=(0,1,t)
1642                         return c;
1643                 }
1644                 if (u > 1.f)
1645                 {
1646                         // The solution is (u,v,t)=(1,0,t)
1647                         return b;
1648                 }
1649                 return a + u*e0 + (1.f-u)*e1;
1650         }
1651         else // All parameters are within range, so the triangle and the line segment intersect, and the intersection point is the closest point.
1652         {
1653                 if (otherPt)
1654                         *otherPt = line.GetPoint(uvt.z);
1655                 return a + uvt.x * e0 + uvt.y * e1;
1656         }
1657 }
1658 #endif
1659
1660 #if 0
1661 /// [groupSyntax]
1662 vec Triangle::ClosestPoint(const Line &other, float *outU, float *outV, float *outD) const
1663 {
1664         /** The implementation of the Triangle-Line test is based on the pseudo-code in
1665                 Schneider, Eberly. Geometric Tools for Computer Graphics pp. 433 - 441. */
1666         ///@todo The Triangle-Line code is currently untested. Run tests to ensure the following code works properly.
1667
1668         // Point on triangle: T(u,v) = a + u*b + v*c;
1669         // Point on line:  L(t) = p + t*d;
1670         // Minimize the function Q(u,v,t) = ||T(u,v) - L(t)||.
1671
1672         vec e0 = b-a;
1673         vec e1 = c-a;
1674         vec d = other.dir;
1675
1676         const float d_e0e0 = Dot(e0, e0);
1677         const float d_e0e1 = Dot(e0, e1);
1678         const float d_e0d = Dot(e0, d);
1679         const float d_e1e1 = Dot(e1, e1);
1680         const float d_e1d = Dot(e1, d);
1681         const float d_dd = Dot(d, d);
1682
1683         float3x3 m;
1684         m[0][0] = d_e0e0;  m[0][1] = d_e0e1;  m[0][2] = -d_e0d;
1685         m[1][0] = d_e0e1;  m[1][1] = d_e1e1;  m[1][2] = -d_e1d;
1686         m[2][0] = -d_e0d;  m[2][1] = -d_e1d;  m[2][2] =   d_dd;
1687
1688         ///@todo Add optimized float3x3::InverseSymmetric().
1689         bool inv = m.Inverse();
1690         if (!inv)
1691                 return ClosestPointToTriangleEdge(other, outU, outV, outD);
1692
1693         vec v_m_p = a - other.pos;
1694         float v_m_p_e0 = v_m_p.Dot(e0);
1695         float v_m_p_e1 = v_m_p.Dot(e1);
1696         float v_m_p_d = v_m_p.Dot(d);
1697         float3 b = float3(-v_m_p_e0, -v_m_p_e1, v_m_p_d);
1698         float3 uvt = m * b;
1699         // We cannot simply clamp the solution to (uv) inside the constraints, since the expression we
1700         // are minimizing is quadratic.
1701         // So, examine case-by-case which part of the space the solution lies in. Because the function is convex,
1702         // we can clamp the search space to the boundary planes.
1703         float u = uvt.x;
1704         float v = uvt.y;
1705         float t = uvt.z;
1706         if (u <= 0)
1707         {
1708                 if (outU) *outU = 0;
1709
1710                 // Solve 2x2 matrix for the (v,t) solution when u == 0.
1711                 v = m[1][1]*b[1] + m[1][2]*b[2];
1712                 t = m[2][1]*b[1] + m[2][2]*b[2];
1713
1714                 // Check if the solution is still out of bounds.
1715                 if (v <= 0)
1716                 {
1717                         if (outV) *outV = 0;
1718                         if (outD) *outD = v_m_p_d / d_dd;
1719                         return Point(0, 0);
1720                 }
1721                 else if (v >= 1)
1722                 {
1723                         if (outV) *outV = 1;
1724                         if (outD) *outD = (v_m_p_d - d_e1d) / d_dd;
1725                         return Point(0, 1);
1726                 }
1727                 else // (0 <= v <= 1).
1728                 {
1729                         if (outV) *outV = v;
1730                         if (outD) *outD = t;
1731                         return Point(0, v);
1732                 }
1733         }
1734         else if (v <= 0)
1735         {
1736                 if (outV) *outV = 0;
1737
1738                 // Solve 2x2 matrix for the (u,t) solution when v == 0.
1739                 u = m[0][0]*b[0] + m[0][2]*b[2];
1740                 t = m[2][0]*b[0] + m[2][2]*b[2];
1741
1742                 // Check if the solution is still out of bounds.
1743                 if (u <= 0)
1744                 {
1745                         if (outU) *outU = 0;
1746                         if (outD) *outD = v_m_p_d / d_dd;
1747                         return Point(0, 0);
1748                 }
1749                 else if (u >= 1)
1750                 {
1751                         if (outU) *outU = 1;
1752                         if (outD) *outD = (v_m_p_d - d_e0d) / d_dd;
1753                         return Point(1, 0);
1754                 }
1755                 else // (0 <= u <= 1).
1756                 {
1757                         if (outU) *outU = u;
1758                         if (outD) *outD = t;
1759                         return Point(u, 0);
1760                 }
1761         }
1762         else if (u + v >= 1.f)
1763         {
1764                 // Set v = 1-u.
1765 #if 0
1766                 float m00 = d_e0e0 + d_e1e1 - 2.f * d_e0e1;
1767                 float m01 = -d_e0d + d_e1d;
1768                 float m10 = -d_e0d + d_e1d;
1769                 float m11 = d_dd;
1770 //              float det = 1.f / (m00*m11 - m01*m10);
1771
1772                 float b0 = d_e1e1 - d_e0e1 + v_m_p_e0 - v_m_p_e1;
1773                 float b1 = d_e1d + v_m_p_d;
1774                 /*
1775                 // Inverse 2x2 matrix.
1776                 Swap(m00, m11);
1777                 Swap(m01, m10);
1778                 m01 = -m01;
1779                 m10 = -m10;
1780                 */
1781                 // 2x2 * 2 matrix*vec mul.
1782                 u = (m00*b0 + m01*b1);// * det;
1783                 t = (m10*b0 + m11*b1);// * det;
1784 #endif
1785 //              u = m[0][0]*b[0] +
1786
1787                 // Check if the solution is still out of bounds.
1788                 if (u <= 0)
1789                 {
1790                         if (outU) *outU = 0;
1791                         if (outV) *outV = 1;
1792                         if (outD) *outD = (d_e1d + v_m_p_d) / d_dd;
1793                         return Point(0, 1);
1794                 }
1795                 else if (u >= 1)
1796                 {
1797                         if (outU) *outU = 1;
1798                         if (outV) *outV = 0;
1799                         if (outD) *outD = (v_m_p_d + d_e0d) / d_dd;
1800                         return Point(1, 0);
1801                 }
1802                 else // (0 <= u <= 1).
1803                 {
1804                         if (outU) *outU = u;
1805                         if (outV) *outV = 1.f - u;
1806                         if (outD) *outD = t;
1807                         return Point(u, 1.f - u);
1808                 }
1809         }
1810         else // Each u, v and t are in appropriate range.
1811         {
1812                 if (outU) *outU = u;
1813                 if (outV) *outV = v;
1814                 if (outD) *outD = t;
1815                 return Point(u, v);
1816         }
1817 }
1818 #endif
1819
1820 /// [groupSyntax]
1821 vec Triangle::ClosestPoint(const Triangle &other, vec *otherPt) const
1822 {
1823         /** The code for computing the closest point pair on two Triangles is based
1824                 on pseudo-code from Christer Ericson's Real-Time Collision Detection, pp. 155-156. */
1825
1826         // First detect if the two triangles are intersecting.
1827         LineSegment l;
1828         bool success = this->Intersects(other, &l);
1829         if (success)
1830         {
1831                 vec cp = l.CenterPoint();
1832                 if (otherPt)
1833                         *otherPt = cp;
1834                 return cp;
1835         }
1836
1837         vec closestThis = this->ClosestPoint(other.a);
1838         vec closestOther = other.a;
1839         float closestDSq = closestThis.DistanceSq(closestOther);
1840
1841         vec pt = this->ClosestPoint(other.b);
1842         float dSq = pt.DistanceSq(other.b);
1843         if (dSq < closestDSq) closestThis = pt, closestOther = other.b, closestDSq = dSq;
1844
1845         pt = this->ClosestPoint(other.c);
1846         dSq = pt.DistanceSq(other.c);
1847         if (dSq < closestDSq) closestThis = pt, closestOther = other.c, closestDSq = dSq;
1848
1849         LineSegment l1[3] = { LineSegment(a,b), LineSegment(a,c), LineSegment(b,c) };
1850         LineSegment l2[3] = { LineSegment(other.a,other.b), LineSegment(other.a,other.c), LineSegment(other.b,other.c) };
1851         float d, d2;
1852         for(int i = 0; i < 3; ++i)
1853                 for(int j = 0; j < 3; ++j)
1854                 {
1855                         float dist = l1[i].Distance(l2[j], d, d2);
1856                         if (dist*dist < closestDSq)
1857                         {
1858                                 closestThis = l1[i].GetPoint(d);
1859                                 closestOther = l2[j].GetPoint(d2);
1860                                 closestDSq = dist*dist;
1861                         }
1862                 }
1863
1864         if (otherPt)
1865                 *otherPt = closestOther;
1866         return closestThis;
1867 }
1868
1869 vec Triangle::RandomPointInside(LCG &rng) const
1870 {
1871         float epsilon = 1e-3f;
1872         ///@todo rng.Float() returns [0,1[, but to be completely uniform, we'd need [0,1] here.
1873         float s = rng.Float(epsilon, 1.f - epsilon);//1e-2f, 1.f - 1e-2f);
1874         float t = rng.Float(epsilon, 1.f - epsilon);//1e-2f, 1.f - 1e-2f
1875         if (s + t >= 1.f)
1876         {
1877                 s = 1.f - s;
1878                 t = 1.f - t;
1879         }
1880 #ifdef MATH_ASSERT_CORRECTNESS
1881         vec pt = Point(s, t);
1882         float2 uv = BarycentricUV(pt);
1883         assert1(uv.x >= 0.f, uv.x);
1884         assert1(uv.y >= 0.f, uv.y);
1885         assert3(uv.x + uv.y <= 1.f, uv.x, uv.y, uv.x + uv.y);
1886         float3 uvw = BarycentricUVW(pt);
1887         assert1(uvw.x >= 0.f, uvw.x);
1888         assert1(uvw.y >= 0.f, uvw.y);
1889         assert1(uvw.z >= 0.f, uvw.z);
1890         assert4(EqualAbs(uvw.x + uvw.y + uvw.z, 1.f), uvw.x, uvw.y, uvw.z, uvw.x + uvw.y + uvw.z);
1891 #endif
1892         return Point(s, t);
1893 }
1894
1895 vec Triangle::RandomVertex(LCG &rng) const
1896 {
1897         return Vertex(rng.Int(0, 2));
1898 }
1899
1900 vec Triangle::RandomPointOnEdge(LCG &rng) const
1901 {
1902         assume(!IsDegenerate());
1903         float ab = a.Distance(b);
1904         float bc = b.Distance(c);
1905         float ca = c.Distance(a);
1906         float r = rng.Float(0, ab + bc + ca);
1907         if (r < ab)
1908                 return a + (b-a) * r / ab;
1909         r -= ab;
1910         if (r < bc)
1911                 return b + (c-b) * r / bc;
1912         r -= bc;
1913         return c + (a-c) * r / ca;
1914 }
1915
1916 Triangle operator *(const float3x3 &transform, const Triangle &triangle)
1917 {
1918         Triangle t(triangle);
1919         t.Transform(transform);
1920         return t;
1921 }
1922
1923 Triangle operator *(const float3x4 &transform, const Triangle &triangle)
1924 {
1925         Triangle t(triangle);
1926         t.Transform(transform);
1927         return t;
1928 }
1929
1930 Triangle operator *(const float4x4 &transform, const Triangle &triangle)
1931 {
1932         Triangle t(triangle);
1933         t.Transform(transform);
1934         return t;
1935 }
1936
1937 Triangle operator *(const Quat &transform, const Triangle &triangle)
1938 {
1939         Triangle t(triangle);
1940         t.Transform(transform);
1941         return t;
1942 }
1943
1944 #ifdef MATH_ENABLE_STL_SUPPORT
1945 std::string Triangle::ToString() const
1946 {
1947         char str[256];
1948         sprintf(str, "Triangle(a:(%.2f, %.2f, %.2f) b:(%.2f, %.2f, %.2f) c:(%.2f, %.2f, %.2f))",
1949                 a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
1950         return str;
1951 }
1952
1953 std::string Triangle::SerializeToString() const
1954 {
1955         char str[256];
1956         char *s = SerializeFloat(a.x, str); *s = ','; ++s;
1957         s = SerializeFloat(a.y, s); *s = ','; ++s;
1958         s = SerializeFloat(a.z, s); *s = ','; ++s;
1959         s = SerializeFloat(b.x, s); *s = ','; ++s;
1960         s = SerializeFloat(b.y, s); *s = ','; ++s;
1961         s = SerializeFloat(b.z, s); *s = ','; ++s;
1962         s = SerializeFloat(c.x, s); *s = ','; ++s;
1963         s = SerializeFloat(c.y, s); *s = ','; ++s;
1964         s = SerializeFloat(c.z, s);
1965         assert(s+1 - str < 256);
1966         MARK_UNUSED(s);
1967         return str;
1968 }
1969
1970 std::string Triangle::SerializeToCodeString() const
1971 {
1972         return "Triangle(" + a.SerializeToCodeString() + "," + b.SerializeToCodeString() + "," + c.SerializeToCodeString() + ")";
1973 }
1974
1975 std::ostream &operator <<(std::ostream &o, const Triangle &triangle)
1976 {
1977         o << triangle.ToString();
1978         return o;
1979 }
1980
1981 #endif
1982
1983 Triangle Triangle::FromString(const char *str, const char **outEndStr)
1984 {
1985         assume(str);
1986         if (!str)
1987                 return Triangle(vec::nan, vec::nan, vec::nan);
1988         Triangle t;
1989         MATH_SKIP_WORD(str, "Triangle(");
1990         MATH_SKIP_WORD(str, "a:(");
1991         t.a = PointVecFromString(str, &str);
1992         MATH_SKIP_WORD(str, " b:(");
1993         t.b = PointVecFromString(str, &str);
1994         MATH_SKIP_WORD(str, " c:(");
1995         t.c = PointVecFromString(str, &str);
1996         if (outEndStr)
1997                 *outEndStr = str;
1998         return t;
1999 }
2000
2001 MATH_END_NAMESPACE