OBB.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 OBB.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Oriented Bounding Box (OBB) geometry object. */
18 #include "OBB.h"
19 #ifdef MATH_ENABLE_STL_SUPPORT
20 #include <iostream>
21 #include <utility>
22 #endif
23 #include "../Math/MathFunc.h"
24 #include "AABB.h"
25 #include "Frustum.h"
26 #include "../Algorithm/Random/LCG.h"
27 #include "LineSegment.h"
28 #include "Line.h"
29 #include "Plane.h"
30 #include "Polygon.h"
31 #include "Polyhedron.h"
32 #include "Sphere.h"
33 #include "Capsule.h"
34 #include "../Math/float3x3.h"
35 #include "../Math/float3x4.h"
36 #include "../Math/float4.h"
37 #include "../Math/float4x4.h"
38 #include "../Math/Quat.h"
39 #include "PBVolume.h"
40 #include "Ray.h"
41 #include "Triangle.h"
42 #include <stdlib.h>
43
44 #ifdef MATH_CONTAINERLIB_SUPPORT
45 #include "Algorithm/Sort/Sort.h"
46 #endif
47
48 #ifdef MATH_GRAPHICSENGINE_INTEROP
49 #include "VertexBuffer.h"
50 #endif
51
52 #if defined(MATH_SSE) && defined(MATH_AUTOMATIC_SSE)
53 #include "../Math/float4_sse.h"
54 #include "../Math/float4x4_sse.h"
55 #endif
56
57 MATH_BEGIN_NAMESPACE
58
59 OBB::OBB(const vec &pos, const vec &r, const vec &axis0, const vec &axis1, const vec &axis2)
60 :pos(pos), r(r)
61 {
62         axis[0] = axis0;
63         axis[1] = axis1;
64         axis[2] = axis2;
65 }
66
67 OBB::OBB(const AABB &aabb)
68 {
69         SetFrom(aabb);
70 }
71
72 void OBB::SetNegativeInfinity()
73 {
74         pos = POINT_VEC_SCALAR(0.f);
75         r.SetFromScalar(-FLOAT_INF);
76         axis[0] = DIR_VEC(1,0,0);
77         axis[1] = DIR_VEC(0, 1, 0);
78         axis[2] = DIR_VEC(0, 0, 1);
79 }
80
81 void OBB::SetFrom(const AABB &aabb)
82 {
83         pos = aabb.CenterPoint();
84         r = aabb.HalfSize();
85         axis[0] = DIR_VEC(1, 0, 0);
86         axis[1] = DIR_VEC(0, 1, 0);
87         axis[2] = DIR_VEC(0, 0, 1);
88 }
89
90 template<typename Matrix>
91 void OBBSetFrom(OBB &obb, const AABB &aabb, const Matrix &m)
92 {
93         assume(m.IsColOrthogonal()); // We cannot convert transform an AABB to OBB if it gets sheared in the process.
94         assume(m.HasUniformScale()); // Nonuniform scale will produce shear as well.
95         obb.pos = m.MulPos(aabb.CenterPoint());
96         obb.r = aabb.HalfSize();
97         obb.axis[0] = DIR_VEC(m.Col(0));
98         obb.axis[1] = DIR_VEC(m.Col(1));
99         obb.axis[2] = DIR_VEC(m.Col(2));
100         // If the matrix m contains scaling, propagate the scaling from the axis vectors to the half-length vectors,
101         // since we want to keep the axis vectors always normalized in our representation.
102         float matrixScale = obb.axis[0].LengthSq();
103         matrixScale = Sqrt(matrixScale);
104         obb.r *= matrixScale;
105         matrixScale = 1.f / matrixScale;
106         obb.axis[0] *= matrixScale;
107         obb.axis[1] *= matrixScale;
108         obb.axis[2] *= matrixScale;
109
110 //      mathassert(vec::AreOrthogonal(obb.axis[0], obb.axis[1], obb.axis[2]));
111 //      mathassert(vec::AreOrthonormal(obb.axis[0], obb.axis[1], obb.axis[2]));
112         ///@todo Would like to simply do the above, but instead numerical stability requires to do the following:
113         vec::Orthonormalize(obb.axis[0], obb.axis[1], obb.axis[2]);
114 }
115
116 void OBB::SetFrom(const AABB &aabb, const float3x3 &transform)
117 {
118         assume(transform.IsColOrthogonal());
119         OBBSetFrom(*this, aabb, transform);
120 }
121
122 void OBB::SetFrom(const AABB &aabb, const float3x4 &transform)
123 {
124         OBBSetFrom(*this, aabb, transform);
125 }
126
127 void OBB::SetFrom(const AABB &aabb, const float4x4 &transform)
128 {
129         assume(transform.Row(3).Equals(0,0,0,1));
130         OBBSetFrom(*this, aabb, transform.Float3x4Part());
131 }
132
133 void OBB::SetFrom(const AABB &aabb, const Quat &transform)
134 {
135         OBBSetFrom(*this, aabb, float3x3(transform));
136 }
137
138 void OBB::SetFrom(const Sphere &sphere)
139 {
140         pos = sphere.pos;
141         r.SetFromScalar(sphere.r);
142         axis[0] = DIR_VEC(1,0,0);
143         axis[1] = DIR_VEC(0,1,0);
144         axis[2] = DIR_VEC(0,0,1);
145 }
146
147 #ifdef MATH_CONTAINERLIB_SUPPORT
148 bool OBB::SetFrom(const Polyhedron &polyhedron)
149 {
150         if (!polyhedron.v.empty())
151         {
152                 *this = OBB::OptimalEnclosingOBB((vec*)&polyhedron.v[0], (int)polyhedron.v.size());
153                 return true;
154         }
155         else
156         {
157                 SetNegativeInfinity();
158                 return false;
159         }
160 }
161 #endif
162
163 #if 0
164 void OBB::SetFromApproximate(const vec *pointArray, int numPoints)
165 {
166         *this = PCAEnclosingOBB(pointArray, numPoints);
167 }
168 #endif
169
170 Polyhedron OBB::ToPolyhedron() const
171 {
172         // Note to maintainer: This function is an exact copy of AABB:ToPolyhedron() and Frustum::ToPolyhedron().
173
174         Polyhedron p;
175         // Populate the corners of this OBB.
176         // The will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
177         for(int i = 0; i < 8; ++i)
178                 p.v.push_back(CornerPoint(i));
179
180         // Generate the 6 faces of this OBB.
181         const int faces[6][4] =
182         {
183                 { 0, 1, 3, 2 }, // X-
184                 { 4, 6, 7, 5 }, // X+
185                 { 0, 4, 5, 1 }, // Y-
186                 { 7, 6, 2, 3 }, // Y+
187                 { 0, 2, 6, 4 }, // Z-
188                 { 1, 5, 7, 3 }, // Z+
189         };
190
191         for(int f = 0; f < 6; ++f)
192         {
193                 Polyhedron::Face face;
194                 for(int v = 0; v < 4; ++v)
195                         face.v.push_back(faces[f][v]);
196                 p.f.push_back(face);
197         }
198
199         return p;
200 }
201
202 PBVolume<6> OBB::ToPBVolume() const
203 {
204         PBVolume<6> pbVolume;
205         for(int i = 0; i < 6; ++i)
206                 pbVolume.p[i] = FacePlane(i);
207
208         return pbVolume;
209 }
210
211 AABB OBB::MinimalEnclosingAABB() const
212 {
213         AABB aabb;
214         aabb.SetFrom(*this);
215         return aabb;
216 }
217
218 #if 0
219
220 AABB OBB::MaximalContainedAABB() const
221 {
222 #ifdef _MSC_VER
223 #pragma warning(OBB::MaximalContainedAABB not implemented!)
224 #else
225 #warning OBB::MaximalContainedAABB not implemented!
226 #endif
227         assume(false && "OBB::MaximalContainedAABB not implemented!"); /// @todo Implement.
228         return AABB();
229 }
230 #endif
231
232 Sphere OBB::MinimalEnclosingSphere() const
233 {
234         Sphere s;
235         s.pos = pos;
236         s.r = HalfDiagonal().Length();
237         return s;
238 }
239
240 Sphere OBB::MaximalContainedSphere() const
241 {
242         Sphere s;
243         s.pos = pos;
244         s.r = r.MinElement();
245         return s;
246 }
247
248 bool OBB::IsFinite() const
249 {
250         return pos.IsFinite() && r.IsFinite() && axis[0].IsFinite() && axis[1].IsFinite() && axis[2].IsFinite();
251 }
252
253 bool OBB::IsDegenerate() const
254 {
255         return !(r.x > 0.f && r.y > 0.f && r.z > 0.f);
256 }
257
258 vec OBB::CenterPoint() const
259 {
260         return pos;
261 }
262
263 vec OBB::PointInside(float x, float y, float z) const
264 {
265         assume(0.f <= x && x <= 1.f);
266         assume(0.f <= y && y <= 1.f);
267         assume(0.f <= z && z <= 1.f);
268
269         return pos + axis[0] * (2.f * r.x * x - r.x)
270                            + axis[1] * (2.f * r.y * y - r.y)
271                            + axis[2] * (2.f * r.z * z - r.z);
272 }
273
274 LineSegment OBB::Edge(int edgeIndex) const
275 {
276         assume(0 <= edgeIndex && edgeIndex <= 11);
277         switch(edgeIndex)
278         {
279                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
280                 case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
281                 case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
282                 case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
283                 case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
284                 case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
285                 case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
286                 case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
287                 case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
288                 case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
289                 case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
290                 case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
291                 case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
292         }
293 }
294
295 vec OBB::CornerPoint(int cornerIndex) const
296 {
297         assume(0 <= cornerIndex && cornerIndex <= 7);
298         switch(cornerIndex)
299         {
300                 default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
301                 case 0: return pos - r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
302                 case 1: return pos - r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
303                 case 2: return pos - r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
304                 case 3: return pos - r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
305                 case 4: return pos + r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
306                 case 5: return pos + r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
307                 case 6: return pos + r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
308                 case 7: return pos + r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
309         }
310 }
311
312 vec OBB::ExtremePoint(const vec &direction) const
313 {
314         vec pt = pos;
315         pt += axis[0] * (Dot(direction, axis[0]) >= 0.f ? r.x : -r.x);
316         pt += axis[1] * (Dot(direction, axis[1]) >= 0.f ? r.y : -r.y);
317         pt += axis[2] * (Dot(direction, axis[2]) >= 0.f ? r.z : -r.z);
318         return pt;
319 }
320
321 vec OBB::ExtremePoint(const vec &direction, float &projectionDistance) const
322 {
323         vec extremePoint = ExtremePoint(direction);
324         projectionDistance = extremePoint.Dot(direction);
325         return extremePoint;
326 }
327
328 void OBB::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
329 {
330         float x = Abs(Dot(direction, axis[0]) * r.x);
331         float y = Abs(Dot(direction, axis[1]) * r.y);
332         float z = Abs(Dot(direction, axis[2]) * r.z);
333         float pt = Dot(direction, pos);
334         outMin = pt - x - y - z;
335         outMax = pt + x + y + z;
336 }
337
338 int OBB::UniqueFaceNormals(vec *out) const
339 {
340         out[0] = axis[0];
341         out[1] = axis[1];
342         out[2] = axis[2];
343         return 3;
344 }
345
346 int OBB::UniqueEdgeDirections(vec *out) const
347 {
348         out[0] = axis[0];
349         out[1] = axis[1];
350         out[2] = axis[2];
351         return 3;
352 }
353
354 vec OBB::PointOnEdge(int edgeIndex, float u) const
355 {
356         assume(0 <= edgeIndex && edgeIndex <= 11);
357         assume(0 <= u && u <= 1.f);
358
359         edgeIndex = Clamp(edgeIndex, 0, 11);
360         vec d = axis[edgeIndex/4] * (2.f * u - 1.f) * r[edgeIndex/4];
361         switch(edgeIndex)
362         {
363         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
364         case 0: return pos - r.y * axis[1] - r.z * axis[2] + d;
365         case 1: return pos - r.y * axis[1] + r.z * axis[2] + d;
366         case 2: return pos + r.y * axis[1] - r.z * axis[2] + d;
367         case 3: return pos + r.y * axis[1] + r.z * axis[2] + d;
368
369         case 4: return pos - r.x * axis[0] - r.z * axis[2] + d;
370         case 5: return pos - r.x * axis[0] + r.z * axis[2] + d;
371         case 6: return pos + r.x * axis[0] - r.z * axis[2] + d;
372         case 7: return pos + r.x * axis[0] + r.z * axis[2] + d;
373
374         case 8: return pos - r.x * axis[0] - r.y * axis[1] + d;
375         case 9: return pos - r.x * axis[0] + r.y * axis[1] + d;
376         case 10: return pos + r.x * axis[0] - r.y * axis[1] + d;
377         case 11: return pos + r.x * axis[0] + r.y * axis[1] + d;
378         }
379 }
380
381 vec OBB::FaceCenterPoint(int faceIndex) const
382 {
383         assume(0 <= faceIndex && faceIndex <= 5);
384
385         switch(faceIndex)
386         {
387         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
388         case 0: return pos - r.x * axis[0];
389         case 1: return pos + r.x * axis[0];
390         case 2: return pos - r.y * axis[1];
391         case 3: return pos + r.y * axis[1];
392         case 4: return pos - r.z * axis[2];
393         case 5: return pos + r.z * axis[2];
394         }
395 }
396
397 vec OBB::FacePoint(int faceIndex, float u, float v) const
398 {
399         assume(0 <= faceIndex && faceIndex <= 5);
400         assume(0 <= u && u <= 1.f);
401         assume(0 <= v && v <= 1.f);
402
403         int uIdx = faceIndex/2;
404         int vIdx = (faceIndex/2 + 1) % 3;
405         vec U = axis[uIdx] * (2.f * u - 1.f) * r[uIdx];
406         vec V = axis[vIdx] * (2.f * v - 1.f) * r[vIdx];
407         switch(faceIndex)
408         {
409         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
410         case 0: return pos - r.z * axis[2] + U + V;
411         case 1: return pos + r.z * axis[2] + U + V;
412         case 2: return pos - r.x * axis[0] + U + V;
413         case 3: return pos + r.x * axis[0] + U + V;
414         case 4: return pos - r.y * axis[1] + U + V;
415         case 5: return pos + r.y * axis[1] + U + V;
416         }
417 }
418
419 Plane OBB::FacePlane(int faceIndex) const
420 {
421         assume(0 <= faceIndex && faceIndex <= 5);
422         switch(faceIndex)
423         {
424         default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
425         case 0: return Plane(FaceCenterPoint(0), -axis[0]);
426         case 1: return Plane(FaceCenterPoint(1), axis[0]);
427         case 2: return Plane(FaceCenterPoint(2), -axis[1]);
428         case 3: return Plane(FaceCenterPoint(3), axis[1]);
429         case 4: return Plane(FaceCenterPoint(4), -axis[2]);
430         case 5: return Plane(FaceCenterPoint(5), axis[2]);
431         }
432 }
433
434 void OBB::GetCornerPoints(vec *outPointArray) const
435 {
436         assume(outPointArray);
437 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
438         if (!outPointArray)
439                 return;
440 #endif
441         for(int i = 0; i < 8; ++i)
442                 outPointArray[i] = CornerPoint(i);
443 }
444
445 void OBB::GetFacePlanes(Plane *outPlaneArray) const
446 {
447         assume(outPlaneArray);
448 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
449         if (!outPlaneArray)
450                 return;
451 #endif
452         for(int i = 0; i < 6; ++i)
453                 outPlaneArray[i] = FacePlane(i);
454 }
455
456 /// See Christer Ericson's book Real-Time Collision Detection, page 83.
457 void OBB::ExtremePointsAlongDirection(const vec &dir, const vec *pointArray, int numPoints, int &idxSmallest, int &idxLargest)
458 {
459         assume(pointArray || numPoints == 0);
460
461         idxSmallest = idxLargest = 0;
462
463 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
464         if (!pointArray)
465                 return;
466 #endif
467
468         float smallestD = FLOAT_INF;
469         float largestD = -FLOAT_INF;
470         for(int i = 0; i < numPoints; ++i)
471         {
472                 float d = Dot(pointArray[i], dir);
473                 if (d < smallestD)
474                 {
475                         smallestD = d;
476                         idxSmallest = i;
477                 }
478                 if (d > largestD)
479                 {
480                         largestD = d;
481                         idxLargest = i;
482                 }
483         }
484 }
485
486 #if 0
487 OBB OBB::PCAEnclosingOBB(const vec * /*pointArray*/, int /*numPoints*/)
488 {
489 #ifdef _MSC_VER
490 #pragma warning(OBB::PCAEnclosingOBB not implemented!)
491 #else
492 #warning OBB::PCAEnclosingOBB not implemented!
493 #endif
494         assume(false && "OBB::PCAEnclosingOBB not implemented!"); /// @todo Implement.
495         return OBB();
496 }
497 #endif
498
499 #define LEX_ORDER(x, y) if ((x) < (y)) return -1; else if ((x) > (y)) return 1;
500 int LexFloat3Cmp(const vec &a, const vec &b)
501 {
502         LEX_ORDER(a.x, b.x);
503         return 0;
504 }
505 int LexFloat3CmpV(const void *a, const void *b) { return LexFloat3Cmp(*(const vec*)a, *(const vec*)b); }
506
507 OBB OBB::OptimalEnclosingOBB(const vec *pointArray, int numPoints)
508 {
509         OBB minOBB;
510 #ifdef MATH_VEC_IS_FLOAT4
511         minOBB.r.w = 0.f;
512 #endif
513         float minVolume = FLOAT_INF;
514
515         std::vector<float2> pts;
516         pts.resize(numPoints);
517         ///\todo Convex hull'ize.
518
519         VecArray dirs;
520         dirs.reserve((numPoints * numPoints-1) / 2);
521         for(int i = 0; i < numPoints; ++i)
522                 for(int j = i+1; j < numPoints; ++j)
523                 {
524                         vec edge = pointArray[i]-pointArray[j];
525                         float oldLength = edge.Normalize();
526                         if (edge.z < 0.f)
527                                 edge = -edge;
528                         if (oldLength > 0.f)
529                                 dirs.push_back(edge);
530                 }
531
532 //      LOGI("Got %d directions.", (int)dirs.size());
533 #ifdef MATH_CONTAINERLIB_SUPPORT
534         sort::QuickSort(&dirs[0], (int)dirs.size(), LexFloat3Cmp);
535 #else
536         qsort(&dirs[0], dirs.size(), sizeof(VecArray::value_type), LexFloat3CmpV);
537 #endif
538         size_t nDistinct = 1;
539         for(size_t i = 1; i < dirs.size(); ++i)
540         {
541                 vec d = dirs[i];
542                 bool removed = false;
543                 for(size_t j = 0; j < nDistinct; ++j)
544                 {
545                         float distX = d.x - dirs[j].x;
546                         if (distX > 1e-3f)
547                                 break;
548                         if (d.DistanceSq(dirs[j]) < 1e-3f)
549                         {
550                                 removed = true;
551                                 break;
552                         }
553                 }
554                 if (!removed)
555                         dirs[nDistinct++] = dirs[i];
556         }
557         if (nDistinct != dirs.size())
558         {
559                 dirs.erase(dirs.begin() + nDistinct, dirs.end());
560 //              LOGI("Pruned %d directions (was %d to %d).", nDistinct, (int)dirs.size()+nDistinct, (int)dirs.size());
561         }
562
563         for(size_t i = 0; i < dirs.size(); ++i)
564         {
565                 vec edge = dirs[i];
566
567                 int e1, e2;
568                 ExtremePointsAlongDirection(edge, pointArray, numPoints, e1, e2);
569                 float edgeLength = Abs(Dot(pointArray[e1] - pointArray[e2], edge));
570
571                 vec u, v;
572                 edge.PerpendicularBasis(u, v);
573                 for(int k = 0; k < numPoints; ++k)
574                         pts[k] = float2(pointArray[k].Dot(u), pointArray[k].Dot(v));
575
576                 float2 rectCenter;
577                 float2 uDir;
578                 float2 vDir;
579                 float minU, maxU, minV, maxV;
580                 float rectArea = float2::MinAreaRectInPlace(&pts[0], (int)pts.size(), rectCenter, uDir, vDir, minU, maxU, minV, maxV);
581                 float volume = rectArea * edgeLength;
582
583                 if (volume < minVolume)
584                 {
585                         float2 c10 = (maxV - minV) * vDir;
586                         float2 c20 = (maxU - minU) * uDir;
587                         float2 center = 0.5f * (uDir * (minU+maxU) + vDir * (minV+maxV));
588                         vec C1 = c10.x*u + c10.y*v;
589                         vec C2 = c20.x*u + c20.y*v;
590                         minOBB.axis[0] = edge;
591                         vec rectCenterPos = vec(POINT_VEC_SCALAR(0.f)) + center.x*u + center.y*v;
592                         minOBB.pos = Dot(pointArray[e1]+pointArray[e2], edge) * 0.5f * edge + rectCenterPos;
593                         minOBB.r[0] = edgeLength * 0.5f;
594                         minOBB.r[1] = C1.Normalize()*0.5f;
595                         minOBB.r[2] = C2.Normalize()*0.5f;
596                         minOBB.axis[1] = C1;
597                         minOBB.axis[2] = C2;
598                         minVolume = volume;
599                 }
600         }
601
602         return minOBB;
603 }
604
605 vec OBB::Size() const
606 {
607         return r * 2.f;
608 }
609
610 vec OBB::HalfSize() const
611 {
612         return r;
613 }
614
615 vec OBB::Diagonal() const
616 {
617         return 2.f * HalfDiagonal();
618 }
619
620 vec OBB::HalfDiagonal() const
621 {
622         return axis[0] * r[0] + axis[1] * r[1] + axis[2] * r[2];
623 }
624
625 float3x4 OBB::WorldToLocal() const
626 {
627         float3x4 m = LocalToWorld();
628         m.InverseOrthonormal();
629         return m;
630 }
631
632 float3x4 OBB::LocalToWorld() const
633 {
634         // To produce a normalized local->world matrix, do the following.
635         /*
636         float3x4 m;
637         vec x = axis[0] * r.x;
638         vec y = axis[1] * r.y;
639         vec z = axis[2] * r.z;
640         m.SetCol(0, 2.f * x);
641         m.SetCol(1, 2.f * y);
642         m.SetCol(2, 2.f * z);
643         m.SetCol(3, pos - x - y - z);
644         return m;
645         */
646
647         assume2(axis[0].IsNormalized(), axis[0], axis[0].LengthSq());
648         assume2(axis[1].IsNormalized(), axis[1], axis[1].LengthSq());
649         assume2(axis[2].IsNormalized(), axis[2], axis[2].LengthSq());
650         float3x4 m; ///\todo sse-matrix
651         m.SetCol(0, axis[0].ptr());
652         m.SetCol(1, axis[1].ptr());
653         m.SetCol(2, axis[2].ptr());
654         vec p = pos - axis[0] * r.x - axis[1] * r.y - axis[2] * r.z;
655         m.SetCol(3, p.ptr());
656         assume(m.IsOrthonormal());
657         return m;
658 }
659
660 /// The implementation of this function is from Christer Ericson's Real-Time Collision Detection, p.133.
661 vec OBB::ClosestPoint(const vec &targetPoint) const
662 {
663 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
664         // Best: 8.833 nsecs / 24 ticks, Avg: 9.044 nsecs, Worst: 9.217 nsecs
665         simd4f d = sub_ps(targetPoint.v, pos.v);
666         simd4f x = xxxx_ps(r.v);
667         simd4f closestPoint = pos.v;
668         closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d, axis[0].v), x), negate_ps(x)), axis[0].v));
669         simd4f y = yyyy_ps(r.v);
670         closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d, axis[1].v), y), negate_ps(y)), axis[1].v));
671         simd4f z = zzzz_ps(r.v);
672         closestPoint = add_ps(closestPoint, mul_ps(max_ps(min_ps(dot4_ps(d, axis[2].v), z), negate_ps(z)), axis[2].v));
673         return closestPoint;
674 #else
675         // Best: 33.412 nsecs / 89.952 ticks, Avg: 33.804 nsecs, Worst: 34.180 nsecs
676         vec d = targetPoint - pos;
677         vec closestPoint = pos; // Start at the center point of the OBB.
678         for(int i = 0; i < 3; ++i) // Project the target onto the OBB axes and walk towards that point.
679                 closestPoint += Clamp(Dot(d, axis[i]), -r[i], r[i]) * axis[i];
680
681         return closestPoint;
682 #endif
683 }
684
685 float OBB::Volume() const
686 {
687         return Size().ProductOfElements();
688 }
689
690 float OBB::SurfaceArea() const
691 {
692         const vec size = Size();
693         return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
694 }
695
696 vec OBB::RandomPointInside(LCG &rng) const
697 {
698         float f1 = rng.Float();
699         float f2 = rng.Float();
700         float f3 = rng.Float();
701         return PointInside(f1, f2, f3);
702 }
703
704 vec OBB::RandomPointOnSurface(LCG &rng) const
705 {
706         int i = rng.Int(0, 5);
707         float f1 = rng.Float();
708         float f2 = rng.Float();
709         return FacePoint(i, f1, f2);
710 }
711
712 vec OBB::RandomPointOnEdge(LCG &rng) const
713 {
714         int i = rng.Int(0, 11);
715         float f = rng.Float();
716         return PointOnEdge(i, f);
717 }
718
719 vec OBB::RandomCornerPoint(LCG &rng) const
720 {
721         return CornerPoint(rng.Int(0, 7));
722 }
723
724 void OBB::Translate(const vec &offset)
725 {
726         pos += offset;
727 }
728
729 void OBB::Scale(const vec &centerPoint, float scaleFactor)
730 {
731         return Scale(centerPoint, DIR_VEC_SCALAR(scaleFactor));
732 }
733
734 void OBB::Scale(const vec &centerPoint, const vec &scaleFactor)
735 {
736         ///@bug This scales in global axes, not local axes.
737         float3x4 transform = float3x4::Scale(scaleFactor, centerPoint);
738         Transform(transform);
739 }
740
741 template<typename Matrix>
742 void OBBTransform(OBB &o, const Matrix &transform)
743 {
744         o.pos = transform.MulPos(o.pos);
745         o.axis[0] = transform.MulDir(o.r.x * o.axis[0]);
746         o.axis[1] = transform.MulDir(o.r.y * o.axis[1]);
747         o.axis[2] = transform.MulDir(o.r.z * o.axis[2]);
748         o.r.x = o.axis[0].Normalize();
749         o.r.y = o.axis[1].Normalize();
750         o.r.z = o.axis[2].Normalize();
751 }
752
753 void OBB::Transform(const float3x3 &transform)
754 {
755         assume(transform.IsColOrthogonal());
756         OBBTransform(*this, transform);
757 }
758
759 void OBB::Transform(const float3x4 &transform)
760 {
761         assume(transform.IsColOrthogonal());
762         OBBTransform(*this, transform);
763 }
764
765 void OBB::Transform(const float4x4 &transform)
766 {
767         assume(transform.IsColOrthogonal3());
768         OBBTransform(*this, transform);
769 }
770
771 void OBB::Transform(const Quat &transform)
772 {
773         OBBTransform(*this, transform.ToFloat3x3());
774 }
775
776 float OBB::Distance(const vec &point) const
777 {
778         ///@todo This code can be optimized a bit. See Christer Ericson's Real-Time Collision Detection,
779         /// p.134.
780         vec closestPoint = ClosestPoint(point);
781         return point.Distance(closestPoint);
782 }
783
784 float OBB::Distance(const Sphere &sphere) const
785 {
786         return Max(0.f, Distance(sphere.pos) - sphere.r);
787 }
788
789 bool OBB::Contains(const vec &point) const
790 {
791 #if defined(MATH_SSE) && defined(MATH_AUTOMATIC_SSE)
792 // Best: 9.985 nsecs / 26.816 ticks, Avg: 10.112 nsecs, Worst: 11.137 nsecs
793         simd4f pt = sub_ps(point.v, pos.v);
794         simd4f s1 = mul_ps(pt, axis[0].v);
795         simd4f s2 = mul_ps(pt, axis[1].v);
796         simd4f s3 = mul_ps(pt, axis[2].v);
797         s1 = abs_ps(sum_xyzw_ps(s1));
798         s2 = abs_ps(sum_xyzw_ps(s2));
799         s3 = abs_ps(sum_xyzw_ps(s3));
800
801         s1 = _mm_sub_ss(s1, r.v);
802         s2 = _mm_sub_ss(s2, yyyy_ps(r.v));
803         simd4f s12 = _mm_max_ss(s1, s2);
804         s3 = _mm_sub_ss(s3, zzzz_ps(r.v));
805         s3 = _mm_max_ss(s12, s3);
806         return _mm_cvtss_f32(s3) <= 0.f; // Note: This might be micro-optimized further out by switching to a signature "float OBB::SignedDistance(point)" instead.
807 #else
808 // Best: 14.978 nsecs / 39.944 ticks, Avg: 15.350 nsecs, Worst: 39.941 nsecs
809         vec pt = point - pos;
810         return Abs(Dot(pt, axis[0])) <= r[0] &&
811                Abs(Dot(pt, axis[1])) <= r[1] &&
812                Abs(Dot(pt, axis[2])) <= r[2];
813 #endif
814 }
815
816 bool OBB::Contains(const LineSegment &lineSegment) const
817 {
818         return Contains(lineSegment.a) && Contains(lineSegment.b);
819 }
820
821 bool OBB::Contains(const AABB &aabb) const
822 {
823         // Since both AABB and OBB are convex objects, this OBB contains the AABB
824         // if and only if it contains all its corner points.
825         for(int i = 0; i < 8; ++i)
826         if (!Contains(aabb.CornerPoint(i)))
827                         return false;
828
829         return true;
830 }
831
832 bool OBB::Contains(const OBB &obb) const
833 {
834         for(int i = 0; i < 8; ++i)
835                 if (!Contains(obb.CornerPoint(i)))
836                         return false;
837
838         return true;
839 }
840
841 bool OBB::Contains(const Triangle &triangle) const
842 {
843         return Contains(triangle.a) && Contains(triangle.b) && Contains(triangle.c);
844 }
845
846 bool OBB::Contains(const Polygon &polygon) const
847 {
848         for(int i = 0; i < polygon.NumVertices(); ++i)
849                 if (!Contains(polygon.Vertex(i)))
850                         return false;
851         return true;
852 }
853
854 bool OBB::Contains(const Frustum &frustum) const
855 {
856         for(int i = 0; i < 8; ++i)
857                 if (!Contains(frustum.CornerPoint(i)))
858                         return false;
859
860         return true;
861 }
862
863 bool OBB::Contains(const Polyhedron &polyhedron) const
864 {
865         assume(polyhedron.IsClosed());
866         for(int i = 0; i < polyhedron.NumVertices(); ++i)
867                 if (!Contains(polyhedron.Vertex(i)))
868                         return false;
869
870         return true;
871 }
872
873 bool OBB::Intersects(const AABB &aabb) const
874 {
875         return Intersects(OBB(aabb));
876 }
877
878 void OBB::Enclose(const vec &point)
879 {
880         vec p = point - pos;
881         for(int i = 0; i < 3; ++i)
882         {
883                 assume2(EqualAbs(axis[i].Length(), 1.f), axis[i], axis[i].Length());
884                 float dist = p.Dot(axis[i]);
885                 float distanceFromOBB = Abs(dist) - r[i];
886                 if (distanceFromOBB > 0.f)
887                 {
888                         r[i] += distanceFromOBB * 0.5f;
889                         if (dist > 0.f) ///\todo Optimize out this comparison!
890                                 pos += axis[i] * distanceFromOBB * 0.5f;
891                         else
892                                 pos -= axis[i] * distanceFromOBB * 0.5f;
893
894                         p = point-pos; ///\todo Can we omit this? (redundant since axis[i] are orthonormal?)
895
896                         mathassert(EqualAbs(Abs(p.Dot(axis[i])), r[i], 1e-1f));
897                 }
898         }
899         // Should now contain the point.
900         assume2(Distance(point) <= 1e-3f, point, Distance(point));
901 }
902
903 void OBB::Triangulate(int x, int y, int z, vec *outPos, vec *outNormal, float2 *outUV, bool ccwIsFrontFacing) const
904 {
905         AABB aabb(POINT_VEC_SCALAR(0), r*2.f);
906         aabb.Triangulate(x, y, z, outPos, outNormal, outUV, ccwIsFrontFacing);
907         float3x4 localToWorld = LocalToWorld();
908         assume(localToWorld.HasUnitaryScale()); // Transforming of normals will fail otherwise.
909         localToWorld.BatchTransformPos(outPos, NumVerticesInTriangulation(x,y,z), sizeof(vec));
910         localToWorld.BatchTransformDir(outNormal, NumVerticesInTriangulation(x,y,z), sizeof(vec));
911 }
912
913 void OBB::ToEdgeList(vec *outPos) const
914 {
915         assume(outPos);
916         if (!outPos)
917                 return;
918         for(int i = 0; i < 12; ++i)
919         {
920                 LineSegment edge = Edge(i);
921                 outPos[i*2] = edge.a;
922                 outPos[i*2+1] = edge.b;
923         }
924 }
925
926 bool OBB::Intersects(const OBB &b, float epsilon) const
927 {
928         assume(pos.IsFinite());
929         assume(b.pos.IsFinite());
930         assume(vec::AreOrthogonal(axis[0], axis[1], axis[2]));
931         assume(vec::AreOrthogonal(b.axis[0], b.axis[1], b.axis[2]));
932
933 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
934         // SSE4.1:
935         // Benchmark 'OBBIntersectsOBB_Random': OBB::Intersects(OBB) Random
936         //    Best: 23.913 nsecs / 40.645 ticks, Avg: 26.490 nsecs, Worst: 43.729 nsecs
937         // Benchmark 'OBBIntersectsOBB_Positive': OBB::Intersects(OBB) Positive
938         //    Best: 42.585 nsecs / 72.413 ticks, Avg: 44.373 nsecs, Worst: 70.774 nsecs
939
940         simd4f bLocalToWorld[3];
941         mat3x4_transpose((const simd4f*)b.axis, bLocalToWorld);
942
943         // Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
944         simd4f R[3];
945         mat3x4_mul_sse(R, (const simd4f*)axis, bLocalToWorld);
946
947         // Express the translation vector in a's coordinate frame.
948         simd4f t = mat3x4_mul_sse((const simd4f*)axis, sub_ps(b.pos, pos));
949
950         // This trashes the w component, which should technically be zero, but this does not matter since
951         // AbsR will only be used with direction vectors.
952         const vec epsilonxyz = set1_ps(epsilon);
953         simd4f AbsR[3];
954         AbsR[0] = add_ps(abs_ps(R[0]), epsilonxyz);
955         AbsR[1] = add_ps(abs_ps(R[1]), epsilonxyz);
956         AbsR[2] = add_ps(abs_ps(R[2]), epsilonxyz);
957
958         // Test the three major axes of this OBB.
959         simd4f res = cmpgt_ps(abs_ps(t), add_ps(r, mat3x4_mul_sse(AbsR, b.r)));
960         if (!allzero_ps(res)) return false;
961
962         // Test the three major axes of the OBB b.
963         simd4f transpR[3];
964         mat3x4_transpose(R, transpR);
965         vec l = abs_ps(mat3x4_mul_sse(transpR, r));
966         simd4f transpAbsR[3];
967         mat3x4_transpose(AbsR, transpAbsR);
968         vec s = mat3x4_mul_sse(transpAbsR, r);
969         res = cmpgt_ps(l, add_ps(s, b.r));
970         if (!allzero_ps(res)) return false;
971
972         // Test the 9 different cross-axes.
973
974         // A.x <cross> B.x
975         // A.x <cross> B.y
976         // A.x <cross> B.z
977         simd4f ra = mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,1,1,1)), AbsR[2]);
978         ra = add_ps(ra, mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,2,2,2)), AbsR[1]));
979         simd4f rb = mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,0,0,1)), shuffle1_ps(AbsR[0], _MM_SHUFFLE(3,1,2,2)));
980         rb = add_ps(rb, mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,1,2,2)), shuffle1_ps(AbsR[0], _MM_SHUFFLE(3,0,0,1))));
981         simd4f lhs = mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,2,2,2)), R[1]);
982         lhs = sub_ps(lhs, mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,1,1,1)), R[2]));
983         res = cmpgt_ps(abs_ps(lhs), add_ps(ra, rb));
984         if (!allzero_ps(res)) return false;
985
986         // A.y <cross> B.x
987         // A.y <cross> B.y
988         // A.y <cross> B.z
989         ra = mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,0,0,0)), AbsR[2]);
990         ra = add_ps(ra, mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,2,2,2)), AbsR[0]));
991         rb = mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,0,0,1)), shuffle1_ps(AbsR[1], _MM_SHUFFLE(3,1,2,2)));
992         rb = add_ps(rb, mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,1,2,2)), shuffle1_ps(AbsR[1], _MM_SHUFFLE(3,0,0,1))));
993         lhs = mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,0,0,0)), R[2]);
994         lhs = sub_ps(lhs, mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,2,2,2)), R[0]));
995         res = cmpgt_ps(abs_ps(lhs), add_ps(ra, rb));
996         if (!allzero_ps(res)) return false;
997
998         // A.z <cross> B.x
999         // A.z <cross> B.y
1000         // A.z <cross> B.z
1001         ra = mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,0,0,0)), AbsR[1]);
1002         ra = add_ps(ra, mul_ps(shuffle1_ps(r, _MM_SHUFFLE(3,1,1,1)), AbsR[0]));
1003         rb = mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,0,0,1)), shuffle1_ps(AbsR[2], _MM_SHUFFLE(3,1,2,2)));
1004         rb = add_ps(rb, mul_ps(shuffle1_ps(b.r, _MM_SHUFFLE(3,1,2,2)), shuffle1_ps(AbsR[2], _MM_SHUFFLE(3,0,0,1))));
1005         lhs = mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,1,1,1)), R[0]);
1006         lhs = sub_ps(lhs, mul_ps(shuffle1_ps(t, _MM_SHUFFLE(3,0,0,0)), R[1]));
1007         res = cmpgt_ps(abs_ps(lhs), add_ps(ra, rb));
1008         return allzero_ps(res) != 0;
1009
1010 #else
1011         // Benchmark 'OBBIntersectsOBB_Random': OBB::Intersects(OBB) Random
1012         //    Best: 100.830 nsecs / 171.37 ticks, Avg: 110.533 nsecs, Worst: 155.582 nsecs
1013         // Benchmark 'OBBIntersectsOBB_Positive': OBB::Intersects(OBB) Positive
1014         //    Best: 95.771 nsecs / 162.739 ticks, Avg: 107.935 nsecs, Worst: 173.110 nsecs
1015
1016         // Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
1017         float3x3 R;
1018         for(int i = 0; i < 3; ++i)
1019                 for(int j = 0; j < 3; ++j)
1020                         R[i][j] = Dot(axis[i], b.axis[j]);
1021
1022         vec t = b.pos - pos;
1023         // Express the translation vector in a's coordinate frame.
1024         t = DIR_VEC(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));
1025
1026         float3x3 AbsR;
1027         for(int i = 0; i < 3; ++i)
1028                 for(int j = 0; j < 3; ++j)
1029                         AbsR[i][j] = Abs(R[i][j]) + epsilon;
1030
1031         // Test the three major axes of this OBB.
1032         for(int i = 0; i < 3; ++i)
1033         {
1034                 float ra = r[i];
1035                 float rb = DOT3(b.r, AbsR[i]);
1036                 if (Abs(t[i]) > ra + rb)
1037                         return false;
1038         }
1039
1040         // Test the three major axes of the OBB b.
1041         for(int i = 0; i < 3; ++i)
1042         {
1043                 float ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];
1044                 float rb = b.r[i];
1045                 if (Abs(t.x * R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)
1046                         return false;
1047         }
1048
1049         // Test the 9 different cross-axes.
1050
1051         // A.x <cross> B.x
1052         float ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];
1053         float rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];
1054         if (Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)
1055                 return false;
1056
1057         // A.x < cross> B.y
1058         ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];
1059         rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];
1060         if (Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)
1061                 return false;
1062
1063         // A.x <cross> B.z
1064         ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];
1065         rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];
1066         if (Abs(t.z * R[1][2] - t.y * R[2][2]) > ra + rb)
1067                 return false;
1068
1069         // A.y <cross> B.x
1070         ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];
1071         rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];
1072         if (Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)
1073                 return false;
1074
1075         // A.y <cross> B.y
1076         ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];
1077         rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];
1078         if (Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)
1079                 return false;
1080
1081         // A.y <cross> B.z
1082         ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];
1083         rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];
1084         if (Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)
1085                 return false;
1086
1087         // A.z <cross> B.x
1088         ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];
1089         rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];
1090         if (Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)
1091                 return false;
1092
1093         // A.z <cross> B.y
1094         ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];
1095         rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];
1096         if (Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)
1097                 return false;
1098
1099         // A.z <cross> B.z
1100         ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];
1101         rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];
1102         if (Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)
1103                 return false;
1104
1105         // No separating axis exists, so the two OBB don't intersect.
1106         return true;
1107 #endif
1108 }
1109
1110 /// The implementation of OBB-Plane intersection test follows Christer Ericson's Real-Time Collision Detection, p. 163. [groupSyntax]
1111 bool OBB::Intersects(const Plane &p) const
1112 {
1113         // Compute the projection interval radius of this OBB onto L(t) = this->pos + x * p.normal;
1114         float t = r[0] * Abs(Dot(p.normal, axis[0])) +
1115                           r[1] * Abs(Dot(p.normal, axis[1])) +
1116                           r[2] * Abs(Dot(p.normal, axis[2]));
1117         // Compute the distance of this OBB center from the plane.
1118         float s = Dot(p.normal, pos) - p.d;
1119         return Abs(s) <= t;
1120 }
1121
1122 bool OBB::Intersects(const Ray &ray) const
1123 {
1124         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1125         Ray r = WorldToLocal() * ray;
1126         return aabb.Intersects(r);
1127 }
1128
1129 bool OBB::Intersects(const Ray &ray, float &dNear, float &dFar) const
1130 {
1131         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1132         Ray r = WorldToLocal() * ray;
1133         return aabb.Intersects(r, dNear, dFar);
1134 }
1135
1136 bool OBB::Intersects(const Line &line) const
1137 {
1138         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1139         Line l = WorldToLocal() * line;
1140         return aabb.Intersects(l);
1141 }
1142
1143 bool OBB::Intersects(const Line &line, float &dNear, float &dFar) const
1144 {
1145         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1146         Line l = WorldToLocal() * line;
1147         return aabb.Intersects(l, dNear, dFar);
1148 }
1149
1150 bool OBB::Intersects(const LineSegment &lineSegment) const
1151 {
1152         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1153         LineSegment l = WorldToLocal() * lineSegment;
1154         return aabb.Intersects(l);
1155 }
1156
1157 bool OBB::Intersects(const LineSegment &lineSegment, float &dNear, float &dFar) const
1158 {
1159         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1160         LineSegment l = WorldToLocal() * lineSegment;
1161         return aabb.Intersects(l, dNear, dFar);
1162 }
1163
1164 /// The implementation of the OBB-Sphere intersection test follows Christer Ericson's Real-Time Collision Detection, p. 166. [groupSyntax]
1165 bool OBB::Intersects(const Sphere &sphere, vec *closestPointOnOBB) const
1166 {
1167         // Find the point on this AABB closest to the sphere center.
1168         vec pt = ClosestPoint(sphere.pos);
1169
1170         // If that point is inside sphere, the AABB and sphere intersect.
1171         if (closestPointOnOBB)
1172                 *closestPointOnOBB = pt;
1173
1174         return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
1175 }
1176
1177 bool OBB::Intersects(const Capsule &capsule) const
1178 {
1179         return capsule.Intersects(*this);
1180 }
1181
1182 bool OBB::Intersects(const Triangle &triangle) const
1183 {
1184         AABB aabb(POINT_VEC_SCALAR(0.f), Size());
1185         Triangle t = WorldToLocal() * triangle;
1186         return t.Intersects(aabb);
1187 }
1188
1189 bool OBB::Intersects(const Polygon &polygon) const
1190 {
1191         return polygon.Intersects(*this);
1192 }
1193
1194 bool OBB::Intersects(const Frustum &frustum) const
1195 {
1196         return frustum.Intersects(*this);
1197 }
1198
1199 bool OBB::Intersects(const Polyhedron &polyhedron) const
1200 {
1201         return polyhedron.Intersects(*this);
1202 }
1203
1204 #ifdef MATH_ENABLE_STL_SUPPORT
1205 std::string OBB::ToString() const
1206 {
1207         char str[256];
1208         sprintf(str, "OBB(Pos:(%.2f, %.2f, %.2f) Halfsize:(%.2f, %.2f, %.2f) X:(%.2f, %.2f, %.2f) Y:(%.2f, %.2f, %.2f) Z:(%.2f, %.2f, %.2f))",
1209                 pos.x, pos.y, pos.z, r.x, r.y, r.z, axis[0].x, axis[0].y, axis[0].z, axis[1].x, axis[1].y, axis[1].z, axis[2].x, axis[2].y, axis[2].z);
1210         return str;
1211 }
1212
1213 std::string OBB::SerializeToString() const
1214 {
1215         std::string s = pos.xyz().SerializeToString() + " "
1216                       + r.xyz().SerializeToString() + " "
1217                       + axis[0].xyz().SerializeToString() + " "
1218                       + axis[1].xyz().SerializeToString() + " "
1219                       + axis[2].xyz().SerializeToString();
1220         return s;
1221 }
1222
1223 std::string OBB::SerializeToCodeString() const
1224 {
1225         return "OBB(" + pos.SerializeToCodeString() + ","
1226                       + r.SerializeToCodeString() + ","
1227                       + axis[0].SerializeToCodeString() + ","
1228                       + axis[1].SerializeToCodeString() + ","
1229                       + axis[2].SerializeToCodeString() + ")";
1230 }
1231
1232 std::ostream &operator <<(std::ostream &o, const OBB &obb)
1233 {
1234         o << obb.ToString();
1235         return o;
1236 }
1237
1238 #endif
1239
1240 OBB OBB::FromString(const char *str, const char **outEndStr)
1241 {
1242         assume(str);
1243         if (!str)
1244                 return OBB(vec::nan, vec::nan, vec::nan, vec::nan, vec::nan);
1245         OBB o;
1246         MATH_SKIP_WORD(str, "OBB(");
1247         MATH_SKIP_WORD(str, "Pos:(");
1248         o.pos = PointVecFromString(str, &str);
1249         MATH_SKIP_WORD(str, " Halfsize:(");
1250         o.r = DirVecFromString(str, &str);
1251         MATH_SKIP_WORD(str, " X:(");
1252         o.axis[0] = DirVecFromString(str, &str);
1253         MATH_SKIP_WORD(str, " Y:(");
1254         o.axis[1] = DirVecFromString(str, &str);
1255         MATH_SKIP_WORD(str, " Z:(");
1256         o.axis[2] = DirVecFromString(str, &str);
1257         if (outEndStr)
1258                 *outEndStr = str;
1259         return o;
1260 }
1261
1262 #ifdef MATH_GRAPHICSENGINE_INTEROP
1263 void OBB::Triangulate(VertexBuffer &vb, int x, int y, int z, bool ccwIsFrontFacing) const
1264 {
1265         Array<vec> pos;
1266         Array<vec> normal;
1267         Array<float2> uv;
1268         int numVertices = (x*y+y*z+x*z)*2*6;
1269         pos.Resize_pod(numVertices);
1270         normal.Resize_pod(numVertices);
1271         uv.Resize_pod(numVertices);
1272         Triangulate(x,y,z, &pos[0], &normal[0], &uv[0], ccwIsFrontFacing);
1273         int startIndex = vb.AppendVertices(numVertices);
1274         for(int i = 0; i < (int)pos.size(); ++i)
1275         {
1276                 vb.Set(startIndex+i, VDPosition, POINT_TO_FLOAT4(pos[i]));
1277                 if (vb.Declaration()->TypeOffset(VDNormal) >= 0)
1278                         vb.Set(startIndex+i, VDNormal, DIR_TO_FLOAT4(normal[i]));
1279                 if (vb.Declaration()->TypeOffset(VDUV) >= 0)
1280                         vb.SetFloat2(startIndex+i, VDUV, 0, uv[i]);
1281         }
1282 }
1283
1284 void OBB::ToLineList(VertexBuffer &vb) const
1285 {
1286         Array<vec> pos;
1287         pos.Resize_pod(NumVerticesInEdgeList());
1288         ToEdgeList(&pos[0]);
1289         int startIndex = vb.AppendVertices((int)pos.size());
1290         for(int i = 0; i < (int)pos.size(); ++i)
1291                 vb.Set(startIndex+i, VDPosition, POINT_TO_FLOAT4(pos[i]));
1292 }
1293
1294 #endif
1295
1296 OBB operator *(const float3x3 &transform, const OBB &obb)
1297 {
1298         OBB o(obb);
1299         o.Transform(transform);
1300         return o;
1301 }
1302
1303 OBB operator *(const float3x4 &transform, const OBB &obb)
1304 {
1305         OBB o(obb);
1306         o.Transform(transform);
1307         return o;
1308 }
1309
1310 OBB operator *(const float4x4 &transform, const OBB &obb)
1311 {
1312         OBB o(obb);
1313         o.Transform(transform);
1314         return o;
1315 }
1316
1317 OBB operator *(const Quat &transform, const OBB &obb)
1318 {
1319         OBB o(obb);
1320         o.Transform(transform);
1321         return o;
1322 }
1323
1324 MATH_END_NAMESPACE