Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/OPCODE/Ice/IceOBB.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 17 years ago

[Physik] add ode-0.9
File size: 12.2 KB
Line 
1///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2/**
3 *      Contains OBB-related code.
4 *      \file           IceOBB.cpp
5 *      \author         Pierre Terdiman
6 *      \date           January, 29, 2000
7 */
8///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
9
10///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
11/**
12 *      An Oriented Bounding Box (OBB).
13 *      \class          OBB
14 *      \author         Pierre Terdiman
15 *      \version        1.0
16 */
17///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
18
19///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
20// Precompiled Header
21#include "Stdafx.h"
22
23using namespace IceMaths;
24
25///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
26/**
27 *      Tests if a point is contained within the OBB.
28 *      \param          p       [in] the world point to test
29 *      \return         true if inside the OBB
30 */
31///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
32bool OBB::ContainsPoint(const Point& p) const
33{
34        // Point in OBB test using lazy evaluation and early exits
35
36        // Translate to box space
37        Point RelPoint = p - mCenter;
38
39        // Point * mRot maps from box space to world space
40        // mRot * Point maps from world space to box space (what we need here)
41
42        float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z;
43        if(f >= mExtents.x || f <= -mExtents.x) return false;
44
45        f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z;
46        if(f >= mExtents.y || f <= -mExtents.y) return false;
47
48        f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z;
49        if(f >= mExtents.z || f <= -mExtents.z) return false;
50        return true;
51}
52
53///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54/**
55 *      Builds an OBB from an AABB and a world transform.
56 *      \param          aabb    [in] the aabb
57 *      \param          mat             [in] the world transform
58 */
59///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60void OBB::Create(const AABB& aabb, const Matrix4x4& mat)
61{
62        // Note: must be coherent with Rotate()
63
64        aabb.GetCenter(mCenter);
65        aabb.GetExtents(mExtents);
66        // Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity).
67
68        // So following what's done in Rotate:
69        // - x-form the center
70        mCenter *= mat;
71        // - combine rotation with identity, i.e. just use given matrix
72        mRot = mat;
73}
74
75///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
76/**
77 *      Computes the obb planes.
78 *      \param          planes  [out] 6 box planes
79 *      \return         true if success
80 */
81///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82bool OBB::ComputePlanes(Plane* planes)  const
83{
84        // Checkings
85        if(!planes)     return false;
86
87        Point Axis0 = mRot[0];
88        Point Axis1 = mRot[1];
89        Point Axis2 = mRot[2];
90
91        // Writes normals
92        planes[0].n = Axis0;
93        planes[1].n = -Axis0;
94        planes[2].n = Axis1;
95        planes[3].n = -Axis1;
96        planes[4].n = Axis2;
97        planes[5].n = -Axis2;
98
99        // Compute a point on each plane
100        Point p0 = mCenter + Axis0 * mExtents.x;
101        Point p1 = mCenter - Axis0 * mExtents.x;
102        Point p2 = mCenter + Axis1 * mExtents.y;
103        Point p3 = mCenter - Axis1 * mExtents.y;
104        Point p4 = mCenter + Axis2 * mExtents.z;
105        Point p5 = mCenter - Axis2 * mExtents.z;
106
107        // Compute d
108        planes[0].d = -(planes[0].n|p0);
109        planes[1].d = -(planes[1].n|p1);
110        planes[2].d = -(planes[2].n|p2);
111        planes[3].d = -(planes[3].n|p3);
112        planes[4].d = -(planes[4].n|p4);
113        planes[5].d = -(planes[5].n|p5);
114
115        return true;
116}
117
118///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
119/**
120 *      Computes the obb points.
121 *      \param          pts     [out] 8 box points
122 *      \return         true if success
123 */
124///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125bool OBB::ComputePoints(Point* pts)     const
126{
127        // Checkings
128        if(!pts)        return false;
129
130        Point Axis0 = mRot[0];
131        Point Axis1 = mRot[1];
132        Point Axis2 = mRot[2];
133
134        Axis0 *= mExtents.x;
135        Axis1 *= mExtents.y;
136        Axis2 *= mExtents.z;
137
138        //     7+------+6                       0 = ---
139        //     /|     /|                        1 = +--
140        //    / |    / |                        2 = ++-
141        //   / 4+---/--+5                       3 = -+-
142        // 3+------+2 /    y   z        4 = --+
143        //  | /    | /     |  /         5 = +-+
144        //  |/     |/      |/           6 = +++
145        // 0+------+1      *---x        7 = -++
146
147        pts[0] = mCenter - Axis0 - Axis1 - Axis2;
148        pts[1] = mCenter + Axis0 - Axis1 - Axis2;
149        pts[2] = mCenter + Axis0 + Axis1 - Axis2;
150        pts[3] = mCenter - Axis0 + Axis1 - Axis2;
151        pts[4] = mCenter - Axis0 - Axis1 + Axis2;
152        pts[5] = mCenter + Axis0 - Axis1 + Axis2;
153        pts[6] = mCenter + Axis0 + Axis1 + Axis2;
154        pts[7] = mCenter - Axis0 + Axis1 + Axis2;
155
156        return true;
157}
158
159///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160/**
161 *      Computes vertex normals.
162 *      \param          pts     [out] 8 box points
163 *      \return         true if success
164 */
165///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
166bool OBB::ComputeVertexNormals(Point* pts)      const
167{
168        static float VertexNormals[] = 
169        {
170                -INVSQRT3,      -INVSQRT3,      -INVSQRT3,
171                INVSQRT3,       -INVSQRT3,      -INVSQRT3,
172                INVSQRT3,       INVSQRT3,       -INVSQRT3,
173                -INVSQRT3,      INVSQRT3,       -INVSQRT3,
174                -INVSQRT3,      -INVSQRT3,      INVSQRT3,
175                INVSQRT3,       -INVSQRT3,      INVSQRT3,
176                INVSQRT3,       INVSQRT3,       INVSQRT3,
177                -INVSQRT3,      INVSQRT3,       INVSQRT3
178        };
179
180        if(!pts)        return false;
181
182        const Point* VN = (const Point*)VertexNormals;
183        for(udword i=0;i<8;i++)
184        {
185                pts[i] = VN[i] * mRot;
186        }
187
188        return true;
189}
190
191///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
192/**
193 *      Returns edges.
194 *      \return         24 indices (12 edges) indexing the list returned by ComputePoints()
195 */
196///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
197const udword* OBB::GetEdges() const
198{
199        static udword Indices[] = {
200        0, 1,   1, 2,   2, 3,   3, 0,
201        7, 6,   6, 5,   5, 4,   4, 7,
202        1, 5,   6, 2,
203        3, 7,   4, 0
204        };
205        return Indices;
206}
207
208///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
209/**
210 *      Returns local edge normals.
211 *      \return         edge normals in local space
212 */
213///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
214const Point* OBB::GetLocalEdgeNormals() const
215{
216        static float EdgeNormals[] = 
217        {
218                0,                      -INVSQRT2,      -INVSQRT2,      // 0-1
219                INVSQRT2,       0,                      -INVSQRT2,      // 1-2
220                0,                      INVSQRT2,       -INVSQRT2,      // 2-3
221                -INVSQRT2,      0,                      -INVSQRT2,      // 3-0
222
223                0,                      INVSQRT2,       INVSQRT2,       // 7-6
224                INVSQRT2,       0,                      INVSQRT2,       // 6-5
225                0,                      -INVSQRT2,      INVSQRT2,       // 5-4
226                -INVSQRT2,      0,                      INVSQRT2,       // 4-7
227
228                INVSQRT2,       -INVSQRT2,      0,                      // 1-5
229                INVSQRT2,       INVSQRT2,       0,                      // 6-2
230                -INVSQRT2,      INVSQRT2,       0,                      // 3-7
231                -INVSQRT2,      -INVSQRT2,      0                       // 4-0
232        };
233        return (const Point*)EdgeNormals;
234}
235
236///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
237/**
238 *      Returns world edge normal
239 *      \param          edge_index              [in] 0 <= edge index < 12
240 *      \param          world_normal    [out] edge normal in world space
241 */
242///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
243void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const
244{
245        ASSERT(edge_index<12);
246        world_normal = GetLocalEdgeNormals()[edge_index] * mRot;
247}
248
249///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
250/**
251 *      Computes an LSS surrounding the OBB.
252 *      \param          lss             [out] the LSS
253 */
254///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
255void OBB::ComputeLSS(LSS& lss) const
256{
257        Point Axis0 = mRot[0];
258        Point Axis1 = mRot[1];
259        Point Axis2 = mRot[2];
260
261        switch(mExtents.LargestAxis())
262        {
263                case 0:
264                        lss.mRadius = (mExtents.y + mExtents.z)*0.5f;
265                        lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius);
266                        lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius);
267                        break;
268                case 1:
269                        lss.mRadius = (mExtents.x + mExtents.z)*0.5f;
270                        lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius);
271                        lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius);
272                        break;
273                case 2:
274                        lss.mRadius = (mExtents.x + mExtents.y)*0.5f;
275                        lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius);
276                        lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius);
277                        break;
278        }
279}
280
281///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
282/**
283 *      Checks the OBB is inside another OBB.
284 *      \param          box             [in] the other OBB
285 *      \return         TRUE if we're inside the other box
286 */
287///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
288BOOL OBB::IsInside(const OBB& box) const
289{
290        // Make a 4x4 from the box & inverse it
291        Matrix4x4 M0Inv;
292        {
293                Matrix4x4 M0 = box.mRot;
294                M0.SetTrans(box.mCenter);
295                InvertPRMatrix(M0Inv, M0);
296        }
297
298        // With our inversed 4x4, create box1 in space of box0
299        OBB _1in0;
300        Rotate(M0Inv, _1in0);
301
302        // This should cancel out box0's rotation, i.e. it's now an AABB.
303        // => Center(0,0,0), Rot(identity)
304
305        // The two boxes are in the same space so now we can compare them.
306
307        // Create the AABB of (box1 in space of box0)
308        const Matrix3x3& mtx = _1in0.mRot;
309
310        float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x;
311        if(f > _1in0.mCenter.x)         return FALSE;
312        if(-f < _1in0.mCenter.x)        return FALSE;
313
314        f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y;
315        if(f > _1in0.mCenter.y)         return FALSE;
316        if(-f < _1in0.mCenter.y)        return FALSE;
317
318        f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z;
319        if(f > _1in0.mCenter.z)         return FALSE;
320        if(-f < _1in0.mCenter.z)        return FALSE;
321
322        return TRUE;
323}
Note: See TracBrowser for help on using the repository browser.