aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h
parentadding ode source to /libraries (diff)
downloadopensim-SC-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h523
1 files changed, 523 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h b/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h
new file mode 100644
index 0000000..5cc50b5
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h
@@ -0,0 +1,523 @@
1
2// Following code from Magic-Software (http://www.magic-software.com/)
3// A bit modified for Opcode
4
5inline_ float OPC_PointAABBSqrDist(const Point& point, const Point& center, const Point& extents)
6{
7 // Compute coordinates of point in box coordinate system
8 Point Closest = point - center;
9
10 float SqrDistance = 0.0f;
11
12 if(Closest.x < -extents.x)
13 {
14 float Delta = Closest.x + extents.x;
15 SqrDistance += Delta*Delta;
16 }
17 else if(Closest.x > extents.x)
18 {
19 float Delta = Closest.x - extents.x;
20 SqrDistance += Delta*Delta;
21 }
22
23 if(Closest.y < -extents.y)
24 {
25 float Delta = Closest.y + extents.y;
26 SqrDistance += Delta*Delta;
27 }
28 else if(Closest.y > extents.y)
29 {
30 float Delta = Closest.y - extents.y;
31 SqrDistance += Delta*Delta;
32 }
33
34 if(Closest.z < -extents.z)
35 {
36 float Delta = Closest.z + extents.z;
37 SqrDistance += Delta*Delta;
38 }
39 else if(Closest.z > extents.z)
40 {
41 float Delta = Closest.z - extents.z;
42 SqrDistance += Delta*Delta;
43 }
44 return SqrDistance;
45}
46
47static void Face(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, const Point& rkPmE, float* pfLParam, float& rfSqrDistance)
48{
49 Point kPpE;
50 float fLSqr, fInv, fTmp, fParam, fT, fDelta;
51
52 kPpE[i1] = rkPnt[i1] + extents[i1];
53 kPpE[i2] = rkPnt[i2] + extents[i2];
54 if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
55 {
56 if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
57 {
58 // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
59 if(pfLParam)
60 {
61 rkPnt[i0] = extents[i0];
62 fInv = 1.0f/rkDir[i0];
63 rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
64 rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
65 *pfLParam = -rkPmE[i0]*fInv;
66 }
67 }
68 else
69 {
70 // v[i1] >= -e[i1], v[i2] < -e[i2]
71 fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
72 fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
73 if(fTmp <= 2.0f*fLSqr*extents[i1])
74 {
75 fT = fTmp/fLSqr;
76 fLSqr += rkDir[i1]*rkDir[i1];
77 fTmp = kPpE[i1] - fT;
78 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
79 fParam = -fDelta/fLSqr;
80 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
81
82 if(pfLParam)
83 {
84 *pfLParam = fParam;
85 rkPnt[i0] = extents[i0];
86 rkPnt[i1] = fT - extents[i1];
87 rkPnt[i2] = -extents[i2];
88 }
89 }
90 else
91 {
92 fLSqr += rkDir[i1]*rkDir[i1];
93 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
94 fParam = -fDelta/fLSqr;
95 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
96
97 if(pfLParam)
98 {
99 *pfLParam = fParam;
100 rkPnt[i0] = extents[i0];
101 rkPnt[i1] = extents[i1];
102 rkPnt[i2] = -extents[i2];
103 }
104 }
105 }
106 }
107 else
108 {
109 if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
110 {
111 // v[i1] < -e[i1], v[i2] >= -e[i2]
112 fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
113 fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
114 if(fTmp <= 2.0f*fLSqr*extents[i2])
115 {
116 fT = fTmp/fLSqr;
117 fLSqr += rkDir[i2]*rkDir[i2];
118 fTmp = kPpE[i2] - fT;
119 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
120 fParam = -fDelta/fLSqr;
121 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
122
123 if(pfLParam)
124 {
125 *pfLParam = fParam;
126 rkPnt[i0] = extents[i0];
127 rkPnt[i1] = -extents[i1];
128 rkPnt[i2] = fT - extents[i2];
129 }
130 }
131 else
132 {
133 fLSqr += rkDir[i2]*rkDir[i2];
134 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
135 fParam = -fDelta/fLSqr;
136 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
137
138 if(pfLParam)
139 {
140 *pfLParam = fParam;
141 rkPnt[i0] = extents[i0];
142 rkPnt[i1] = -extents[i1];
143 rkPnt[i2] = extents[i2];
144 }
145 }
146 }
147 else
148 {
149 // v[i1] < -e[i1], v[i2] < -e[i2]
150 fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
151 fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
152 if(fTmp >= 0.0f)
153 {
154 // v[i1]-edge is closest
155 if ( fTmp <= 2.0f*fLSqr*extents[i1] )
156 {
157 fT = fTmp/fLSqr;
158 fLSqr += rkDir[i1]*rkDir[i1];
159 fTmp = kPpE[i1] - fT;
160 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
161 fParam = -fDelta/fLSqr;
162 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
163
164 if(pfLParam)
165 {
166 *pfLParam = fParam;
167 rkPnt[i0] = extents[i0];
168 rkPnt[i1] = fT - extents[i1];
169 rkPnt[i2] = -extents[i2];
170 }
171 }
172 else
173 {
174 fLSqr += rkDir[i1]*rkDir[i1];
175 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
176 fParam = -fDelta/fLSqr;
177 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
178
179 if(pfLParam)
180 {
181 *pfLParam = fParam;
182 rkPnt[i0] = extents[i0];
183 rkPnt[i1] = extents[i1];
184 rkPnt[i2] = -extents[i2];
185 }
186 }
187 return;
188 }
189
190 fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
191 fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
192 if(fTmp >= 0.0f)
193 {
194 // v[i2]-edge is closest
195 if(fTmp <= 2.0f*fLSqr*extents[i2])
196 {
197 fT = fTmp/fLSqr;
198 fLSqr += rkDir[i2]*rkDir[i2];
199 fTmp = kPpE[i2] - fT;
200 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
201 fParam = -fDelta/fLSqr;
202 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
203
204 if(pfLParam)
205 {
206 *pfLParam = fParam;
207 rkPnt[i0] = extents[i0];
208 rkPnt[i1] = -extents[i1];
209 rkPnt[i2] = fT - extents[i2];
210 }
211 }
212 else
213 {
214 fLSqr += rkDir[i2]*rkDir[i2];
215 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
216 fParam = -fDelta/fLSqr;
217 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
218
219 if(pfLParam)
220 {
221 *pfLParam = fParam;
222 rkPnt[i0] = extents[i0];
223 rkPnt[i1] = -extents[i1];
224 rkPnt[i2] = extents[i2];
225 }
226 }
227 return;
228 }
229
230 // (v[i1],v[i2])-corner is closest
231 fLSqr += rkDir[i2]*rkDir[i2];
232 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
233 fParam = -fDelta/fLSqr;
234 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
235
236 if(pfLParam)
237 {
238 *pfLParam = fParam;
239 rkPnt[i0] = extents[i0];
240 rkPnt[i1] = -extents[i1];
241 rkPnt[i2] = -extents[i2];
242 }
243 }
244 }
245}
246
247static void CaseNoZeros(Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
248{
249 Point kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
250
251 float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
252
253 fProdDxPy = rkDir.x*kPmE.y;
254 fProdDyPx = rkDir.y*kPmE.x;
255 if(fProdDyPx >= fProdDxPy)
256 {
257 fProdDzPx = rkDir.z*kPmE.x;
258 fProdDxPz = rkDir.x*kPmE.z;
259 if(fProdDzPx >= fProdDxPz)
260 {
261 // line intersects x = e0
262 Face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
263 }
264 else
265 {
266 // line intersects z = e2
267 Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
268 }
269 }
270 else
271 {
272 fProdDzPy = rkDir.z*kPmE.y;
273 fProdDyPz = rkDir.y*kPmE.z;
274 if(fProdDzPy >= fProdDyPz)
275 {
276 // line intersects y = e1
277 Face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
278 }
279 else
280 {
281 // line intersects z = e2
282 Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
283 }
284 }
285}
286
287static void Case0(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
288{
289 float fPmE0 = rkPnt[i0] - extents[i0];
290 float fPmE1 = rkPnt[i1] - extents[i1];
291 float fProd0 = rkDir[i1]*fPmE0;
292 float fProd1 = rkDir[i0]*fPmE1;
293 float fDelta, fInvLSqr, fInv;
294
295 if(fProd0 >= fProd1)
296 {
297 // line intersects P[i0] = e[i0]
298 rkPnt[i0] = extents[i0];
299
300 float fPpE1 = rkPnt[i1] + extents[i1];
301 fDelta = fProd0 - rkDir[i0]*fPpE1;
302 if(fDelta >= 0.0f)
303 {
304 fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
305 rfSqrDistance += fDelta*fDelta*fInvLSqr;
306 if(pfLParam)
307 {
308 rkPnt[i1] = -extents[i1];
309 *pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
310 }
311 }
312 else
313 {
314 if(pfLParam)
315 {
316 fInv = 1.0f/rkDir[i0];
317 rkPnt[i1] -= fProd0*fInv;
318 *pfLParam = -fPmE0*fInv;
319 }
320 }
321 }
322 else
323 {
324 // line intersects P[i1] = e[i1]
325 rkPnt[i1] = extents[i1];
326
327 float fPpE0 = rkPnt[i0] + extents[i0];
328 fDelta = fProd1 - rkDir[i1]*fPpE0;
329 if(fDelta >= 0.0f)
330 {
331 fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
332 rfSqrDistance += fDelta*fDelta*fInvLSqr;
333 if(pfLParam)
334 {
335 rkPnt[i0] = -extents[i0];
336 *pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
337 }
338 }
339 else
340 {
341 if(pfLParam)
342 {
343 fInv = 1.0f/rkDir[i1];
344 rkPnt[i0] -= fProd1*fInv;
345 *pfLParam = -fPmE1*fInv;
346 }
347 }
348 }
349
350 if(rkPnt[i2] < -extents[i2])
351 {
352 fDelta = rkPnt[i2] + extents[i2];
353 rfSqrDistance += fDelta*fDelta;
354 rkPnt[i2] = -extents[i2];
355 }
356 else if ( rkPnt[i2] > extents[i2] )
357 {
358 fDelta = rkPnt[i2] - extents[i2];
359 rfSqrDistance += fDelta*fDelta;
360 rkPnt[i2] = extents[i2];
361 }
362}
363
364static void Case00(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
365{
366 float fDelta;
367
368 if(pfLParam)
369 *pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
370
371 rkPnt[i0] = extents[i0];
372
373 if(rkPnt[i1] < -extents[i1])
374 {
375 fDelta = rkPnt[i1] + extents[i1];
376 rfSqrDistance += fDelta*fDelta;
377 rkPnt[i1] = -extents[i1];
378 }
379 else if(rkPnt[i1] > extents[i1])
380 {
381 fDelta = rkPnt[i1] - extents[i1];
382 rfSqrDistance += fDelta*fDelta;
383 rkPnt[i1] = extents[i1];
384 }
385
386 if(rkPnt[i2] < -extents[i2])
387 {
388 fDelta = rkPnt[i2] + extents[i2];
389 rfSqrDistance += fDelta*fDelta;
390 rkPnt[i1] = -extents[i2];
391 }
392 else if(rkPnt[i2] > extents[i2])
393 {
394 fDelta = rkPnt[i2] - extents[i2];
395 rfSqrDistance += fDelta*fDelta;
396 rkPnt[i2] = extents[i2];
397 }
398}
399
400static void Case000(Point& rkPnt, const Point& extents, float& rfSqrDistance)
401{
402 float fDelta;
403
404 if(rkPnt.x < -extents.x)
405 {
406 fDelta = rkPnt.x + extents.x;
407 rfSqrDistance += fDelta*fDelta;
408 rkPnt.x = -extents.x;
409 }
410 else if(rkPnt.x > extents.x)
411 {
412 fDelta = rkPnt.x - extents.x;
413 rfSqrDistance += fDelta*fDelta;
414 rkPnt.x = extents.x;
415 }
416
417 if(rkPnt.y < -extents.y)
418 {
419 fDelta = rkPnt.y + extents.y;
420 rfSqrDistance += fDelta*fDelta;
421 rkPnt.y = -extents.y;
422 }
423 else if(rkPnt.y > extents.y)
424 {
425 fDelta = rkPnt.y - extents.y;
426 rfSqrDistance += fDelta*fDelta;
427 rkPnt.y = extents.y;
428 }
429
430 if(rkPnt.z < -extents.z)
431 {
432 fDelta = rkPnt.z + extents.z;
433 rfSqrDistance += fDelta*fDelta;
434 rkPnt.z = -extents.z;
435 }
436 else if(rkPnt.z > extents.z)
437 {
438 fDelta = rkPnt.z - extents.z;
439 rfSqrDistance += fDelta*fDelta;
440 rkPnt.z = extents.z;
441 }
442}
443
444static float SqrDistance(const Ray& rkLine, const Point& center, const Point& extents, float* pfLParam)
445{
446 // compute coordinates of line in box coordinate system
447 Point kDiff = rkLine.mOrig - center;
448 Point kPnt = kDiff;
449 Point kDir = rkLine.mDir;
450
451 // Apply reflections so that direction vector has nonnegative components.
452 bool bReflect[3];
453 for(int i=0;i<3;i++)
454 {
455 if(kDir[i]<0.0f)
456 {
457 kPnt[i] = -kPnt[i];
458 kDir[i] = -kDir[i];
459 bReflect[i] = true;
460 }
461 else
462 {
463 bReflect[i] = false;
464 }
465 }
466
467 float fSqrDistance = 0.0f;
468
469 if(kDir.x>0.0f)
470 {
471 if(kDir.y>0.0f)
472 {
473 if(kDir.z>0.0f) CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,+)
474 else Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,0)
475 }
476 else
477 {
478 if(kDir.z>0.0f) Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,+)
479 else Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,0)
480 }
481 }
482 else
483 {
484 if(kDir.y>0.0f)
485 {
486 if(kDir.z>0.0f) Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,+)
487 else Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,0)
488 }
489 else
490 {
491 if(kDir.z>0.0f) Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,0,+)
492 else
493 {
494 Case000(kPnt, extents, fSqrDistance); // (0,0,0)
495 if(pfLParam) *pfLParam = 0.0f;
496 }
497 }
498 }
499 return fSqrDistance;
500}
501
502inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0)
503{
504 float fLP;
505 float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP);
506 if(fLP>=0.0f)
507 {
508 if(fLP<=1.0f) return fSqrDistance;
509 else return OPC_PointAABBSqrDist(segment.mP1, c0, e0);
510 }
511 else return OPC_PointAABBSqrDist(segment.mP0, c0, e0);
512}
513
514inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents)
515{
516 // Stats
517 mNbVolumeBVTests++;
518
519 float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents);
520 if(s2<mRadius2) return TRUE;
521
522 return FALSE;
523}
generated by cgit v1.1 at 2024-12-28 10:01:16 +0000