Frustum.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 Frustum.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Frustum geometry object. */
18 #include "Frustum.h"
19 #include "AABB.h"
20 #include "Circle.h"
21 #include "../Math/MathFunc.h"
22 #include "Plane.h"
23 #include "Line.h"
24 #include "OBB.h"
25 #include "Polyhedron.h"
26 #include "Polygon.h"
27 #include "Ray.h"
28 #include "Sphere.h"
29 #include "Capsule.h"
30 #include "Triangle.h"
31 #include "LineSegment.h"
32 #include "PBVolume.h"
33 #include "../Math/float2.h"
34 #include "../Math/float3x3.h"
35 #include "../Math/float3x4.h"
36 #include "../Math/float4x4.h"
37 #include "../Math/float4.h"
38 #include "../Math/Quat.h"
39 #include "../Algorithm/Random/LCG.h"
40 #include "../Algorithm/GJK.h"
41
42 #ifdef MATH_ENABLE_STL_SUPPORT
43 #include <iostream>
44 #endif
45
46 #if defined(MATH_TINYXML_INTEROP) && defined(MATH_CONTAINERLIB_SUPPORT)
47 #include "Container/UString.h"
48 #endif
49
50 #if defined(MATH_SSE) && defined(MATH_AUTOMATIC_SSE)
51 #include "../Math/float4_sse.h"
52 #endif
53
54 MATH_BEGIN_NAMESPACE
55
56 Frustum::Frustum()
57 :type(InvalidFrustum),
58 pos(vec::nan),
59 front(vec::nan),
60 up(vec::nan),
61 nearPlaneDistance(FLOAT_NAN),
62 farPlaneDistance(FLOAT_NAN),
63 worldMatrix(float3x4::nan),
64 viewProjMatrix(float4x4::nan)
65 {
66         // For conveniency, allow automatic initialization of the graphics API and handedness in use.
67         // If neither of the #defines are set, user must specify per-instance.
68
69 #ifdef MATH_USE_DIRECT3D
70         projectiveSpace = FrustumSpaceD3D;
71 #elif defined(MATH_USE_OPENGL)
72         projectiveSpace = FrustumSpaceGL;
73 #else
74         projectiveSpace = FrustumSpaceInvalid;
75 #endif
76
77 #ifdef MATH_LEFTHANDED_CAMERA
78         handedness = FrustumLeftHanded;
79 #elif defined(MATH_RIGHTHANDED_CAMERA)
80         handedness = FrustumRightHanded;
81 #else
82         handedness = FrustumHandednessInvalid;
83 #endif
84 }
85
86 void Frustum::SetKind(FrustumProjectiveSpace p, FrustumHandedness h)
87 {
88         projectiveSpace = p;
89         handedness = h;
90         if (up.IsFinite())
91                 WorldMatrixChanged(); // Setting handedness affects world matrix.
92         ProjectionMatrixChanged();
93 }
94
95 void Frustum::SetViewPlaneDistances(float n, float f)
96 {
97         nearPlaneDistance = n;
98         farPlaneDistance = f;
99         ProjectionMatrixChanged();
100 }
101
102 void Frustum::SetFrame(const vec &p, const vec &f, const vec &u)
103 {
104         pos = p;
105         front = f;
106         up = u;
107         WorldMatrixChanged();
108 }
109
110 void Frustum::SetPos(const vec &p)
111 {
112         pos = p;
113         WorldMatrixChanged();
114 }
115
116 void Frustum::SetFront(const vec &f)
117 {
118         front = f;
119         WorldMatrixChanged();
120 }
121
122 void Frustum::SetUp(const vec &u)
123 {
124         up = u;
125         WorldMatrixChanged();
126 }
127
128 void Frustum::SetPerspective(float h, float v)
129 {
130         type = PerspectiveFrustum;
131         horizontalFov = h;
132         verticalFov = v;
133         ProjectionMatrixChanged();
134 }
135
136 void Frustum::SetOrthographic(float w, float h)
137 {
138         type = OrthographicFrustum;
139         orthographicWidth = w;
140         orthographicHeight = h;
141         ProjectionMatrixChanged();
142 }
143
144 void Frustum::WorldMatrixChanged()
145 {
146         worldMatrix = ComputeWorldMatrix();
147         float3x4 viewMatrix = worldMatrix;
148         viewMatrix.InverseOrthonormal();
149         viewProjMatrix = projectionMatrix * viewMatrix;
150 }
151
152 void Frustum::ProjectionMatrixChanged()
153 {
154         projectionMatrix = ComputeProjectionMatrix();
155         if (!IsNan(worldMatrix[0][0]))
156         {
157                 float3x4 viewMatrix = worldMatrix;
158                 viewMatrix.InverseOrthonormal();
159                 viewProjMatrix = projectionMatrix * viewMatrix;
160         }
161 }
162
163 float Frustum::AspectRatio() const
164 {
165         return Tan(horizontalFov*0.5f) / Tan(verticalFov*0.5f);
166 }
167
168 void Frustum::SetHorizontalFovAndAspectRatio(float hFov, float aspectRatio)
169 {
170         type = PerspectiveFrustum;
171         horizontalFov = hFov;
172         verticalFov = 2.f * Atan(Tan(hFov*0.5f)/aspectRatio);
173         ProjectionMatrixChanged();
174 }
175
176 void Frustum::SetVerticalFovAndAspectRatio(float vFov, float aspectRatio)
177 {
178         type = PerspectiveFrustum;
179         verticalFov = vFov;
180         horizontalFov = 2.f * Atan(Tan(vFov*0.5f)*aspectRatio);
181         ProjectionMatrixChanged();
182 }
183
184 vec Frustum::WorldRight() const
185 {
186         if (handedness == FrustumRightHanded)
187                 return Cross(front, up);
188         else
189                 return Cross(up, front);
190 }
191
192 float Frustum::NearPlaneWidth() const
193 {
194         if (type == PerspectiveFrustum)
195                 return Tan(horizontalFov*0.5f)*2.f * nearPlaneDistance;
196         else
197                 return orthographicWidth;
198 }
199
200 float Frustum::NearPlaneHeight() const
201 {
202         if (type == PerspectiveFrustum)
203                 return Tan(verticalFov*0.5f)*2.f * nearPlaneDistance;
204         else
205                 return orthographicHeight;
206 }
207
208 Plane Frustum::NearPlane() const
209 {
210         return Plane(pos + front * nearPlaneDistance, -front);
211 }
212
213 Plane Frustum::FarPlane() const
214 {
215         return Plane(pos + front * farPlaneDistance, front);
216 }
217
218 Plane Frustum::LeftPlane() const
219 {
220         if (type == PerspectiveFrustum)
221         {
222                 vec left = -WorldRight();
223                 left.ScaleToLength(Tan(horizontalFov*0.5f));
224                 vec leftSide = front + left;
225                 vec leftSideNormal = ((handedness == FrustumRightHanded) ? Cross(up, leftSide) : Cross(leftSide, up)).Normalized();
226                 return Plane(pos, leftSideNormal);
227         }
228         else
229         {
230                 vec left = -WorldRight();
231                 return Plane(NearPlanePos(-1.f, 0.f), left.Normalized());
232         }
233 }
234
235 Plane Frustum::RightPlane() const
236 {
237         if (type == PerspectiveFrustum)
238         {
239                 vec right = WorldRight();
240                 right.ScaleToLength(Tan(horizontalFov*0.5f));
241                 vec rightSide = front + right;
242                 vec rightSideNormal = ((handedness == FrustumRightHanded) ? Cross(rightSide, up) : Cross(up, rightSide)).Normalized();
243                 return Plane(pos, rightSideNormal);
244         }
245         else
246         {
247                 vec right = WorldRight();
248                 return Plane(NearPlanePos(1.f, 0.f), right.Normalized());
249         }
250 }
251
252 Plane Frustum::TopPlane() const
253 {
254         if (type == PerspectiveFrustum)
255         {
256                 vec topSide = front + Tan(verticalFov * 0.5f) * up;
257                 vec right = WorldRight();
258                 vec topSideNormal = ((handedness == FrustumRightHanded) ? Cross(right, topSide) : Cross(topSide, right)).Normalized();
259                 return Plane(pos, topSideNormal);
260         }
261         else
262         {
263                 return Plane(NearPlanePos(0.f, 1.f), up);
264         }
265 }
266
267 Plane Frustum::BottomPlane() const
268 {
269         if (type == PerspectiveFrustum)
270         {
271                 vec bottomSide = front - Tan(verticalFov * 0.5f) * up;
272                 vec left = -WorldRight();
273                 vec bottomSideNormal = ((handedness == FrustumRightHanded) ? Cross(left, bottomSide) : Cross(bottomSide, left)).Normalized();
274                 return Plane(pos, bottomSideNormal);
275         }
276         else
277         {
278                 return Plane(NearPlanePos(0.f, -1.f), -up);
279         }
280 }
281
282 void Frustum::SetWorldMatrix(const float3x4 &worldTransform)
283 {
284         pos = POINT_VEC(worldTransform.TranslatePart());
285         if (handedness == FrustumRightHanded)
286                 front = -DIR_VEC(worldTransform.Col(2)); // The camera looks towards -Z axis of the given transform.
287         else
288                 front = DIR_VEC(worldTransform.Col(2)); // The camera looks towards +Z axis of the given transform.
289         up = DIR_VEC(worldTransform.Col(1)); // The camera up points towards +Y of the given transform.
290         assume1(pos.IsFinite(), pos);
291         assume2(up.IsNormalized(1e-3f), up, up.Length());
292         assume2(front.IsNormalized(1e-3f), front, front.Length());
293         assume3(up.IsPerpendicular(front), up, front, up.Dot(front));
294         assume(worldTransform.IsColOrthogonal3()); // Front and up must be orthogonal to each other.
295         assume(EqualAbs(worldTransform.Determinant(), 1.f)); // The matrix cannot contain mirroring.
296
297         // Finally store the passed world matrix as the cached matrix.
298         worldMatrix = worldTransform;
299 }
300
301 float3x4 Frustum::ComputeWorldMatrix() const
302 {
303         assume1(pos.IsFinite(), pos);
304         assume2(up.IsNormalized(1e-3f), up, up.Length());
305         assume2(front.IsNormalized(1e-3f), front, front.Length());
306         assume3(up.IsPerpendicular(front), up, front, up.Dot(front));
307         float3x4 m;
308         m.SetCol(0, DIR_TO_FLOAT3(WorldRight().Normalized()));
309         m.SetCol(1, DIR_TO_FLOAT3(up));
310         if (handedness == FrustumRightHanded)
311                 m.SetCol(2, DIR_TO_FLOAT3(-front)); // In right-handed convention, the -Z axis must map towards the front vector. (so +Z maps to -front)
312         else
313                 m.SetCol(2, DIR_TO_FLOAT3(front)); // In left-handed convention, the +Z axis must map towards the front vector.
314         m.SetCol(3, POINT_TO_FLOAT3(pos));
315         assume(!m.HasNegativeScale());
316         return m;
317 }
318
319 float3x4 Frustum::ComputeViewMatrix() const
320 {
321         float3x4 world = ComputeWorldMatrix();
322         world.InverseOrthonormal();
323         return world;
324 }
325
326 float4x4 Frustum::ComputeViewProjMatrix() const
327 {
328         return ComputeProjectionMatrix() * ComputeViewMatrix();
329 }
330
331 float4x4 Frustum::ComputeProjectionMatrix() const
332 {
333         if (type == InvalidFrustum || projectiveSpace == FrustumSpaceInvalid)
334                 return float4x4::nan;
335         assume(type == PerspectiveFrustum || type == OrthographicFrustum);
336         assume(projectiveSpace == FrustumSpaceGL || projectiveSpace == FrustumSpaceD3D);
337         assume(handedness == FrustumLeftHanded || handedness == FrustumRightHanded);
338         if (type == PerspectiveFrustum)
339         {
340                 if (projectiveSpace == FrustumSpaceGL)
341                 {
342                         if (handedness == FrustumRightHanded)
343                                 return float4x4::OpenGLPerspProjRH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
344                         else if (handedness == FrustumLeftHanded)
345                                 return float4x4::OpenGLPerspProjLH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
346                 }
347                 else if (projectiveSpace == FrustumSpaceD3D)
348                 {
349                         if (handedness == FrustumRightHanded)
350                                 return float4x4::D3DPerspProjRH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
351                         else if (handedness == FrustumLeftHanded)
352                                 return float4x4::D3DPerspProjLH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
353                 }
354         }
355         else if (type == OrthographicFrustum)
356         {
357                 if (projectiveSpace == FrustumSpaceGL)
358                 {
359                         if (handedness == FrustumRightHanded)
360                                 return float4x4::OpenGLOrthoProjRH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
361                         else if (handedness == FrustumLeftHanded)
362                                 return float4x4::OpenGLOrthoProjLH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
363                 }
364                 else if (projectiveSpace == FrustumSpaceD3D)
365                 {
366                         if (handedness == FrustumRightHanded)
367                                 return float4x4::D3DOrthoProjRH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
368                         else if (handedness == FrustumLeftHanded)
369                                 return float4x4::D3DOrthoProjLH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
370                 }
371         }
372 #ifndef OPTIMIZED_RELEASE
373         LOGE("Not all values of Frustum were initialized properly! Please initialize correctly before calling Frustum::ProjectionMatrix()!");
374 #endif
375         return float4x4::nan;
376 }
377
378 vec Frustum::NearPlanePos(float x, float y) const
379 {
380         assume(type == PerspectiveFrustum || type == OrthographicFrustum);
381
382         if (type == PerspectiveFrustum)
383         {
384                 float frontPlaneHalfWidth = Tan(horizontalFov*0.5f)*nearPlaneDistance;
385                 float frontPlaneHalfHeight = Tan(verticalFov*0.5f)*nearPlaneDistance;
386                 x = x * frontPlaneHalfWidth; // Map [-1,1] to [-width/2, width/2].
387                 y = y * frontPlaneHalfHeight;  // Map [-1,1] to [-height/2, height/2].
388                 vec right = WorldRight();
389                 return pos + front * nearPlaneDistance + x * right + y * up;
390         }
391         else
392         {
393                 vec right = WorldRight();
394                 return pos + front * nearPlaneDistance
395                                    + x * orthographicWidth * 0.5f * right
396                                    + y * orthographicHeight * 0.5f * up;
397         }
398 }
399
400 vec Frustum::NearPlanePos(const float2 &point) const
401 {
402         return NearPlanePos(point.x, point.y);
403 }
404
405 vec Frustum::FarPlanePos(float x, float y) const
406 {
407         assume(type == PerspectiveFrustum || type == OrthographicFrustum);
408
409         if (type == PerspectiveFrustum)
410         {
411                 float farPlaneHalfWidth = Tan(horizontalFov*0.5f)*farPlaneDistance;
412                 float farPlaneHalfHeight = Tan(verticalFov*0.5f)*farPlaneDistance;
413                 x = x * farPlaneHalfWidth;
414                 y = y * farPlaneHalfHeight;
415                 vec right = WorldRight();
416                 return pos + front * farPlaneDistance + x * right + y * up;
417         }
418         else
419         {
420                 vec right = WorldRight();
421                 return pos + front * farPlaneDistance
422                                    + x * orthographicWidth * 0.5f * right
423                                    + y * orthographicHeight * 0.5f * up;
424         }
425 }
426
427 vec Frustum::FarPlanePos(const float2 &point) const
428 {
429         return FarPlanePos(point.x, point.y);
430 }
431
432 float2 Frustum::ViewportToScreenSpace(float x, float y, int screenWidth, int screenHeight)
433 {
434         return float2((x + 1.f) * 0.5f * (screenWidth-1.f), (1.f - y) * 0.5f * (screenHeight-1.f));
435 }
436
437 float2 Frustum::ViewportToScreenSpace(const float2 &point, int screenWidth, int screenHeight)
438 {
439         return ViewportToScreenSpace(point.x, point.y, screenWidth, screenHeight);
440 }
441
442 float2 Frustum::ScreenToViewportSpace(float x, float y, int screenWidth, int screenHeight)
443 {
444         return float2(x * 2.f / (screenWidth-1.f) - 1.f, 1.f - y * 2.f / (screenHeight - 1.f));
445 }
446
447 float2 Frustum::ScreenToViewportSpace(const float2 &point, int screenWidth, int screenHeight)
448 {
449         return ScreenToViewportSpace(point.x, point.y, screenWidth, screenHeight);
450 }
451
452 Ray Frustum::UnProject(float x, float y) const
453 {
454         assume1(x >= -1.f, x);
455         assume1(x <= 1.f, x);
456         assume1(y >= -1.f, y);
457         assume1(y <= 1.f, y);
458         if (type == PerspectiveFrustum)
459         {
460                 vec nearPlanePos = NearPlanePos(x, y);
461                 return Ray(pos, (nearPlanePos - pos).Normalized());
462         }
463         else
464                 return UnProjectFromNearPlane(x, y);
465 }
466
467 LineSegment Frustum::UnProjectLineSegment(float x, float y) const
468 {
469         vec nearPlanePos = NearPlanePos(x, y);
470         vec farPlanePos = FarPlanePos(x, y);
471         return LineSegment(nearPlanePos, farPlanePos);
472 }
473
474 Ray Frustum::UnProjectFromNearPlane(float x, float y) const
475 {
476         return UnProjectLineSegment(x, y).ToRay();
477 }
478
479 vec Frustum::PointInside(float x, float y, float z) const
480 {
481         assume(z >= 0.f);
482         assume(z <= 1.f);
483         return UnProjectLineSegment(x, y).GetPoint(z);
484 }
485
486 vec Frustum::Project(const vec &point) const
487 {
488         float4 projectedPoint = ViewProjMatrix().Mul(POINT_TO_FLOAT4(point));
489         projectedPoint.NormalizeW(); // Post-projective perspective divide.
490         return FLOAT4_TO_POINT(projectedPoint);
491 }
492
493 bool Frustum::Contains(const vec &point) const
494 {
495 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
496         // SSE 4.1 32-bit: Best: 6.913 nsecs / 18.52 ticks, Avg: 6.978 nsecs, Worst: 7.297 nsecs
497         vec projected = Project(point);
498         simd4f a = abs_ps(projected);
499         simd4f y = yyyy_ps(a);
500         a = max_ps(a, y);
501         y = zzzz_ps(a);
502         a = max_ps(a, y);
503         const float eps = 1e-3f;
504         return _mm_cvtss_f32(a) <= 1.f + eps &&
505                 (projectiveSpace == FrustumSpaceGL || s4f_z(projected) >= -eps);
506 #else
507         // SSE 4.1 32-bit: Best: 7.681 nsecs / 21.096 ticks, Avg : 7.954 nsecs, Worst : 9.217 nsecs
508         const float eps = 1e-3f;
509         const float pos = 1.f + eps;
510         const float neg = -pos;
511         vec projected = Project(point);
512         if (projectiveSpace == FrustumSpaceD3D)
513         {
514                 return neg <= projected.x && projected.x <= pos &&
515                         neg <= projected.y && projected.y <= pos &&
516                         -eps <= projected.z && projected.z <= pos;
517         }
518         else if (projectiveSpace == FrustumSpaceGL)
519         {
520                 return neg <= projected.x && projected.x <= pos &&
521                         neg <= projected.y && projected.y <= pos &&
522                         neg <= projected.z && projected.z <= pos;
523         }
524         else
525         {
526 #ifndef OPTIMIZED_RELEASE
527                 ///\todo Make Frustum::Contains agnostic of the projection settings.
528                 LOGE("Not all values of Frustum were initialized properly! Please initialize correctly before calling Frustum::Contains()!");
529 #endif
530                 return false;
531         }
532 #endif
533 }
534
535 bool Frustum::Contains(const LineSegment &lineSegment) const
536 {
537 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
538         // SSE 4.1: Best: 15.746 nsecs / 42.152 ticks, Avg: 15.842 nsecs, Worst: 16.130 nsecs
539         vec pa = Project(lineSegment.a);
540         simd4f a = abs_ps(pa);
541         vec pb = Project(lineSegment.b);
542         simd4f b = abs_ps(pb);
543         a = max_ps(a, b);
544         simd4f y = yyyy_ps(a);
545         a = max_ps(a, y);
546         y = zzzz_ps(a);
547         a = max_ps(a, y);
548         const float eps = 1e-3f;
549         return _mm_cvtss_f32(a) <= 1.f + eps &&
550                 (projectiveSpace == FrustumSpaceGL || s4f_z(min_ps(pa, pb)) >= -eps);
551 #else
552         // SSE 4.1: 16.514 nsecs / 44.784 ticks, Avg: 16.825 nsecs, Worst: 16.898 nsecs
553         return Contains(lineSegment.a) && Contains(lineSegment.b);
554 #endif
555 }
556
557 bool Frustum::Contains(const Triangle &triangle) const
558 {
559 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
560         // Best: 21.206 nsecs / 55.496 ticks, Avg: 21.702 nsecs, Worst: 21.891 nsecs
561         vec pa = Project(triangle.a);
562         simd4f a = abs_ps(pa);
563         vec pb = Project(triangle.b);
564         simd4f b = abs_ps(pb);
565         a = max_ps(a, b);
566         vec pc = Project(triangle.c);
567         simd4f c = abs_ps(pc);
568         a = max_ps(a, c);
569         simd4f y = yyyy_ps(a);
570         a = max_ps(a, y);
571         y = zzzz_ps(a);
572         a = max_ps(a, y);
573         const float eps = 1e-3f;
574         return _mm_cvtss_f32(a) <= 1.f + eps &&
575                 (projectiveSpace == FrustumSpaceGL || s4f_z(min_ps(min_ps(pa, pb), pc)) >= -eps);
576 #else
577         // Best: 21.122 nsecs / 56.512 ticks, Avg: 21.226 nsecs, Worst: 21.506 nsecs
578         return Contains(triangle.a) && Contains(triangle.b) && Contains(triangle.c);
579 #endif
580 }
581
582 bool Frustum::Contains(const Polygon &polygon) const
583 {
584         for(int i = 0; i < polygon.NumVertices(); ++i)
585                 if (!Contains(polygon.Vertex(i)))
586                         return false;
587         return true;
588 }
589
590 bool Frustum::Contains(const AABB &aabb) const
591 {
592         for(int i = 0; i < 8; ++i)
593                 if (!Contains(aabb.CornerPoint(i)))
594                         return false;
595
596         return true;
597 }
598
599 bool Frustum::Contains(const OBB &obb) const
600 {
601         for(int i = 0; i < 8; ++i)
602                 if (!Contains(obb.CornerPoint(i)))
603                         return false;
604
605         return true;
606 }
607
608 bool Frustum::Contains(const Frustum &frustum) const
609 {
610         for(int i = 0; i < 8; ++i)
611                 if (!Contains(frustum.CornerPoint(i)))
612                         return false;
613
614         return true;
615 }
616
617 bool Frustum::Contains(const Polyhedron &polyhedron) const
618 {
619         assume(polyhedron.IsClosed());
620         for(int i = 0; i < polyhedron.NumVertices(); ++i)
621                 if (!Contains(polyhedron.Vertex(i)))
622                         return false;
623
624         return true;
625 }
626
627 vec Frustum::ClosestPoint(const vec &point) const
628 {
629         if (type == OrthographicFrustum)
630         {
631                 float frontHalfSize = (farPlaneDistance - nearPlaneDistance) * 0.5f;
632                 float halfWidth = orthographicWidth * 0.5f;
633                 float halfHeight = orthographicHeight * 0.5f;
634                 vec frustumCenter = pos + (frontHalfSize + nearPlaneDistance) * front;
635                 vec right = Cross(front, up);
636                 assert(right.IsNormalized());
637                 vec d = point - frustumCenter;
638                 vec closestPoint = frustumCenter;
639 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
640                 // Best: 21.506 nsecs / 57.224 ticks, Avg: 21.683 nsecs, Worst: 22.659 nsecs
641                 simd4f z = set1_ps(frontHalfSize);
642                 closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d.v, front.v), z), negate_ps(z)), front.v));
643                 simd4f y = set1_ps(halfHeight);
644                 closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d.v, up.v), y), negate_ps(y)), up.v));
645                 simd4f x = set1_ps(halfWidth);
646                 closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d.v, right.v), x), negate_ps(x)), right.v));
647 #else
648                 // Best: 30.724 nsecs / 82.192 ticks, Avg: 31.069 nsecs, Worst: 39.172 nsecs
649                 closestPoint += Clamp(Dot(d, front), -frontHalfSize, frontHalfSize) * front;
650                 closestPoint += Clamp(Dot(d, right), -halfWidth, halfWidth) * right;
651                 closestPoint += Clamp(Dot(d, up), -halfHeight, halfHeight) * up;
652 #endif
653                 return closestPoint;
654         }
655         else
656         {
657                 // Best: 12.339 usecs / 32900.7 ticks, Avg: 12.521 usecs, Worst: 13.308 usecs
658                 return ToPolyhedron().ClosestPoint(point);
659         }
660
661 ///\todo Improve numerical stability enough to do effectively this - but do so without temporary memory allocations.
662 //      return ToPolyhedron().ClosestPointConvex(point);
663 }
664
665 float Frustum::Distance(const vec &point) const
666 {
667         vec pt = ClosestPoint(point);
668         return pt.Distance(point);
669 }
670
671 bool Frustum::IsFinite() const
672 {
673         return pos.IsFinite() && front.IsFinite() && up.IsFinite() && MATH_NS::IsFinite(nearPlaneDistance)
674                 && MATH_NS::IsFinite(farPlaneDistance) && MATH_NS::IsFinite(horizontalFov) && MATH_NS::IsFinite(verticalFov);
675 }
676
677 Plane Frustum::GetPlane(int faceIndex) const
678 {
679         assume(0 <= faceIndex && faceIndex <= 5);
680         switch(faceIndex)
681         {
682                 default: // For release builds where assume() is disabled, always return the first option if out-of-bounds.
683                 case 0: return NearPlane();
684                 case 1: return FarPlane();
685                 case 2: return LeftPlane();
686                 case 3: return RightPlane();
687                 case 4: return TopPlane();
688                 case 5: return BottomPlane();
689         }
690 }
691
692 float Frustum::Volume() const
693 {
694         return ToPolyhedron().Volume();
695 }
696
697 vec Frustum::FastRandomPointInside(LCG &rng) const
698 {
699         float f1 = rng.Float(-1.f, 1.f);
700         float f2 = rng.Float(-1.f, 1.f);
701         float f3 = rng.Float(0.f, 1.f);
702         return PointInside(f1, f2, f3);
703 }
704
705 vec Frustum::UniformRandomPointInside(LCG &rng) const
706 {
707         if (type == OrthographicFrustum)
708                 return FastRandomPointInside(rng);
709         else
710         {
711                 OBB o = MinimalEnclosingOBB();
712                 for(int numTries = 0; numTries < 1000; ++numTries)
713                 {
714                         vec pt = o.RandomPointInside(rng);
715                         if (Contains(pt))
716                                 return pt;
717                 }
718                 LOGW("Rejection sampling failed in Frustum::UniformRandomPointInside! Producing a non-uniformly distributed point inside the frustum!");
719                 return FastRandomPointInside(rng);
720         }
721 }
722
723 void Frustum::Translate(const vec &offset)
724 {
725         pos += offset;
726 }
727
728 void Frustum::Transform(const float3x3 &transform)
729 {
730         assume(transform.HasUniformScale());
731         pos = transform * pos;
732         front = transform * front;
733         float scaleFactor = front.Normalize();
734         up = (transform * up).Normalized();
735         nearPlaneDistance *= scaleFactor;
736         farPlaneDistance *= scaleFactor;
737         if (type == OrthographicFrustum)
738         {
739                 orthographicWidth *= scaleFactor;
740                 orthographicHeight *= scaleFactor;
741         }
742 }
743
744 void Frustum::Transform(const float3x4 &transform)
745 {
746         assume(transform.HasUniformScale());
747         pos = transform.MulPos(pos);
748         front = transform.MulDir(front);
749         float scaleFactor = front.Normalize();
750         up = transform.MulDir(up).Normalized();
751         nearPlaneDistance *= scaleFactor;
752         farPlaneDistance *= scaleFactor;
753         if (type == OrthographicFrustum)
754         {
755                 orthographicWidth *= scaleFactor;
756                 orthographicHeight *= scaleFactor;
757         }
758 }
759
760 void Frustum::Transform(const float4x4 &transform)
761 {
762         assume(transform.Row(3).Equals(0,0,0,1));
763         Transform(transform.Float3x4Part());
764 }
765
766 void Frustum::Transform(const Quat &transform)
767 {
768         Transform(transform.ToFloat3x3());
769 }
770
771 void Frustum::GetPlanes(Plane *outArray) const
772 {
773         assume(outArray);
774 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
775         if (!outArray)
776                 return;
777 #endif
778         for(int i = 0; i < 6; ++i)
779                 outArray[i] = GetPlane(i);
780 }
781
782 vec Frustum::CenterPoint() const
783 {
784         return pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
785 }
786
787 void Frustum::GetCornerPoints(vec *outPointArray) const
788 {
789         assume(outPointArray);
790 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
791         if (!outPointArray)
792                 return;
793 #endif
794
795         if (type == PerspectiveFrustum)
796         {
797                 float tanhfov = Tan(horizontalFov*0.5f);
798                 float tanvfov = Tan(verticalFov*0.5f);
799                 float frontPlaneHalfWidth = tanhfov*nearPlaneDistance;
800                 float frontPlaneHalfHeight = tanvfov*nearPlaneDistance;
801                 float farPlaneHalfWidth = tanhfov*farPlaneDistance;
802                 float farPlaneHalfHeight = tanvfov*farPlaneDistance;
803
804                 vec right = WorldRight();
805
806                 vec nearCenter = pos + front * nearPlaneDistance;
807                 vec nearHalfWidth = frontPlaneHalfWidth*right;
808                 vec nearHalfHeight = frontPlaneHalfHeight*up;
809                 outPointArray[0] = nearCenter - nearHalfWidth - nearHalfHeight;
810                 outPointArray[1] = nearCenter + nearHalfWidth - nearHalfHeight;
811                 outPointArray[2] = nearCenter - nearHalfWidth + nearHalfHeight;
812                 outPointArray[3] = nearCenter + nearHalfWidth + nearHalfHeight;
813
814                 vec farCenter = pos + front * farPlaneDistance;
815                 vec farHalfWidth = farPlaneHalfWidth*right;
816                 vec farHalfHeight = farPlaneHalfHeight*up;
817                 outPointArray[4] = farCenter - farHalfWidth - farHalfHeight;
818                 outPointArray[5] = farCenter + farHalfWidth - farHalfHeight;
819                 outPointArray[6] = farCenter - farHalfWidth + farHalfHeight;
820                 outPointArray[7] = farCenter + farHalfWidth + farHalfHeight;
821         }
822         else
823         {
824                 vec right = WorldRight();
825                 vec nearCenter = pos + front * nearPlaneDistance;
826                 vec farCenter = pos + front * farPlaneDistance;
827                 vec halfWidth = orthographicWidth * 0.5f * right;
828                 vec halfHeight = orthographicHeight * 0.5f * up;
829
830                 outPointArray[0] = nearCenter - halfWidth - halfHeight;
831                 outPointArray[1] = nearCenter + halfWidth - halfHeight;
832                 outPointArray[2] = nearCenter - halfWidth + halfHeight;
833                 outPointArray[3] = nearCenter + halfWidth + halfHeight;
834                 outPointArray[4] = farCenter - halfWidth - halfHeight;
835                 outPointArray[5] = farCenter + halfWidth - halfHeight;
836                 outPointArray[6] = farCenter - halfWidth + halfHeight;
837                 outPointArray[7] = farCenter + halfWidth + halfHeight;
838         }
839 }
840
841 LineSegment Frustum::Edge(int edgeIndex) const
842 {
843         assume(0 <= edgeIndex && edgeIndex <= 11);
844         switch(edgeIndex)
845         {
846                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
847                 case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
848                 case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
849                 case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
850                 case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
851                 case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
852                 case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
853                 case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
854                 case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
855                 case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
856                 case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
857                 case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
858                 case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
859         }
860 }
861
862 vec Frustum::CornerPoint(int cornerIndex) const
863 {
864         assume(0 <= cornerIndex && cornerIndex <= 7);
865         switch(cornerIndex)
866         {
867                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
868                 case 0: return NearPlanePos(-1, -1);
869                 case 1: return FarPlanePos(-1, -1);
870                 case 2: return NearPlanePos(-1, 1);
871                 case 3: return FarPlanePos(-1, 1);
872                 case 4: return NearPlanePos(1, -1);
873                 case 5: return FarPlanePos(1, -1);
874                 case 6: return NearPlanePos(1, 1);
875                 case 7: return FarPlanePos(1, 1);
876         }
877 }
878
879 vec Frustum::ExtremePoint(const vec &direction, float &projectionDistance) const
880 {
881         vec corners[8];
882         GetCornerPoints(corners);
883         vec mostExtreme = vec::nan;
884         projectionDistance = -FLOAT_INF;
885         for(int i = 0; i < 8; ++i)
886         {
887                 float d = Dot(direction, corners[i]);
888                 if (d > projectionDistance)
889                 {
890                         projectionDistance = d;
891                         mostExtreme = corners[i];
892                 }
893         }
894         return mostExtreme;
895 }
896
897 void Frustum::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
898 {
899         vec corners[8];
900         GetCornerPoints(corners);
901         outMax = -FLOAT_INF;
902         outMin = FLOAT_INF;
903         for(int i = 0; i < 8; ++i)
904         {
905                 float d = Dot(direction, corners[i]);
906                 outMax = Max(outMax, d);
907                 outMin = Min(outMin, d);
908         }
909 }
910
911 int Frustum::UniqueFaceNormals(vec *out) const
912 {
913         if (type == PerspectiveFrustum)
914         {
915                 out[0] = front;
916                 out[1] = LeftPlane().normal;
917                 out[2] = RightPlane().normal;
918                 out[3] = TopPlane().normal;
919                 out[4] = BottomPlane().normal;
920                 return 5;
921         }
922         else
923         {
924                 out[0] = front;
925                 out[1] = up;
926                 out[2] = Cross(front, up);
927                 return 3;
928         }
929 }
930
931 int Frustum::UniqueEdgeDirections(vec *out) const
932 {
933         if (type == PerspectiveFrustum)
934         {
935                 out[0] = NearPlanePos(-1, -1) - NearPlanePos(1, -1);
936                 out[1] = NearPlanePos(-1, -1) - NearPlanePos(-1, 1);
937                 out[2] = FarPlanePos(-1, -1) - NearPlanePos(-1, -1);
938                 out[3] = FarPlanePos( 1, -1) - NearPlanePos( 1, -1);
939                 out[4] = FarPlanePos(-1,  1) - NearPlanePos(-1,  1);
940                 out[5] = FarPlanePos( 1,  1) - NearPlanePos( 1,  1);
941                 return 6;
942         }
943         else
944         {
945                 out[0] = front;
946                 out[1] = up;
947                 out[2] = Cross(front, up);
948                 return 3;
949         }
950 }
951
952 AABB Frustum::MinimalEnclosingAABB() const
953 {
954         AABB aabb;
955         aabb.SetNegativeInfinity();
956         for(int i = 0; i < 8; ++i)
957                 aabb.Enclose(CornerPoint(i));
958         return aabb;
959 }
960
961 OBB Frustum::MinimalEnclosingOBB(float expandGuardband) const
962 {
963         assume(IsFinite());
964         assume(front.IsNormalized());
965         assume(up.IsNormalized());
966
967         OBB obb;
968         obb.pos = pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
969         obb.axis[1] = up;
970         obb.axis[2] = -front;
971         obb.axis[0] = Cross(obb.axis[1], obb.axis[2]);
972         obb.r = vec::zero;
973         for(int i = 0; i < 8; ++i)
974                 obb.Enclose(CornerPoint(i));
975
976         // Expand the generated OBB very slightly to avoid numerical issues when
977         // testing whether this Frustum actually is contained inside the generated OBB.
978         obb.r.x += expandGuardband;
979         obb.r.y += expandGuardband;
980         obb.r.z += expandGuardband;
981         return obb;
982 }
983
984 Polyhedron Frustum::ToPolyhedron() const
985 {
986         // Note to maintainer: This function is an exact copy of AABB:ToPolyhedron() and OBB::ToPolyhedron().
987
988         Polyhedron p;
989         // Populate the corners of this Frustum.
990         // The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
991         for(int i = 0; i < 8; ++i)
992                 p.v.push_back(CornerPoint(i));
993
994         // Generate the 6 faces of this Frustum. The function Frustum::GetPlane() has a convention of returning
995         // the planes in order near,far,left,right,top,bottom, so follow the same convention here.
996         const int faces[6][4] =
997         {
998                 { 0, 4, 6, 2 }, // Z-: near plane
999                 { 1, 3, 7, 5 }, // Z+: far plane
1000                 { 0, 2, 3, 1 }, // X-: left plane
1001                 { 4, 5, 7, 6 }, // X+: right plane
1002                 { 7, 3, 2, 6 }, // Y+: top plane
1003                 { 0, 1, 5, 4 }, // Y-: bottom plane
1004         };
1005
1006         for(int f = 0; f < 6; ++f)
1007         {
1008                 Polyhedron::Face face;
1009                 if (this->handedness == FrustumLeftHanded)
1010                 {
1011                         for(int v = 0; v < 4; ++v)
1012                                 face.v.push_back(faces[f][3-v]);
1013                 }
1014                 else
1015                 {
1016                         for(int v = 0; v < 4; ++v)
1017                                 face.v.push_back(faces[f][v]);
1018                 }
1019                 p.f.push_back(face);
1020         }
1021
1022         return p;
1023 }
1024
1025 PBVolume<6> Frustum::ToPBVolume() const
1026 {
1027         PBVolume<6> frustumVolume;
1028         frustumVolume.p[0] = NearPlane();
1029         frustumVolume.p[1] = LeftPlane();
1030         frustumVolume.p[2] = RightPlane();
1031         frustumVolume.p[3] = TopPlane();
1032         frustumVolume.p[4] = BottomPlane();
1033         frustumVolume.p[5] = FarPlane();
1034
1035         return frustumVolume;
1036 }
1037
1038 bool Frustum::Intersects(const Ray &ray) const
1039 {
1040         ///@todo This is a naive test. Implement a faster version.
1041         return this->ToPolyhedron().Intersects(ray);
1042 }
1043
1044 bool Frustum::Intersects(const Line &line) const
1045 {
1046         ///@todo This is a naive test. Implement a faster version.
1047         return this->ToPolyhedron().Intersects(line);
1048 }
1049
1050 bool Frustum::Intersects(const LineSegment &lineSegment) const
1051 {
1052         return GJKIntersect(*this, lineSegment);
1053 }
1054
1055 bool Frustum::Intersects(const AABB &aabb) const
1056 {
1057         return GJKIntersect(*this, aabb);
1058 }
1059
1060 bool Frustum::Intersects(const OBB &obb) const
1061 {
1062         return GJKIntersect(*this, obb);
1063 }
1064
1065 bool Frustum::Intersects(const Plane &plane) const
1066 {
1067         return plane.Intersects(*this);
1068 }
1069
1070 bool Frustum::Intersects(const Triangle &triangle) const
1071 {
1072         return GJKIntersect(*this, triangle);
1073 }
1074
1075 bool Frustum::Intersects(const Polygon &polygon) const
1076 {
1077         return polygon.Intersects(*this);
1078 }
1079
1080 bool Frustum::Intersects(const Sphere &sphere) const
1081 {
1082         return GJKIntersect(*this, sphere);
1083 }
1084
1085 bool Frustum::Intersects(const Capsule &capsule) const
1086 {
1087         return GJKIntersect(*this, capsule);
1088 }
1089
1090 bool Frustum::Intersects(const Frustum &frustum) const
1091 {
1092         return GJKIntersect(*this, frustum);
1093 }
1094
1095 bool Frustum::Intersects(const Polyhedron &polyhedron) const
1096 {
1097         return this->ToPolyhedron().Intersects(polyhedron);
1098 }
1099
1100 #if defined(MATH_TINYXML_INTEROP) && defined(MATH_CONTAINERLIB_SUPPORT)
1101
1102 void Frustum::DeserializeFromXml(TiXmlElement *e)
1103 {
1104         type = StrCaseEq(e->Attribute("orthographic"), "true") ? OrthographicFrustum : PerspectiveFrustum;
1105         pos = POINT_VEC(float3::FromString(e->Attribute("pos")));
1106         front = DIR_VEC(float3::FromString(e->Attribute("front")));
1107         up = DIR_VEC(float3::FromString(e->Attribute("up")));
1108         e->QueryFloatAttribute("nearPlaneDistance", &nearPlaneDistance);
1109         e->QueryFloatAttribute("farPlaneDistance", &farPlaneDistance);
1110         e->QueryFloatAttribute("horizontalFov", &horizontalFov);
1111         e->QueryFloatAttribute("verticalFov", &verticalFov);
1112 }
1113
1114 #endif
1115
1116 #ifdef MATH_ENABLE_STL_SUPPORT
1117
1118 std::string FrustumTypeToString(FrustumType t)
1119 {
1120         if (t == InvalidFrustum) return "InvalidFrustum";
1121         if (t == OrthographicFrustum) return "OrthographicFrustum";
1122         if (t == PerspectiveFrustum) return "PerspectiveFrustum";
1123         return "(invalid frustum type)";
1124 }
1125
1126 std::string Frustum::ToString() const
1127 {
1128         char str[256];
1129         sprintf(str, "Frustum(%s pos:(%.2f, %.2f, %.2f) front:(%.2f, %.2f, %.2f), up:(%.2f, %.2f, %.2f), near: %.2f, far: %.2f, %s: %.2f, %s: %.2f)",
1130                 FrustumTypeToString(type).c_str(), pos.x, pos.y, pos.z, front.x, front.y, front.z,
1131                 up.x, up.y, up.z, nearPlaneDistance, farPlaneDistance,
1132                 type == OrthographicFrustum ? "ortho width:" : "hFov",
1133                 horizontalFov,
1134                 type == OrthographicFrustum ? "ortho height:" : "vFov",
1135                 verticalFov);
1136         return str;
1137 }
1138
1139 std::ostream &operator <<(std::ostream &o, const Frustum &frustum)
1140 {
1141         o << frustum.ToString();
1142         return o;
1143 }
1144
1145 #endif
1146
1147 Frustum operator *(const float3x3 &transform, const Frustum &frustum)
1148 {
1149         Frustum f(frustum);
1150         f.Transform(transform);
1151         return f;
1152 }
1153
1154 Frustum operator *(const float3x4 &transform, const Frustum &frustum)
1155 {
1156         Frustum f(frustum);
1157         f.Transform(transform);
1158         return f;
1159 }
1160
1161 Frustum operator *(const float4x4 &transform, const Frustum &frustum)
1162 {
1163         Frustum f(frustum);
1164         f.Transform(transform);
1165         return f;
1166 }
1167
1168 Frustum operator *(const Quat &transform, const Frustum &frustum)
1169 {
1170         Frustum f(frustum);
1171         f.Transform(transform);
1172         return f;
1173 }
1174
1175 MATH_END_NAMESPACE