1
2 // Following code from Magic-Software (http://www.magic-software.com/)
3 // A bit modified for Opcode
4
OPC_PointAABBSqrDist(const Point & point,const Point & center,const Point & extents)5 inline_ 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
Face(int i0,int i1,int i2,Point & rkPnt,const Point & rkDir,const Point & extents,const Point & rkPmE,float * pfLParam,float & rfSqrDistance)47 static 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
CaseNoZeros(Point & rkPnt,const Point & rkDir,const Point & extents,float * pfLParam,float & rfSqrDistance)247 static 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
Case0(int i0,int i1,int i2,Point & rkPnt,const Point & rkDir,const Point & extents,float * pfLParam,float & rfSqrDistance)287 static 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
Case00(int i0,int i1,int i2,Point & rkPnt,const Point & rkDir,const Point & extents,float * pfLParam,float & rfSqrDistance)364 static 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
Case000(Point & rkPnt,const Point & extents,float & rfSqrDistance)400 static 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
SqrDistance(const Ray & rkLine,const Point & center,const Point & extents,float * pfLParam)444 static 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 #if 0
452 // Apply reflections so that direction vector has nonnegative components.
453 bool bReflect[3];
454 for(int i=0;i<3;i++)
455 {
456 if(kDir[i]<0.0f)
457 {
458 kPnt[i] = -kPnt[i];
459 kDir[i] = -kDir[i];
460 bReflect[i] = true;
461 }
462 else
463 {
464 bReflect[i] = false;
465 }
466 }
467 #endif
468
469 float fSqrDistance = 0.0f;
470
471 if(kDir.x>0.0f)
472 {
473 if(kDir.y>0.0f)
474 {
475 if(kDir.z>0.0f) CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,+)
476 else Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,0)
477 }
478 else
479 {
480 if(kDir.z>0.0f) Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,+)
481 else Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,0)
482 }
483 }
484 else
485 {
486 if(kDir.y>0.0f)
487 {
488 if(kDir.z>0.0f) Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,+)
489 else Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,0)
490 }
491 else
492 {
493 if(kDir.z>0.0f) Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,0,+)
494 else
495 {
496 Case000(kPnt, extents, fSqrDistance); // (0,0,0)
497 if(pfLParam) *pfLParam = 0.0f;
498 }
499 }
500 }
501 return fSqrDistance;
502 }
503
OPC_SegmentOBBSqrDist(const Segment & segment,const Point & c0,const Point & e0)504 inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0)
505 {
506 float fLP;
507 float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP);
508 if(fLP>=0.0f)
509 {
510 if(fLP<=1.0f) return fSqrDistance;
511 else return OPC_PointAABBSqrDist(segment.mP1, c0, e0);
512 }
513 else return OPC_PointAABBSqrDist(segment.mP0, c0, e0);
514 }
515
LSSAABBOverlap(const Point & center,const Point & extents)516 inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents)
517 {
518 // Stats
519 mNbVolumeBVTests++;
520
521 float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents);
522 if(s2<mRadius2) return TRUE;
523
524 return FALSE;
525 }
526