Polyhedron.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 Polyhedron.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Polyhedron geometry object. */
18 #include "Polyhedron.h"
19 #include <set>
20 #include <map>
21 #include <utility>
22 #include <list>
23 #include <sstream>
24 #include <stdlib.h>
25 #include "../Math/assume.h"
26 #include "../Math/MathFunc.h"
27 #include "../Math/float3x3.h"
28 #include "../Math/float3x4.h"
29 #include "../Math/float4x4.h"
30 #include "../Math/Quat.h"
31 #include "AABB.h"
32 #include "OBB.h"
33 #include "Frustum.h"
34 #include "Plane.h"
35 #include "Polygon.h"
36 #include "Line.h"
37 #include "Ray.h"
38 #include "LineSegment.h"
39 #include "Triangle.h"
40 #include "Sphere.h"
41 #include "Capsule.h"
42
43 #ifdef MATH_GRAPHICSENGINE_INTEROP
44 #include "VertexBuffer.h"
45 #endif
46
47 MATH_BEGIN_NAMESPACE
48
49 void Polyhedron::Face::FlipWindingOrder()
50 {
51         for(size_t i = 0; i < v.size()/2; ++i)
52                 Swap(v[i], v[v.size()-1-i]);
53 }
54
55 std::string Polyhedron::Face::ToString() const
56 {
57         std::stringstream ss;
58         for(size_t i = 0; i < v.size(); ++i)
59                 ss << v[i] << ((i!=v.size()-1) ? ", " : "");
60         return ss.str();
61 }
62
63 Polyhedron::Face Polyhedron::Face::FromString(const char *str)
64 {
65         Face f;
66         if (!str)
67                 return f;
68         while(*str)
69         {
70                 char *endptr = 0;
71                 int idx = (int)strtol(str, &endptr, 10);
72                 str = endptr;
73                 while(*str == ',' || *str == ' ')
74                         ++str;
75                 f.v.push_back(idx);
76         }
77         return f;
78 }
79
80 int Polyhedron::NumEdges() const
81 {
82         int numEdges = 0;
83         for(size_t i = 0; i < f.size(); ++i)
84                 numEdges += (int)f[i].v.size();
85         return numEdges / 2;
86 }
87
88 vec Polyhedron::Vertex(int vertexIndex) const
89 {
90         assume(vertexIndex >= 0);
91         assume(vertexIndex < (int)v.size());
92 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
93         if (vertexIndex < 0 || vertexIndex >= (int)v.size())
94                 return vec::nan;
95 #endif
96
97         return v[vertexIndex];
98 }
99
100 LineSegment Polyhedron::Edge(int edgeIndex) const
101 {
102         assume(edgeIndex >= 0);
103         LineSegmentArray edges = Edges();
104         assume(edgeIndex < (int)edges.size());
105 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
106         if (edgeIndex < 0 || edgeIndex >= (int)edges.size())
107                 return LineSegment(vec::nan, vec::nan);
108 #endif
109         return edges[edgeIndex];
110 }
111
112 LineSegmentArray Polyhedron::Edges() const
113 {
114         std::vector<std::pair<int, int> > edges = EdgeIndices();
115         LineSegmentArray edgeLines;
116         edgeLines.reserve(edges.size());
117         for(size_t i = 0; i < edges.size(); ++i)
118                 edgeLines.push_back(LineSegment(Vertex(edges[i].first), Vertex(edges[i].second)));
119
120         return edgeLines;
121 }
122
123 std::vector<std::pair<int, int> > Polyhedron::EdgeIndices() const
124 {
125         std::set<std::pair<int, int> > uniqueEdges;
126         for(int i = 0; i < NumFaces(); ++i)
127         {
128                 assume(f[i].v.size() >= 3);
129                 if (f[i].v.size() < 3)
130                         continue; // Degenerate face with less than three vertices, skip!
131                 int x = f[i].v.back();
132                 for(size_t j = 0; j < f[i].v.size(); ++j)
133                 {
134                         int y = f[i].v[j];
135                         uniqueEdges.insert(std::make_pair(Min(x, y), Max(x, y)));
136                         x = y;
137                 }
138         }
139
140         std::vector<std::pair<int, int> >edges;
141         edges.insert(edges.end(), uniqueEdges.begin(), uniqueEdges.end());
142         return edges;
143 }
144
145 std::vector<Polygon> Polyhedron::Faces() const
146 {
147         std::vector<Polygon> faces;
148         faces.reserve(NumFaces());
149         for(int i = 0; i < NumFaces(); ++i)
150                 faces.push_back(FacePolygon(i));
151         return faces;
152 }
153
154 Polygon Polyhedron::FacePolygon(int faceIndex) const
155 {
156         Polygon p;
157         assume(faceIndex >= 0);
158         assume(faceIndex < (int)f.size());
159 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
160         if (faceIndex < 0 || faceIndex >= (int)f.size())
161                 return Polygon();
162 #endif
163
164         p.p.reserve(f[faceIndex].v.size());
165         for(size_t v = 0; v < f[faceIndex].v.size(); ++v)
166                 p.p.push_back(Vertex(f[faceIndex].v[v]));
167         return p;
168 }
169
170 Plane Polyhedron::FacePlane(int faceIndex) const
171 {
172         const Face &face = f[faceIndex];
173         if (face.v.size() >= 3)
174                 return Plane(v[face.v[0]], v[face.v[1]], v[face.v[2]]);
175         else if (face.v.size() == 2)
176                 return Plane(Line(v[face.v[0]], v[face.v[1]]), ((vec)v[face.v[0]]-(vec)v[face.v[1]]).Perpendicular());
177         else if (face.v.size() == 1)
178                 return Plane(v[face.v[0]], DIR_VEC(0,1,0));
179         else
180                 return Plane();
181 }
182
183 vec Polyhedron::FaceNormal(int faceIndex) const
184 {
185         const Face &face = f[faceIndex];
186         if (face.v.size() >= 3)
187                 return ((vec)v[face.v[1]]-(vec)v[face.v[0]]).Cross((vec)v[face.v[2]]-(vec)v[face.v[0]]).Normalized();
188         else if (face.v.size() == 2)
189                 return ((vec)v[face.v[1]]-(vec)v[face.v[0]]).Cross(((vec)v[face.v[0]]-(vec)v[face.v[1]]).Perpendicular()-v[face.v[0]]).Normalized();
190         else if (face.v.size() == 1)
191                 return DIR_VEC(0,1,0);
192         else
193                 return vec::nan;
194 }
195
196 int Polyhedron::ExtremeVertex(const vec &direction) const
197 {
198         int mostExtreme = -1;
199         float mostExtremeDist = -FLT_MAX;
200         for(int i = 0; i < NumVertices(); ++i)
201         {
202                 float d = Dot(direction, Vertex(i));
203                 if (d > mostExtremeDist)
204                 {
205                         mostExtremeDist = d;
206                         mostExtreme = i;
207                 }
208         }
209         return mostExtreme;
210 }
211
212 vec Polyhedron::ExtremePoint(const vec &direction) const
213 {
214         return Vertex(ExtremeVertex(direction));
215 }
216
217 void Polyhedron::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
218 {
219         ///\todo Optimize!
220         vec minPt = ExtremePoint(-direction);
221         vec maxPt = ExtremePoint(direction);
222         outMin = Dot(minPt, direction);
223         outMax = Dot(maxPt, direction);
224 }
225
226 vec Polyhedron::ApproximateConvexCentroid() const
227 {
228         // Since this shape is convex, the averaged position of all vertices is inside this polyhedron.
229         vec arbitraryCenterVertex = vec::zero;
230         for(int i = 0; i < NumVertices(); ++i)
231                 arbitraryCenterVertex += Vertex(i);
232         return arbitraryCenterVertex / (float)NumVertices();
233 }
234
235 vec Polyhedron::ConvexCentroid() const
236 {
237         if (v.size() <= 3)
238         {
239                 if (v.size() == 3)
240                         return (vec(v[0]) + vec(v[1]) + vec(v[2])) / 3.f;
241                 else if (v.size() == 2)
242                         return (vec(v[0]) + vec(v[1])) / 2.f;
243                 else if (v.size() == 1)
244                         return vec(v[0]);
245                 else
246                         return vec::nan;
247         }
248         // Since this shape is convex, the averaged position of all vertices is inside this polyhedron.
249         vec arbitraryCenterVertex = ApproximateConvexCentroid();
250
251         vec centroid = vec::zero;
252         float totalVolume = 0.f;
253         // Decompose the polyhedron to tetrahedrons and compute the mass of center of each, and the total
254         // mass of center of the polyhedron will be the weighted average of the tetrahedrons' mass of centers.
255         for(size_t i = 0; i < f.size(); ++i)
256         {
257                 const Face &fa = f[i];
258                 if (fa.v.size() < 3)
259                         continue;
260                 for(int v = 0; v < (int)fa.v.size()-2; ++v)
261                 {
262                         vec a = Vertex(fa.v[v]);
263                         vec b = Vertex(fa.v[v+1]);
264                         vec c = Vertex(fa.v[v+2]);
265                         vec center = (a + b + c + arbitraryCenterVertex) * 0.25f;
266                         float volume = Abs((a - arbitraryCenterVertex).Dot((b - arbitraryCenterVertex).Cross(c - arbitraryCenterVertex))); // This is actually volume*6, but can ignore the scale.
267                         totalVolume += volume;
268                         centroid += volume * center;
269                 }
270         }
271         return centroid / totalVolume;
272 }
273
274 float Polyhedron::SurfaceArea() const
275 {
276         float area = 0.f;
277         for(int i = 0; i < NumFaces(); ++i)
278                 area += FacePolygon(i).Area(); ///@todo Optimize temporary copies.
279         return area;
280 }
281
282 /** The implementation of this function is based on Graphics Gems 2, p. 170: "IV.1. Area of Planar Polygons and Volume of Polyhedra." */
283 float Polyhedron::Volume() const
284 {
285         float volume = 0.f;
286         for(int i = 0; i < NumFaces(); ++i)
287         {
288                 Polygon face = FacePolygon(i); ///@todo Optimize temporary copies.
289                 volume += face.Vertex(0).Dot(face.NormalCCW()) * face.Area();
290         }
291         return Abs(volume) / 3.f;
292 }
293
294 AABB Polyhedron::MinimalEnclosingAABB() const
295 {
296         AABB aabb;
297         aabb.SetNegativeInfinity();
298         for(int i = 0; i < NumVertices(); ++i)
299                 aabb.Enclose(Vertex(i));
300         return aabb;
301 }
302
303 #ifdef MATH_CONTAINERLIB_SUPPORT
304 OBB Polyhedron::MinimalEnclosingOBB() const
305 {
306         return OBB::OptimalEnclosingOBB((vec*)&v[0], (int)v.size());
307 }
308 #endif
309
310 bool Polyhedron::FaceIndicesValid() const
311 {
312         // Test condition 1: Face indices in proper range.
313         for(int i = 0; i < NumFaces(); ++i)
314                 for(int j = 0; j < (int)f[i].v.size(); ++j)
315                         if (f[i].v[j] < 0 || f[i].v[j] >= (int)v.size())
316                                 return false;
317
318         // Test condition 2: Each face has at least three vertices.
319         for(int i = 0; i < NumFaces(); ++i)
320                 if (f[i].v.size() < 3)
321                         return false;
322
323         // Test condition 3: Each face may refer to a vertex at most once. (Complexity O(n^2)).
324         for(int i = 0; i < NumFaces(); ++i)
325                 for(int j = 0; j < (int)f[i].v.size(); ++j)
326                         for(size_t k = j+1; k < f[i].v.size(); ++k)
327                                 if (f[i].v[j] == f[i].v[k])
328                                         return false;
329
330         return true;
331 }
332
333 void Polyhedron::FlipWindingOrder()
334 {
335         for(size_t i = 0; i < f.size(); ++i)
336                 f[i].FlipWindingOrder();
337 }
338
339 bool Polyhedron::IsClosed() const
340 {
341         std::set<std::pair<int, int> > uniqueEdges;
342         for(int i = 0; i < NumFaces(); ++i)
343         {
344                 assume1(FacePolygon(i).IsPlanar(), FacePolygon(i).SerializeToString());
345                 assume(FacePolygon(i).IsSimple());
346                 int x = f[i].v.back();
347                 for(size_t j = 0; j < f[i].v.size(); ++j)
348                 {
349                         int y = f[i].v[j];
350                         if (uniqueEdges.find(std::make_pair(x, y)) != uniqueEdges.end())
351                         {
352                                 LOGW("The edge (%d,%d) is used twice. Polyhedron is not simple and closed!", x, y);
353                                 return false; // This edge is being used twice! Cannot be simple and closed.
354                         }
355                         uniqueEdges.insert(std::make_pair(x, y));
356                         x = y;
357                 }
358         }
359
360         for(std::set<std::pair<int, int> >::iterator iter = uniqueEdges.begin();
361                 iter != uniqueEdges.end(); ++iter)
362         {
363                 std::pair<int, int> reverse = std::make_pair(iter->second, iter->first);
364                 if (uniqueEdges.find(reverse) == uniqueEdges.end())
365                 {
366                         LOGW("The edge (%d,%d) does not exist. Polyhedron is not closed!", iter->second, iter->first);
367                         return false;
368                 }
369         }
370
371         return true;
372 }
373
374 bool Polyhedron::IsConvex() const
375 {
376         // This function is O(n^2).
377         /** @todo Real-Time Collision Detection, p. 64:
378                 A faster O(n) approach is to compute for each face F of P the centroid C of F,
379                 and for all neighboring faces G of F test if C lies behind the supporting plane of
380                 G. If some C fails to lie behind the supporting plane of one or more neighboring
381                 faces, P is concave, and is otherwise assumed convex. However, note that just as the
382                 corresponding polygonal convexity test may fail for a pentagram this test may fail for,
383                 for example, a pentagram extruded out of its plane and capped at the ends. */
384
385         for(int f = 0; f < NumFaces(); ++f)
386         {
387                 Plane p = FacePlane(f);
388                 for(int i = 0; i < NumVertices(); ++i)
389                 {
390                         float d = p.SignedDistance(Vertex(i));
391                         if (d > 1e-3f) // Tolerate a small epsilon error.
392                         {
393 //                              LOGW("Distance of vertex %s/%d from plane %s/%d: %f", Vertex(i).ToString().c_str(), i, p.ToString().c_str(), f, d);
394                                 return false;
395                         }
396                 }
397         }
398         return true;
399 }
400
401 bool Polyhedron::EulerFormulaHolds() const
402 {
403         return NumVertices() + NumFaces() - NumEdges() == 2;
404 }
405
406 bool Polyhedron::FacesAreNondegeneratePlanar(float epsilon) const
407 {
408         for(int i = 0; i < (int)f.size(); ++i)
409         {
410                 const Face &face = f[i];
411                 if (face.v.size() < 3)
412                         return false;
413                 if (face.v.size() >= 4)
414                 {
415                         Plane facePlane = FacePlane(i);
416                         for(int j = 0; j < (int)face.v.size(); ++j)
417                                 if (facePlane.Distance(v[face.v[j]]) > epsilon)
418                                         return false;
419                 }
420         }
421
422         return true;
423 }
424
425 int Polyhedron::NearestVertex(const vec &point) const
426 {
427         int nearest = -1;
428         float nearestDistSq = FLOAT_INF;
429         for(int i = 0; i < (int)v.size(); ++i)
430         {
431                 float dSq = point.DistanceSq(v[i]);
432                 if (dSq < nearestDistSq)
433                 {
434                         nearestDistSq = dSq;
435                         nearest = i;
436                 }
437         }
438         return nearest;
439 }
440
441 float Polyhedron::FaceContainmentDistance2D(int faceIndex, const vec &worldSpacePoint, float polygonThickness) const
442 {
443         // N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation
444         // related to converting the face of a Polyhedron to a Polygon object.
445
446         // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
447
448         const Face &face = f[faceIndex];
449         const std::vector<int> &vertices = face.v;
450
451         if (vertices.size() < 3)
452                 return -FLOAT_INF; // Certainly not intersecting, so return -inf denoting "strongly not contained"
453
454         Plane p = FacePlane(faceIndex);
455         if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness)
456                 return -FLOAT_INF;
457
458         int numIntersections = 0;
459
460         vec basisU = (vec)v[vertices[1]] - (vec)v[vertices[0]];
461         basisU.Normalize();
462         vec basisV = Cross(p.normal, basisU).Normalized();
463         mathassert(basisU.IsNormalized());
464         mathassert(basisV.IsNormalized());
465         mathassert(basisU.IsPerpendicular(basisV));
466         mathassert(basisU.IsPerpendicular(p.normal));
467         mathassert(basisV.IsPerpendicular(p.normal));
468
469         // Tracks a pseudo-distance of the point to the ~nearest edge of the polygon. If the point is very close to the polygon
470         // edge, this is very small, and it's possible that due to numerical imprecision we cannot rely on the result in higher-level
471         // algorithms that invoke this function.
472         float faceContainmentDistance = FLOAT_INF;
473         const float epsilon = 1e-4f;
474
475         vec vt = vec(v[vertices.back()]) - worldSpacePoint;
476         float2 p0 = float2(Dot(vt, basisU), Dot(vt, basisV));
477         if (Abs(p0.y) < epsilon)
478                 p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
479
480         for(size_t i = 0; i < vertices.size(); ++i)
481         {
482                 vt = vec(v[vertices[i]]) - worldSpacePoint;
483                 float2 p1 = float2(Dot(vt, basisU), Dot(vt, basisV));
484                 if (Abs(p1.y) < epsilon)
485                         p1.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
486
487                 if (p0.y * p1.y < 0.f)
488                 {
489                         float minX = Min(p0.x, p1.x);
490                         if (minX > 0.f)
491                         {
492                                 faceContainmentDistance = Min(faceContainmentDistance, minX);
493                                 ++numIntersections;
494                         }
495                         else if (Max(p0.x, p1.x) > 0.f)
496                         {
497                                 // P = p0 + t*(p1-p0) == (x,0)
498                                 //     p0.x + t*(p1.x-p0.x) == x
499                                 //     p0.y + t*(p1.y-p0.y) == 0
500                                 //                 t == -p0.y / (p1.y - p0.y)
501
502                                 // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate.
503                                 float2 d = p1 - p0;
504                                 if (d.y != 0.f)
505                                 {
506                                         float t = -p0.y / d.y;
507                                         float x = p0.x + t * d.x;
508                                         if (t >= 0.f && t <= 1.f)
509                                         {
510                                                 // Remember how close the point was to the edge, for tracking robustness/goodness of the result.
511                                                 // If this is very large, then we can conclude that the point was contained or not contained in the face.
512                                                 faceContainmentDistance = Min(faceContainmentDistance, Abs(x));
513                                                 if (x >= 0.f)
514                                                         ++numIntersections;
515                                         }
516                                 }
517                         }
518                 }
519                 p0 = p1;
520         }
521
522         // Positive return value: face contains the point. Negative: face did not contain the point.
523         return (numIntersections % 2 == 1) ? faceContainmentDistance : -faceContainmentDistance;
524 }
525
526 bool Polyhedron::FaceContains(int faceIndex, const vec &worldSpacePoint, float polygonThickness) const
527 {
528         float faceContainmentDistance = FaceContainmentDistance2D(faceIndex, worldSpacePoint, polygonThickness);
529         return faceContainmentDistance >= 0.f;
530 }
531
532 bool Polyhedron::Contains(const vec &point) const
533 {
534         if (v.size() <= 3)
535         {
536                 if (v.size() == 3)
537                         return Triangle(vec(v[0]), vec(v[1]), vec(v[2])).Contains(point);
538                 else if (v.size() == 2)
539                         return LineSegment(vec(v[0]), vec(v[1])).Contains(point);
540                 else if (v.size() == 1)
541                         return vec(v[0]).Equals(point);
542                 else
543                         return false;
544         }
545         int bestNumIntersections = 0;
546         float bestFaceContainmentDistance = 0.f;
547
548         // General strategy: pick a ray from the query point to a random direction, and count the number of times the ray intersects
549         // a face. If it intersects an odd number of times, the given point must have been inside the polyhedron.
550         // But unfortunately for numerical stability, we must be smart with the choice of the ray direction. If we pick a ray direction
551         // which exits the polyhedron precisely at a vertex, or at an edge of two adjoining faces, we might count those twice. Therefore
552         // try to pick a ray direction that passes safely through a center of some face. If we detect that there was a tricky face that
553         // the ray passed too close to an edge, we have no choice but to pick another ray direction and hope that it passes through
554         // the polyhedron in a safer manner.
555
556         // Loop through each face to choose the ray direction. If our choice was good, we only do this once and the algorithm can exit
557         // after the first iteration at j == 0. If not, we iterate more faces of the polyhedron to try to find one that is safe for
558         // ray-polyhedron examination.
559         for(int j = 0; j < (int)f.size(); ++j)
560         {
561                 if (f[j].v.size() < 3)
562                         continue;
563
564                 // Accumulate how many times the ray intersected a face of the polyhedron.
565                 int numIntersections = 0;
566                 // Track a pseudo-distance of the closest edge of a face that the ray passed through. If this distance ends up being too
567                 // small, we decide to not trust the result we got, and proceed to another iteration of j, hoping to guess a better-behaving
568                 // direction for the test ray.
569                 float faceContainmentDistance = FLOAT_INF;
570
571                 vec dir = ((vec)v[f[j].v[0]] + (vec)v[f[j].v[1]] + (vec)v[f[j].v[2]]) * 0.33333333333f - point;
572 #ifdef MATH_VEC_IS_FLOAT4
573                 dir.w = 0.f;
574 #endif
575                 if (dir.Normalize() <= 0.f)
576                         continue;
577                 Ray r(POINT_VEC_SCALAR(0.f), dir);
578
579                 for(int i = 0; i < (int)f.size(); ++i)
580                 {
581                         Plane p((vec)v[f[i].v[0]] - point, (vec)v[f[i].v[1]] - point, (vec)v[f[i].v[2]] - point);
582
583                         float d;
584                         // Find the intersection of the plane and the ray.
585                         if (p.Intersects(r, &d))
586                         {
587                                 float containmentDistance2D = FaceContainmentDistance2D(i, r.GetPoint(d) + point);
588                                 if (containmentDistance2D >= 0.f)
589                                         ++numIntersections;
590                                 faceContainmentDistance = Min(faceContainmentDistance, Abs(containmentDistance2D));
591                         }
592                 }
593                 if (faceContainmentDistance > 1e-2f) // The nearest edge was far enough, we conclude the result is believable.
594                         return (numIntersections % 2) == 1;
595                 else if (faceContainmentDistance >= bestFaceContainmentDistance)
596                 {
597                         // The ray passed too close to a face edge. Remember this result, but proceed to another test iteration to see if we can
598                         // find a more plausible test ray.
599                         bestNumIntersections = numIntersections;
600                         bestFaceContainmentDistance = faceContainmentDistance;
601                 }
602         }
603         // We tested rays through each face of the polyhedron, but all rays passed too close to edges of the polyhedron faces. Return
604         // the result from the test that was farthest to any of the face edges.
605         return (bestNumIntersections % 2) == 1;
606 }
607
608 bool Polyhedron::Contains(const LineSegment &lineSegment) const
609 {
610         return Contains(lineSegment.a) && Contains(lineSegment.b);
611 }
612
613 bool Polyhedron::Contains(const Triangle &triangle) const
614 {
615         return Contains(triangle.a) && Contains(triangle.b) && Contains(triangle.c);
616 }
617
618 bool Polyhedron::Contains(const Polygon &polygon) const
619 {
620         for(int i = 0; i < polygon.NumVertices(); ++i)
621                 if (!Contains(polygon.Vertex(i)))
622                         return false;
623         return true;
624 }
625
626 bool Polyhedron::Contains(const AABB &aabb) const
627 {
628         for(int i = 0; i < 8; ++i)
629                 if (!Contains(aabb.CornerPoint(i)))
630                         return false;
631
632         return true;
633 }
634
635 bool Polyhedron::Contains(const OBB &obb) const
636 {
637         for(int i = 0; i < 8; ++i)
638                 if (!Contains(obb.CornerPoint(i)))
639                         return false;
640
641         return true;
642 }
643
644 bool Polyhedron::Contains(const Frustum &frustum) const
645 {
646         for(int i = 0; i < 8; ++i)
647                 if (!Contains(frustum.CornerPoint(i)))
648                         return false;
649
650         return true;
651 }
652
653 bool Polyhedron::Contains(const Polyhedron &polyhedron) const
654 {
655         assume(polyhedron.IsClosed());
656         for(int i = 0; i < polyhedron.NumVertices(); ++i)
657                 if (!Contains(polyhedron.Vertex(i)))
658                         return false;
659
660         return true;
661 }
662
663 bool Polyhedron::ContainsConvex(const vec &point, float epsilon) const
664 {
665         assume(IsConvex());
666         for(int i = 0; i < NumFaces(); ++i)
667                 if (FacePlane(i).SignedDistance(point) > epsilon)
668                         return false;
669
670         return true;
671 }
672
673 bool Polyhedron::ContainsConvex(const LineSegment &lineSegment) const
674 {
675         return ContainsConvex(lineSegment.a) && ContainsConvex(lineSegment.b);
676 }
677
678 bool Polyhedron::ContainsConvex(const Triangle &triangle) const
679 {
680         return ContainsConvex(triangle.a) && ContainsConvex(triangle.b) && ContainsConvex(triangle.c);
681 }
682
683 vec Polyhedron::ClosestPointConvex(const vec &point) const
684 {
685         assume(IsConvex());
686         if (ContainsConvex(point))
687                 return point;
688         vec closestPoint = vec::nan;
689         float closestDistance = FLT_MAX;
690         for(int i = 0; i < NumFaces(); ++i)
691         {
692                 vec closestOnPoly = FacePolygon(i).ClosestPoint(point);
693                 float d = closestOnPoly.DistanceSq(point);
694                 if (d < closestDistance)
695                 {
696                         closestPoint = closestOnPoly;
697                         closestDistance = d;
698                 }
699         }
700         return closestPoint;
701 }
702
703 vec Polyhedron::ClosestPoint(const vec &point) const
704 {
705         if (Contains(point))
706                 return point;
707         vec closestPoint = vec::nan;
708         float closestDistance = FLT_MAX;
709         for(int i = 0; i < NumFaces(); ++i)
710         {
711                 vec closestOnPoly = FacePolygon(i).ClosestPoint(point);
712                 float d = closestOnPoly.DistanceSq(point);
713                 if (d < closestDistance)
714                 {
715                         closestPoint = closestOnPoly;
716                         closestDistance = d;
717                 }
718         }
719         return closestPoint;
720 }
721
722 vec Polyhedron::ClosestPoint(const LineSegment &lineSegment) const
723 {
724         return ClosestPoint(lineSegment, 0);
725 }
726
727 vec Polyhedron::ClosestPoint(const LineSegment &lineSegment, vec *lineSegmentPt) const
728 {
729         if (Contains(lineSegment.a))
730         {
731                 if (lineSegmentPt)
732                         *lineSegmentPt = lineSegment.a;
733                 return lineSegment.a;
734         }
735         if (Contains(lineSegment.b))
736         {
737                 if (lineSegmentPt)
738                         *lineSegmentPt = lineSegment.b;
739                 return lineSegment.b;
740         }
741         vec closestPt = vec::nan;
742         float closestDistance = FLT_MAX;
743         vec closestLineSegmentPt = vec::nan;
744         for(int i = 0; i < NumFaces(); ++i)
745         {
746                 vec lineSegPt;
747                 vec pt = FacePolygon(i).ClosestPoint(lineSegment, &lineSegPt);
748                 float d = pt.DistanceSq(lineSegPt);
749                 if (d < closestDistance)
750                 {
751                         closestDistance = d;
752                         closestPt = pt;
753                         closestLineSegmentPt = lineSegPt;
754                 }
755         }
756         if (lineSegmentPt)
757                 *lineSegmentPt = closestLineSegmentPt;
758         return closestPt;
759 }
760
761 float Polyhedron::Distance(const vec &point) const
762 {
763         vec pt = ClosestPoint(point);
764         return pt.Distance(point);
765 }
766
767 bool Polyhedron::ClipLineSegmentToConvexPolyhedron(const vec &ptA, const vec &dir,
768                                                    float &tFirst, float &tLast) const
769 {
770         assume(IsConvex());
771
772         // Intersect line segment against each plane.
773         for(int i = 0; i < NumFaces(); ++i)
774         {
775                 /* Denoting the dot product of vectors a and b with <a,b>, we have:
776
777                    The points P on the plane p satisfy the equation <P, p.normal> == p.d.
778                    The points P on the line have the parametric equation P = ptA + dir * t.
779                    Solving for the distance along the line for intersection gives
780
781                    t = (p.d - <p.normal, ptA>) / <p.normal, dir>.
782                 */
783
784                 Plane p = FacePlane(i);
785                 float denom = Dot(p.normal, dir);
786                 float dist = p.d - Dot(p.normal, ptA);
787
788                 // Avoid division by zero. In this case the line segment runs parallel to the plane.
789                 if (Abs(denom) < 1e-5f)
790                 {
791                         // If <P, p.normal> < p.d, then the point lies in the negative halfspace of the plane, which is inside the polyhedron.
792                         // If <P, p.normal> > p.d, then the point lies in the positive halfspace of the plane, which is outside the polyhedron.
793                         // Therefore, if p.d - <ptA, p.normal> == dist < 0, then the whole line is outside the polyhedron.
794                         if (dist < 0.f)
795                                 return false;
796                 }
797                 else
798                 {
799                         float t = dist / denom;
800                         if (denom < 0.f) // When entering halfspace, update tFirst if t is larger.
801                                 tFirst = Max(t, tFirst);
802                         else // When exiting halfspace, updeate tLast if t is smaller.
803                                 tLast = Min(t, tLast);
804
805                         if (tFirst > tLast)
806                                 return false; // We clipped the whole line segment.
807                 }
808         }
809         return true;
810 }
811
812 bool Polyhedron::Intersects(const LineSegment &lineSegment) const
813 {
814         if (Contains(lineSegment))
815                 return true;
816         for(int i = 0; i < NumFaces(); ++i)
817         {
818                 float t;
819                 Plane plane = FacePlane(i);
820                 bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
821                 if (intersects && t >= 0.f && t <= 1.f)
822                         if (FaceContains(i, lineSegment.GetPoint(t)))
823                                 return true;
824         }
825
826         return false;
827 }
828
829 bool Polyhedron::Intersects(const Line &line) const
830 {
831         for(int i = 0; i < NumFaces(); ++i)
832                 if (FacePolygon(i).Intersects(line))
833                         return true;
834
835         return false;
836 }
837
838 bool Polyhedron::Intersects(const Ray &ray) const
839 {
840         for(int i = 0; i < NumFaces(); ++i)
841                 if (FacePolygon(i).Intersects(ray))
842                         return true;
843
844         return false;
845 }
846
847 bool Polyhedron::Intersects(const Plane &plane) const
848 {
849         return plane.Intersects(*this);
850 }
851
852 /** The algorithm for Polyhedron-Polyhedron intersection is from Christer Ericson's Real-Time Collision Detection, p. 384.
853         As noted by the author, the algorithm is very naive (and here unoptimized), and better methods exist. [groupSyntax] */
854 bool Polyhedron::Intersects(const Polyhedron &polyhedron) const
855 {
856         vec c = this->ApproximateConvexCentroid();
857         if (polyhedron.Contains(c) && this->Contains(c))
858                 return true;
859         c = polyhedron.ApproximateConvexCentroid();
860         if (polyhedron.Contains(c) && this->Contains(c))
861                 return true;
862
863         // This test assumes that both this and the other polyhedron are closed.
864         // This means that for each edge running through vertices i and j, there's a face
865         // that contains the line segment (i,j) and another neighboring face that contains
866         // the line segment (j,i). These represent the same line segment (but in opposite direction)
867         // so we only have to test one of them for intersection. Take i < j as the canonical choice
868         // and skip the other winding order.
869
870         // Test for each edge of this polyhedron whether the other polyhedron intersects it.
871         for(size_t i = 0; i < f.size(); ++i)
872         {
873                 assert(!f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
874                 int v0 = f[i].v.back();
875                 vec l0 = v[v0];
876                 for(size_t j = 0; j < f[i].v.size(); ++j)
877                 {
878                         int v1 = f[i].v[j];
879                         vec l1 = v[v1];
880                         if (v0 < v1 && polyhedron.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
881                                 return true;
882                         l0 = l1;
883                         v0 = v1;
884                 }
885         }
886
887         // Test for each edge of the other polyhedron whether this polyhedron intersects it.
888         for(size_t i = 0; i < polyhedron.f.size(); ++i)
889         {
890                 assert(!polyhedron.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
891                 int v0 = polyhedron.f[i].v.back();
892                 vec l0 = polyhedron.v[v0];
893                 for(size_t j = 0; j < polyhedron.f[i].v.size(); ++j)
894                 {
895                         int v1 = polyhedron.f[i].v[j];
896                         vec l1 = polyhedron.v[v1];
897                         if (v0 < v1 && Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
898                                 return true;
899                         l0 = l1;
900                         v0 = v1;
901                 }
902         }
903
904         return false;
905 }
906
907 template<typename T>
908 bool PolyhedronIntersectsAABB_OBB(const Polyhedron &p, const T &obj)
909 {
910         if (p.Contains(obj.CenterPoint()))
911                 return true;
912         if (obj.Contains(p.ApproximateConvexCentroid())) // @bug: This is not correct for concave polyhedrons!
913                 return true;
914
915         // Test for each edge of the AABB/OBB whether this polyhedron intersects it.
916         for(int i = 0; i < obj.NumEdges(); ++i)
917                 if (p.Intersects(obj.Edge(i)))
918                         return true;
919
920         // Test for each edge of this polyhedron whether the AABB/OBB intersects it.
921         for(size_t i = 0; i < p.f.size(); ++i)
922         {
923                 assert(!p.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
924                 int v0 = p.f[i].v.back();
925                 vec l0 = p.v[v0];
926                 for(size_t j = 0; j < p.f[i].v.size(); ++j)
927                 {
928                         int v1 = p.f[i].v[j];
929                         vec l1 = p.v[v1];
930                         if (v0 < v1 && obj.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
931                                 return true;
932                         l0 = l1;
933                         v0 = v1;
934                 }
935         }
936
937         return false;
938 }
939
940 bool Polyhedron::Intersects(const AABB &aabb) const
941 {
942         return PolyhedronIntersectsAABB_OBB(*this, aabb);
943 }
944
945 bool Polyhedron::Intersects(const OBB &obb) const
946 {
947         return PolyhedronIntersectsAABB_OBB(*this, obb);
948 }
949
950 bool Polyhedron::Intersects(const Triangle &triangle) const
951 {
952         return PolyhedronIntersectsAABB_OBB(*this, triangle);
953 }
954
955 bool Polyhedron::Intersects(const Polygon &polygon) const
956 {
957         return Intersects(polygon.ToPolyhedron());
958 }
959
960 bool Polyhedron::Intersects(const Frustum &frustum) const
961 {
962         return PolyhedronIntersectsAABB_OBB(*this, frustum);
963 }
964
965 bool Polyhedron::Intersects(const Sphere &sphere) const
966 {
967         vec closestPt = ClosestPoint(sphere.pos);
968         return closestPt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
969 }
970
971 bool Polyhedron::Intersects(const Capsule &capsule) const
972 {
973         vec pt, ptOnLineSegment;
974         pt = ClosestPoint(capsule.l, &ptOnLineSegment);
975         return pt.DistanceSq(ptOnLineSegment) <= capsule.r * capsule.r;
976 }
977
978 bool Polyhedron::IntersectsConvex(const Line &line) const
979 {
980         float tFirst = -FLT_MAX;
981         float tLast = FLT_MAX;
982         return ClipLineSegmentToConvexPolyhedron(line.pos, line.dir, tFirst, tLast);
983 }
984
985 bool Polyhedron::IntersectsConvex(const Ray &ray) const
986 {
987         float tFirst = 0.f;
988         float tLast = FLT_MAX;
989         return ClipLineSegmentToConvexPolyhedron(ray.pos, ray.dir, tFirst, tLast);
990 }
991
992 bool Polyhedron::IntersectsConvex(const LineSegment &lineSegment) const
993 {
994         float tFirst = 0.f;
995         float tLast = 1.f;
996         return ClipLineSegmentToConvexPolyhedron(lineSegment.a, lineSegment.b - lineSegment.a, tFirst, tLast);
997 }
998
999 #if 0
1000 void Polyhedron::MergeConvex(const vec &point)
1001 {
1002 //      LOGI("mergeconvex.");
1003         std::set<std::pair<int, int> > deletedEdges;
1004         std::map<std::pair<int, int>, int> remainingEdges;
1005
1006         for(size_t i = 0; i < v.size(); ++i)
1007                 if (point.DistanceSq(v[i]) < 1e-3f)
1008                         return;
1009
1010 //      bool hadDisconnectedHorizon = false;
1011
1012         for(int i = 0; i < (int)f.size(); ++i)
1013         {
1014                 // Delete all faces that don't contain the given point. (they have point in their positive side)
1015                 Plane p = FacePlane(i);
1016                 Face &face = f[i];
1017                 if (p.SignedDistance(point) > 1e-5f)
1018                 {
1019                         bool isConnected = (deletedEdges.empty());
1020
1021                         int v0 = face.v.back();
1022                         for(size_t j = 0; j < face.v.size() && !isConnected; ++j)
1023                         {
1024                                 int v1 = face.v[j];
1025                                 if (deletedEdges.find(std::make_pair(v1, v0)) != deletedEdges.end())
1026                                 {
1027                                         isConnected = true;
1028                                         break;
1029                                 }
1030                                 v0 = v1;
1031                         }
1032
1033                         if (isConnected)
1034                         {
1035                                 v0 = face.v.back();
1036                                 for(size_t j = 0; j < face.v.size(); ++j)
1037                                 {
1038                                         int v1 = face.v[j];
1039                                         deletedEdges.insert(std::make_pair(v0, v1));
1040                         //              LOGI("Edge %d,%d is to be deleted.", v0, v1);
1041                                         v0 = v1;
1042                                 }
1043                 //              LOGI("Deleting face %d: %s. Distance to vertex %f", i, face.ToString().c_str(), p.SignedDistance(point));
1044                                 std::swap(f[i], f.back());
1045                                 f.pop_back();
1046                                 --i;
1047                                 continue;
1048                         }
1049 //                      else
1050 //                              hadDisconnectedHorizon = true;
1051                 }
1052
1053                 int v0 = face.v.back();
1054                 for(size_t j = 0; j < face.v.size(); ++j)
1055                 {
1056                         int v1 = face.v[j];
1057                         remainingEdges[std::make_pair(v0, v1)] = i;
1058         //              LOGI("Edge %d,%d is to be deleted.", v0, v1);
1059                         v0 = v1;
1060                 }
1061
1062         }
1063
1064         // The polyhedron contained our point, nothing to merge.
1065         if (deletedEdges.empty())
1066                 return;
1067
1068         // Add the new point to this polyhedron.
1069 //      if (!v.back().Equals(point))
1070                 v.push_back(point);
1071
1072 /*
1073         // Create a look-up index of all remaining uncapped edges of the polyhedron.
1074         std::map<std::pair<int,int>, int> edgesToFaces;
1075         for(size_t i = 0; i < f.size(); ++i)
1076         {
1077                 Face &face = f[i];
1078                 int v0 = face.v.back();
1079                 for(size_t j = 0; j < face.v.size(); ++j)
1080                 {
1081                         int v1 = face.v[j];
1082                         edgesToFaces[std::make_pair(v1, v0)] = i;
1083                         v0 = v1;
1084                 }
1085         }
1086 */
1087         // Now fix all edges by adding new triangular faces for the point.
1088 //      for(size_t i = 0; i < deletedEdges.size(); ++i)
1089         for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
1090         {
1091                 std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
1092                 if (deletedEdges.find(opposite) != deletedEdges.end())
1093                         continue;
1094
1095 //              std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
1096 //              std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
1097 //              if (iter != edgesToFaces.end())
1098                 {
1099                         // If the adjoining face is planar to the triangle we'd like to add, instead extend the face to enclose
1100                         // this vertex.
1101                         //vec newTriangleNormal = (v[v.size()-1]-v[iter->second]).Cross(v[iter->first]-v[iter->second]).Normalized();
1102
1103                         std::map<std::pair<int, int>, int>::iterator existing = remainingEdges.find(opposite);
1104                         assert(existing != remainingEdges.end());
1105                         MARK_UNUSED(existing);
1106
1107 #if 0
1108                         int adjoiningFace = existing->second;
1109
1110                         if (FaceNormal(adjoiningFace).Dot(newTriangleNormal) >= 0.99999f) ///\todo vec::IsCollinear
1111                         {
1112                                 bool added = false;
1113                                 Face &adjoining = f[adjoiningFace];
1114                                 for(size_t i = 0; i < adjoining.v.size(); ++i)
1115                                         if (adjoining.v[i] == iter->second)
1116                                         {
1117                                                 adjoining.v.insert(adjoining.v.begin() + i + 1, v.size()-1);
1118                                                 added = true;
1119                                                 /*
1120                                                 int prev2 = (i + adjoining.v.size() - 1) % adjoining.v.size();
1121                                                 int prev = i;
1122                                                 int cur = i + 1;
1123                                                 int next = (i + 2) % adjoining.v.size();
1124                                                 int next2 = (i + 3) % adjoining.v.size();
1125
1126                                                 if (vec::AreCollinear(v[prev2], v[prev], v[cur]))
1127                                                         adjoining.v.erase(adjoining.v.begin() + prev);
1128                                                 else if (vec::AreCollinear(v[prev], v[cur], v[next]))
1129                                                         adjoining.v.erase(adjoining.v.begin() + cur);
1130                                                 else if (vec::AreCollinear(v[cur], v[next], v[next2]))
1131                                                         adjoining.v.erase(adjoining.v.begin() + next2);
1132                                                         */
1133
1134                                                 break;
1135                                         }
1136                                 assert(added);
1137                                 assume(added);
1138                         }
1139                         else
1140 #endif
1141 //                      if (!v[deletedEdges[i].first].Equals(point) && !v[deletedEdges[i].second].Equals(point))
1142                         {
1143                                 Face tri;
1144                                 tri.v.push_back(iter->first);
1145                                 tri.v.push_back(iter->second);
1146                                 tri.v.push_back((int)v.size()-1);
1147                                 f.push_back(tri);
1148                                 LOGI("Added face %d: %s.", (int)f.size()-1, tri.ToString().c_str());
1149                         }
1150                 }
1151         }
1152
1153 #define mathasserteq(lhs, op, rhs) do { if (!((lhs) op (rhs))) { LOGE("Condition %s %s %s (%g %s %g) failed!", #lhs, #op, #rhs, (double)(lhs), #op, (double)(rhs)); assert(false); } } while(0)
1154
1155 //      mathasserteq(NumVertices() + NumFaces(), ==, 2 + NumEdges());
1156         assert(FaceIndicesValid());
1157 //      assert(EulerFormulaHolds());
1158 //      assert(IsClosed());
1159 //      assert(FacesAreNondegeneratePlanar());
1160 //      assert(IsConvex());
1161
1162 //      if (hadDisconnectedHorizon)
1163 //              MergeConvex(point);
1164 }
1165 #endif
1166
1167 void Polyhedron::MergeConvex(const vec &point)
1168 {
1169 //      assert(IsClosed());
1170 //      assert(IsConvex());
1171
1172         std::set<std::pair<int, int> > deletedEdges;
1173
1174         int nFacesAlive = 0;
1175         for(int i = 0; i < (int)f.size(); ++i)
1176         {
1177                 // Delete all faces that don't contain the given point. (they have point in their positive side)
1178                 Plane p = FacePlane(i);
1179                 Face &face = f[i];
1180
1181                 if (p.SignedDistance(point) <= 0.f)
1182                 {
1183                         if (i != nFacesAlive)
1184                                 f[nFacesAlive] = f[i];
1185                         ++nFacesAlive;
1186                         continue;
1187                 }
1188
1189                 int v0 = face.v.back();
1190                 for(size_t j = 0; j < face.v.size(); ++j)
1191                 {
1192                         deletedEdges.insert(std::make_pair(v0, face.v[j]));
1193                         v0 = face.v[j];
1194                 }
1195         }
1196         f.erase(f.begin()+nFacesAlive, f.end());
1197
1198         // If the polyhedron contained our point, then there is nothing to merge.
1199         if (deletedEdges.empty())
1200                 return;
1201
1202         // Add the new point to this polyhedron.
1203         v.push_back(point);
1204
1205         // Now fix all edges by adding new triangular faces for the point.
1206         for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
1207         {
1208                 std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
1209                 if (deletedEdges.find(opposite) != deletedEdges.end())
1210                         continue;
1211
1212                 Face tri;
1213                 tri.v.push_back(iter->first);
1214                 tri.v.push_back(iter->second);
1215                 tri.v.push_back((int)v.size()-1);
1216                 f.push_back(tri);
1217         }
1218
1219 //      assert(FaceIndicesValid());
1220 //      assert(EulerFormulaHolds());
1221 //      assert(IsClosed());
1222 //      assert(FacesAreNondegeneratePlanar());
1223 //      assert(IsConvex());
1224 }
1225
1226 void Polyhedron::Translate(const vec &offset)
1227 {
1228         for(size_t i = 0; i < v.size(); ++i)
1229                 v[i] = (vec)v[i] + offset;
1230 }
1231
1232 void Polyhedron::Transform(const float3x3 &transform)
1233 {
1234         if (!v.empty())
1235                 transform.BatchTransform((vec*)&v[0], (int)v.size());
1236 }
1237
1238 void Polyhedron::Transform(const float3x4 &transform)
1239 {
1240         if (!v.empty())
1241                 transform.BatchTransformPos((vec*)&v[0], (int)v.size());
1242 }
1243
1244 void Polyhedron::Transform(const float4x4 &transform)
1245 {
1246         for(size_t i = 0; i < v.size(); ++i)
1247                 v[i] = transform.MulPos(v[i]); ///\todo Add float4x4::BatchTransformPos.
1248 }
1249
1250 void Polyhedron::Transform(const Quat &transform)
1251 {
1252         for(size_t i = 0; i < v.size(); ++i)
1253                 v[i] = transform * v[i];
1254 }
1255
1256 void Polyhedron::OrientNormalsOutsideConvex()
1257 {
1258         vec center = v[0];
1259         for(size_t i = 1; i < v.size(); ++i)
1260                 center += v[i];
1261
1262         center /= (float)v.size();
1263         for(int i = 0; i < (int)f.size(); ++i)
1264                 if (FacePlane(i).SignedDistance(center) > 0.f)
1265                         f[i].FlipWindingOrder();
1266 }
1267
1268 /// Edge from v1->v2.
1269 struct AdjEdge
1270 {
1271 //      int v1;
1272 //      int v2;
1273         int f1; // The face that has v1->v2.
1274         int f2; // The face that has v2->v1.
1275 };
1276
1277 #include <list>
1278
1279 struct CHullHelp
1280 {
1281         std::map<std::pair<int,int>, AdjEdge> edges;
1282         std::list<int> livePlanes;
1283 };
1284
1285 Polyhedron Polyhedron::ConvexHull(const vec *pointArray, int numPoints)
1286 {
1287         std::set<int> extremes;
1288
1289         Polyhedron p;
1290
1291         const vec dirs[] =
1292         {
1293                 DIR_VEC(1, 0, 0), DIR_VEC(0, 1, 0), DIR_VEC(0, 0, 1),
1294                 DIR_VEC(1, 1, 0), DIR_VEC(1, 0, 1), DIR_VEC(0, 1, 1),
1295                 DIR_VEC(1, -1, 0), DIR_VEC(1, 0, -1), DIR_VEC(0, 1, -1),
1296                 DIR_VEC(1, 1, 1), DIR_VEC(-1, 1, 1), DIR_VEC(1, -1, 1),
1297                 DIR_VEC(1, 1, -1)
1298         };
1299
1300         for(size_t i = 0; i < ARRAY_LENGTH(dirs); ++i)
1301         {
1302                 int idx1, idx2;
1303                 OBB::ExtremePointsAlongDirection(dirs[i], pointArray, numPoints, idx1, idx2);
1304                 extremes.insert(idx1);
1305                 extremes.insert(idx2);
1306         }
1307
1308         assume(extremes.size() >= 3);
1309         if (extremes.size() < 3)
1310                 return p; // This might happen if there's NaNs in the vertex data, or duplicates.
1311
1312         // Handle degenerate case when the predefined directions did not find a nonzero volume.
1313         if (extremes.size() == 3)
1314         {
1315                 std::set<int>::iterator iter = extremes.begin();
1316                 int v0 = *iter++;
1317                 int v1 = *iter++;
1318                 int v2 = *iter;
1319                 Plane p(pointArray[v0], pointArray[v1], pointArray[v2]);
1320                 for(int i = 0; i < numPoints; ++i)
1321                 {
1322                         if (!p.Contains(pointArray[i]))
1323                                 extremes.insert(i);
1324                         if (extremes.size() >= 4)
1325                                 break;
1326                 }
1327         }
1328
1329         if (extremes.size() == 3)
1330         {
1331                 Face f;
1332                 for(int i = 0; i < numPoints; ++i)
1333                 {
1334                         p.v.push_back(pointArray[i]);
1335                         f.v.push_back(i);
1336                 }
1337                 if (numPoints > 0)
1338                         p.f.push_back(f);
1339                 return p;
1340         }
1341
1342         int i = 0;
1343         std::set<int>::iterator iter = extremes.begin();
1344         for(; iter != extremes.end() && i < 4; ++iter, ++i)
1345                 p.v.push_back(pointArray[*iter]);
1346
1347         Face face;
1348         face.v.resize(3);
1349         face.v[0] = 0; face.v[1] = 1; face.v[2] = 2; p.f.push_back(face);
1350         face.v[0] = 0; face.v[1] = 1; face.v[2] = 3; p.f.push_back(face);
1351         face.v[0] = 0; face.v[1] = 2; face.v[2] = 3; p.f.push_back(face);
1352         face.v[0] = 1; face.v[1] = 2; face.v[2] = 3; p.f.push_back(face);
1353         p.OrientNormalsOutsideConvex(); // Ensure that the winding order of the generated tetrahedron is correct for each face.
1354
1355         assert(p.IsClosed());
1356         assert(p.IsConvex());
1357         assert(p.FaceIndicesValid());
1358         assert(p.EulerFormulaHolds());
1359 //      assert(p.FacesAreNondegeneratePlanar());
1360
1361         // For better performance, merge the remaining extreme points first.
1362         for(; iter != extremes.end(); ++iter)
1363         {
1364                 p.MergeConvex(pointArray[*iter]);
1365
1366 //              assert(p.FaceIndicesValid());
1367 //              assert(p.IsClosed());
1368 //              assert(p.FacesAreNondegeneratePlanar());
1369 //              assert(p.IsConvex());
1370         }
1371
1372         // Merge all the rest of the points.
1373         for(int j = 0; j < numPoints; ++j)
1374         {
1375                 if (extremes.find(j) != extremes.end())
1376                         continue; // The extreme points have already been merged.
1377                 p.MergeConvex(pointArray[j]);
1378
1379 //              assert(p.FaceIndicesValid());
1380 //              assert(p.IsClosed());
1381 //              mathassert(p.FacesAreNondegeneratePlanar());
1382 //              assert(p.IsConvex());
1383
1384 //              if (p.f.size() > 5000)
1385 //                      break;
1386         }
1387
1388         assert(p.FaceIndicesValid());
1389         assert(p.IsClosed());
1390         assert(p.IsConvex());
1391         p.RemoveRedundantVertices();
1392         return p;
1393 }
1394
1395 /// See http://paulbourke.net/geometry/platonic/
1396 Polyhedron Polyhedron::Tetrahedron(const vec &centerPos, float scale, bool ccwIsFrontFacing)
1397 {
1398         const vec vertices[4] = { DIR_VEC(1, 1, 1),
1399                 DIR_VEC(-1, 1, -1),
1400                 DIR_VEC(1, -1, -1),
1401                 DIR_VEC(-1, -1, 1) };
1402         const int faces[4][3] = { { 0, 1, 2 },
1403                                   { 1, 3, 2 },
1404                                   { 0, 2, 3 },
1405                                   { 0, 3, 1 } };
1406
1407         scale /= 2.f;
1408         Polyhedron p;
1409
1410         for(int i = 0; i < 4; ++i)
1411                 p.v.push_back(vertices[i]*scale + centerPos);
1412
1413         for(int i = 0; i < 4; ++i)
1414         {
1415                 Face f;
1416                 for(int j = 0; j < 3; ++j)
1417                         f.v.push_back(faces[i][j]);
1418                 p.f.push_back(f);
1419         }
1420
1421         assume(p.Contains(centerPos));
1422
1423         if (!ccwIsFrontFacing)
1424                 p.FlipWindingOrder();
1425
1426         assume(p.Contains(centerPos));
1427
1428         return p;
1429 }
1430
1431 /// See http://paulbourke.net/geometry/platonic/
1432 Polyhedron Polyhedron::Octahedron(const vec &centerPos, float scale, bool ccwIsFrontFacing)
1433 {
1434         float a = 1.f / (2.f * Sqrt(2.f));
1435         float b = 0.5f;
1436
1437         const vec vertices[6] = { DIR_VEC(-a, 0, a),
1438                 DIR_VEC(-a, 0, -a),
1439                 DIR_VEC(0, b, 0),
1440                 DIR_VEC(a, 0, -a),
1441                 DIR_VEC(0, -b, 0),
1442                 DIR_VEC(a, 0, a) };
1443         const int faces[8][3] = { { 0, 1, 2 },
1444                                   { 1, 3, 2 },
1445                                   { 3, 5, 2 },
1446                                   { 5, 0, 2 },
1447                                   { 3, 1, 4 },
1448                                   { 1, 0, 4 },
1449                                   { 5, 3, 4 },
1450                                   { 0, 5, 4 } };
1451
1452         scale /= 2.f;
1453         Polyhedron p;
1454
1455         for(int i = 0; i < 6; ++i)
1456                 p.v.push_back(vertices[i]*scale + centerPos);
1457
1458         for(int i = 0; i < 8; ++i)
1459         {
1460                 Face f;
1461                 for(int j = 0; j < 3; ++j)
1462                         f.v.push_back(faces[i][j]);
1463                 p.f.push_back(f);
1464         }
1465
1466         assume(p.Contains(centerPos));
1467
1468         if (!ccwIsFrontFacing)
1469                 p.FlipWindingOrder();
1470
1471         assume(p.Contains(centerPos));
1472
1473         return p;
1474 }
1475
1476 /// See http://paulbourke.net/geometry/platonic/
1477 Polyhedron Polyhedron::Hexahedron(const vec &centerPos, float scale, bool ccwIsFrontFacing)
1478 {
1479         AABB aabb(DIR_VEC(-1, -1, -1), DIR_VEC(1, 1, 1));
1480         aabb.Scale(DIR_VEC_SCALAR(0.f), scale * 0.5f);
1481         aabb.Translate(centerPos);
1482         Polyhedron p = aabb.ToPolyhedron();
1483
1484         assume(p.Contains(centerPos));
1485
1486         if (ccwIsFrontFacing)
1487                 p.FlipWindingOrder();
1488
1489         assume(p.Contains(centerPos));
1490
1491         return p;
1492 }
1493
1494 /// See http://paulbourke.net/geometry/platonic/
1495 Polyhedron Polyhedron::Icosahedron(const vec &centerPos, float scale, bool ccwIsFrontFacing)
1496 {
1497         float a = 0.5f;
1498         float phi = (1.f + Sqrt(5.f)) * 0.5f;
1499         float b = 1.f / (2.f * phi);
1500
1501         const vec vertices[12] = { DIR_VEC( 0,  b, -a),
1502                 DIR_VEC(b, a, 0),
1503                 DIR_VEC(-b, a, 0),
1504                 DIR_VEC(0, b, a),
1505                 DIR_VEC(0, -b, a),
1506                 DIR_VEC(-a, 0, b),
1507                 DIR_VEC(a, 0, b),
1508                 DIR_VEC(0, -b, -a),
1509                 DIR_VEC(-a, 0, -b),
1510                 DIR_VEC(-b, -a, 0),
1511                 DIR_VEC(b, -a, 0),
1512                 DIR_VEC(a, 0, -b) };
1513         const int faces[20][3] = { { 0,  1,  2 },
1514                                    { 3,  2,  1 },
1515                                    { 3,  4,  5 },
1516                                    { 3,  6,  4 },
1517                                    { 0,  7, 11 },
1518                                    { 0,  8,  7 },
1519                                    { 4, 10,  9 },
1520                                    { 7,  9, 10 },
1521                                    { 2,  5,  8 },
1522                                    { 9,  8,  5 },
1523                                    { 1, 11,  6 },
1524                                    { 10, 6, 11 },
1525                                    { 3,  5,  2 },
1526                                    { 3,  1,  6 },
1527                                    { 0,  2,  8 },
1528                                    { 0, 11,  1 },
1529                                    { 7,  8,  9 },
1530                                    { 7, 10, 11 },
1531                                    { 4,  9,  5 },
1532                                    { 4,  6, 10 } };
1533
1534         Polyhedron p;
1535
1536         for(int i = 0; i < 12; ++i)
1537                 p.v.push_back(vertices[i]*scale + centerPos);
1538
1539         for(int i = 0; i < 20; ++i)
1540         {
1541                 Face f;
1542                 for(int j = 0; j < 3; ++j)
1543                         f.v.push_back(faces[i][j]);
1544                 p.f.push_back(f);
1545         }
1546
1547         assume(p.Contains(centerPos));
1548
1549         if (!ccwIsFrontFacing)
1550                 p.FlipWindingOrder();
1551
1552         assume(p.Contains(centerPos));
1553
1554         return p;
1555 }
1556
1557 /// See http://paulbourke.net/geometry/platonic/
1558 Polyhedron Polyhedron::Dodecahedron(const vec &centerPos, float scale, bool ccwIsFrontFacing)
1559 {
1560         float phi = (1.f + Sqrt(5.f)) * 0.5f;
1561         float b = 1.f / phi;
1562         float c = 2.f - phi;
1563
1564         const vec vertices[20] = { DIR_VEC( c,  0,  1),
1565                 DIR_VEC(-c, 0, 1),
1566                 DIR_VEC(-b, b, b),
1567                 DIR_VEC(0, 1, c),
1568                 DIR_VEC(b, b, b),
1569                 DIR_VEC(b, -b, b),
1570                 DIR_VEC(0, -1, c),
1571                 DIR_VEC(-b, -b, b),
1572                 DIR_VEC(0, -1, -c),
1573                 DIR_VEC(b, -b, -b),
1574                 DIR_VEC(-c, 0, -1),
1575                 DIR_VEC(c, 0, -1),
1576                 DIR_VEC(-b, -b, -b),
1577                 DIR_VEC(b, b, -b),
1578                 DIR_VEC(0, 1, -c),
1579                 DIR_VEC(-b, b, -b),
1580                 DIR_VEC(1, c, 0),
1581                 DIR_VEC(-1, c, 0),
1582                 DIR_VEC(-1, -c, 0),
1583                 DIR_VEC(1, -c, 0) };
1584
1585         const int faces[12][5] = { {  0,  1,  2,  3,  4 },
1586                                    {  1,  0,  5,  6,  7 },
1587                                    { 11, 10, 12,  8,  9 },
1588                                    { 10, 11, 13, 14, 15 },
1589                                    { 13, 16,  4,  3, 14 }, // Note: The winding order of this face was flipped from PBourke's original representation.
1590                                    {  2, 17, 15, 14,  3 }, //       Winding order flipped.
1591                                    { 12, 18,  7,  6,  8 }, //       Winding order flipped.
1592                                    {  5, 19,  9,  8,  6 }, //       Winding order flipped.
1593                                    { 16, 19,  5,  0,  4 },
1594                                    { 19, 16, 13, 11,  9 },
1595                                    { 17, 18, 12, 10, 15 },
1596                                    { 18, 17,  2,  1,  7 } };
1597
1598         scale /= 2.f;
1599         Polyhedron p;
1600
1601         for(int i = 0; i < 20; ++i)
1602                 p.v.push_back(vertices[i]*scale + centerPos);
1603
1604         for(int i = 0; i < 12; ++i)
1605         {
1606                 Face f;
1607                 for(int j = 0; j < 5; ++j)
1608                         f.v.push_back(faces[i][j]);
1609                 p.f.push_back(f);
1610         }
1611
1612         assume(p.Contains(centerPos));
1613
1614         if (!ccwIsFrontFacing)
1615                 p.FlipWindingOrder();
1616
1617         assume(p.Contains(centerPos));
1618
1619         return p;
1620 }
1621
1622 int IntTriCmp(int a, int b)
1623 {
1624         return a - b;
1625 }
1626
1627 /** Does a binary search on the array list that is sorted in ascending order.
1628         @param list [in] A pointer to the array to search.
1629         @param numItems The number of elements in the array 'list'.
1630         @param value The element to search for.
1631         @param cmp The comparison operator to use. The comparison function is of form
1632                 int CmpFunc(const T &a, const T &b), and it tests the mutual order of a and b.
1633                 It should return -1 if a < b, +1 if a > b, and 0 if a == b.
1634         @return The index where a matching element lies, or -1 if not found. Note that if there are more than
1635                 one matching element, the first that is found is returned. */
1636 template<typename T, typename CmpFunc>
1637 int ArrayBinarySearch(const T *list, int numItems, const T &value, CmpFunc &cmp)
1638 {
1639         int left = 0;
1640         int right = numItems-1;
1641         int order = cmp(list[left], value);
1642         if (order > 0) return -1;
1643         if (order == 0) return left;
1644
1645         order = cmp(list[right], value);
1646         if (order < 0) return -1;
1647         if (order == 0) return right;
1648
1649         int round = 0; // Counter to alternatingly round up or down.
1650         do
1651         {
1652                 int middle = (left + right + round) / 2;
1653                 round = (round+1)&1;
1654                 order = cmp(list[middle], value);
1655                 if (order == 0)
1656                         return middle;
1657                 if (order < 0)
1658                         left = middle;
1659                 else right = middle;
1660         } while(left < right);
1661         return -1;
1662 }
1663
1664 void Polyhedron::RemoveRedundantVertices()
1665 {
1666         std::set<int> usedVertices;
1667
1668         // Gather all used vertices.
1669         for(size_t i = 0; i < f.size(); ++i)
1670                 for(size_t j = 0; j < f[i].v.size(); ++j)
1671                         usedVertices.insert(f[i].v[j]);
1672
1673         // Turn the used vertices set into a vector for random access.
1674         std::vector<int> usedVerticesArray;
1675         usedVerticesArray.reserve(usedVertices.size());
1676         for(std::set<int>::iterator iter = usedVertices.begin(); iter != usedVertices.end(); ++iter)
1677                 usedVerticesArray.push_back(*iter);
1678
1679         // Shift all face indices to point to the new vertex array.
1680         for(size_t i = 0; i < f.size(); ++i)
1681                 for(size_t j = 0; j < f[i].v.size(); ++j)
1682                 {
1683                         int oldIndex = f[i].v[j];
1684                         int newIndex = ArrayBinarySearch(&usedVerticesArray[0], (int)usedVerticesArray.size(), oldIndex, IntTriCmp);
1685                         assert(newIndex != -1);
1686                         f[i].v[j] = newIndex;
1687                 }
1688
1689         // Delete all unused vertices from the vertex array.
1690         for(size_t i = 0; i < usedVerticesArray.size(); ++i)
1691                 v[i] = v[usedVerticesArray[i]];
1692         v.resize(usedVerticesArray.size());
1693
1694         assert(FaceIndicesValid());
1695 }
1696
1697 void Polyhedron::MergeAdjacentPlanarFaces()
1698 {
1699         ///\todo
1700 }
1701
1702 int CmpFaces(const Polyhedron::Face &a, const Polyhedron::Face &b)
1703 {
1704         if (a.v.size() != b.v.size())
1705                 return (int)b.v.size() - (int)a.v.size();
1706         for(size_t i = 0; i < a.v.size(); ++i)
1707         {
1708                 if (a.v[i] != b.v[i])
1709                         return a.v[i] - b.v[i];
1710         }
1711         return 0;
1712 }
1713
1714 void Polyhedron::CanonicalizeFaceArray()
1715 {
1716         if (f.empty())
1717                 return;
1718
1719         for(size_t i = 0; i < f.size(); ++i)
1720         {
1721                 Face &fc = f[i];
1722                 int smallestJ = 0;
1723                 for(size_t j = 1; j < fc.v.size(); ++j)
1724                 {
1725                         if (fc.v[j] < fc.v[smallestJ])
1726                                 smallestJ = (int)j;
1727                 }
1728                 while(smallestJ-- > 0) // Cycle smallest to front.
1729                 {
1730                         int j = fc.v.front();
1731                         fc.v.erase(fc.v.begin(), fc.v.begin() + 1);
1732                         fc.v.push_back(j);
1733                 }
1734         }
1735
1736         // Quick&dirty selection sort with custom predicate. Don't want to implement operate < for Face for std::sort.
1737         for(size_t i = 0; i < f.size()-1; ++i)
1738                 for(size_t j = i+1; j < f.size(); ++j)
1739                         if (CmpFaces(f[i], f[j]) > 0)
1740                                 Swap(f[i], f[j]);
1741 }
1742
1743 bool Polyhedron::SetEquals(Polyhedron &p2)
1744 {
1745         if (NumVertices() != p2.NumVertices() || NumFaces() != p2.NumFaces() || NumEdges() != p2.NumEdges())
1746                 return false;
1747
1748         // Match all corner vertices.
1749         const float epsilonSq = 1e-4f;
1750         for(int i = 0; i < (int)v.size(); ++i)
1751         {
1752                 float dSq;
1753                 int j = p2.FindClosestVertex(v[i], dSq);
1754                 if (j < i || dSq > epsilonSq)
1755                         return false; // No corresponding vertex found.
1756                 p2.SwapVertices(i, j);
1757         }
1758
1759         // Canonicalize face lists
1760         CanonicalizeFaceArray();
1761         p2.CanonicalizeFaceArray();
1762
1763         // Match all faces.
1764         for(size_t i = 0; i < f.size(); ++i)
1765                 if (!p2.ContainsFace(f[i]))
1766                         return false;
1767         return true;
1768 }
1769
1770 bool Polyhedron::ContainsFace(const Face &face) const
1771 {
1772         for(size_t i = 0; i < f.size(); ++i)
1773         {
1774                 const Face &f2 = f[i];
1775                 if (f2.v.size() != face.v.size())
1776                         continue;
1777                 if (face.v.empty())
1778                         return true;
1779                 // Find presumed cyclic shift
1780                 int shift = -1;
1781                 for(size_t j = 0; j < f2.v.size(); ++j)
1782                 {
1783                         if (f2.v[j] == face.v[0])
1784                         {
1785                                 shift = (int)j; // Assuming that each Face only contains each vertex once, like all good Faces do.
1786                                 break;
1787                         }
1788                 }
1789                 if (shift == -1)
1790                         continue; // Was not found?
1791
1792                 // Match all vertices with the found shift.
1793                 bool matches = true;
1794                 for(size_t j = 0; j < f2.v.size(); ++j)
1795                 {
1796                         if (f2.v[(j+f2.v.size()-shift) % f2.v.size()] != face.v[j])
1797                         {
1798                                 matches = false;
1799                                 break;
1800                         }
1801                 }
1802                 if (matches)
1803                         return true;
1804         }
1805         return false;
1806 }
1807
1808 void Polyhedron::SwapVertices(int i, int j)
1809 {
1810         if (i == j)
1811                 return;
1812         Swap(v[i], v[j]);
1813         for(size_t F = 0; F < f.size(); ++F)
1814         {
1815                 for(size_t V = 0; V < f[F].v.size(); ++V)
1816                 {
1817                         if (f[F].v[V] == i)
1818                                 f[F].v[V] = j;
1819                         else if (f[F].v[V] == j)
1820                                 f[F].v[V] = i;
1821                 }
1822         }
1823 }
1824
1825 int Polyhedron::FindClosestVertex(const vec &pt, float &outDistanceSq) const
1826 {
1827         outDistanceSq = FLOAT_INF;
1828         int closestI = -1;
1829         for(size_t i = 0; i < v.size(); ++i)
1830         {
1831                 float distSq = pt.DistanceSq(v[i]);
1832                 if (distSq < outDistanceSq)
1833                 {
1834                         outDistanceSq = distSq;
1835                         closestI = (int)i;
1836                 }
1837         }
1838         return closestI;
1839 }
1840
1841 TriangleArray Polyhedron::Triangulate() const
1842 {
1843         TriangleArray outTriangleList;
1844         for(int i = 0; i < NumFaces(); ++i)
1845         {
1846                 Polygon p = FacePolygon(i);
1847                 TriangleArray tris = p.Triangulate();
1848                 outTriangleList.insert(outTriangleList.end(), tris.begin(), tris.end());
1849         }
1850         return outTriangleList;
1851 }
1852
1853 std::string Polyhedron::ToString() const
1854 {
1855         if (v.empty())
1856                 return "Polyhedron(0 vertices)";
1857
1858         std::stringstream ss;
1859         ss << "Polyhedron(" << v.size() << " vertices:";
1860         for(size_t i = 0; i < v.size(); ++i)
1861         {
1862                 if (i != 0)
1863                         ss << ",";
1864                 ss << v[i];
1865         }
1866         ss << ")";
1867         return ss.str();
1868 }
1869
1870 #ifdef MATH_GRAPHICSENGINE_INTEROP
1871 void Polyhedron::Triangulate(VertexBuffer &vb, bool ccwIsFrontFacing, int faceStart, int faceEnd) const
1872 {
1873         for(int i = faceStart; i < Min(NumFaces(), faceEnd); ++i)
1874         {
1875                 Polygon p = FacePolygon(i);
1876                 TriangleArray tris = p.Triangulate();
1877                 int idx = vb.AppendVertices(3*(int)tris.size());
1878                 for(size_t j = 0; j < tris.size(); ++j)
1879                 {
1880                         vb.Set(idx, VDPosition, POINT_TO_FLOAT4(TRIANGLE(tris[j]).a));
1881                         if (ccwIsFrontFacing)
1882                         {
1883                                 vb.Set(idx+1, VDPosition, POINT_TO_FLOAT4(TRIANGLE(tris[j]).c));
1884                                 vb.Set(idx+2, VDPosition, POINT_TO_FLOAT4(TRIANGLE(tris[j]).b));
1885                         }
1886                         else
1887                         {
1888                                 vb.Set(idx+1, VDPosition, POINT_TO_FLOAT4(TRIANGLE(tris[j]).b));
1889                                 vb.Set(idx+2, VDPosition, POINT_TO_FLOAT4(TRIANGLE(tris[j]).c));
1890                         }
1891                         // Generate flat normals if VB has space for normals.
1892                         if (vb.Declaration()->TypeOffset(VDNormal) >= 0)
1893                         {
1894                                 vec normal = ccwIsFrontFacing ? TRIANGLE(tris[j]).NormalCCW() : TRIANGLE(tris[j]).NormalCW();
1895                                 vb.Set(idx, VDNormal, DIR_TO_FLOAT4(normal));
1896                                 vb.Set(idx+1, VDNormal, DIR_TO_FLOAT4(normal));
1897                                 vb.Set(idx+2, VDNormal, DIR_TO_FLOAT4(normal));
1898                         }
1899                         idx += 3;
1900                 }
1901         }
1902 }
1903
1904 void Polyhedron::ToLineList(VertexBuffer &vb) const
1905 {
1906         LineSegmentArray edges = Edges();
1907
1908         int startIndex = vb.AppendVertices((int)edges.size()*2);
1909         for(int i = 0; i < (int)edges.size(); ++i)
1910         {
1911                 vb.Set(startIndex+2*i, VDPosition, POINT_TO_FLOAT4(edges[i].a));
1912                 vb.Set(startIndex+2*i+1, VDPosition, POINT_TO_FLOAT4(edges[i].b));
1913         }
1914 }
1915 #endif
1916
1917 Polyhedron operator *(const float3x3 &transform, const Polyhedron &polyhedron)
1918 {
1919         Polyhedron p(polyhedron);
1920         p.Transform(transform);
1921         return p;
1922 }
1923
1924 Polyhedron operator *(const float3x4 &transform, const Polyhedron &polyhedron)
1925 {
1926         Polyhedron p(polyhedron);
1927         p.Transform(transform);
1928         return p;
1929 }
1930
1931 Polyhedron operator *(const float4x4 &transform, const Polyhedron &polyhedron)
1932 {
1933         Polyhedron p(polyhedron);
1934         p.Transform(transform);
1935         return p;
1936 }
1937
1938 Polyhedron operator *(const Quat &transform, const Polyhedron &polyhedron)
1939 {
1940         Polyhedron p(polyhedron);
1941         p.Transform(transform);
1942         return p;
1943 }
1944
1945 MATH_END_NAMESPACE