Triangle.cpp

1 /* Copyright 2011 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 "Math/MathFunc.h"
19 #include "Math/float2.h"
20 #include "Math/float3.h"
21 #include "Math/float3x3.h"
22 #include "Math/float3x4.h"
23 #include "Math/float4x4.h"
24 #include "Math/Quat.h"
25 #include "Geometry/Capsule.h"
26 #include "Geometry/Frustum.h"
27 #include "Geometry/Triangle.h"
28 #include "Geometry/Plane.h"
29 #include "Geometry/Polygon.h"
30 #include "Geometry/Polyhedron.h"
31 #include "Geometry/Line.h"
32 #include "Geometry/LineSegment.h"
33 #include "Geometry/Ray.h"
34 #include "Geometry/Sphere.h"
35 #include "Geometry/AABB.h"
36 #include "Geometry/OBB.h"
37 #include "Algorithm/Random/LCG.h"
38
39 MATH_BEGIN_NAMESPACE
40
41 Triangle::Triangle(const float3 &a_, const float3 &b_, const float3 &c_)
42 :a(a_), b(b_), c(c_)
43 {
44 }
45
46 float3 Triangle::BarycentricUVW(const float3 &point) const
47 {
48         /// @note An alternate mechanism to compute the barycentric is given in Christer Ericson's
49         /// Real-Time Collision Detection, pp. 51-52, which might be slightly faster than the current implementation.
50         float3 v0 = b - a;
51         float3 v1 = c - a;
52         float3 v2 = point - a;
53         float d00 = Dot(v0, v0);
54         float d01 = Dot(v0, v1);
55         float d11 = Dot(v1, v1);
56         float d20 = Dot(v2, v0);
57         float d21 = Dot(v2, v1);
58         float denom = 1.f / (d00 * d11 - d01 * d01);
59         float v = (d11 * d20 - d01 * d21) * denom;
60         float w = (d00 * d21 - d01 * d20) * denom;
61         float u = 1.0f - v - w;
62         return float3(u, v, w);
63 }
64
65 float2 Triangle::BarycentricUV(const float3 &point) const
66 {
67         float3 uvw = BarycentricUVW(point);
68         return float2(uvw.y, uvw.z);
69 }
70
71 bool Triangle::BarycentricInsideTriangle(const float3 &barycentric)
72 {
73         return barycentric.x >= 0.f && barycentric.y >= 0.f && barycentric.z >= 0.f &&
74                 EqualAbs(barycentric.x + barycentric.y + barycentric.z, 1.f);
75 }
76
77 float3 Triangle::Point(float u, float v, float w) const
78 {
79         return u * a + v * b + w * c;
80 }
81
82 float3 Triangle::Point(const float3 &b) const
83 {
84         return Point(b.x, b.y, b.z);
85 }
86
87 float3 Triangle::Point(const float2 &b) const
88 {
89         return Point(b.x, b.y);
90 }
91
92 float3 Triangle::Centroid() const
93 {
94         return (a + b + c) / 3.f;
95 }
96
97 float Triangle::Area() const
98 {
99         return 0.5f * Cross(b-a, c-a).Length();
100 }
101
102 float Triangle::Perimeter() const
103 {
104         return a.Distance(b) + b.Distance(c) + c.Distance(a);
105 }
106
107 LineSegment Triangle::Edge(int i) const
108 {
109         assume(0 <= i);
110         assume(i <= 2);
111         if (i == 0)
112                 return LineSegment(a, b);
113         else if (i == 1)
114                 return LineSegment(b, c);
115         else if (i == 2)
116                 return LineSegment(c, a);
117         else
118                 return LineSegment(float3::nan, float3::nan);
119 }
120
121 float3 Triangle::Vertex(int i) const
122 {
123         assume(0 <= i);
124         assume(i <= 2);
125         if (i == 0)
126                 return a;
127         else if (i == 1)
128                 return b;
129         else if (i == 2)
130                 return c;
131         else
132                 return float3::nan;
133 }
134
135 Plane Triangle::PlaneCCW() const
136 {
137         return Plane(a, b, c);
138 }
139
140 Plane Triangle::PlaneCW() const
141 {
142         return Plane(a, c, b);
143 }
144
145 float3 Triangle::NormalCCW() const
146 {
147         return UnnormalizedNormalCCW().Normalized();
148 }
149
150 float3 Triangle::NormalCW() const
151 {
152         return UnnormalizedNormalCW().Normalized();
153 }
154
155 float3 Triangle::UnnormalizedNormalCCW() const
156 {
157         return Cross(b-a, c-a);
158 }
159
160 float3 Triangle::UnnormalizedNormalCW() const
161 {
162         return Cross(c-a, b-a);
163 }
164
165 Polygon Triangle::ToPolygon() const
166 {
167         Polygon p;
168         p.p.push_back(a);
169         p.p.push_back(b);
170         p.p.push_back(c);
171         return p;
172 }
173
174 Polyhedron Triangle::ToPolyhedron() const
175 {
176         return ToPolygon().ToPolyhedron();
177 }
178
179 float Triangle::Area2D(const float2 &p1, const float2 &p2, const float2 &p3)
180 {
181         return (p1.x - p2.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p2.y);
182 }
183
184 float Triangle::SignedArea(const float3 &pt, const float3 &a, const float3 &b, const float3 &c)
185 {
186         return Dot(Cross(b-pt, c-pt), Cross(b-a, c-a).Normalized());
187 }
188
189 bool Triangle::IsFinite() const
190 {
191         return a.IsFinite() && b.IsFinite() && c.IsFinite();
192 }
193
194 bool Triangle::IsDegenerate(float epsilon) const
195 {
196         return IsDegenerate(a, b, c);
197 }
198
199 bool Triangle::IsDegenerate(const float3 &a, const float3 &b, const float3 &c, float epsilon)
200 {
201         return a.Equals(b, epsilon) || a.Equals(c, epsilon) || b.Equals(c, epsilon);
202 }
203
204 bool Triangle::Contains(const float3 &point, float triangleThickness) const
205 {
206         if (PlaneCCW().Distance(point) > triangleThickness) // The winding order of the triangle plane does not matter.
207                 return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
208
209         float3 br = BarycentricUVW(point);
210         return br.y >= 0.f && br.z >= 0.f && (br.y + br.z) <= 1.f;
211 }
212
213 bool Triangle::Contains(const LineSegment &lineSegment, float triangleThickness) const
214 {
215         return Contains(lineSegment.a, triangleThickness) && Contains(lineSegment.b, triangleThickness);
216 }
217
218 bool Triangle::Contains(const Triangle &triangle, float triangleThickness) const
219 {
220         return Contains(triangle.a, triangleThickness) && Contains(triangle.b, triangleThickness)
221           && Contains(triangle.c, triangleThickness);
222 }
223
224 /*
225 bool Triangle::Contains(const Polygon &polygon, float triangleThickness) const
226 {
227         if (polygon.points.size() == 0)
228                 return false;
229         for(int i = 0; i < polygon.points.size(); ++i)
230                 if (!Contains(polygon.points[i], triangleThickness))
231                         return false;
232         return true;
233 }
234 */
235 float Triangle::Distance(const float3 &point) const
236 {
237         return ClosestPoint(point).Distance(point);
238 }
239
240 float Triangle::Distance(const Sphere &sphere) const
241 {
242         return Max(0.f, Distance(sphere.pos) - sphere.r);
243 }
244
245 /** Calculates the intersection between a ray and a triangle. The facing is not accounted for, so
246         rays are reported to intersect triangles that are both front and backfacing.
247         According to "T. Möller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
248         http://jgt.akpeters.com/papers/MollerTrumbore97/
249         @param ray The ray to test.
250         @param v0 Vertex 0 of the triangle.
251         @param v1 Vertex 1 of the triangle.
252         @param v2 Vertex 2 of the triangle.
253         @param u [out] The barycentric u coordinate is returned here if an intersection occurred.
254         @param v [out] The barycentric v coordinate is returned here if an intersection occurred.
255         @param t [out] The signed distance from ray origin to ray intersection position will be returned here. (if intersection occurred)
256         @return True if an intersection occurred. If no intersection, then u,v and t will contain undefined values. */
257 bool Triangle::IntersectLineTri(const float3 &linePos, const float3 &lineDir,
258                 const float3 &v0, const float3 &v1, const float3 &v2,
259                 float &u, float &v, float &t)
260 {
261         float3 vE1, vE2;
262         float3 vT, vP, vQ;
263
264         const float epsilon = 1e-6f;
265
266         // Edge vectors
267         vE1 = v1 - v0;
268         vE2 = v2 - v0;
269
270         // begin calculating determinant - also used to calculate U parameter
271         vP = Cross(lineDir, vE2);
272
273         // If det < 0, intersecting backfacing tri, > 0, intersecting frontfacing tri, 0, parallel to plane.
274         const float det = Dot(vE1, vP);
275
276         // If determinant is near zero, ray lies in plane of triangle.
277         if (fabs(det) <= epsilon)
278                 return false;
279         const float recipDet = 1.f / det;
280
281         // Calculate distance from v0 to ray origin
282         vT = linePos - v0;
283
284         // Output barycentric u
285         u = Dot(vT, vP) * recipDet;
286         if (u < 0.f || u > 1.f)
287                 return false; // Barycentric U is outside the triangle - early out.
288
289         // Prepare to test V parameter
290         vQ = Cross(vT, vE1);
291
292         // Output barycentric v
293         v = Dot(lineDir, vQ) * recipDet;
294         if (v < 0.f || u + v > 1.f) // Barycentric V or the combination of U and V are outside the triangle - no intersection.
295                 return false;
296
297         // Barycentric u and v are in limits, the ray intersects the triangle.
298
299         // Output signed distance from ray to triangle.
300         t = Dot(vE2, vQ) * recipDet;
301         return true;
302 //      return (det < 0.f) ? IntersectBackface : IntersectFrontface;
303 }
304
305 /// [groupSyntax]
306 bool Triangle::Intersects(const LineSegment &l, float *d, float3 *intersectionPoint) const
307 {
308         /** The Triangle-Line/LineSegment/Ray intersection tests are based on Möller-Trumbore method:
309                 "T. Möller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
310                 http://jgt.akpeters.com/papers/MollerTrumbore97/. */
311         float u, v, t;
312         bool success = IntersectLineTri(l.a, l.Dir(), a, b, c, u, v, t);
313         if (!success)
314                 return false;
315         float length = l.LengthSq();
316         if (t < 0.f || t*t >= length)
317                 return false;
318         length = sqrtf(length);
319         if (d)
320         {
321                 float len = t / length;
322                 *d = len;
323                 if (intersectionPoint)
324                         *intersectionPoint = l.GetPoint(len);
325         }
326         else if (intersectionPoint)
327                 *intersectionPoint = l.GetPoint(t / length);
328         return true;
329 }
330
331 bool Triangle::Intersects(const Line &l, float *d, float3 *intersectionPoint) const
332 {
333         float u, v, t;
334         bool success = IntersectLineTri(l.pos, l.dir, a, b, c, u, v, t);
335         if (!success)
336                 return false;
337         if (d)
338                 *d = t;
339         if (intersectionPoint)
340                 *intersectionPoint = l.GetPoint(t);
341         return success;
342 }
343
344 bool Triangle::Intersects(const Ray &r, float *d, float3 *intersectionPoint) const
345 {
346         float u, v, t;
347         bool success = IntersectLineTri(r.pos, r.dir, a, b, c, u, v, t);
348         if (!success || t <= 0.f)
349                 return false;
350         if (d)
351                 *d = t;
352         if (intersectionPoint)
353                 *intersectionPoint = r.GetPoint(t);
354         return success;
355 }
356
357 bool Triangle::Intersects(const Plane &plane) const
358 {
359         return plane.Intersects(*this);
360 }
361
362 /// [groupSyntax]
363 /** For Triangle-Sphere intersection code, see Christer Ericson's Real-Time Collision Detection, p.167. */
364 bool Triangle::Intersects(const Sphere &sphere, float3 *closestPointOnTriangle) const
365 {
366         float3 pt = ClosestPoint(sphere.pos);
367
368         if (closestPointOnTriangle)
369                 *closestPointOnTriangle = pt;
370
371         return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
372 }
373
374 bool Triangle::Intersects(const Sphere &sphere) const
375 {
376         return Intersects(sphere, 0);
377 }
378
379 static void FindIntersectingLineSegments(const Triangle &t, float da, float db, float dc, LineSegment &l1, LineSegment &l2)
380 {
381         if (da*db > 0.f)
382         {
383                 l1 = LineSegment(t.a, t.c);
384                 l2 = LineSegment(t.b, t.c);
385         }
386         else if (db*dc > 0.f)
387         {
388                 l1 = LineSegment(t.a, t.b);
389                 l1 = LineSegment(t.a, t.c);
390         }
391         else
392         {
393                 l1 = LineSegment(t.a, t.b);
394                 l1 = LineSegment(t.b, t.c);
395         }
396 }
397
398 /// [groupSyntax]
399 /** The Triangle-Triangle test implementation is based on pseudo-code from Tomas Möller's
400         "A Fast Triangle-Triangle Intersection Test": http://jgt.akpeters.com/papers/Moller97/.
401         See also Christer Ericson's Real-Time Collision Detection, p. 172. */
402 bool Triangle::Intersects(const Triangle &t2, LineSegment *outLine) const
403 {
404         // Is the triangle t2 completely on one side of the plane of this triangle?
405         Plane p1 = this->PlaneCCW();
406         float t2da = p1.SignedDistance(t2.a);
407         float t2db = p1.SignedDistance(t2.b);
408         float t2dc = p1.SignedDistance(t2.c);
409         if (t2da*t2db > 0.f && t2da*t2dc > 0.f)
410                 return false;
411         // Is this triangle completely on one side of the plane of the triangle t2?
412         Plane p2 = t2.PlaneCCW();
413         float t1da = p2.SignedDistance(this->a);
414         float t1db = p2.SignedDistance(this->b);
415         float t1dc = p2.SignedDistance(this->c);
416         if (t1da*t1db > 0.f && t1da*t1dc > 0.f)
417                 return false;
418
419         // Find the intersection line of the two planes.
420         Line l;
421         bool success = p1.Intersects(p2, &l);
422         assume(success); // We already determined the two triangles have intersecting planes, so this should always succeed.
423         if (!success)
424                 return false;
425
426         // Find the two line segments of both triangles which straddle the intersection line.
427         LineSegment l1a, l1b;
428         LineSegment l2a, l2b;
429         FindIntersectingLineSegments(*this, t1da, t1db, t1dc, l1a, l1b);
430         FindIntersectingLineSegments(t2, t2da, t2db, t2dc, l2a, l2b);
431
432         // Find the projection intervals on the intersection line.
433         float d1a, d1b, d2a, d2b;
434         l.Distance(l1a, &d1a);
435         l.Distance(l1b, &d1b);
436         l.Distance(l2a, &d2a);
437         l.Distance(l2b, &d2b);
438         if (d1a > d1b)
439                 Swap(d1a, d1b);
440         if (d2a > d2b)
441                 Swap(d2a, d2b);
442         float rStart = Max(d1a, d2a);
443         float rEnd = Min(d1b, d2b);
444         if (rStart <= rEnd)
445         {
446                 if (outLine)
447                         *outLine = LineSegment(l.GetPoint(rStart), l.GetPoint(rEnd));
448                 return true;
449         }
450         return false;
451 }
452
453 bool RangesOverlap(float start1, float end1, float start2, float end2)
454 {
455         return end1 >= start2 && end2 >= start1;
456 }
457
458 /// [groupSyntax]
459 bool Triangle::Intersects(const AABB &aabb) const
460 {
461 /** The AABB-Triangle test implementation is based on the pseudo-code in
462         Christer Ericson's Real-Time Collision Detection, pp. 169-172. */
463         ///@todo The Triangle-AABB intersection test can be greatly optimized by manually unrolling loops, trivial math and by avoiding
464         /// unnecessary copying.
465         float t1, t2, a1, a2;
466         const float3 e[3] = { float3(1,0,0), float3(0,1,0), float3(0,0,1) };
467
468         for(int i = 0; i < 3; ++i)
469         {
470                 ProjectToAxis(e[i], t1, t2);
471                 aabb.ProjectToAxis(e[i], a1, a2);
472                 if (!RangesOverlap(t1, t2, a1, a2))
473                         return false;
474         }
475
476         float3 n = UnnormalizedNormalCCW();
477         ProjectToAxis(n, t1, t2);
478         aabb.ProjectToAxis(n, a1, a2);
479         if (!RangesOverlap(t1, t2, a1, a2))
480                 return false;
481
482         const float3 t[3] = { b-a, c-a, c-b };
483
484         for(int i = 0; i < 3; ++i)
485                 for(int j = 0; j < 3; ++j)
486                 {
487                         float3 axis = Cross(e[i], t[j]);
488                         float len = axis.LengthSq();
489                         if (len <= 1e-4f)
490                                 continue; // Ignore tests on degenerate axes.
491
492                         ProjectToAxis(axis, t1, t2);
493                         aabb.ProjectToAxis(axis, a1, a2);
494                         if (!RangesOverlap(t1, t2, a1, a2))
495                                 return false;
496                 }
497
498         // No separating axis exists, the AABB and triangle intersect.
499         return true;
500 }
501
502 bool Triangle::Intersects(const OBB &obb) const
503 {
504         return obb.Intersects(*this);
505 }
506
507 bool Triangle::Intersects(const Polygon &polygon) const
508 {
509         return polygon.Intersects(*this);
510 }
511
512 bool Triangle::Intersects(const Frustum &frustum) const
513 {
514         return frustum.Intersects(*this);
515 }
516
517 bool Triangle::Intersects(const Polyhedron &polyhedron) const
518 {
519         return polyhedron.Intersects(*this);
520 }
521
522 bool Triangle::Intersects(const Capsule &capsule) const
523 {
524         return capsule.Intersects(*this);
525 }
526
527 void Triangle::ProjectToAxis(const float3 &axis, float &dMin, float &dMax) const
528 {
529         dMin = dMax = Dot(axis, a);
530         float t = Dot(axis, b);
531         dMin = Min(t, dMin);
532         dMax = Max(t, dMax);
533         t = Dot(axis, c);
534         dMin = Min(t, dMin);
535         dMax = Max(t, dMax);
536 }
537
538 /// [groupSyntax]
539 float3 Triangle::ClosestPoint(const float3 &p) const
540 {
541         /** The code for Triangle-float3 test is from Christer Ericson's Real-Time Collision Detection, pp. 141-142. */
542
543         // Check if P is in vertex region outside A.
544         float3 ab = b - a;
545         float3 ac = c - a;
546         float3 ap = p - a;
547         float d1 = Dot(ab, ap);
548         float d2 = Dot(ac, ap);
549         if (d1 <= 0.f && d2 <= 0.f)
550                 return a; // Barycentric coordinates are (1,0,0).
551
552         // Check if P is in vertex region outside B.
553         float3 bp = p - b;
554         float d3 = Dot(ab, bp);
555         float d4 = Dot(ac, bp);
556         if (d3 >= 0.f && d4 <= d3)
557                 return b; // Barycentric coordinates are (0,1,0).
558
559         // Check if P is in edge region of AB, and if so, return the projection of P onto AB.
560         float vc = d1*d4 - d3*d2;
561         if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
562         {
563                 float v = d1 / (d1 - d3);
564                 return a + v * ab; // The barycentric coordinates are (1-v, v, 0).
565         }
566
567         // Check if P is in vertex region outside C.
568         float3 cp = p - c;
569         float d5 = Dot(ab, cp);
570         float d6 = Dot(ac, cp);
571         if (d6 >= 0.f && d5 <= d6)
572                 return c; // The barycentric coordinates are (0,0,1).
573
574         // Check if P is in edge region of AC, and if so, return the projection of P onto AC.
575         float vb = d5*d2 - d1*d6;
576         if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
577         {
578                 float w = d2 / (d2 - d6);
579                 return a + w * ac; // The barycentric coordinates are (1-w, 0, w).
580         }
581
582         // Check if P is in edge region of BC, and if so, return the projection of P onto BC.
583         float va = d3*d6 - d5*d4;
584         if (va <= 0.f && d4 - d3 >= 0.f && d5 - d6 >= 0.f)
585         {
586                 float w = (d4 - d3) / (d4 - d3 + d5 - d6);
587                 return b + w * (c - b); // The barycentric coordinates are (0, 1-w, w).
588         }
589
590         // P must be inside the face region. Compute the closest point through its barycentric coordinates (u,v,w).
591         float denom = 1.f / (va + vb + vc);
592         float v = vb * denom;
593         float w = vc * denom;
594         return a + ab * v + ac * w;
595 }
596
597 /// [groupSyntax]
598 float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const
599 {
600         ///@todo The Triangle-LineSegment test is naive. Optimize!
601         float3 closestToA = ClosestPoint(lineSegment.a);
602         float3 closestToB = ClosestPoint(lineSegment.b);
603         float d;
604         float3 closestToSegment = ClosestPointToTriangleEdge(lineSegment, 0, 0, &d);
605         float3 segmentPt = lineSegment.GetPoint(d);
606         float distA = closestToA.DistanceSq(lineSegment.a);
607         float distB = closestToB.DistanceSq(lineSegment.b);
608         float distC = closestToSegment.DistanceSq(segmentPt);
609         if (distA <= distB && distA <= distC)
610         {
611                 if (otherPt)
612                         *otherPt = lineSegment.a;
613                 return closestToA;
614         }
615         else if (distB <= distC)
616         {
617                 if (otherPt)
618                         *otherPt = lineSegment.b;
619                 return closestToB;
620         }
621         else
622         {
623                 if (otherPt)
624                         *otherPt = segmentPt;
625                 return closestToSegment;
626         }
627 }
628
629 /*
630 float3 Triangle::ClosestPoint(const LineSegment &line, float3 *otherPt) const
631 {
632         float3 intersectionPoint;
633         bool success = Intersects(line, 0, &intersectionPoint);
634         if (success)
635                 return intersectionPoint;
636
637         Plane p = PlaneCCW();
638         float d1 = p.Distance(line.a);
639         float d2 = p.Distance(line.b);
640         /// \bug The following two lines are not correct. This algorithm does not
641         /// produce correct answers. Rewrite.
642         bool aProjectsInsideTriangle = BarycentricInsideTriangle(line.a);
643         bool bProjectsInsideTriangle = BarycentricInsideTriangle(line.b);
644
645         if (aProjectsInsideTriangle && bProjectsInsideTriangle)
646         {
647                 // We tested above for intersection, so cannot intersect now.
648                 if (d1 <= d2)
649                 {
650                         if (otherPt)
651                                 *otherPt = line.a;
652                         return p.Project(line.a);
653                 }
654                 else
655                 {
656                         if (otherPt)
657                                 *otherPt = line.b;
658                         return p.Project(line.b);
659                 }
660         }
661         LineSegment ab(a, b);
662         LineSegment ac(a, c);
663         LineSegment bc(b, c);
664
665         float tab, tac, tbc;
666         float tab2, tac2, tbc2;
667
668         float dab = ab.Distance(line, &tab, &tab2);
669         float dac = ac.Distance(line, &tac, &tac2);
670         float dbc = bc.Distance(line, &tbc, &tbc2);
671
672         if (dab <= dac && dab <= dbc && dab <= d1 && dab <= d2)
673         {
674                 if (otherPt)
675                         *otherPt = line.GetPoint(tab2);
676                 return ab.GetPoint(tab);
677         }
678         else if (dac <= dbc && dac <= d1 && dac <= d2)
679         {
680                 if (otherPt)
681                         *otherPt = line.GetPoint(tac2);
682                 return ab.GetPoint(tac);
683         }
684         else if (dbc <= d1 && dbc <= d2)
685         {
686                 if (otherPt)
687                         *otherPt = line.GetPoint(tbc2);
688                 return ab.GetPoint(tbc);
689         }
690         else if (d1 <= d2)
691         {
692                 if (otherPt)
693                         *otherPt = line.a;
694                 return p.Project(line.a);
695         }
696         else
697         {
698                 if (otherPt)
699                         *otherPt = line.b;
700                 return p.Project(line.b);
701         }
702 }
703 */
704
705 float3 Triangle::ClosestPointToTriangleEdge(const Line &other, float *outU, float *outV, float *outD) const
706 {
707         ///@todo Optimize!
708         // The line is parallel to the triangle.
709         float d1, d2, d3;
710         float3 pt1 = Edge(0).ClosestPoint(other, 0, &d1);
711         float3 pt2 = Edge(1).ClosestPoint(other, 0, &d2);
712         float3 pt3 = Edge(2).ClosestPoint(other, 0, &d3);
713         float dist1 = pt1.DistanceSq(other.GetPoint(d1));
714         float dist2 = pt2.DistanceSq(other.GetPoint(d2));
715         float dist3 = pt3.DistanceSq(other.GetPoint(d3));
716         if (dist1 <= dist2 && dist1 <= dist3)
717         {
718                 if (outU) *outU = BarycentricUV(pt1).x;
719                 if (outV) *outV = BarycentricUV(pt1).y;
720                 if (outD) *outD = d1;
721                 return pt1;
722         }
723         else if (dist2 <= dist3)
724         {
725                 if (outU) *outU = BarycentricUV(pt2).x;
726                 if (outV) *outV = BarycentricUV(pt2).y;
727                 if (outD) *outD = d2;
728                 return pt2;
729         }
730         else
731         {
732                 if (outU) *outU = BarycentricUV(pt3).x;
733                 if (outV) *outV = BarycentricUV(pt3).y;
734                 if (outD) *outD = d3;
735                 return pt3;
736         }
737 }
738
739 float3 Triangle::ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const
740 {
741         ///@todo Optimize!
742         // The line is parallel to the triangle.
743         float d1, d2, d3;
744         float3 pt1 = Edge(0).ClosestPoint(lineSegment, 0, &d1);
745         float3 pt2 = Edge(1).ClosestPoint(lineSegment, 0, &d2);
746         float3 pt3 = Edge(2).ClosestPoint(lineSegment, 0, &d3);
747         float dist1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
748         float dist2 = pt2.DistanceSq(lineSegment.GetPoint(d2));
749         float dist3 = pt3.DistanceSq(lineSegment.GetPoint(d3));
750         if (dist1 <= dist2 && dist1 <= dist3)
751         {
752                 if (outU) *outU = BarycentricUV(pt1).x;
753                 if (outV) *outV = BarycentricUV(pt1).y;
754                 if (outD) *outD = d1;
755                 return pt1;
756         }
757         else if (dist2 <= dist3)
758         {
759                 if (outU) *outU = BarycentricUV(pt2).x;
760                 if (outV) *outV = BarycentricUV(pt2).y;
761                 if (outD) *outD = d2;
762                 return pt2;
763         }
764         else
765         {
766                 if (outU) *outU = BarycentricUV(pt3).x;
767                 if (outV) *outV = BarycentricUV(pt3).y;
768                 if (outD) *outD = d3;
769                 return pt3;
770         }
771 }
772
773 /// [groupSyntax]
774 float3 Triangle::ClosestPoint(const Line &other, float *outU, float *outV, float *outD) const
775 {
776         /** The implementation of the Triangle-Line test is based on the pseudo-code in
777                 Schneider, Eberly. Geometric Tools for Computer Graphics pp. 433 - 441. */
778         ///@todo The Triangle-Line code is currently untested. Run tests to ensure the following code works properly.
779
780         // Point on triangle: T(u,v) = a + u*b + v*c;
781         // Point on line:  L(t) = p + t*d;
782         // Minimize the function Q(u,v,t) = ||T(u,v) - L(t)||.
783
784         float3 e0 = b-a;
785         float3 e1 = c-a;
786         float3 d = other.dir;
787
788         const float d_e0e0 = Dot(e0, e0);
789         const float d_e0e1 = Dot(e0, e1);
790         const float d_e0d = Dot(e0, d);
791         const float d_e1e1 = Dot(e1, e1);
792         const float d_e1d = Dot(e1, d);
793         const float d_dd = Dot(d, d);
794
795         float3x3 m;
796         m[0][0] = d_e0e0;  m[0][1] = d_e0e1;  m[0][2] = -d_e0d;
797         m[1][0] = d_e0e1;  m[1][1] = d_e1e1;  m[1][2] = -d_e1d;
798         m[2][0] = -d_e0d;  m[2][1] = -d_e1d;  m[2][2] = d_dd;
799
800         ///@todo Add optimized float3x3::InverseSymmetric().
801         bool inv = m.Inverse();
802         if (!inv)
803                 return ClosestPointToTriangleEdge(other, outU, outV, outD);
804
805         float3 v_m_p = a - other.pos;
806         float v_m_p_e0 = v_m_p.Dot(e0);
807         float v_m_p_e1 = v_m_p.Dot(e1);
808         float v_m_p_d = v_m_p.Dot(d);
809         float3 b = float3(-v_m_p_e0, -v_m_p_e1, v_m_p_d);
810         float3 uvt = m * b;
811         // We cannot simply clamp the solution to (uv) inside the constraints, since the expression we
812         // are minimizing is quadratic.
813         // So, examine case-by-case which part of the space the solution lies in. Because the function is convex,
814         // we can clamp the search space to the boundary planes.
815         float u = uvt.x;
816         float v = uvt.y;
817         float t = uvt.z;
818         if (u < 0)
819         {
820                 if (outU) *outU = 0;
821
822                 // Solve 2x2 matrix for the (v,t) solution when u == 0.
823                 float m00 = m[2][2];
824                 float m01 = -m[2][1];
825                 float m10 = -m[1][2];
826                 float m11 = m[1][1];
827                 float det = m00*m11 - m01*m10;
828
829                 // 2x2 * 2 matrix*vec mul.
830                 v = m00*b[1] + m01*b[2];
831                 t = m10*b[1] + m11*b[2];
832                 if (outD) *outD = t;
833
834                 // Check if the solution is still out of bounds.
835                 if (v <= 0)
836                 {
837                         if (outV) *outV = 0;
838                         t = v_m_p_d / d_dd;
839                         return Point(0, 0);
840                 }
841                 else if (v >= 1)
842                 {
843                         if (outV) *outV = 1;
844                         t = (v_m_p_d - d_e1d) / d_dd;
845                         return Point(0, 1);
846                 }
847                 else // (0 <= v <= 1).
848                 {
849                         if (outV) *outV = v;
850                         return Point(0, v);
851                 }
852         }
853         else if (v <= 0)
854         {
855                 if (outV) *outV = 0;
856
857                 // Solve 2x2 matrix for the (u,t) solution when v == 0.
858                 float m00 = m[2][2];
859                 float m01 = -m[2][0];
860                 float m10 = -m[0][2];
861                 float m11 = m[0][0];
862                 float det = 1.f / (m00*m11 - m01*m10);
863
864                 // 2x2 * 2 matrix*vec mul.
865                 u = (m00*b[0] + m01*b[2]) * det;
866                 t = (m10*b[0] + m11*b[2]) * det;
867                 if (outD) *outD = t;
868
869                 // Check if the solution is still out of bounds.
870                 if (u <= 0)
871                 {
872                         if (outU) *outU = 0;
873                         t = v_m_p_d / d_dd;
874                         return Point(0, 0);
875                 }
876                 else if (u >= 1)
877                 {
878                         if (outU) *outU = 1;
879                         t = (v_m_p_d - d_e0d) / d_dd;
880                         return Point(1, 0);
881                 }
882                 else // (0 <= u <= 1).
883                 {
884                         if (outU) *outU = u;
885                         return Point(u, 0);
886                 }
887         }
888         else if (u + v >= 1.f)
889         {
890                 // Set v = 1-u.
891                 float m00 = d_e0e0 + d_e1e1 - 2.f * d_e0e1;
892                 float m01 = -d_e0d + d_e1d;
893                 float m10 = -d_e0d + d_e1d;
894                 float m11 = d_dd;
895                 float det = 1.f / (m00*m11 - m01*m10);
896
897                 float b0 = d_e1e1 - d_e0e1 + v_m_p_e0 - v_m_p_e1;
898                 float b1 = d_e1d + v_m_p_d;
899                 // Inverse 2x2 matrix.
900                 Swap(m00, m11);
901                 Swap(m01, m10);
902                 m01 = -m01;
903                 m10 = -m10;
904
905                 // 2x2 * 2 matrix*vec mul.
906                 u = (m00*b0 + m01*b1) * det;
907                 t = (m10*b0 + m11*b1) * det;
908                 if (outD) *outD = t;
909
910                 // Check if the solution is still out of bounds.
911                 if (u <= 0)
912                 {
913                         if (outU) *outU = 0;
914                         if (outV) *outV = 1;
915                         t = (d_e1d + v_m_p_d) / d_dd;
916                         return Point(0, 1);
917                 }
918                 else if (u >= 1)
919                 {
920                         if (outU) *outU = 1;
921                         if (outV) *outV = 0;
922                         t = (v_m_p_d + d_e0d) / d_dd;
923                         return Point(1, 0);
924                 }
925                 else // (0 <= u <= 1).
926                 {
927                         if (outU) *outU = u;
928                         if (outV) *outV = 1.f - u;
929                         return Point(u, 1.f - u);
930                 }
931         }
932         else // Each u, v and t are in appropriate range.
933         {
934                 if (outU) *outU = u;
935                 if (outV) *outV = v;
936                 if (outD) *outD = t;
937                 return Point(u, v);
938         }
939 }
940
941 /// [groupSyntax]
942 float3 Triangle::ClosestPoint(const Triangle &other, float3 *otherPt) const
943 {
944         /** The code for computing the closest point pair on two Triangles is based
945                 on pseudo-code from Christer Ericson's Real-Time Collision Detection, pp. 155-156. */
946
947         // First detect if the two triangles are intersecting.
948         LineSegment l;
949         bool success = this->Intersects(other, &l);
950         if (success)
951         {
952                 float3 cp = l.CenterPoint();
953                 if (otherPt)
954                         *otherPt = cp;
955                 return cp;
956         }
957
958         float3 closestThis = this->ClosestPoint(other.a);
959         float3 closestOther = other.a;
960         float closestDSq = closestThis.DistanceSq(closestOther);
961
962         float3 pt = this->ClosestPoint(other.b);
963         float dSq = pt.DistanceSq(other.b);
964         if (dSq < closestDSq) closestThis = pt, closestOther = other.b, closestDSq = dSq;
965
966         pt = this->ClosestPoint(other.c);
967         dSq = pt.DistanceSq(other.c);
968         if (dSq < closestDSq) closestThis = pt, closestOther = other.c, closestDSq = dSq;
969
970         LineSegment l1[3] = { LineSegment(a,b), LineSegment(a,c), LineSegment(b,c) };
971         LineSegment l2[3] = { LineSegment(other.a,other.b), LineSegment(other.a,other.c), LineSegment(other.b,other.c) };
972         float d, d2;
973         for(int i = 0; i < 3; ++i)
974                 for(int j = 0; j < 3; ++j)
975                 {
976                         float dist = l1[i].Distance(l2[j], &d, &d2);
977                         if (dist*dist < closestDSq)
978                         {
979                                 closestThis = l1[i].GetPoint(d);
980                                 closestOther = l2[j].GetPoint(d2);
981                                 closestDSq = dist*dist;
982                         }
983                 }
984
985         if (otherPt)
986                 *otherPt = closestOther;
987         return closestThis;
988 }
989
990 float3 Triangle::RandomPointInside(LCG &rng) const
991 {
992         ///@todo rng.Float() returns [0,1[, but to be completely uniform, we'd need [0,1] here.
993         float s = rng.Float();
994         float t = rng.Float();
995         if (s + t > 1.f)
996         {
997                 s = 1.f - s;
998                 t = 1.f - t;
999         }
1000         return Point(s, t);
1001 }
1002
1003 float3 Triangle::RandomVertex(LCG &rng) const
1004 {
1005         return Vertex(rng.Int(0, 2));
1006 }
1007
1008 float3 Triangle::RandomPointOnEdge(LCG &rng) const
1009 {
1010         assume(!IsDegenerate());
1011         float ab = a.Distance(b);
1012         float bc = b.Distance(c);
1013         float ca = c.Distance(a);
1014         float r = rng.Float(0, ab + bc + ca);
1015         if (r < ab)
1016                 return a + (b-a) * r / ab;
1017         r -= ab;
1018         if (r < bc)
1019                 return b + (c-b) * r / bc;
1020         r -= bc;
1021         return c + (a-c) * r / ca;
1022 }
1023
1024 Triangle operator *(const float3x3 &transform, const Triangle &t)
1025 {
1026         return Triangle(transform*t.a, transform*t.b, transform*t.c);
1027 }
1028
1029 Triangle operator *(const float3x4 &transform, const Triangle &t)
1030 {
1031         return Triangle(transform.MulPos(t.a), transform.MulPos(t.b), transform.MulPos(t.c));
1032 }
1033
1034 Triangle operator *(const float4x4 &transform, const Triangle &t)
1035 {
1036         return Triangle(transform.MulPos(t.a), transform.MulPos(t.b), transform.MulPos(t.c));
1037 }
1038
1039 Triangle operator *(const Quat &transform, const Triangle &t)
1040 {
1041         return Triangle(transform*t.a, transform*t.b, transform*t.c);
1042 }
1043
1044 #ifdef MATH_ENABLE_STL_SUPPORT
1045 std::string Triangle::ToString() const
1046 {
1047         char str[256];
1048         sprintf(str, "Triangle(a:(%.2f, %.2f, %.2f) b:(%.2f, %.2f, %.2f) c:(%.2f, %.2f, %.2f))",
1049                 a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
1050         return str;
1051 }
1052 #endif
1053
1054 MATH_END_NAMESPACE