aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:24:38 +0000
committerdan miller2007-10-19 05:24:38 +0000
commitf205de7847da7ae1c10212d82e7042d0100b4ce0 (patch)
tree9acc9608a6880502aaeda43af52c33e278e95b9c /libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp
parenttrying to fix my screwup part deux (diff)
downloadopensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.zip
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.gz
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.bz2
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.xz
from the start... checking in ode-0.9
Diffstat (limited to 'libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp')
-rw-r--r--libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp323
1 files changed, 323 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp b/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp
new file mode 100644
index 0000000..0b1b6f7
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp
@@ -0,0 +1,323 @@
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}
generated by cgit v1.1 at 2024-12-26 23:59:07 +0000