Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

[Physik] add ode-0.9
File size: 20.7 KB
Line 
1// Following code from Magic-Software (http://www.magic-software.com/)
2// A bit modified for Opcode
3
4static const float gs_fTolerance = 1e-05f;
5
6static float OPC_PointTriangleSqrDist(const Point& point, const Point& p0, const Point& p1, const Point& p2)
7{
8        // Hook
9        Point TriEdge0 = p1 - p0;
10        Point TriEdge1 = p2 - p0;
11
12        Point kDiff     = p0 - point;
13        float fA00      = TriEdge0.SquareMagnitude();
14        float fA01      = TriEdge0 | TriEdge1;
15        float fA11      = TriEdge1.SquareMagnitude();
16        float fB0       = kDiff | TriEdge0;
17        float fB1       = kDiff | TriEdge1;
18        float fC        = kDiff.SquareMagnitude();
19        float fDet      = fabsf(fA00*fA11 - fA01*fA01);
20        float fS        = fA01*fB1-fA11*fB0;
21        float fT        = fA01*fB0-fA00*fB1;
22        float fSqrDist;
23
24        if(fS + fT <= fDet)
25        {
26                if(fS < 0.0f)
27                {
28                        if(fT < 0.0f)  // region 4
29                        {
30                                if(fB0 < 0.0f)
31                                {
32                                        if(-fB0 >= fA00)                fSqrDist = fA00+2.0f*fB0+fC;
33                                        else                                    fSqrDist = fB0*(-fB0/fA00)+fC;
34                                }
35                                else
36                                {
37                                        if(fB1 >= 0.0f)                 fSqrDist = fC;
38                                        else if(-fB1 >= fA11)   fSqrDist = fA11+2.0f*fB1+fC;
39                                        else                                    fSqrDist = fB1*(-fB1/fA11)+fC;
40                                }
41                        }
42                        else  // region 3
43                        {
44                                if(fB1 >= 0.0f)                         fSqrDist = fC;
45                                else if(-fB1 >= fA11)           fSqrDist = fA11+2.0f*fB1+fC;
46                                else                                            fSqrDist = fB1*(-fB1/fA11)+fC;
47                        }
48                }
49                else if(fT < 0.0f)  // region 5
50                {
51                        if(fB0 >= 0.0f)                                 fSqrDist = fC;
52                        else if(-fB0 >= fA00)                   fSqrDist = fA00+2.0f*fB0+fC;
53                        else                                                    fSqrDist = fB0*(-fB0/fA00)+fC;
54                }
55                else  // region 0
56                {
57                        // minimum at interior point
58                        if(fDet==0.0f)
59                        {
60                                fSqrDist = MAX_FLOAT;
61                        }
62                        else
63                        {
64                                float fInvDet = 1.0f/fDet;
65                                fS *= fInvDet;
66                                fT *= fInvDet;
67                                fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
68                        }
69                }
70        }
71        else
72        {
73                float fTmp0, fTmp1, fNumer, fDenom;
74
75                if(fS < 0.0f)  // region 2
76                {
77                        fTmp0 = fA01 + fB0;
78                        fTmp1 = fA11 + fB1;
79                        if(fTmp1 > fTmp0)
80                        {
81                                fNumer = fTmp1 - fTmp0;
82                                fDenom = fA00-2.0f*fA01+fA11;
83                                if(fNumer >= fDenom)
84                                {
85                                        fSqrDist = fA00+2.0f*fB0+fC;
86                                }
87                                else
88                                {
89                                        fS = fNumer/fDenom;
90                                        fT = 1.0f - fS;
91                                        fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
92                                }
93                        }
94                        else
95                        {
96                                if(fTmp1 <= 0.0f)               fSqrDist = fA11+2.0f*fB1+fC;
97                                else if(fB1 >= 0.0f)    fSqrDist = fC;
98                                else                                    fSqrDist = fB1*(-fB1/fA11)+fC;
99                        }
100                }
101                else if(fT < 0.0f)  // region 6
102                {
103                        fTmp0 = fA01 + fB1;
104                        fTmp1 = fA00 + fB0;
105                        if(fTmp1 > fTmp0)
106                        {
107                                fNumer = fTmp1 - fTmp0;
108                                fDenom = fA00-2.0f*fA01+fA11;
109                                if(fNumer >= fDenom)
110                                {
111                                        fSqrDist = fA11+2.0f*fB1+fC;
112                                }
113                                else
114                                {
115                                        fT = fNumer/fDenom;
116                                        fS = 1.0f - fT;
117                                        fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
118                                }
119                        }
120                        else
121                        {
122                                if(fTmp1 <= 0.0f)               fSqrDist = fA00+2.0f*fB0+fC;
123                                else if(fB0 >= 0.0f)    fSqrDist = fC;
124                                else                                    fSqrDist = fB0*(-fB0/fA00)+fC;
125                        }
126                }
127                else  // region 1
128                {
129                        fNumer = fA11 + fB1 - fA01 - fB0;
130                        if(fNumer <= 0.0f)
131                        {
132                                fSqrDist = fA11+2.0f*fB1+fC;
133                        }
134                        else
135                        {
136                                fDenom = fA00-2.0f*fA01+fA11;
137                                if(fNumer >= fDenom)
138                                {
139                                        fSqrDist = fA00+2.0f*fB0+fC;
140                                }
141                                else
142                                {
143                                        fS = fNumer/fDenom;
144                                        fT = 1.0f - fS;
145                                        fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
146                                }
147                        }
148                }
149        }
150        return fabsf(fSqrDist);
151}
152
153static float OPC_SegmentSegmentSqrDist(const Segment& rkSeg0, const Segment& rkSeg1)
154{
155        // Hook
156        Point rkSeg0Direction   = rkSeg0.ComputeDirection();
157        Point rkSeg1Direction   = rkSeg1.ComputeDirection();
158
159        Point kDiff     = rkSeg0.mP0 - rkSeg1.mP0;
160        float fA00      = rkSeg0Direction.SquareMagnitude();
161        float fA01      = -rkSeg0Direction.Dot(rkSeg1Direction);
162        float fA11      = rkSeg1Direction.SquareMagnitude();
163        float fB0       = kDiff.Dot(rkSeg0Direction);
164        float fC        = kDiff.SquareMagnitude();
165        float fDet      = fabsf(fA00*fA11-fA01*fA01);
166
167        float fB1, fS, fT, fSqrDist, fTmp;
168
169        if(fDet>=gs_fTolerance)
170        {
171                // line segments are not parallel
172                fB1 = -kDiff.Dot(rkSeg1Direction);
173                fS = fA01*fB1-fA11*fB0;
174                fT = fA01*fB0-fA00*fB1;
175
176                if(fS >= 0.0f)
177                {
178                        if(fS <= fDet)
179                        {
180                                if(fT >= 0.0f)
181                                {
182                                        if(fT <= fDet)  // region 0 (interior)
183                                        {
184                                                // minimum at two interior points of 3D lines
185                                                float fInvDet = 1.0f/fDet;
186                                                fS *= fInvDet;
187                                                fT *= fInvDet;
188                                                fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
189                                        }
190                                        else  // region 3 (side)
191                                        {
192                                                fTmp = fA01+fB0;
193                                                if(fTmp>=0.0f)                  fSqrDist = fA11+2.0f*fB1+fC;
194                                                else if(-fTmp>=fA00)    fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
195                                                else                                    fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
196                                        }
197                                }
198                                else  // region 7 (side)
199                                {
200                                        if(fB0>=0.0f)                           fSqrDist = fC;
201                                        else if(-fB0>=fA00)                     fSqrDist = fA00+2.0f*fB0+fC;
202                                        else                                            fSqrDist = fB0*(-fB0/fA00)+fC;
203                                }
204                        }
205                        else
206                        {
207                                if ( fT >= 0.0 )
208                                {
209                                        if ( fT <= fDet )  // region 1 (side)
210                                        {
211                                                fTmp = fA01+fB1;
212                                                if(fTmp>=0.0f)                  fSqrDist = fA00+2.0f*fB0+fC;
213                                                else if(-fTmp>=fA11)    fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
214                                                else                                    fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
215                                        }
216                                        else  // region 2 (corner)
217                                        {
218                                                fTmp = fA01+fB0;
219                                                if ( -fTmp <= fA00 )
220                                                {
221                                                        if(fTmp>=0.0f)          fSqrDist = fA11+2.0f*fB1+fC;
222                                                        else                            fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
223                                                }
224                                                else
225                                                {
226                                                        fTmp = fA01+fB1;
227                                                        if(fTmp>=0.0f)                  fSqrDist = fA00+2.0f*fB0+fC;
228                                                        else if(-fTmp>=fA11)    fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
229                                                        else                                    fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
230                                                }
231                                        }
232                                }
233                                else  // region 8 (corner)
234                                {
235                                        if ( -fB0 < fA00 )
236                                        {
237                                                if(fB0>=0.0f)   fSqrDist = fC;
238                                                else                    fSqrDist = fB0*(-fB0/fA00)+fC;
239                                        }
240                                        else
241                                        {
242                                                fTmp = fA01+fB1;
243                                                if(fTmp>=0.0f)                  fSqrDist = fA00+2.0f*fB0+fC;
244                                                else if(-fTmp>=fA11)    fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
245                                                else                                    fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
246                                        }
247                                }
248                        }
249                }
250                else 
251                {
252                        if ( fT >= 0.0f )
253                        {
254                                if ( fT <= fDet )  // region 5 (side)
255                                {
256                                        if(fB1>=0.0f)           fSqrDist = fC;
257                                        else if(-fB1>=fA11)     fSqrDist = fA11+2.0f*fB1+fC;
258                                        else                            fSqrDist = fB1*(-fB1/fA11)+fC;
259                                }
260                                else  // region 4 (corner)
261                                {
262                                        fTmp = fA01+fB0;
263                                        if ( fTmp < 0.0f )
264                                        {
265                                                if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
266                                                else                    fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
267                                        }
268                                        else
269                                        {
270                                                if(fB1>=0.0f)           fSqrDist = fC;
271                                                else if(-fB1>=fA11)     fSqrDist = fA11+2.0f*fB1+fC;
272                                                else                            fSqrDist = fB1*(-fB1/fA11)+fC;
273                                        }
274                                }
275                        }
276                        else   // region 6 (corner)
277                        {
278                                if ( fB0 < 0.0f )
279                                {
280                                        if(-fB0>=fA00)  fSqrDist = fA00+2.0f*fB0+fC;
281                                        else                    fSqrDist = fB0*(-fB0/fA00)+fC;
282                                }
283                                else
284                                {
285                                        if(fB1>=0.0f)           fSqrDist = fC;
286                                        else if(-fB1>=fA11)     fSqrDist = fA11+2.0f*fB1+fC;
287                                        else                            fSqrDist = fB1*(-fB1/fA11)+fC;
288                                }
289                        }
290                }
291        }
292        else
293        {
294                // line segments are parallel
295                if ( fA01 > 0.0f )
296                {
297                        // direction vectors form an obtuse angle
298                        if ( fB0 >= 0.0f )
299                        {
300                                fSqrDist = fC;
301                        }
302                        else if ( -fB0 <= fA00 )
303                        {
304                                fSqrDist = fB0*(-fB0/fA00)+fC;
305                        }
306                        else
307                        {
308                                fB1 = -kDiff.Dot(rkSeg1Direction);
309                                fTmp = fA00+fB0;
310                                if ( -fTmp >= fA01 )
311                                {
312                                        fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
313                                }
314                                else
315                                {
316                                        fT = -fTmp/fA01;
317                                        fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
318                                }
319                        }
320                }
321                else
322                {
323                        // direction vectors form an acute angle
324                        if ( -fB0 >= fA00 )
325                        {
326                                fSqrDist = fA00+2.0f*fB0+fC;
327                        }
328                        else if ( fB0 <= 0.0f )
329                        {
330                                fSqrDist = fB0*(-fB0/fA00)+fC;
331                        }
332                        else
333                        {
334                                fB1 = -kDiff.Dot(rkSeg1Direction);
335                                if ( fB0 >= -fA01 )
336                                {
337                                        fSqrDist = fA11+2.0f*fB1+fC;
338                                }
339                                else
340                                {
341                                        fT = -fB0/fA01;
342                                        fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
343                                }
344                        }
345                }
346        }
347        return fabsf(fSqrDist);
348}
349
350inline_ float OPC_SegmentRaySqrDist(const Segment& rkSeg0, const Ray& rkSeg1)
351{
352        return OPC_SegmentSegmentSqrDist(rkSeg0, Segment(rkSeg1.mOrig, rkSeg1.mOrig + rkSeg1.mDir));
353}
354
355static float OPC_SegmentTriangleSqrDist(const Segment& segment, const Point& p0, const Point& p1, const Point& p2)
356{
357        // Hook
358        const Point TriEdge0 = p1 - p0;
359        const Point TriEdge1 = p2 - p0;
360
361        const Point& rkSegOrigin        = segment.GetOrigin();
362        Point rkSegDirection            = segment.ComputeDirection();
363
364        Point kDiff = p0 - rkSegOrigin;
365        float fA00 = rkSegDirection.SquareMagnitude();
366        float fA01 = -rkSegDirection.Dot(TriEdge0);
367        float fA02 = -rkSegDirection.Dot(TriEdge1);
368        float fA11 = TriEdge0.SquareMagnitude();
369        float fA12 = TriEdge0.Dot(TriEdge1);
370        float fA22 = TriEdge1.Dot(TriEdge1);
371        float fB0  = -kDiff.Dot(rkSegDirection);
372        float fB1  = kDiff.Dot(TriEdge0);
373        float fB2  = kDiff.Dot(TriEdge1);
374        float fCof00 = fA11*fA22-fA12*fA12;
375        float fCof01 = fA02*fA12-fA01*fA22;
376        float fCof02 = fA01*fA12-fA02*fA11;
377        float fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
378
379        Ray kTriSeg;
380        Point kPt;
381        float fSqrDist, fSqrDist0;
382
383        if(fabsf(fDet)>=gs_fTolerance)
384        {
385                float fCof11 = fA00*fA22-fA02*fA02;
386                float fCof12 = fA02*fA01-fA00*fA12;
387                float fCof22 = fA00*fA11-fA01*fA01;
388                float fInvDet = 1.0f/fDet;
389                float fRhs0 = -fB0*fInvDet;
390                float fRhs1 = -fB1*fInvDet;
391                float fRhs2 = -fB2*fInvDet;
392
393                float fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
394                float fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
395                float fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
396
397                if ( fR < 0.0f )
398                {
399                        if ( fS+fT <= 1.0f )
400                        {
401                                if ( fS < 0.0f )
402                                {
403                                        if ( fT < 0.0f )  // region 4m
404                                        {
405                                                // min on face s=0 or t=0 or r=0
406                                                kTriSeg.mOrig = p0;
407                                                kTriSeg.mDir = TriEdge1;
408                                                fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
409                                                kTriSeg.mOrig = p0;
410                                                kTriSeg.mDir = TriEdge0;
411                                                fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
412                                                if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
413                                                fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
414                                                if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
415                    }
416                    else  // region 3m
417                    {
418                        // min on face s=0 or r=0
419                        kTriSeg.mOrig = p0;
420                        kTriSeg.mDir = TriEdge1;
421                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
422                        fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
423                        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
424                    }
425                }
426                else if ( fT < 0.0f )  // region 5m
427                {
428                    // min on face t=0 or r=0
429                    kTriSeg.mOrig = p0;
430                    kTriSeg.mDir = TriEdge0;
431                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
432                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
433                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
434                }
435                else  // region 0m
436                {
437                    // min on face r=0
438                    fSqrDist = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
439                }
440            }
441            else
442            {
443                if ( fS < 0.0f )  // region 2m
444                {
445                    // min on face s=0 or s+t=1 or r=0
446                    kTriSeg.mOrig = p0;
447                    kTriSeg.mDir = TriEdge1;
448                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
449                    kTriSeg.mOrig = p1;
450                    kTriSeg.mDir = TriEdge1-TriEdge0;
451                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
452                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
453                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
454                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
455                }
456                else if ( fT < 0.0f )  // region 6m
457                {
458                    // min on face t=0 or s+t=1 or r=0
459                    kTriSeg.mOrig = p0;
460                    kTriSeg.mDir = TriEdge0;
461                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
462                    kTriSeg.mOrig = p1;
463                    kTriSeg.mDir = TriEdge1-TriEdge0;
464                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
465                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
466                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
467                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
468                }
469                else  // region 1m
470                {
471                    // min on face s+t=1 or r=0
472                    kTriSeg.mOrig = p1;
473                    kTriSeg.mDir = TriEdge1-TriEdge0;
474                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
475                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
476                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
477                }
478            }
479        }
480        else if ( fR <= 1.0f )
481        {
482            if ( fS+fT <= 1.0f )
483            {
484                if ( fS < 0.0f )
485                {
486                    if ( fT < 0.0f )  // region 4
487                    {
488                        // min on face s=0 or t=0
489                        kTriSeg.mOrig = p0;
490                        kTriSeg.mDir = TriEdge1;
491                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
492                        kTriSeg.mOrig = p0;
493                        kTriSeg.mDir = TriEdge0;
494                        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
495                        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
496                    }
497                    else  // region 3
498                    {
499                        // min on face s=0
500                        kTriSeg.mOrig = p0;
501                        kTriSeg.mDir = TriEdge1;
502                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
503                    }
504                }
505                else if ( fT < 0.0f )  // region 5
506                {
507                    // min on face t=0
508                    kTriSeg.mOrig = p0;
509                    kTriSeg.mDir = TriEdge0;
510                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
511                }
512                else  // region 0
513                {
514                    // global minimum is interior, done
515                    fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0)
516                          +fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1)
517                          +fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2)
518                          +kDiff.SquareMagnitude();
519                }
520            }
521            else
522            {
523                if ( fS < 0.0f )  // region 2
524                {
525                    // min on face s=0 or s+t=1
526                    kTriSeg.mOrig = p0;
527                    kTriSeg.mDir = TriEdge1;
528                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
529                    kTriSeg.mOrig = p1;
530                    kTriSeg.mDir = TriEdge1-TriEdge0;
531                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
532                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
533                }
534                else if ( fT < 0.0f )  // region 6
535                {
536                    // min on face t=0 or s+t=1
537                    kTriSeg.mOrig = p0;
538                    kTriSeg.mDir = TriEdge0;
539                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
540                    kTriSeg.mOrig = p1;
541                    kTriSeg.mDir = TriEdge1-TriEdge0;
542                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
543                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
544                }
545                else  // region 1
546                {
547                    // min on face s+t=1
548                    kTriSeg.mOrig = p1;
549                    kTriSeg.mDir = TriEdge1-TriEdge0;
550                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
551                }
552            }
553        }
554        else  // fR > 1
555        {
556            if ( fS+fT <= 1.0f )
557            {
558                if ( fS < 0.0f )
559                {
560                    if ( fT < 0.0f )  // region 4p
561                    {
562                        // min on face s=0 or t=0 or r=1
563                        kTriSeg.mOrig = p0;
564                        kTriSeg.mDir = TriEdge1;
565                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
566                        kTriSeg.mOrig = p0;
567                        kTriSeg.mDir = TriEdge0;
568                        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
569                        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
570                        kPt = rkSegOrigin+rkSegDirection;
571                        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
572                        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
573                    }
574                    else  // region 3p
575                    {
576                        // min on face s=0 or r=1
577                        kTriSeg.mOrig = p0;
578                        kTriSeg.mDir = TriEdge1;
579                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
580                        kPt = rkSegOrigin+rkSegDirection;
581                        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
582                        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
583                    }
584                }
585                else if ( fT < 0.0f )  // region 5p
586                {
587                    // min on face t=0 or r=1
588                    kTriSeg.mOrig = p0;
589                    kTriSeg.mDir = TriEdge0;
590                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
591                    kPt = rkSegOrigin+rkSegDirection;
592                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
593                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
594                }
595                else  // region 0p
596                {
597                    // min face on r=1
598                    kPt = rkSegOrigin+rkSegDirection;
599                    fSqrDist = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
600                }
601            }
602            else
603            {
604                if ( fS < 0.0f )  // region 2p
605                {
606                    // min on face s=0 or s+t=1 or r=1
607                    kTriSeg.mOrig = p0;
608                    kTriSeg.mDir = TriEdge1;
609                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
610                    kTriSeg.mOrig = p1;
611                    kTriSeg.mDir = TriEdge1-TriEdge0;
612                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
613                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
614                    kPt = rkSegOrigin+rkSegDirection;
615                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
616                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
617                }
618                else if ( fT < 0.0f )  // region 6p
619                {
620                    // min on face t=0 or s+t=1 or r=1
621                    kTriSeg.mOrig = p0;
622                    kTriSeg.mDir = TriEdge0;
623                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
624                    kTriSeg.mOrig = p1;
625                    kTriSeg.mDir = TriEdge1-TriEdge0;
626                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
627                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
628                    kPt = rkSegOrigin+rkSegDirection;
629                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
630                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
631                }
632                else  // region 1p
633                {
634                    // min on face s+t=1 or r=1
635                    kTriSeg.mOrig = p1;
636                    kTriSeg.mDir = TriEdge1-TriEdge0;
637                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
638                    kPt = rkSegOrigin+rkSegDirection;
639                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
640                    if(fSqrDist0<fSqrDist)      fSqrDist = fSqrDist0;
641                }
642            }
643        }
644    }
645    else
646    {
647        // segment and triangle are parallel
648        kTriSeg.mOrig = p0;
649        kTriSeg.mDir = TriEdge0;
650        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
651
652        kTriSeg.mDir = TriEdge1;
653        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
654        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
655
656        kTriSeg.mOrig = p1;
657        kTriSeg.mDir = TriEdge1 - TriEdge0;
658        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
659        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
660
661        fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
662        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
663
664        kPt = rkSegOrigin+rkSegDirection;
665        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
666        if(fSqrDist0<fSqrDist)  fSqrDist = fSqrDist0;
667    }
668    return fabsf(fSqrDist);
669}
670
671inline_ BOOL LSSCollider::LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
672{
673        // Stats
674        mNbVolumePrimTests++;
675
676        float s2 = OPC_SegmentTriangleSqrDist(mSeg, vert0, vert1, vert2);
677        if(s2<mRadius2) return TRUE;
678        return FALSE;
679}
Note: See TracBrowser for help on using the repository browser.