Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h @ 216

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

[Physik] add ode-0.9
File size: 15.9 KB
Line 
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}
Note: See TracBrowser for help on using the repository browser.