Plane.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 Plane.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Plane geometry object. */
18 #include "Plane.h"
19 #include "../Math/MathFunc.h"
20 #include "../Math/Polynomial.h"
21 #include "AABB.h"
22 #include "Circle.h"
23 #include "Line.h"
24 #include "OBB.h"
25 #include "Polygon.h"
26 #include "Polyhedron.h"
27 #include "Ray.h"
28 #include "Capsule.h"
29 #include "Sphere.h"
30 #include "Triangle.h"
31 #include "LineSegment.h"
32 #include "../Math/float3x3.h"
33 #include "../Math/float3x4.h"
34 #include "../Math/float4x4.h"
35 #include "../Math/float4.h"
36 #include "../Math/Quat.h"
37 #include "Frustum.h"
38
39 #ifdef MATH_ENABLE_STL_SUPPORT
40 #include <iostream>
41 #endif
42
43 #ifdef MATH_GRAPHICSENGINE_INTEROP
44 #include "VertexBuffer.h"
45 #endif
46
47 MATH_BEGIN_NAMESPACE
48
49 Plane::Plane(const vec &normal_, float d_)
50 :normal(normal_), d(d_)
51 {
52         assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
53 }
54
55 Plane::Plane(const vec &v1, const vec &v2, const vec &v3)
56 {
57         Set(v1, v2, v3);
58 }
59
60 Plane::Plane(const vec &point, const vec &normal_)
61 {
62         Set(point, normal_);
63 }
64
65 Plane::Plane(const Ray &ray, const vec &normal)
66 {
67         vec perpNormal = normal - normal.ProjectToNorm(ray.dir);
68         Set(ray.pos, perpNormal.Normalized());
69 }
70
71 Plane::Plane(const Line &line, const vec &normal)
72 {
73         vec perpNormal = normal - normal.ProjectToNorm(line.dir);
74         Set(line.pos, perpNormal.Normalized());
75 }
76
77 Plane::Plane(const LineSegment &lineSegment, const vec &normal)
78 {
79         vec perpNormal = normal - normal.ProjectTo(lineSegment.b - lineSegment.a);
80         Set(lineSegment.a, perpNormal.Normalized());
81 }
82
83 bool Plane::IsDegenerate() const
84 {
85         return !normal.IsFinite() || normal.IsZero() || !IsFinite(d);
86 }
87
88 void Plane::Set(const vec &v1, const vec &v2, const vec &v3)
89 {
90         normal = (v2-v1).Cross(v3-v1);
91         float len = normal.Length();
92         assume1(len > 1e-10f, len);
93         normal /= len;
94         assume2(normal.IsNormalized(), normal, normal.LengthSq());
95         d = normal.Dot(v1);
96 }
97
98 void Plane::Set(const vec &point, const vec &normal_)
99 {
100         normal = normal_;
101         assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
102         d = point.Dot(normal);
103
104 #ifdef MATH_ASSERT_CORRECTNESS
105         assert1(EqualAbs(SignedDistance(point), 0.f, 0.01f), SignedDistance(point));
106         assert1(EqualAbs(SignedDistance(point + normal_), 1.f, 0.01f), SignedDistance(point + normal_));
107 #endif
108 }
109
110 void Plane::ReverseNormal()
111 {
112         normal = -normal;
113         d = -d;
114 }
115
116 vec Plane::PointOnPlane() const
117 {
118 #ifdef MATH_AUTOMATIC_SSE
119         return normal * d + vec(POINT_VEC_SCALAR(0.f));
120 #else
121         return normal * d;
122 #endif
123 }
124
125 vec Plane::Point(float u, float v) const
126 {
127         vec b1, b2;
128         normal.PerpendicularBasis(b1, b2);
129         return PointOnPlane() + u * b1 + v * b2;
130 }
131
132 vec Plane::Point(float u, float v, const vec &referenceOrigin) const
133 {
134         vec b1, b2;
135         normal.PerpendicularBasis(b1, b2);
136         return Project(referenceOrigin) + u * b1 + v * b2;
137 }
138
139 void Plane::Translate(const vec &offset)
140 {
141         d -= normal.Dot(offset);
142 }
143
144 void Plane::Transform(const float3x3 &transform)
145 {
146         float3x3 it = transform.InverseTransposed(); ///@todo Could optimize the inverse here by assuming orthogonality or orthonormality.
147         normal = it * normal;
148 }
149
150 /// For Plane-float3x4 transform code, see Eric Lengyel's Mathematics for 3D Game Programming And Computer Graphics 2nd ed., p.110, chapter 4.2.3. [groupSyntax]
151 void Plane::Transform(const float3x4 &transform)
152 {
153         ///@todo Could optimize this function by switching to plane convention ax+by+cz+d=0 instead of ax+by+cz=d.
154         float3x3 r = transform.Float3x3Part();
155         bool success = r.Inverse(); ///@todo Can optimize the inverse here by assuming orthogonality or orthonormality.
156         assume(success);
157         MARK_UNUSED(success);
158         d = d + normal.Dot(DIR_VEC(r * transform.TranslatePart()));
159         normal = normal * r;
160 }
161
162 void Plane::Transform(const float4x4 &transform)
163 {
164         assume(transform.Row(3).Equals(float4(0,0,0,1)));
165         Transform(transform.Float3x4Part());
166 }
167
168 void Plane::Transform(const Quat &transform)
169 {
170         float3x3 r = transform.ToFloat3x3();
171         Transform(r);
172 }
173
174 bool Plane::IsInPositiveDirection(const vec &directionVector) const
175 {
176         return normal.Dot(directionVector) >= 0.f;
177 }
178
179 bool Plane::IsOnPositiveSide(const vec &point) const
180 {
181         return SignedDistance(point) >= 0.f;
182 }
183
184 int Plane::ExamineSide(const Triangle &triangle) const
185 {
186         float a = SignedDistance(triangle.a);
187         float b = SignedDistance(triangle.b);
188         float c = SignedDistance(triangle.c);
189         const float epsilon = 1e-4f; // Allow a small epsilon amount for tests for floating point inaccuracies.
190         if (a >= -epsilon && b >= -epsilon && c >= -epsilon)
191                 return 1;
192         if (a <= epsilon && b <= epsilon && c <= epsilon)
193                 return -1;
194         return 0;
195 }
196
197 bool Plane::AreOnSameSide(const vec &p1, const vec &p2) const
198 {
199         return SignedDistance(p1) * SignedDistance(p2) >= 0.f;
200 }
201
202 float Plane::Distance(const vec &point) const
203 {
204         return Abs(SignedDistance(point));
205 }
206
207 float Plane::Distance(const LineSegment &lineSegment) const
208 {
209         return lineSegment.Distance(*this);
210 }
211
212 float Plane::Distance(const Sphere &sphere) const
213 {
214         return Max(0.f, Distance(sphere.pos) - sphere.r);
215 }
216
217 float Plane::Distance(const Capsule &capsule) const
218 {
219         return Max(0.f, Distance(capsule.l) - capsule.r);
220 }
221
222 float Plane::SignedDistance(const vec &point) const
223 {
224         assume2(normal.IsNormalized(), normal, normal.Length());
225 #ifdef MATH_VEC_IS_FLOAT4
226         assert1(normal.w == 0.f, normal.w);
227 #endif
228         return normal.Dot(point) - d;
229 }
230
231 template<typename T>
232 float Plane_SignedDistance(const Plane &plane, const T &object)
233 {
234         float pMin, pMax;
235         assume(plane.normal.IsNormalized());
236         object.ProjectToAxis(plane.normal, pMin, pMax);
237         pMin -= plane.d;
238         pMax -= plane.d;
239         if (pMin * pMax <= 0.f)
240                 return 0.f;
241         return Abs(pMin) < Abs(pMax) ? pMin : pMax;
242 }
243
244 float Plane::SignedDistance(const AABB &aabb) const { return Plane_SignedDistance(*this, aabb); }
245 float Plane::SignedDistance(const OBB &obb) const { return Plane_SignedDistance(*this, obb); }
246 float Plane::SignedDistance(const Capsule &capsule) const { return Plane_SignedDistance(*this, capsule); }
247 //float Plane::SignedDistance(const Circle &circle) const { return Plane_SignedDistance(*this, circle); }
248 float Plane::SignedDistance(const Frustum &frustum) const { return Plane_SignedDistance(*this, frustum); }
249 float Plane::SignedDistance(const Line &line) const { return Plane_SignedDistance(*this, line); }
250 float Plane::SignedDistance(const LineSegment &lineSegment) const { return Plane_SignedDistance(*this, lineSegment); }
251 float Plane::SignedDistance(const Ray &ray) const { return Plane_SignedDistance(*this, ray); }
252 //float Plane::SignedDistance(const Plane &plane) const { return Plane_SignedDistance(*this, plane); }
253 float Plane::SignedDistance(const Polygon &polygon) const { return Plane_SignedDistance(*this, polygon); }
254 float Plane::SignedDistance(const Polyhedron &polyhedron) const { return Plane_SignedDistance(*this, polyhedron); }
255 float Plane::SignedDistance(const Sphere &sphere) const { return Plane_SignedDistance(*this, sphere); }
256 float Plane::SignedDistance(const Triangle &triangle) const { return Plane_SignedDistance(*this, triangle); }
257
258 float3x4 Plane::OrthoProjection() const
259 {
260         return float3x4::OrthographicProjection(*this);
261 }
262
263 #if 0
264 float3x4 Plane::ObliqueProjection(const vec & /*obliqueProjectionDir*/) const
265 {
266 #ifdef _MSC_VER
267 #pragma warning(Plane::ObliqueProjection not implemented!)
268 #else
269 #warning Plane::ObliqueProjection not implemented!
270 #endif
271         assume(false && "Plane::ObliqueProjection not implemented!"); /// @todo Implement.
272         return float3x4();
273 }
274 #endif
275
276 float3x4 Plane::MirrorMatrix() const
277 {
278         return float3x4::Mirror(*this);
279 }
280
281 vec Plane::Mirror(const vec &point) const
282 {
283 #ifdef MATH_ASSERT_CORRECTNESS
284         float signedDistance = SignedDistance(point);
285 #endif
286         assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
287         vec reflected = point - 2.f * (point.Dot(normal) - d) * normal;
288         mathassert(EqualAbs(signedDistance, -SignedDistance(reflected), 1e-2f));
289         mathassert(reflected.Equals(MirrorMatrix().MulPos(point)));
290         return reflected;
291 }
292
293 vec Plane::Refract(const vec &vec, float negativeSideRefractionIndex, float positiveSideRefractionIndex) const
294 {
295         return vec.Refract(normal, negativeSideRefractionIndex, positiveSideRefractionIndex);
296 }
297
298 vec Plane::Project(const vec &point) const
299 {
300         vec projected = point - (normal.Dot(point) - d) * normal;
301         return projected;
302 }
303
304 vec Plane::ProjectToNegativeHalf(const vec &point) const
305 {
306         return point - Max(0.f, (normal.Dot(point) - d)) * normal;
307 }
308
309 vec Plane::ProjectToPositiveHalf(const vec &point) const
310 {
311         return point - Min(0.f, (Dot(normal, point) - d)) * normal;
312 }
313
314 LineSegment Plane::Project(const LineSegment &lineSegment) const
315 {
316         return LineSegment(Project(lineSegment.a), Project(lineSegment.b));
317 }
318
319 Line Plane::Project(const Line &line, bool *nonDegenerate) const
320 {
321         Line l;
322         l.pos = Project(line.pos);
323         l.dir = l.dir - l.dir.ProjectToNorm(normal);
324         float len = l.dir.Normalize();
325         if (nonDegenerate)
326                 *nonDegenerate = (len > 0.f);
327         return l;
328 }
329
330 Ray Plane::Project(const Ray &ray, bool *nonDegenerate) const
331 {
332         Ray r;
333         r.pos = Project(ray.pos);
334         r.dir = r.dir - r.dir.ProjectToNorm(normal);
335         float len = r.dir.Normalize();
336         if (nonDegenerate)
337                 *nonDegenerate = (len > 0.f);
338         return r;
339 }
340
341 Triangle Plane::Project(const Triangle &triangle) const
342 {
343         Triangle t;
344         t.a = Project(triangle.a);
345         t.b = Project(triangle.b);
346         t.c = Project(triangle.c);
347         return t;
348 }
349
350 Polygon Plane::Project(const Polygon &polygon) const
351 {
352         Polygon p;
353         for(size_t i = 0; i < polygon.p.size(); ++i)
354                 p.p.push_back(Project(polygon.p[i]));
355
356         return p;
357 }
358
359 vec Plane::ClosestPoint(const Ray &ray) const
360 {
361         assume(ray.IsFinite());
362         assume(!IsDegenerate());
363
364         // The plane and a ray have three configurations:
365         // 1) the ray and the plane don't intersect: the closest point is the ray origin point.
366         // 2) the ray and the plane do intersect: the closest point is the intersection point.
367         // 3) the ray is parallel to the plane: any point on the ray projected to the plane is a closest point.
368         float denom = Dot(normal, ray.dir);
369         if (denom == 0.f)
370                 return Project(ray.pos); // case 3)
371         float t = (d - Dot(normal, ray.pos)) / denom;
372         if (t >= 0.f && t < 1e6f) // Numerical stability check: Instead of checking denom against epsilon, check the resulting t for very large values.
373                 return ray.GetPoint(t); // case 2)
374         else
375                 return Project(ray.pos); // case 1)
376 }
377
378 vec Plane::ClosestPoint(const LineSegment &lineSegment) const
379 {
380         /*
381         ///@todo Output parametric d as well.
382         float d;
383         if (lineSegment.Intersects(*this, &d))
384                 return lineSegment.GetPoint(d);
385         else
386                 if (Distance(lineSegment.a) < Distance(lineSegment.b))
387                         return Project(lineSegment.a);
388                 else
389                         return Project(lineSegment.b);
390         */
391
392         assume(lineSegment.IsFinite());
393         assume(!IsDegenerate());
394
395         float aDist = Dot(normal, lineSegment.a);
396         float bDist = Dot(normal, lineSegment.b);
397
398         float denom = bDist - aDist;
399         if (EqualAbs(denom, 0.f))
400                 return Project(Abs(aDist) < Abs(bDist) ? lineSegment.a : lineSegment.b); // Project()ing the result here is not strictly necessary,
401                                                                                          // but done for numerical stability, so that Plane::Contains()
402                                                                                          // will return true for the returned point.
403         else
404         {
405                 ///@todo Output parametric t along the ray as well.
406                 float t = (d - Dot(normal, lineSegment.a)) / (bDist - aDist);
407                 t = Clamp01(t);
408                 // Project()ing the result here is necessary only if we clamped, but done for numerical stability, so that Plane::Contains() will
409                 // return true for the returned point.
410                 return Project(lineSegment.GetPoint(t));
411         }
412 }
413
414 #if 0
415 vec Plane::ObliqueProject(const vec & /*point*/, const vec & /*obliqueProjectionDir*/) const
416 {
417 #ifdef _MSC_VER
418 #pragma warning(Plane::ObliqueProject not implemented!)
419 #else
420 #warning Plane::ObliqueProject not implemented!
421 #endif
422         assume(false && "Plane::ObliqueProject not implemented!"); /// @todo Implement.
423         return vec();
424 }
425 #endif
426
427 bool Plane::Contains(const vec &point, float distanceThreshold) const
428 {
429         return Distance(point) <= distanceThreshold;
430 }
431
432 bool Plane::Contains(const Line &line, float epsilon) const
433 {
434         return Contains(line.pos) && line.dir.IsPerpendicular(normal, epsilon);
435 }
436
437 bool Plane::Contains(const Ray &ray, float epsilon) const
438 {
439         return Contains(ray.pos) && ray.dir.IsPerpendicular(normal, epsilon);
440 }
441
442 bool Plane::Contains(const LineSegment &lineSegment, float epsilon) const
443 {
444         return Contains(lineSegment.a, epsilon) && Contains(lineSegment.b, epsilon);
445 }
446
447 bool Plane::Contains(const Triangle &triangle, float epsilon) const
448 {
449         return Contains(triangle.a, epsilon) && Contains(triangle.b, epsilon) && Contains(triangle.c, epsilon);
450 }
451
452 bool Plane::Contains(const Circle &circle, float epsilon) const
453 {
454         return Contains(circle.pos, epsilon) && (EqualAbs(Abs(Dot(normal, circle.normal)), 1.f) || circle.r <= epsilon);
455 }
456
457 bool Plane::Contains(const Polygon &polygon, float epsilon) const
458 {
459         switch(polygon.NumVertices())
460         {
461         case 0: assume(false && "Plane::Contains(Polygon) called with a degenerate polygon of 0 vertices!"); return false;
462         case 1: return Contains(polygon.Vertex(0), epsilon);
463         case 2: return Contains(polygon.Vertex(0), epsilon) && Contains(polygon.Vertex(1), epsilon);
464         default:
465                 return SetEquals(polygon.PlaneCCW(), epsilon);
466           }
467 }
468
469 bool Plane::SetEquals(const Plane &plane, float epsilon) const
470 {
471         return (normal.Equals(plane.normal) && EqualAbs(d, plane.d, epsilon)) ||
472                 (normal.Equals(-plane.normal) && EqualAbs(-d, plane.d, epsilon));
473 }
474
475 bool Plane::Equals(const Plane &other, float epsilon) const
476 {
477         return IsParallel(other, epsilon) && EqualAbs(d, other.d, epsilon);
478 }
479
480 bool Plane::Intersects(const Plane &plane, Line *outLine) const
481 {
482         vec perp = normal.Perpendicular(plane.normal);//vec::Perpendicular Cross(normal, plane.normal);
483
484         float3x3 m;
485         m.SetRow(0, DIR_TO_FLOAT3(normal));
486         m.SetRow(1, DIR_TO_FLOAT3(plane.normal));
487         m.SetRow(2, DIR_TO_FLOAT3(perp)); // This is arbitrarily chosen, to produce m invertible.
488         float3 intersectionPos;
489         bool success = m.SolveAxb(float3(d, plane.d, 0.f),intersectionPos);
490         if (!success) // Inverse failed, so the planes must be parallel.
491         {
492                 float normalDir = Dot(normal,plane.normal);
493                 if ((normalDir > 0.f && EqualAbs(d, plane.d)) || (normalDir < 0.f && EqualAbs(d, -plane.d)))
494                 {
495                         if (outLine)
496                                 *outLine = Line(normal*d, plane.normal.Perpendicular());
497                         return true;
498                 }
499                 else
500                         return false;
501         }
502         if (outLine)
503                 *outLine = Line(POINT_VEC(intersectionPos), perp.Normalized());
504         return true;
505 }
506
507 bool Plane::Intersects(const Plane &plane, const Plane &plane2, Line *outLine, vec *outPoint) const
508 {
509         Line dummy;
510         if (!outLine)
511                 outLine = &dummy;
512
513         // First check all planes for parallel pairs.
514         if (this->IsParallel(plane) || this->IsParallel(plane2))
515         {
516                 if (EqualAbs(d, plane.d) || EqualAbs(d, plane2.d))
517                 {
518                         bool intersect = plane.Intersects(plane2, outLine);
519                         if (intersect && outPoint)
520                                 *outPoint = outLine->GetPoint(0);
521                         return intersect;
522                 }
523                 else
524                         return false;
525         }
526         if (plane.IsParallel(plane2))
527         {
528                 if (EqualAbs(plane.d, plane2.d))
529                 {
530                         bool intersect = this->Intersects(plane, outLine);
531                         if (intersect && outPoint)
532                                 *outPoint = outLine->GetPoint(0);
533                         return intersect;
534                 }
535                 else
536                         return false;
537         }
538
539         // All planes point to different directions.
540         float3x3 m;
541         m.SetRow(0, DIR_TO_FLOAT3(normal));
542         m.SetRow(1, DIR_TO_FLOAT3(plane.normal));
543         m.SetRow(2, DIR_TO_FLOAT3(plane2.normal));
544         float3 intersectionPos;
545         bool success = m.SolveAxb(float3(d, plane.d, plane2.d), intersectionPos);
546         if (!success)
547                 return false;
548         if (outPoint)
549                 *outPoint = POINT_VEC(intersectionPos);
550         return true;
551 }
552
553 bool Plane::Intersects(const Polygon &polygon) const
554 {
555         return polygon.Intersects(*this);
556 }
557
558 #if 0
559 bool Plane::IntersectLinePlane(const vec &p, const vec &n, const vec &a, const vec &d, float &t)
560 {
561         /* The set of points x lying on a plane is defined by the equation
562
563                 (x - p)*n == 0, where p is a point on the plane, and n is the plane normal.
564
565         The set of points x on a line is constructed explicitly by a single parameter t by
566
567                 x = a + t*d, where a is a point on the line, and d is the direction vector of the line.
568
569         To solve the intersection of these two objects, substitute the second equation to the first above,
570         and we get
571
572                   (a + t*d - p)*n == 0, or
573                 t*(d*n) + (a-p)*n == 0, or
574                             t == (p-a)*n / (d*n), assuming that d*n != 0.
575
576         If d*n == 0, then the line is parallel to the plane, and either no intersection occurs, or the whole line
577         is embedded on the plane, and infinitely many intersections occur. */
578
579         float denom = Dot(d, n);
580         if (EqualAbs(denom, 0.f))
581         {
582                 t = 0.f;
583                 float f = Dot(a-p, n);
584                 bool b = EqualAbs(Dot(a-p, n), 0.f);
585                 return EqualAbs(Dot(a-p, n), 0.f); // If (a-p)*n == 0, then then above equation holds for all t, and return true.
586         }
587         else
588         {
589                 // Compute the distance from the line starting point to the point of intersection.
590                 t = Dot(p - a, n) / denom;
591                 return true;
592         }
593 }
594 #endif
595
596 bool Plane::IntersectLinePlane(const vec &planeNormal, float planeD, const vec &linePos, const vec &lineDir, float &t)
597 {
598         /* The set of points x lying on a plane is defined by the equation
599
600                 <planeNormal, x> = planeD.
601
602         The set of points x on a line is constructed explicitly by a single parameter t by
603
604                 x = linePos + t*lineDir.
605
606         To solve the intersection of these two objects, substitute the second equation to the first above,
607         and we get
608
609                              <planeNormal, linePos + t*lineDir> == planeD, or
610             <planeNormal, linePos> + t * <planeNormal, lineDir> == planeD, or
611                                                               t == (planeD - <planeNormal, linePos>) / <planeNormal, lineDir>,
612
613                                                                    assuming that <planeNormal, lineDir> != 0.
614
615         If <planeNormal, lineDir> == 0, then the line is parallel to the plane, and either no intersection occurs, or the whole line
616         is embedded on the plane, and infinitely many intersections occur. */
617
618         float denom = Dot(planeNormal, lineDir);
619         if (Abs(denom) > 1e-4f)
620         {
621                 // Compute the distance from the line starting point to the point of intersection.
622                 t = (planeD - Dot(planeNormal, linePos)) / denom;
623                 return true;
624         }
625
626         if (denom != 0.f)
627         {
628                 t = (planeD - Dot(planeNormal, linePos)) / denom;
629                 if (Abs(t) < 1e4f)
630                         return true;
631         }
632         t = 0.f;
633         return EqualAbs(Dot(planeNormal, linePos), planeD, 1e-3f);
634 }
635
636
637 bool Plane::Intersects(const Ray &ray, float *d) const
638 {
639         float t;
640         bool success = IntersectLinePlane(normal, this->d, ray.pos, ray.dir, t);
641         if (d)
642                 *d = t;
643         return success && t >= 0.f;
644 }
645
646 bool Plane::Intersects(const Line &line, float *d) const
647 {
648         float t;
649         bool intersects = IntersectLinePlane(normal, this->d, line.pos, line.dir, t);
650         if (d)
651                 *d = t;
652         return intersects;
653 }
654
655 bool Plane::Intersects(const LineSegment &lineSegment, float *d) const
656 {
657         float t;
658         bool success = IntersectLinePlane(normal, this->d, lineSegment.a, lineSegment.Dir(), t);
659         const float lineSegmentLength = lineSegment.Length();
660         if (d)
661                 *d = t / lineSegmentLength;
662         return success && t >= 0.f && t <= lineSegmentLength;
663 }
664
665 bool Plane::Intersects(const Sphere &sphere) const
666 {
667         return Distance(sphere.pos) <= sphere.r;
668 }
669
670 bool Plane::Intersects(const Capsule &capsule) const
671 {
672         return capsule.Intersects(*this);
673 }
674
675 /// The Plane-AABB intersection is implemented according to Christer Ericson's Real-Time Collision Detection, p.164. [groupSyntax]
676 bool Plane::Intersects(const AABB &aabb) const
677 {
678         vec c = aabb.CenterPoint();
679         vec e = aabb.HalfDiagonal();
680
681         // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
682         float r = e[0]*Abs(normal[0]) + e[1]*Abs(normal[1]) + e[2]*Abs(normal[2]);
683         // Compute the distance of the box center from plane.
684 //      float s = Dot(normal, c) - d;
685         float s = Dot(normal.xyz(), c.xyz()) - d; ///\todo Use the above form when Plane is SSE'ized.
686         return Abs(s) <= r;
687 }
688
689 bool Plane::Intersects(const OBB &obb) const
690 {
691         return obb.Intersects(*this);
692 }
693
694 bool Plane::Intersects(const Triangle &triangle) const
695 {
696         float a = SignedDistance(triangle.a);
697         float b = SignedDistance(triangle.b);
698         float c = SignedDistance(triangle.c);
699         return (a*b <= 0.f || a*c <= 0.f);
700 }
701
702 bool Plane::Intersects(const Frustum &frustum) const
703 {
704         bool sign = IsOnPositiveSide(frustum.CornerPoint(0));
705         for(int i = 1; i < 8; ++i)
706                 if (sign != IsOnPositiveSide(frustum.CornerPoint(i)))
707                         return true;
708         return false;
709 }
710
711 bool Plane::Intersects(const Polyhedron &polyhedron) const
712 {
713         if (polyhedron.NumVertices() == 0)
714                 return false;
715         bool sign = IsOnPositiveSide(polyhedron.Vertex(0));
716         for(int i = 1; i < polyhedron.NumVertices(); ++i)
717                 if (sign != IsOnPositiveSide(polyhedron.Vertex(i)))
718                         return true;
719         return false;
720 }
721
722 int Plane::Intersects(const Circle &circle, vec *pt1, vec *pt2) const
723 {
724         Line line;
725         bool planeIntersects = Intersects(circle.ContainingPlane(), &line);
726         if (!planeIntersects)
727                 return false;
728
729         // Offset both line and circle position so the circle origin is at center.
730         line.pos -= circle.pos;
731
732         float a = 1.f;
733         float b = 2.f * Dot(line.pos, line.dir);
734         float c = line.pos.LengthSq() - circle.r * circle.r;
735         float r1, r2;
736         int numRoots = Polynomial::SolveQuadratic(a, b, c, r1, r2);
737         if (numRoots >= 1 && pt1)
738                 *pt1 = circle.pos + line.GetPoint(r1);
739         if (numRoots >= 2 && pt2)
740                 *pt2 = circle.pos + line.GetPoint(r2);
741         return numRoots;
742 }
743
744 int Plane::Intersects(const Circle &circle) const
745 {
746         return Intersects(circle, 0, 0);
747 }
748
749 bool Plane::Clip(vec &a, vec &b) const
750 {
751         float t;
752         bool intersects = IntersectLinePlane(normal, d, a, b-a, t);
753         if (!intersects || t <= 0.f || t >= 1.f)
754         {
755                 if (SignedDistance(a) <= 0.f)
756                         return false; // Discard the whole line segment, it's completely behind the plane.
757                 else
758                         return true; // The whole line segment is in the positive halfspace. Keep all of it.
759         }
760         vec pt = a + (b-a) * t; // The intersection point.
761         // We are either interested in the line segment [a, pt] or the segment [pt, b]. Which one is in the positive side?
762         if (IsOnPositiveSide(a))
763                 b = pt;
764         else
765                 a = pt;
766
767         return true;
768 }
769
770 bool Plane::Clip(LineSegment &line) const
771 {
772         return Clip(line.a, line.b);
773 }
774
775 int Plane::Clip(const Line &line, Ray &outRay) const
776 {
777         float t;
778         bool intersects = IntersectLinePlane(normal, d, line.pos, line.dir, t);
779         if (!intersects)
780         {
781                 if (SignedDistance(line.pos) <= 0.f)
782                         return 0; // Discard the whole line, it's completely behind the plane.
783                 else
784                         return 2; // The whole line is in the positive halfspace. Keep all of it.
785         }
786
787         outRay.pos = line.pos + line.dir * t; // The intersection point
788         if (Dot(line.dir, normal) >= 0.f)
789                 outRay.dir = line.dir;
790         else
791                 outRay.dir = -line.dir;
792
793         return 1; // Clipping resulted in a ray being generated.
794 }
795
796 int Plane::Clip(const Triangle &triangle, Triangle &t1, Triangle &t2) const
797 {
798         bool side[3];
799         side[0] = IsOnPositiveSide(triangle.a);
800         side[1] = IsOnPositiveSide(triangle.b);
801         side[2] = IsOnPositiveSide(triangle.c);
802         int nPos = (side[0] ? 1 : 0) + (side[1] ? 1 : 0) + (side[2] ? 1 : 0);
803         if (nPos == 0) // Everything should be clipped?
804                 return 0;
805         // We will output at least one triangle, so copy the input to t1 for processing.
806         t1 = triangle;
807
808         if (nPos == 3) // All vertices of the triangle are in positive side?
809                 return 1;
810
811         if (nPos == 1)
812         {
813                 if (side[1])
814                 {
815                         vec tmp = t1.a;
816                         t1.a = t1.b;
817                         t1.b = t1.c;
818                         t1.c = tmp;
819                 }
820                 else if (side[2])
821                 {
822                         vec tmp = t1.a;
823                         t1.a = t1.c;
824                         t1.c = t1.b;
825                         t1.b = tmp;
826                 }
827
828                 // After the above cycling, t1.a is the triangle on the positive side.
829                 float t;
830                 Intersects(LineSegment(t1.a, t1.b), &t);
831                 t1.b = t1.a + (t1.b-t1.a)*t;
832                 Intersects(LineSegment(t1.a, t1.c), &t);
833                 t1.c = t1.a + (t1.c-t1.a)*t;
834                 return 1;
835         }
836         // Must be nPos == 2.
837         if (!side[1])
838         {
839                 vec tmp = t1.a;
840                 t1.a = t1.b;
841                 t1.b = t1.c;
842                 t1.c = tmp;
843         }
844         else if (!side[2])
845         {
846                 vec tmp = t1.a;
847                 t1.a = t1.c;
848                 t1.c = t1.b;
849                 t1.b = tmp;
850         }
851         // After the above cycling, t1.a is the triangle on the negative side.
852
853         float t, r;
854         Intersects(LineSegment(t1.a, t1.b), &t);
855         vec ab = t1.a + (t1.b-t1.a)*t;
856         Intersects(LineSegment(t1.a, t1.c), &r);
857         vec ac = t1.a + (t1.c-t1.a)*r;
858         t1.a = ab;
859
860         t2.a = t1.c;
861         t2.b = ac;
862         t2.c = ab;
863
864         return 2;
865 }
866
867 bool Plane::IsParallel(const Plane &plane, float epsilon) const
868 {
869         return normal.Equals(plane.normal, epsilon);
870 }
871
872 bool Plane::PassesThroughOrigin(float epsilon) const
873 {
874         return MATH_NS::Abs(d) <= epsilon;
875 }
876
877 float Plane::DihedralAngle(const Plane &plane) const
878 {
879         return Dot(normal, plane.normal);
880 }
881
882 Circle Plane::GenerateCircle(const vec &circleCenter, float radius) const
883 {
884         return Circle(Project(circleCenter), normal, radius);
885 }
886
887 Plane operator *(const float3x3 &transform, const Plane &plane)
888 {
889         Plane p(plane);
890         p.Transform(transform);
891         return p;
892 }
893
894 Plane operator *(const float3x4 &transform, const Plane &plane)
895 {
896         Plane p(plane);
897         p.Transform(transform);
898         return p;
899 }
900
901 Plane operator *(const float4x4 &transform, const Plane &plane)
902 {
903         Plane p(plane);
904         p.Transform(transform);
905         return p;
906 }
907
908 Plane operator *(const Quat &transform, const Plane &plane)
909 {
910         Plane p(plane);
911         p.Transform(transform);
912         return p;
913 }
914
915 #ifdef MATH_ENABLE_STL_SUPPORT
916 std::string Plane::ToString() const
917 {
918         char str[256];
919         sprintf(str, "Plane(Normal:(%.2f, %.2f, %.2f) d:%.2f)", normal.x, normal.y, normal.z, d);
920         return str;
921 }
922
923 std::string Plane::SerializeToString() const
924 {
925         char str[256];
926         char *s = SerializeFloat(normal.x, str); *s = ','; ++s;
927         s = SerializeFloat(normal.y, s); *s = ','; ++s;
928         s = SerializeFloat(normal.z, s); *s = ','; ++s;
929         s = SerializeFloat(d, s);
930         assert(s+1 - str < 256);
931         MARK_UNUSED(s);
932         return str;
933 }
934
935 std::string Plane::SerializeToCodeString() const
936 {
937         char str[256];
938         sprintf(str, "%.9g", d);
939         return "Plane(" + normal.SerializeToCodeString() + "," + str + ")";
940 }
941
942 Plane Plane::FromString(const char *str, const char **outEndStr)
943 {
944         assume(str);
945         if (!str)
946                 return Plane(vec::nan, FLOAT_NAN);
947         Plane p;
948         MATH_SKIP_WORD(str, "Plane(");
949         MATH_SKIP_WORD(str, "Normal:(");
950         p.normal = DirVecFromString(str, &str);
951         MATH_SKIP_WORD(str, " d:");
952         p.d = DeserializeFloat(str, &str);
953         if (outEndStr)
954                 *outEndStr = str;
955         return p;
956 }
957
958 std::ostream &operator <<(std::ostream &o, const Plane &plane)
959 {
960         o << plane.ToString();
961         return o;
962 }
963
964 #endif
965
966 #ifdef MATH_GRAPHICSENGINE_INTEROP
967 void Plane::Triangulate(VertexBuffer &vb, float uWidth, float vHeight, const vec &centerPoint, int numFacesU, int numFacesV, bool ccwIsFrontFacing) const
968 {
969         vec topLeft = Point(-uWidth*0.5f, -vHeight *0.5f, centerPoint);
970         vec uEdge = (Point(uWidth*0.5f, -vHeight *0.5f, centerPoint) - topLeft) / (float)numFacesU;
971         vec vEdge = (Point(-uWidth*0.5f, vHeight *0.5f, centerPoint) - topLeft) / (float)numFacesV;
972
973         int i = vb.AppendVertices(numFacesU * numFacesV * 6);
974         for(int y = 0; y < numFacesV; ++y)
975                 for(int x = 0; x < numFacesU; ++x)
976                 {
977                         float4 tl = POINT_TO_FLOAT4(topLeft + uEdge * (float)x + vEdge * (float)y);
978                         float4 tr = POINT_TO_FLOAT4(topLeft + uEdge * (float)(x+1) + vEdge * (float)y);
979                         float4 bl = POINT_TO_FLOAT4(topLeft + uEdge * (float)x + vEdge * (float)(y+1));
980                         float4 br = POINT_TO_FLOAT4(topLeft + uEdge * (float)(x+1) + vEdge * (float)(y+1));
981                         int i0 = ccwIsFrontFacing ? i : i+5;
982                         int i1 = ccwIsFrontFacing ? i+5 : i;
983                         vb.Set(i0, VDPosition, tl);
984                         vb.Set(i+1, VDPosition, tr);
985                         vb.Set(i+2, VDPosition, bl);
986                         vb.Set(i+3, VDPosition, bl);
987                         vb.Set(i+4, VDPosition, tr);
988                         vb.Set(i1, VDPosition, br);
989
990                         if (vb.Declaration()->HasType(VDUV))
991                         {
992                                 float4 uvTL((float)x/numFacesU, (float)y/numFacesV, 0.f, 1.f);
993                                 float4 uvTR((float)(x+1)/numFacesU, (float)y/numFacesV, 0.f, 1.f);
994                                 float4 uvBL((float)x/numFacesU, (float)(y+1)/numFacesV, 0.f, 1.f);
995                                 float4 uvBR((float)(x+1)/numFacesU, (float)(y+1)/numFacesV, 0.f, 1.f);
996
997                                 vb.Set(i0, VDUV, uvTL);
998                                 vb.Set(i+1, VDUV, uvTR);
999                                 vb.Set(i+2, VDUV, uvBL);
1000                                 vb.Set(i+3, VDUV, uvBL);
1001                                 vb.Set(i+4, VDUV, uvTR);
1002                                 vb.Set(i1, VDUV, uvBR);
1003                         }
1004
1005                         if (vb.Declaration()->HasType(VDNormal))
1006                         {
1007                                 for(int k = 0; k < 6; ++k)
1008                                         vb.Set(i+k, VDNormal, DIR_TO_FLOAT4(normal));
1009                         }
1010
1011                         i += 6;
1012                 }
1013 }
1014
1015 void Plane::ToLineList(VertexBuffer &vb, float uWidth, float vHeight, const vec &centerPoint, int numLinesU, int numLinesV) const
1016 {
1017         vec topLeft = Point(-uWidth*0.5f, -vHeight *0.5f, centerPoint);
1018         vec uEdge = (Point(uWidth*0.5f, -vHeight *0.5f, centerPoint) - topLeft) / (float)numLinesU;
1019         vec vEdge = (Point(-uWidth*0.5f, vHeight *0.5f, centerPoint) - topLeft) / (float)numLinesV;
1020
1021         int i = vb.AppendVertices((numLinesU + numLinesV) * 2);
1022         for(int y = 0; y < numLinesV; ++y)
1023         {
1024                 float4 start = POINT_TO_FLOAT4(topLeft + vEdge * (float)y);
1025                 float4 end   = POINT_TO_FLOAT4(topLeft + uWidth * uEdge + vEdge * (float)y);
1026                 vb.Set(i, VDPosition, start);
1027                 vb.Set(i+1, VDPosition, end);
1028                 i += 2;
1029         }
1030
1031         for(int x = 0; x < numLinesU; ++x)
1032         {
1033                 float4 start = POINT_TO_FLOAT4(topLeft + uEdge * (float)x);
1034                 float4 end   = POINT_TO_FLOAT4(topLeft + vHeight * vEdge + uEdge * (float)x);
1035                 vb.Set(i, VDPosition, start);
1036                 vb.Set(i+1, VDPosition, end);
1037                 i += 2;
1038         }
1039 }
1040
1041 #endif
1042 MATH_END_NAMESPACE