Polygon.cpp

1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file Polygon.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Polygon geometry object. */
18 #include "Polygon.h"
19 #ifdef MATH_ENABLE_STL_SUPPORT
20 #include "../Math/myassert.h"
21 #include <utility>
22 #include <list>
23 #endif
24
25 #include "AABB.h"
26 #include "OBB.h"
27 #include "Frustum.h"
28 #include "Polyhedron.h"
29 #include "Plane.h"
30 #include "Line.h"
31 #include "Ray.h"
32 #include "LineSegment.h"
33 #include "Triangle.h"
34 #include "Sphere.h"
35 #include "../Algorithm/Random/LCG.h"
36 #include "../Math/MathFunc.h"
37 #include "../Math/float3x3.h"
38 #include "../Math/float3x4.h"
39 #include "../Math/float4x4.h"
40 #include "../Math/Quat.h"
41 #include "../Math/float2.h"
42 #include "../Math/MathConstants.h"
43 #include "../Algorithm/GJK.h"
44
45 MATH_BEGIN_NAMESPACE
46
47 int Polygon::NumVertices() const
48 {
49         return (int)p.size();
50 }
51
52 int Polygon::NumEdges() const
53 {
54         return (int)p.size();
55 }
56
57 vec Polygon::Vertex(int vertexIndex) const
58 {
59         assume(vertexIndex >= 0);
60         assume(vertexIndex < (int)p.size());
61 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
62         if (vertexIndex < 0 || vertexIndex >= (int)p.size())
63                 return vec::nan;
64 #endif
65         return p[vertexIndex];
66 }
67
68 LineSegment Polygon::Edge(int i) const
69 {
70         if (p.empty())
71                 return LineSegment(vec::nan, vec::nan);
72         if (p.size() == 1)
73                 return LineSegment(p[0], p[0]);
74         return LineSegment(p[i], p[(i+1)%p.size()]);
75 }
76
77 LineSegment Polygon::Edge2D(int i) const
78 {
79         if (p.empty())
80                 return LineSegment(vec::nan, vec::nan);
81         if (p.size() == 1)
82                 return LineSegment(vec::zero, vec::zero);
83         return LineSegment(POINT_VEC(MapTo2D(i), 0), POINT_VEC(MapTo2D((i+1)%p.size()), 0));
84 }
85
86 bool Polygon::DiagonalExists(int i, int j) const
87 {
88         assume(p.size() >= 3);
89         assume(i >= 0);
90         assume(j >= 0);
91         assume(i < (int)p.size());
92         assume(j < (int)p.size());
93 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
94         if (p.size() < 3 || i < 0 || j < 0 || i >= (int)p.size() || j >= (int)p.size())
95                 return false;
96 #endif
97         assume(IsPlanar());
98         assume(i != j);
99         if (i == j) // Degenerate if i == j.
100                 return false;
101         if (i > j)
102                 Swap(i, j);
103         assume(i+1 != j);
104         if (i+1 == j) // Is this LineSegment an edge of this polygon?
105                 return false;
106
107         Plane polygonPlane = PlaneCCW();
108         LineSegment diagonal = polygonPlane.Project(LineSegment(p[i], p[j]));
109
110         // First check that this diagonal line is not intersected by an edge of this polygon.
111         for(int k = 0; k < (int)p.size(); ++k)
112                 if (!(k == i || k+1 == i || k == j))
113                         if (polygonPlane.Project(LineSegment(p[k], p[k+1])).Intersects(diagonal))
114                                 return false;
115
116         return IsConvex();
117 }
118
119 vec Polygon::BasisU() const
120 {
121         if (p.size() < 2)
122                 return vec::unitX;
123         vec u = (vec)p[1] - (vec)p[0];
124         u.Normalize(); // Always succeeds, even if u was zero (generates (1,0,0)).
125         return u;
126 }
127
128 vec Polygon::BasisV() const
129 {
130         if (p.size() < 2)
131                 return vec::unitY;
132         return Cross(PlaneCCW().normal, BasisU()).Normalized();
133 }
134
135 LineSegment Polygon::Diagonal(int i, int j) const
136 {
137         assume(i >= 0);
138         assume(j >= 0);
139         assume(i < (int)p.size());
140         assume(j < (int)p.size());
141 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
142         if (i < 0 || j < 0 || i >= (int)p.size() || j >= (int)p.size())
143                 return LineSegment(vec::nan, vec::nan);
144 #endif
145         return LineSegment(p[i], p[j]);
146 }
147
148 bool Polygon::IsConvex() const
149 {
150         assume(IsPlanar());
151         if (p.empty())
152                 return false;
153         if (p.size() <= 3)
154                 return true;
155         int i = (int)p.size()-2;
156         int j = (int)p.size()-1;
157         int k = 0;
158
159         while(k < (int)p.size())
160         {
161                 float2 a = MapTo2D(i);
162                 float2 b = MapTo2D(j);
163                 float2 c = MapTo2D(k);
164                 if (!float2::OrientedCCW(a, b, c))
165                         return false;
166                 i = j;
167                 j = k;
168                 ++k;
169         }
170         return true;
171 }
172
173 float2 Polygon::MapTo2D(int i) const
174 {
175         assume(i >= 0);
176         assume(i < (int)p.size());
177 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
178         if (i < 0 || i >= (int)p.size())
179                 return float2::nan;
180 #endif
181         return MapTo2D(p[i]);
182 }
183
184 float2 Polygon::MapTo2D(const vec &point) const
185 {
186         assume(!p.empty());
187 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
188         if (p.empty())
189                 return float2::nan;
190 #endif
191         vec basisU = BasisU();
192         vec basisV = BasisV();
193         vec pt = point - p[0];
194         return float2(Dot(pt, basisU), Dot(pt, basisV));
195 }
196
197 vec Polygon::MapFrom2D(const float2 &point) const
198 {
199         assume(!p.empty());
200 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
201         if (p.empty())
202                 return vec::nan;
203 #endif
204         return (vec)p[0] + point.x * BasisU() + point.y * BasisV();
205 }
206
207 bool Polygon::IsPlanar(float epsilonSq) const
208 {
209         if (p.empty())
210                 return false;
211         if (p.size() <= 3)
212                 return true;
213         vec normal = (vec(p[1])-vec(p[0])).Cross(vec(p[2])-vec(p[0]));
214         float lenSq = normal.LengthSq();
215         for(size_t i = 3; i < p.size(); ++i)
216         {
217                 float d = normal.Dot(vec(p[i])-vec(p[0]));
218                 if (d*d > epsilonSq * lenSq)
219                         return false;
220         }
221         return true;
222 }
223
224 bool Polygon::IsSimple() const
225 {
226         assume(IsPlanar());
227         Plane plane = PlaneCCW();
228         for(int i = 0; i < (int)p.size(); ++i)
229         {
230                 LineSegment si = plane.Project(Edge(i));
231                 for(int j = i+2; j < (int)p.size(); ++j)
232                 {
233                         if (i == 0 && j == (int)p.size() - 1)
234                                 continue; // These two edges are consecutive and share a vertex. Don't check that pair.
235                         LineSegment sj = plane.Project(Edge(j));
236                         if (si.Intersects(sj))
237                                 return false;
238                 }
239         }
240         return true;
241 }
242
243 bool Polygon::IsNull() const
244 {
245         return p.empty();
246 }
247
248 bool Polygon::IsFinite() const
249 {
250         for(size_t i = 0; i < p.size(); ++i)
251                 if (!((vec)p[i]).IsFinite())
252                         return false;
253
254         return true;
255 }
256
257 bool Polygon::IsDegenerate(float epsilon) const
258 {
259         return p.size() < 3 || Area() <= epsilon;
260 }
261
262 vec Polygon::NormalCCW() const
263 {
264         ///@todo Optimize temporaries.
265         return PlaneCCW().normal;
266 }
267
268 vec Polygon::NormalCW() const
269 {
270         ///@todo Optimize temporaries.
271         return PlaneCW().normal;
272 }
273
274 Plane Polygon::PlaneCCW() const
275 {
276         if (p.size() > 3)
277         {
278                 Plane plane;
279                 for(size_t i = 0; i < p.size()-2; ++i)
280                         for(size_t j = i+1; j < p.size()-1; ++j)
281                         {
282                                 vec pij = vec(p[j])-vec(p[i]);
283                                 for(size_t k = j+1; k < p.size(); ++k)
284                                 {
285                                         plane.normal = pij.Cross(vec(p[k])-vec(p[i]));
286                                         float lenSq = plane.normal.LengthSq();
287                                         if (lenSq > 1e-8f)
288                                         {
289                                                 plane.normal /= Sqrt(lenSq);
290                                                 plane.d = plane.normal.Dot(vec(p[i]));
291                                                 return plane;
292                                         }
293                                 }
294                         }
295
296 #ifndef MATH_SILENT_ASSUME
297                 LOGW("Polygon contains %d points, but they are all collinear! Cannot form a plane for the Polygon using three points! %s", (int)p.size(), this->SerializeToString().c_str());
298 #endif
299                 // Polygon contains multiple points, but they are all collinear.
300                 // Pick an arbitrary plane along the line as the polygon plane (as if the polygon had only two points)
301                 vec dir = (vec(p[1])-vec(p[0])).Normalized();
302                 return Plane(Line(p[0], dir), dir.Perpendicular());
303         }
304         if (p.size() == 3)
305                 return Plane(p[0], p[1], p[2]);
306         if (p.size() == 2)
307         {
308                 vec dir = (vec(p[1])-vec(p[0])).Normalized();
309                 return Plane(Line(p[0], dir), dir.Perpendicular());
310         }
311         if (p.size() == 1)
312                 return Plane(p[0], DIR_VEC(0,1,0));
313         return Plane();
314 }
315
316 Plane Polygon::PlaneCW() const
317 {
318         Plane plane = PlaneCCW();
319         plane.ReverseNormal();
320         return plane;
321 }
322
323 void Polygon::Translate(const vec &offset)
324 {
325         for(size_t i = 0; i < p.size(); ++i)
326                 p[i] = (vec)p[i] + offset;
327 }
328
329 void Polygon::Transform(const float3x3 &transform)
330 {
331         if (!p.empty())
332                 transform.BatchTransform((vec*)&p[0], (int)p.size());
333 }
334
335 void Polygon::Transform(const float3x4 &transform)
336 {
337         if (!p.empty())
338                 transform.BatchTransformPos((vec*)&p[0], (int)p.size());
339 }
340
341 void Polygon::Transform(const float4x4 &transform)
342 {
343         for(size_t i = 0; i < p.size(); ++i)
344                 p[i] = transform.MulPos(p[i]);
345 }
346
347 void Polygon::Transform(const Quat &transform)
348 {
349         for(size_t i = 0; i < p.size(); ++i)
350                 p[i] = transform * p[i];
351 }
352
353 bool Polygon::Contains(const Polygon &worldSpacePolygon, float polygonThickness) const
354 {
355         for(int i = 0; i < worldSpacePolygon.NumVertices(); ++i)
356                 if (!Contains(worldSpacePolygon.Vertex(i), polygonThickness))
357                         return false;
358         return true;
359 }
360
361 bool Polygon::Contains(const vec &worldSpacePoint, float polygonThicknessSq) const
362 {
363         // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
364
365         if (p.size() < 3)
366                 return false;
367
368         vec basisU = BasisU();
369         vec basisV = BasisV();
370         assert1(basisU.IsNormalized(), basisU);
371         assert1(basisV.IsNormalized(), basisV);
372         assert2(basisU.IsPerpendicular(basisV), basisU, basisV);
373         assert3(basisU.IsPerpendicular(PlaneCCW().normal), basisU, PlaneCCW().normal, basisU.Dot(PlaneCCW().normal));
374         assert3(basisV.IsPerpendicular(PlaneCCW().normal), basisV, PlaneCCW().normal, basisV.Dot(PlaneCCW().normal));
375
376         vec normal = basisU.Cross(basisV);
377 //      float lenSq = normal.LengthSq(); ///\todo Could we treat basisU and basisV unnormalized here?
378         float dot = normal.Dot(vec(p[0]) - worldSpacePoint);
379         if (dot*dot > polygonThicknessSq)
380                 return false;
381
382         int numIntersections = 0;
383
384         const float epsilon = 1e-4f;
385
386         // General strategy: transform all points on the polygon onto 2D face plane of the polygon, where the target query point is
387         // centered to lie in the origin.
388         // If the test ray (0,0) -> (+inf, 0) intersects exactly an odd number of polygon edge segments, then the query point must have been
389         // inside the polygon. The test ray is chosen like that to avoid all extra per-edge computations.
390         vec vt = vec(p.back()) - worldSpacePoint;
391         float2 p0 = float2(Dot(vt, basisU), Dot(vt, basisV));
392         if (Abs(p0.y) < epsilon)
393                 p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
394
395         for(int i = 0; i < (int)p.size(); ++i)
396         {
397                 vt = vec(p[i]) - worldSpacePoint;
398                 float2 p1 = float2(Dot(vt, basisU), Dot(vt, basisV));
399                 if (Abs(p1.y) < epsilon)
400                         p1.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
401
402                 if (p0.y * p1.y < 0.f) // If the line segment p0 -> p1 straddles the line x=0, it could intersect the ray (0,0) -> (+inf, 0)
403                 {
404                         if (Min(p0.x, p1.x) > 0.f) // If both x-coordinates are positive, then there certainly is an intersection with the ray.
405                                 ++numIntersections;
406                         else if (Max(p0.x, p1.x) > 0.f) // If one of them is positive, there could be an intersection. (otherwise both are negative and they can't intersect ray)
407                         {
408                                 // P = p0 + t*(p1-p0) == (x,0)
409                                 //     p0.x + t*(p1.x-p0.x) == x
410                                 //     p0.y + t*(p1.y-p0.y) == 0
411                                 //                 t == -p0.y / (p1.y - p0.y)
412
413                                 // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate?
414                                 float2 d = p1 - p0;
415                                 if (d.y != 0.f)
416                                 {
417                                         float t = -p0.y / d.y; // The line segment parameter, t \in [0,1] forms the line segment p0->p1.
418                                         float x = p0.x + t * d.x; // The x-coordinate of intersection with the ray.
419                                         if (t >= 0.f && t <= 1.f && x > 0.f)
420                                                 ++numIntersections;
421                                 }
422                         }
423                 }
424                 p0 = p1;
425         }
426
427         return numIntersections % 2 == 1;
428 }
429
430 bool Polygon::Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const
431 {
432         if (p.size() < 3)
433                 return false;
434
435         Plane plane = PlaneCCW();
436         if (plane.Distance(worldSpaceLineSegment.a) > polygonThickness ||
437                 plane.Distance(worldSpaceLineSegment.b) > polygonThickness)
438                 return false;
439
440         // For robustness, project onto the polygon plane.
441         LineSegment l = plane.Project(worldSpaceLineSegment);
442
443         if (!Contains(l.a) || !Contains(l.b))
444                 return false;
445
446         for(int i = 0; i < (int)p.size(); ++i)
447                 if (plane.Project(Edge(i)).Intersects(l))
448                         return false;
449
450         return true;
451 }
452
453 bool Polygon::Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const
454 {
455         return Contains(worldSpaceTriangle.Edge(0), polygonThickness) &&
456                 Contains(worldSpaceTriangle.Edge(1), polygonThickness) &&
457                 Contains(worldSpaceTriangle.Edge(2), polygonThickness);
458 }
459
460 bool Polygon::Contains2D(const LineSegment &localSpaceLineSegment) const
461 {
462         if (p.size() < 3)
463                 return false;
464
465 ///\todo Reimplement this!
466 //      if (!Contains2D(localSpaceLineSegment.a.xy()) || !Contains2D(localSpaceLineSegment.b.xy()))
467 //              return false;
468
469         for(int i = 0; i < (int)p.size(); ++i)
470                 if (Edge2D(i).Intersects(localSpaceLineSegment))
471                         return false;
472
473         return true;
474 }
475
476 bool Polygon::Intersects(const Line &line) const
477 {
478         float d;
479         if (!PlaneCCW().Intersects(line, &d))
480                 return false;
481         return Contains(line.GetPoint(d));
482 }
483
484 bool Polygon::Intersects(const Ray &ray) const
485 {
486         float d;
487         if (!PlaneCCW().Intersects(ray, &d))
488                 return false;
489         return Contains(ray.GetPoint(d));
490 }
491
492 bool Polygon::Intersects(const LineSegment &lineSegment) const
493 {
494         Plane plane = PlaneCCW();
495         float t;
496         bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
497         if (!intersects || t < 0.f || t > 1.f)
498                 return false;
499
500         return Contains(lineSegment.GetPoint(t));
501 }
502
503 bool Polygon::Intersects(const Plane &plane) const
504 {
505         // Project the points of this polygon onto the 1D axis of the plane normal.
506         // If there are points on both sides of the plane, then the polygon intersects the plane.
507         float minD = inf;
508         float maxD = -inf;
509         for(size_t i = 0; i < p.size(); ++i)
510         {
511                 float d = plane.SignedDistance(p[i]);
512                 minD = Min(minD, d);
513                 maxD = Max(maxD, d);
514         }
515         // Allow a very small epsilon tolerance.
516         return minD <= 1e-4f && maxD >= -1e-4f;
517 }
518
519 bool Polygon::ConvexIntersects(const AABB &aabb) const
520 {
521         return GJKIntersect(*this, aabb);
522 }
523
524 bool Polygon::ConvexIntersects(const OBB &obb) const
525 {
526         return GJKIntersect(*this, obb);
527 }
528
529 bool Polygon::ConvexIntersects(const Frustum &frustum) const
530 {
531         return GJKIntersect(*this, frustum);
532 }
533
534 template<typename Convex /* = AABB, OBB, Frustum. */>
535 bool Convex_Intersects_Polygon(const Convex &c, const Polygon &p)
536 {
537         LineSegment l;
538         l.a = p.p.back();
539         for(size_t i = 0; i < p.p.size(); ++i)
540         {
541                 l.b = p.p[i];
542                 if (c.Intersects(l))
543                         return true;
544                 l.a = l.b;
545         }
546
547         // Check all the edges of the convex shape against the polygon.
548         for(int i = 0; i < c.NumEdges(); ++i)
549         {
550                 l = c.Edge(i);
551                 if (p.Intersects(l))
552                         return true;
553         }
554
555         return false;
556 }
557
558 bool Polygon::Intersects(const AABB &aabb) const
559 {
560         // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the aabb)
561         bool convexIntersects = ConvexIntersects(aabb);
562         if (!convexIntersects)
563                 return false;
564
565         return Convex_Intersects_Polygon(aabb, *this);
566 }
567
568 bool Polygon::Intersects(const OBB &obb) const
569 {
570         // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the obb)
571         bool convexIntersects = ConvexIntersects(obb);
572         if (!convexIntersects)
573                 return false;
574
575         return Convex_Intersects_Polygon(obb, *this);
576 }
577
578 bool Polygon::Intersects(const Frustum &frustum) const
579 {
580         // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the frustum)
581         bool convexIntersects = ConvexIntersects(frustum);
582         if (!convexIntersects)
583                 return false;
584
585         return Convex_Intersects_Polygon(frustum, *this);
586 }
587
588 template<typename T /* = Polygon or Triangle */>
589 bool Polygon_Intersects_Polygon(const Polygon &poly, const T &other, float polygonThickness)
590 {
591         Plane plane = poly.PlaneCCW();
592         Plane plane2 = other.PlaneCCW();
593
594         if (!plane.normal.Cross(plane2.normal).IsZero())
595         {
596                 // General strategy: If two polygon/triangle objects intersect, one
597                 // of them must have an edge that passes through the interior of the other object.
598                 // Test each edge of the this object against intersection of the interior of the other polygon,
599                 // and vice versa.
600                 for(int i = 0; i < other.NumEdges(); ++i)
601                 {
602                         LineSegment lineSegment = other.Edge(i);
603                         float t;
604                         bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
605                         if (!intersects || t < 0.f || t > 1.f)
606                                 continue;
607
608                         if (poly.Contains(lineSegment.GetPoint(t)))
609                                 return true;
610                 }
611
612                 for(int i = 0; i < poly.NumEdges(); ++i)
613                 {
614                         LineSegment lineSegment = poly.Edge(i);
615                         float t;
616                         bool intersects = Plane::IntersectLinePlane(plane2.normal, plane2.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
617                         if (!intersects || t < 0.f || t > 1.f)
618                                 continue;
619
620                         if (other.Contains(lineSegment.GetPoint(t)))
621                                 return true;
622                 }
623                 return false;
624         }
625         else // The two polygons are coplanar. Perform the intersection test in 2D.
626         {
627                 float poly0Pos = plane.normal.Dot(poly.Vertex(0));
628                 float poly1Pos = plane.normal.Dot(other.Vertex(0));
629                 if (Abs(poly0Pos-poly1Pos) > polygonThickness)
630                         return false;
631
632                 if (other.Contains(poly.Vertex(0), FLOAT_INF) || poly.Contains(other.Vertex(0), FLOAT_INF))
633                         return true;
634
635                 vec basisU, basisV;
636                 plane.normal.PerpendicularBasis(basisU, basisV);
637
638                 vec pivot = poly.Vertex(0);
639                 vec pt = poly.Vertex(poly.NumVertices()-1)-pivot;
640                 float2 a1 = float2(basisU.Dot(pt), basisV.Dot(pt));
641                 for(int i = 0; i < poly.NumVertices(); ++i)
642                 {
643                         pt = poly.Vertex(i)-pivot;
644                         float2 a2 = float2(basisU.Dot(pt), basisV.Dot(pt));
645
646                         pt = other.Vertex(other.NumVertices()-1)-pivot;
647                         float2 b1 = float2(basisU.Dot(pt), basisV.Dot(pt));
648                         for(int j = 0; j < other.NumVertices(); ++j)
649                         {
650                                 pt = other.Vertex(j)-pivot;
651                                 float2 b2 = float2(basisU.Dot(pt), basisV.Dot(pt));
652
653                                 float s, t;
654                                 if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b1, b2-b1, s, t))
655                                         return true;
656
657                                 b1 = b2;
658                         }
659                         a1 = a2;
660                 }
661
662                 return false;
663         }
664 }
665
666
667 bool Polygon::Intersects(const Triangle &triangle, float polygonThickness) const
668 {
669         return Polygon_Intersects_Polygon(*this, triangle, polygonThickness);
670 }
671
672 bool Polygon::Intersects(const Polygon &polygon, float polygonThickness) const
673 {
674         return Polygon_Intersects_Polygon(*this, polygon, polygonThickness);
675 }
676
677 bool Polygon::Intersects(const Polyhedron &polyhedron) const
678 {
679         return polyhedron.Intersects(*this);
680 }
681
682 bool Polygon::Intersects(const Sphere &sphere) const
683 {
684         ///@todo Optimize.
685         TriangleArray tris = Triangulate();
686         for(size_t i = 0; i < tris.size(); ++i)
687                 if (TRIANGLE(tris[i]).Intersects(sphere))
688                         return true;
689
690         return false;
691 }
692
693 bool Polygon::Intersects(const Capsule &capsule) const
694 {
695         ///@todo Optimize.
696         TriangleArray tris = Triangulate();
697         for(size_t i = 0; i < tris.size(); ++i)
698                 if (TRIANGLE(tris[i]).Intersects(capsule))
699                         return true;
700
701         return false;
702 }
703
704 vec Polygon::ClosestPoint(const vec &point) const
705 {
706         assume(IsPlanar());
707
708         TriangleArray tris = Triangulate();
709         vec closestPt = vec::nan;
710         float closestDist = FLT_MAX;
711         for(size_t i = 0; i < tris.size(); ++i)
712         {
713                 vec pt = TRIANGLE(tris[i]).ClosestPoint(point);
714                 float d = pt.DistanceSq(point);
715                 if (d < closestDist)
716                 {
717                         closestPt = pt;
718                         closestDist = d;
719                 }
720         }
721         return closestPt;
722 }
723
724 vec Polygon::ClosestPoint(const LineSegment &lineSegment) const
725 {
726         return ClosestPoint(lineSegment, 0);
727 }
728
729 vec Polygon::ClosestPoint(const LineSegment &lineSegment, vec *lineSegmentPt) const
730 {
731         TriangleArray tris = Triangulate();
732         vec closestPt = vec::nan;
733         vec closestLineSegmentPt = vec::nan;
734         float closestDist = FLT_MAX;
735         for(size_t i = 0; i < tris.size(); ++i)
736         {
737                 vec lineSegPt;
738                 vec pt = TRIANGLE(tris[i]).ClosestPoint(lineSegment, &lineSegPt);
739                 float d = pt.DistanceSq(lineSegPt);
740                 if (d < closestDist)
741                 {
742                         closestPt = pt;
743                         closestLineSegmentPt = lineSegPt;
744                         closestDist = d;
745                 }
746         }
747         if (lineSegmentPt)
748                 *lineSegmentPt = closestLineSegmentPt;
749         return closestPt;
750 }
751
752 float Polygon::Distance(const vec &point) const
753 {
754         vec pt = ClosestPoint(point);
755         return pt.Distance(point);
756 }
757
758 vec Polygon::EdgeNormal(int edgeIndex) const
759 {
760         return Cross(Edge(edgeIndex).Dir(), PlaneCCW().normal).Normalized();
761 }
762
763 Plane Polygon::EdgePlane(int edgeIndex) const
764 {
765         return Plane(Edge(edgeIndex).a, EdgeNormal(edgeIndex));
766 }
767
768 vec Polygon::ExtremePoint(const vec &direction) const
769 {
770         float projectionDistance;
771         return ExtremePoint(direction, projectionDistance);
772 }
773
774 vec Polygon::ExtremePoint(const vec &direction, float &projectionDistance) const
775 {
776         vec mostExtreme = vec::nan;
777         projectionDistance = -FLOAT_INF;
778         for(int i = 0; i < NumVertices(); ++i)
779         {
780                 vec pt = Vertex(i);
781                 float d = Dot(direction, pt);
782                 if (d > projectionDistance)
783                 {
784                         projectionDistance = d;
785                         mostExtreme = pt;
786                 }
787         }
788         return mostExtreme;
789 }
790
791 void Polygon::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
792 {
793         ///\todo Optimize!
794         vec minPt = ExtremePoint(-direction);
795         vec maxPt = ExtremePoint(direction);
796         outMin = Dot(minPt, direction);
797         outMax = Dot(maxPt, direction);
798 }
799
800 /*
801 /// Returns true if the edges of this polygon self-intersect.
802 bool IsSelfIntersecting() const;
803
804 /// Projects all vertices of this polygon to the given plane.
805 void ProjectToPlane(const Plane &plane);
806
807 /// Returns true if the edges of this polygon self-intersect when viewed from the given direction.
808 bool IsSelfIntersecting(const vec &viewDirection) const;
809
810 bool Contains(const vec &point, const vec &viewDirection) const;
811
812 */
813
814 /** Implementation based on Graphics Gems 2, p. 170: "IV.1. Area of Planar Polygons and Volume of Polyhedra." */
815 float Polygon::Area() const
816 {
817         assume(IsPlanar());
818         vec area = vec::zero;
819         if (p.size() <= 2)
820                 return 0.f;
821
822         int i = NumEdges()-1;
823         for(int j = 0; j < NumEdges(); ++j)
824         {
825                 area += Vertex(i).Cross(Vertex(j));
826                 i = j;
827         }
828         return 0.5f * Abs(NormalCCW().Dot(area));
829 }
830
831 float Polygon::Perimeter() const
832 {
833         float perimeter = 0.f;
834         for(int i = 0; i < NumEdges(); ++i)
835                 perimeter += Edge(i).Length();
836         return perimeter;
837 }
838
839 ///\bug This function does not properly compute the centroid.
840 vec Polygon::Centroid() const
841 {
842         if (NumVertices() == 0)
843                 return vec::nan;
844         vec centroid = vec::zero;
845         for(int i = 0; i < NumVertices(); ++i)
846                 centroid += Vertex(i);
847         return centroid / (float)NumVertices();
848 }
849
850 vec Polygon::PointOnEdge(float normalizedDistance) const
851 {
852         if (p.empty())
853                 return vec::nan;
854         if (p.size() < 2)
855                 return p[0];
856         normalizedDistance = Frac(normalizedDistance); // Take modulo 1 so we have the range [0,1[.
857         float perimeter = Perimeter();
858         float d = normalizedDistance * perimeter;
859         for(int i = 0; i < NumVertices(); ++i)
860         {
861                 LineSegment edge = Edge(i);
862                 float len = edge.Length();
863                 assume(len != 0.f && "Degenerate Polygon detected!");
864                 if (d <= len)
865                         return edge.GetPoint(d / len);
866                 d -= len;
867         }
868         mathassert(false && "Polygon::PointOnEdge reached end of loop which shouldn't!");
869         return p[0];
870 }
871
872 vec Polygon::RandomPointOnEdge(LCG &rng) const
873 {
874         return PointOnEdge(rng.Float());
875 }
876
877 vec Polygon::FastRandomPointInside(LCG &rng) const
878 {
879         TriangleArray tris = Triangulate();
880         if (tris.empty())
881                 return vec::nan;
882         int i = rng.Int(0, (int)tris.size()-1);
883         return TRIANGLE(tris[i]).RandomPointInside(rng);
884 }
885
886 Polyhedron Polygon::ToPolyhedron() const
887 {
888         Polyhedron poly;
889         poly.v = p;
890         poly.f.push_back(Polyhedron::Face());
891         poly.f.push_back(Polyhedron::Face());
892         for(int i = 0; i < NumVertices(); ++i)
893         {
894                 poly.f[0].v.push_back(i);
895                 poly.f[1].v.push_back(NumVertices()-1-i);
896         }
897         return poly;
898 }
899
900 // A(u) = a1 + u * (a2-a1).
901 // B(v) = b1 + v * (b2-b1).
902 // Returns (u,v).
903 bool IntersectLineLine2D(const float2 &a1, const float2 &a2, const float2 &b1, const float2 &b2, float2 &out)
904 {
905         float u = (b2.x - b1.x)*(a1.y - b1.y) - (b2.y - b1.y)*(a1.x - b1.x);
906         float v = (a2.x - a1.x)*(a1.y - b1.y) - (a2.y - a1.y)*(a1.x - b1.x);
907
908         float det = (b2.y - b1.y)*(a2.x - a1.x) - (b2.x - b1.x)*(a2.y - a1.y);
909         if (Abs(det) < 1e-4f)
910                 return false;
911         det = 1.f / det;
912         out.x = u * det;
913         out.y = v * det;
914
915         return true;
916 }
917
918 bool IntersectLineSegmentLineSegment2D(const float2 &a1, const float2 &a2, const float2 &b1, const float2 &b2, float2 &out)
919 {
920         bool ret = IntersectLineLine2D(a1, a2, b1, b2, out);
921         return ret && out.x >= 0.f && out.x <= 1.f && out.y >= 0.f && out.y <= 1.f;
922 }
923
924 /// Returns true if poly[i+1] is an ear.
925 /// Precondition: i+2 == j (mod poly.size()).
926 bool IsAnEar(const std::vector<float2> &poly, int i, int j)
927 {
928         float2 dummy;
929         int x = (int)poly.size()-1;
930         for(int y = 0; y < i; ++y)
931         {
932                 if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
933                         return false;
934                 x = y;
935         }
936         x = j+1;
937         for(int y = x+1; y < (int)poly.size(); ++y)
938         {
939                 if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
940                         return false;
941                 x = y;
942         }
943         return true;
944 }
945
946 /** The implementation of this function is based on the paper
947         "Kong, Everett, Toussant. The Graham Scan Triangulates Simple Polygons."
948         See also p. 772-775 of Geometric Tools for Computer Graphics.
949         The running time of this function is O(n^2). */
950 TriangleArray Polygon::Triangulate() const
951 {
952         assume1(IsPlanar(), this->SerializeToString());
953
954         TriangleArray t;
955         // Handle degenerate cases.
956         if (NumVertices() < 3)
957                 return t;
958         if (NumVertices() == 3)
959         {
960                 t.push_back(Triangle(Vertex(0), Vertex(1), Vertex(2)));
961                 return t;
962         }
963         std::vector<float2> p2d;
964         std::vector<int> polyIndices;
965         for(int v = 0; v < NumVertices(); ++v)
966         {
967                 p2d.push_back(MapTo2D(v));
968                 polyIndices.push_back(v);
969         }
970
971         // Clip ears of the polygon until it has been reduced to a triangle.
972         int i = 0;
973         int j = 1;
974         int k = 2;
975         size_t numTries = 0; // Avoid creating an infinite loop.
976         while(p2d.size() > 3 && numTries < p2d.size())
977         {
978                 if (float2::OrientedCCW(p2d[i], p2d[j], p2d[k]) && IsAnEar(p2d, i, k))
979                 {
980                         // The vertex j is an ear. Clip it off.
981                         t.push_back(Triangle(p[polyIndices[i]], p[polyIndices[j]], p[polyIndices[k]]));
982                         p2d.erase(p2d.begin() + j);
983                         polyIndices.erase(polyIndices.begin() + j);
984
985                         // The previous index might now have become an ear. Move back one index to see if so.
986                         if (i > 0)
987                         {
988                                 i = (i + (int)p2d.size() - 1) % p2d.size();
989                                 j = (j + (int)p2d.size() - 1) % p2d.size();
990                                 k = (k + (int)p2d.size() - 1) % p2d.size();
991                         }
992                         numTries = 0;
993                 }
994                 else
995                 {
996                         // The vertex at j is not an ear. Move to test next vertex.
997                         i = j;
998                         j = k;
999                         k = (k+1) % p2d.size();
1000                         ++numTries;
1001                 }
1002         }
1003
1004         assume3(p2d.size() == 3, (int)p2d.size(), (int)polyIndices.size(), (int)NumVertices());
1005         if (p2d.size() > 3) // If this occurs, then the polygon is NOT counter-clockwise oriented.
1006                 return t;
1007 /*
1008         {
1009                 // For conveniency, create a copy that has the winding order fixed, and triangulate that instead.
1010                 // (Causes a large performance hit!)
1011                 Polygon p2 = *this;
1012                 for(size_t i = 0; i < p2.p.size()/2; ++i)
1013                         std::swap(p2.p[i], p2.p[p2.p.size()-1-i]);
1014                 return p2.Triangulate();
1015         }
1016 */
1017         // Add the last poly.
1018         t.push_back(Triangle(p[polyIndices[0]], p[polyIndices[1]], p[polyIndices[2]]));
1019
1020         return t;
1021 }
1022
1023 AABB Polygon::MinimalEnclosingAABB() const
1024 {
1025         AABB aabb;
1026         aabb.SetNegativeInfinity();
1027         for(int i = 0; i < NumVertices(); ++i)
1028                 aabb.Enclose(Vertex(i));
1029         return aabb;
1030 }
1031
1032 std::string Polygon::ToString() const
1033 {
1034         if (p.empty())
1035                 return "Polygon(0 vertices)";
1036
1037         std::stringstream ss;
1038         ss << "Polygon(" << p.size() << " vertices:";
1039         for(size_t i = 0; i < p.size(); ++i)
1040         {
1041                 if (i != 0)
1042                         ss << ",";
1043                 ss << p[i];
1044         }
1045         ss << ")";
1046         return ss.str();
1047 }
1048
1049 std::string Polygon::SerializeToString() const
1050 {
1051         std::stringstream ss;
1052         ss << "(";
1053         for(size_t i = 0; i < p.size(); ++i)
1054                 ss << "(" << vec(p[i]).xyz().SerializeToString() + (i+1 != p.size() ? ")," : ")");
1055         ss << ")";
1056         return ss.str();
1057 }
1058
1059 Polygon Polygon::FromString(const char *str, const char **outEndStr)
1060 {
1061         MATH_SKIP_WORD(str, "Polygon");
1062         MATH_SKIP_WORD(str, "(");
1063         Polygon p;
1064         while(*str == '(' || *str == ',')
1065         {
1066                 MATH_SKIP_WORD(str, ",");
1067                 float3 pt = float3::FromString(str, &str);
1068                 p.p.push_back(POINT_VEC(pt));
1069         }
1070         MATH_SKIP_WORD(str, ")");
1071
1072         if (outEndStr)
1073                 *outEndStr = str;
1074
1075         return p;
1076 }
1077
1078 bool Polygon::Equals(const Polygon &other) const
1079 {
1080         if (p.size() != other.p.size())
1081                 return false;
1082
1083         for(size_t i = 0; i < p.size(); ++i)
1084                 if (!Vertex((int)i).Equals(other.Vertex((int)i)))
1085                         return false;
1086
1087         return true;
1088 }
1089
1090 bool Polygon::BitEquals(const Polygon &other) const
1091 {
1092         if (p.size() != other.p.size())
1093                 return false;
1094
1095         for(size_t i = 0; i < p.size(); ++i)
1096                 if (!Vertex((int)i).BitEquals(other.Vertex((int)i)))
1097                         return false;
1098
1099         return true;
1100 }
1101
1102 /*
1103 /// Returns true if the given vertex is a concave vertex. Otherwise the vertex is a convex vertex.
1104 bool IsConcaveVertex(int i) const;
1105
1106 /// Computes the conves hull of this polygon.
1107 Polygon ConvexHull() const;
1108
1109 bool IsSupportingPoint(int i) const;
1110
1111 bool IsSupportingPoint(const vec &point) const;
1112
1113 /// Returns true if the quadrilateral defined by the four points is convex (and not concave or bowtie).
1114 static bool IsConvexQuad(const vec &pointA, const vec &pointB, const vec &pointC, const vec &pointD);
1115 */
1116
1117 Polygon operator *(const float3x3 &transform, const Polygon &polygon)
1118 {
1119         Polygon p(polygon);
1120         p.Transform(transform);
1121         return p;
1122 }
1123
1124 Polygon operator *(const float3x4 &transform, const Polygon &polygon)
1125 {
1126         Polygon p(polygon);
1127         p.Transform(transform);
1128         return p;
1129 }
1130
1131 Polygon operator *(const float4x4 &transform, const Polygon &polygon)
1132 {
1133         Polygon p(polygon);
1134         p.Transform(transform);
1135         return p;
1136 }
1137
1138 Polygon operator *(const Quat &transform, const Polygon &polygon)
1139 {
1140         Polygon p(polygon);
1141         p.Transform(transform);
1142         return p;
1143 }
1144
1145 MATH_END_NAMESPACE