Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/ode/src/collision_trimesh_distance.cpp @ 216

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

[Physik] add ode-0.9
File size: 37.3 KB
Line 
1// This file contains some code based on the code from Magic Software.
2// That code is available under a Free Source License Agreement
3// that can be found at http://www.magic-software.com/License/free.pdf
4 
5#include <ode/common.h>
6#include <ode/odemath.h>
7#include <ode/collision.h>
8#define TRIMESH_INTERNAL
9#include "collision_trimesh_internal.h"
10
11//------------------------------------------------------------------------------
12/**
13  @brief Finds the shortest distance squared between a point and a triangle.
14 
15  @param pfSParam  Barycentric coordinate of triangle at point closest to p (u)
16  @param pfTParam  Barycentric coordinate of triangle at point closest to p (v)
17  @return Shortest distance squared.
18 
19  The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
20 
21  Taken from:
22  Magic Software, Inc.
23  http://www.magic-software.com
24*/
25dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, 
26                           const dVector3 triEdge0, const dVector3 triEdge1,
27                           dReal* pfSParam, dReal* pfTParam )
28{
29  dVector3 kDiff;
30  Vector3Subtract( triOrigin, p, kDiff );
31  dReal fA00 = dDOT( triEdge0, triEdge0 );
32  dReal fA01 = dDOT( triEdge0, triEdge1 );
33  dReal fA11 = dDOT( triEdge1, triEdge1 );
34  dReal fB0 = dDOT( kDiff, triEdge0 );
35  dReal fB1 = dDOT( kDiff, triEdge1 );
36  dReal fC = dDOT( kDiff, kDiff );
37  dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
38  dReal fS = fA01*fB1-fA11*fB0;
39  dReal fT = fA01*fB0-fA00*fB1;
40  dReal fSqrDist;
41
42  if ( fS + fT <= fDet )
43  {
44    if ( fS < REAL(0.0) )
45    {
46      if ( fT < REAL(0.0) )  // region 4
47      {
48        if ( fB0 < REAL(0.0) )
49        {
50          fT = REAL(0.0);
51          if ( -fB0 >= fA00 )
52          {
53            fS = REAL(1.0);
54            fSqrDist = fA00+REAL(2.0)*fB0+fC;
55          }
56          else
57          {
58            fS = -fB0/fA00;
59            fSqrDist = fB0*fS+fC;
60          }
61        }
62        else
63        {
64          fS = REAL(0.0);
65          if ( fB1 >= REAL(0.0) )
66          {
67            fT = REAL(0.0);
68            fSqrDist = fC;
69          }
70          else if ( -fB1 >= fA11 )
71          {
72            fT = REAL(1.0);
73            fSqrDist = fA11+REAL(2.0)*fB1+fC;
74          }
75          else
76          {
77            fT = -fB1/fA11;
78            fSqrDist = fB1*fT+fC;
79          }
80        }
81      }
82      else  // region 3
83      {
84        fS = REAL(0.0);
85        if ( fB1 >= REAL(0.0) )
86        {
87          fT = REAL(0.0);
88          fSqrDist = fC;
89        }
90        else if ( -fB1 >= fA11 )
91        {
92          fT = REAL(1.0);
93          fSqrDist = fA11+REAL(2.0)*fB1+fC;
94        }
95        else
96        {
97          fT = -fB1/fA11;
98          fSqrDist = fB1*fT+fC;
99        }
100      }
101    }
102    else if ( fT < REAL(0.0) )  // region 5
103    {
104      fT = REAL(0.0);
105      if ( fB0 >= REAL(0.0) )
106      {
107        fS = REAL(0.0);
108        fSqrDist = fC;
109      }
110      else if ( -fB0 >= fA00 )
111      {
112        fS = REAL(1.0);
113        fSqrDist = fA00+REAL(2.0)*fB0+fC;
114      }
115      else
116      {
117        fS = -fB0/fA00;
118        fSqrDist = fB0*fS+fC;
119      }
120    }
121    else  // region 0
122    {
123      // minimum at interior point
124      if ( fDet == REAL(0.0) )
125      {
126        fS = REAL(0.0);
127        fT = REAL(0.0);
128        fSqrDist = dInfinity;
129      } 
130      else
131      {
132        dReal fInvDet = REAL(1.0)/fDet;
133        fS *= fInvDet;
134        fT *= fInvDet;
135        fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
136                   fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
137      }
138    }
139  }
140  else
141  {
142    dReal fTmp0, fTmp1, fNumer, fDenom;
143
144    if ( fS < REAL(0.0) )  // region 2
145    {
146      fTmp0 = fA01 + fB0;
147      fTmp1 = fA11 + fB1;
148      if ( fTmp1 > fTmp0 )
149      {
150        fNumer = fTmp1 - fTmp0;
151        fDenom = fA00-REAL(2.0)*fA01+fA11;
152        if ( fNumer >= fDenom )
153        {
154          fS = REAL(1.0);
155          fT = REAL(0.0);
156          fSqrDist = fA00+REAL(2.0)*fB0+fC;
157        }
158        else
159        {
160          fS = fNumer/fDenom;
161          fT = REAL(1.0) - fS;
162          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
163                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
164        }
165      }
166      else
167      {
168        fS = REAL(0.0);
169        if ( fTmp1 <= REAL(0.0) )
170        {
171          fT = REAL(1.0);
172          fSqrDist = fA11+REAL(2.0)*fB1+fC;
173        }
174        else if ( fB1 >= REAL(0.0) )
175        {
176          fT = REAL(0.0);
177          fSqrDist = fC;
178        }
179        else
180        {
181          fT = -fB1/fA11;
182          fSqrDist = fB1*fT+fC;
183        }
184      }
185    }
186    else if ( fT < REAL(0.0) )  // region 6
187    {
188      fTmp0 = fA01 + fB1;
189      fTmp1 = fA00 + fB0;
190      if ( fTmp1 > fTmp0 )
191      {
192        fNumer = fTmp1 - fTmp0;
193        fDenom = fA00-REAL(2.0)*fA01+fA11;
194        if ( fNumer >= fDenom )
195        {
196          fT = REAL(1.0);
197          fS = REAL(0.0);
198          fSqrDist = fA11+REAL(2.0)*fB1+fC;
199        }
200        else
201        {
202          fT = fNumer/fDenom;
203          fS = REAL(1.0) - fT;
204          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
205                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
206        }
207      }
208      else
209      {
210        fT = REAL(0.0);
211        if ( fTmp1 <= REAL(0.0) )
212        {
213          fS = REAL(1.0);
214          fSqrDist = fA00+REAL(2.0)*fB0+fC;
215        }
216        else if ( fB0 >= REAL(0.0) )
217        {
218          fS = REAL(0.0);
219          fSqrDist = fC;
220        }
221        else
222        {
223          fS = -fB0/fA00;
224          fSqrDist = fB0*fS+fC;
225        }
226      }
227    }
228    else  // region 1
229    {
230      fNumer = fA11 + fB1 - fA01 - fB0;
231      if ( fNumer <= REAL(0.0) )
232      {
233        fS = REAL(0.0);
234        fT = REAL(1.0);
235        fSqrDist = fA11+REAL(2.0)*fB1+fC;
236      }
237      else
238      {
239        fDenom = fA00-REAL(2.0)*fA01+fA11;
240        if ( fNumer >= fDenom )
241        {
242          fS = REAL(1.0);
243          fT = REAL(0.0);
244          fSqrDist = fA00+REAL(2.0)*fB0+fC;
245        }
246        else
247        {
248          fS = fNumer/fDenom;
249          fT = REAL(1.0) - fS;
250          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
251                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
252        }
253      }
254    }
255  }
256
257  if ( pfSParam )
258      *pfSParam = (float)fS;
259
260  if ( pfTParam )
261      *pfTParam = (float)fT;
262
263  return dReal(fabs(fSqrDist));
264}
265
266//------------------------------------------------------------------------------
267/**
268  @brief Finds the shortest distance squared between two line segments.
269  @param pfSegP0  t value for seg1 where the shortest distance between
270                  the segments exists.
271  @param pfSegP0  t value for seg2 where the shortest distance between
272                  the segments exists.
273  @return Shortest distance squared.
274 
275  Taken from:
276  Magic Software, Inc.
277  http://www.magic-software.com
278*/
279dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, 
280                           const dVector3 seg2Origin, const dVector3 seg2Direction,
281                           dReal* pfSegP0, dReal* pfSegP1 )
282{
283  const dReal gs_fTolerance = 1e-05f;
284  dVector3 kDiff, kNegDiff, seg1NegDirection;
285  Vector3Subtract( seg1Origin, seg2Origin, kDiff );
286  Vector3Negate( kDiff, kNegDiff );
287  dReal fA00 = dDOT( seg1Direction, seg1Direction );
288  Vector3Negate( seg1Direction, seg1NegDirection );
289  dReal fA01 = dDOT( seg1NegDirection, seg2Direction );
290  dReal fA11 = dDOT( seg2Direction, seg2Direction );
291  dReal fB0 = dDOT( kDiff, seg1Direction );
292  dReal fC = dDOT( kDiff, kDiff );
293  dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
294  dReal fB1, fS, fT, fSqrDist, fTmp;
295
296  if ( fDet >= gs_fTolerance )
297  {
298    // line segments are not parallel
299    fB1 = dDOT( kNegDiff, seg2Direction );
300    fS = fA01*fB1-fA11*fB0;
301    fT = fA01*fB0-fA00*fB1;
302       
303    if ( fS >= REAL(0.0) )
304    {
305      if ( fS <= fDet )
306      {
307        if ( fT >= REAL(0.0) )
308        {
309          if ( fT <= fDet )  // region 0 (interior)
310          {
311            // minimum at two interior points of 3D lines
312            dReal fInvDet = REAL(1.0)/fDet;
313            fS *= fInvDet;
314            fT *= fInvDet;
315            fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
316                       fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
317          }
318          else  // region 3 (side)
319          {
320            fT = REAL(1.0);
321            fTmp = fA01+fB0;
322            if ( fTmp >= REAL(0.0) )
323            {
324              fS = REAL(0.0);
325              fSqrDist = fA11+REAL(2.0)*fB1+fC;
326            }
327            else if ( -fTmp >= fA00 )
328            {
329              fS = REAL(1.0);
330              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
331            }
332            else
333            {
334              fS = -fTmp/fA00;
335              fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
336            }
337          }
338        }
339        else  // region 7 (side)
340        {
341          fT = REAL(0.0);
342          if ( fB0 >= REAL(0.0) )
343          {
344            fS = REAL(0.0);
345            fSqrDist = fC;
346          }
347          else if ( -fB0 >= fA00 )
348          {
349            fS = REAL(1.0);
350            fSqrDist = fA00+REAL(2.0)*fB0+fC;
351          }
352          else
353          {
354            fS = -fB0/fA00;
355            fSqrDist = fB0*fS+fC;
356          }
357        }
358      }
359      else
360      {
361        if ( fT >= REAL(0.0) )
362        {
363          if ( fT <= fDet )  // region 1 (side)
364          {
365            fS = REAL(1.0);
366            fTmp = fA01+fB1;
367            if ( fTmp >= REAL(0.0) )
368            {
369              fT = REAL(0.0);
370              fSqrDist = fA00+REAL(2.0)*fB0+fC;
371            }
372            else if ( -fTmp >= fA11 )
373            {
374              fT = REAL(1.0);
375              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
376            }
377            else
378            {
379              fT = -fTmp/fA11;
380              fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
381            }
382          }
383          else  // region 2 (corner)
384          {
385            fTmp = fA01+fB0;
386            if ( -fTmp <= fA00 )
387            {
388              fT = REAL(1.0);
389              if ( fTmp >= REAL(0.0) )
390              {
391                fS = REAL(0.0);
392                fSqrDist = fA11+REAL(2.0)*fB1+fC;
393              }
394              else
395              {
396                fS = -fTmp/fA00;
397                fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
398              }
399            }
400            else
401            {
402              fS = REAL(1.0);
403              fTmp = fA01+fB1;
404              if ( fTmp >= REAL(0.0) )
405              {
406                fT = REAL(0.0);
407                fSqrDist = fA00+REAL(2.0)*fB0+fC;
408              }
409              else if ( -fTmp >= fA11 )
410              {
411                fT = REAL(1.0);
412                fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
413              }
414              else
415              {
416                fT = -fTmp/fA11;
417                fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
418              }
419            }
420          }
421        }
422        else  // region 8 (corner)
423        {
424          if ( -fB0 < fA00 )
425          { 
426            fT = REAL(0.0);
427            if ( fB0 >= REAL(0.0) )
428            {
429              fS = REAL(0.0);
430              fSqrDist = fC;
431            }
432            else
433            {
434              fS = -fB0/fA00;
435              fSqrDist = fB0*fS+fC;
436            }
437          }
438          else
439          {
440            fS = REAL(1.0);
441            fTmp = fA01+fB1;
442            if ( fTmp >= REAL(0.0) )
443            {
444              fT = REAL(0.0);
445              fSqrDist = fA00+REAL(2.0)*fB0+fC;
446            }
447            else if ( -fTmp >= fA11 )
448            {
449              fT = REAL(1.0);
450              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
451            }
452            else
453            {
454              fT = -fTmp/fA11;
455              fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
456            }
457          }
458        }
459      }
460    }
461    else 
462    {
463      if ( fT >= REAL(0.0) )
464      {
465        if ( fT <= fDet )  // region 5 (side)
466        {
467          fS = REAL(0.0);
468          if ( fB1 >= REAL(0.0) )
469          {
470            fT = REAL(0.0);
471            fSqrDist = fC;
472          }
473          else if ( -fB1 >= fA11 )
474          {
475            fT = REAL(1.0);
476            fSqrDist = fA11+REAL(2.0)*fB1+fC;
477          }
478          else
479          {
480            fT = -fB1/fA11;
481            fSqrDist = fB1*fT+fC;
482          }
483        }
484        else  // region 4 (corner)
485        {
486          fTmp = fA01+fB0;
487          if ( fTmp < REAL(0.0) )
488          {
489            fT = REAL(1.0);
490            if ( -fTmp >= fA00 )
491            {
492              fS = REAL(1.0);
493              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
494            }
495            else
496            {
497              fS = -fTmp/fA00;
498              fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
499            }
500          }
501          else
502          {
503            fS = REAL(0.0);
504            if ( fB1 >= REAL(0.0) )
505            {
506              fT = REAL(0.0);
507              fSqrDist = fC;
508            }
509            else if ( -fB1 >= fA11 )
510            {
511              fT = REAL(1.0);
512              fSqrDist = fA11+REAL(2.0)*fB1+fC;
513            }
514            else
515            {
516              fT = -fB1/fA11;
517              fSqrDist = fB1*fT+fC;
518            }
519          }
520        }
521      }
522      else   // region 6 (corner)
523      {
524        if ( fB0 < REAL(0.0) )
525        {
526          fT = REAL(0.0);
527          if ( -fB0 >= fA00 )
528          {
529            fS = REAL(1.0);
530            fSqrDist = fA00+REAL(2.0)*fB0+fC;
531          }
532          else
533          {
534            fS = -fB0/fA00;
535            fSqrDist = fB0*fS+fC;
536          }
537        }
538        else
539        {
540          fS = REAL(0.0);
541          if ( fB1 >= REAL(0.0) )
542          {
543            fT = REAL(0.0);
544            fSqrDist = fC;
545          }
546          else if ( -fB1 >= fA11 )
547          {
548            fT = REAL(1.0);
549            fSqrDist = fA11+REAL(2.0)*fB1+fC;
550          }
551          else
552          {
553            fT = -fB1/fA11;
554            fSqrDist = fB1*fT+fC;
555          }
556        }
557      }
558    }
559  }
560  else
561  {
562    // line segments are parallel
563    if ( fA01 > REAL(0.0) )
564    {
565      // direction vectors form an obtuse angle
566      if ( fB0 >= REAL(0.0) )
567      {
568        fS = REAL(0.0);
569        fT = REAL(0.0);
570        fSqrDist = fC;
571      }
572      else if ( -fB0 <= fA00 )
573      {
574        fS = -fB0/fA00;
575        fT = REAL(0.0);
576        fSqrDist = fB0*fS+fC;
577      }
578      else
579      {
580        //fB1 = -kDiff % seg2.m;
581        fB1 = dDOT( kNegDiff, seg2Direction );
582        fS = REAL(1.0);
583        fTmp = fA00+fB0;
584        if ( -fTmp >= fA01 )
585        {
586          fT = REAL(1.0);
587          fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1);
588        }
589        else
590        {
591          fT = -fTmp/fA01;
592          fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1));
593        }
594      }
595    }
596    else
597    {
598      // direction vectors form an acute angle
599      if ( -fB0 >= fA00 )
600      {
601        fS = REAL(1.0);
602        fT = REAL(0.0);
603        fSqrDist = fA00+REAL(2.0)*fB0+fC;
604      }
605      else if ( fB0 <= REAL(0.0) )
606      {
607        fS = -fB0/fA00;
608        fT = REAL(0.0);
609        fSqrDist = fB0*fS+fC;
610      }
611      else
612      {
613        fB1 = dDOT( kNegDiff, seg2Direction );
614        fS = REAL(0.0);
615        if ( fB0 >= -fA01 )
616        {
617          fT = REAL(1.0);
618          fSqrDist = fA11+REAL(2.0)*fB1+fC;
619        }
620        else
621        {
622          fT = -fB0/fA01;
623          fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT);
624        }
625      }
626    }
627  }
628   
629  if ( pfSegP0 )
630    *pfSegP0 = fS;
631 
632  if ( pfSegP1 )
633    *pfSegP1 = fT;
634   
635  return dReal(fabs(fSqrDist));
636}
637
638//------------------------------------------------------------------------------
639/**
640  @brief Finds the shortest distance squared between a line segment and
641         a triangle.
642         
643  @param pfSegP   t value for the line segment where the shortest distance between
644                  the segment and the triangle occurs.
645                  So the point along the segment that is the shortest distance
646                  away from the triangle can be obtained by (seg.end - seg.start) * t.
647  @param pfTriP0  Barycentric coordinate of triangle at point closest to seg (u)
648  @param pfTriP1  Barycentric coordinate of triangle at point closest to seg (v)
649  @return Shortest distance squared.
650 
651  The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
652         
653  Taken from:
654  Magic Software, Inc.
655  http://www.magic-software.com
656*/
657dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, 
658                         const dVector3 triOrigin, 
659                         const dVector3 triEdge0, const dVector3 triEdge1,
660                         dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 )
661{
662  const dReal gs_fTolerance = 1e-06f;
663  dVector3 segDirection, segNegDirection, kDiff, kNegDiff;
664  Vector3Subtract( segEnd, segOrigin, segDirection );
665  Vector3Negate( segDirection, segNegDirection );
666  Vector3Subtract( triOrigin, segOrigin, kDiff );
667  Vector3Negate( kDiff, kNegDiff );
668  dReal fA00 = dDOT( segDirection, segDirection );
669  dReal fA01 = dDOT( segNegDirection, triEdge0 );
670  dReal fA02 = dDOT( segNegDirection, triEdge1 );
671  dReal fA11 = dDOT( triEdge0, triEdge0 );
672  dReal fA12 = dDOT( triEdge0, triEdge1 );
673  dReal fA22 = dDOT( triEdge1, triEdge1 );
674  dReal fB0  = dDOT( kNegDiff, segDirection );
675  dReal fB1  = dDOT( kDiff, triEdge0 );
676  dReal fB2  = dDOT( kDiff, triEdge1 );
677
678  dVector3 kTriSegOrigin, kTriSegDirection, kPt;
679  dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
680
681  // Set up for a relative error test on the angle between ray direction
682  // and triangle normal to determine parallel/nonparallel status.
683  dVector3 kN;
684  dCROSS( kN, =, triEdge0, triEdge1 );
685  dReal fNSqrLen = dDOT( kN, kN );
686  dReal fDot = dDOT( segDirection, kN );
687  bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen);
688
689  if ( bNotParallel )
690  {
691    dReal fCof00 = fA11*fA22-fA12*fA12;
692    dReal fCof01 = fA02*fA12-fA01*fA22;
693    dReal fCof02 = fA01*fA12-fA02*fA11;
694    dReal fCof11 = fA00*fA22-fA02*fA02;
695    dReal fCof12 = fA02*fA01-fA00*fA12;
696    dReal fCof22 = fA00*fA11-fA01*fA01;
697    dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02);
698    dReal fRhs0 = -fB0*fInvDet;
699    dReal fRhs1 = -fB1*fInvDet;
700    dReal fRhs2 = -fB2*fInvDet;
701
702    fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
703    fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
704    fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
705
706    if ( fR < REAL(0.0) )
707    {
708      if ( fS+fT <= REAL(1.0) )
709      {
710        if ( fS < REAL(0.0) )
711        {
712          if ( fT < REAL(0.0) )  // region 4m
713          {
714            // min on face s=0 or t=0 or r=0
715            Vector3Copy( triOrigin, kTriSegOrigin );
716            Vector3Copy( triEdge1, kTriSegDirection );
717            fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
718                                            kTriSegOrigin, kTriSegDirection, 
719                                            &fR, &fT );
720            fS = REAL(0.0);
721            Vector3Copy( triOrigin, kTriSegOrigin );
722            Vector3Copy( triEdge0, kTriSegDirection );
723            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
724                                             kTriSegOrigin, kTriSegDirection, 
725                                             &fR0, &fS0 );
726            fT0 = REAL(0.0);
727            if ( fSqrDist0 < fSqrDist )
728            {
729              fSqrDist = fSqrDist0;
730              fR = fR0;
731              fS = fS0;
732              fT = fT0;
733            }
734            fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
735                                             &fS0, &fT0 );
736            fR0 = REAL(0.0);
737            if ( fSqrDist0 < fSqrDist )
738            {
739              fSqrDist = fSqrDist0;
740              fR = fR0;
741              fS = fS0;
742              fT = fT0;
743            }
744          }
745          else  // region 3m
746          {
747            // min on face s=0 or r=0
748            Vector3Copy( triOrigin, kTriSegOrigin );
749            Vector3Copy( triEdge1, kTriSegDirection );
750            fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
751                                            kTriSegOrigin, kTriSegDirection,
752                                            &fR,&fT );
753            fS = REAL(0.0);
754            fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
755                                             &fS0, &fT0 );
756            fR0 = REAL(0.0);
757            if ( fSqrDist0 < fSqrDist )
758            {
759              fSqrDist = fSqrDist0;
760              fR = fR0;
761              fS = fS0;
762              fT = fT0;
763            }
764          }
765        }
766        else if ( fT < REAL(0.0) )  // region 5m
767        {
768          // min on face t=0 or r=0
769          Vector3Copy( triOrigin, kTriSegOrigin );
770          Vector3Copy( triEdge0, kTriSegDirection );
771          fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
772                                          kTriSegOrigin, kTriSegDirection, 
773                                          &fR, &fS );
774          fT = REAL(0.0);
775          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
776                                           &fS0, &fT0 );
777          fR0 = REAL(0.0);
778          if ( fSqrDist0 < fSqrDist )
779          {
780            fSqrDist = fSqrDist0;
781            fR = fR0;
782            fS = fS0;
783            fT = fT0;
784          }
785        }
786        else  // region 0m
787        {
788          // min on face r=0
789          fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
790                                          &fS, &fT );
791          fR = REAL(0.0);
792        }
793      }
794      else
795      {
796        if ( fS < REAL(0.0) )  // region 2m
797        {
798          // min on face s=0 or s+t=1 or r=0
799          Vector3Copy( triOrigin, kTriSegOrigin );
800          Vector3Copy( triEdge1, kTriSegDirection );
801          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
802                                          kTriSegOrigin, kTriSegDirection,
803                                          &fR, &fT );
804          fS = REAL(0.0);
805          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
806          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
807          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
808                                           kTriSegOrigin, kTriSegDirection,
809                                           &fR0, &fT0 );
810          fS0 = REAL(1.0) - fT0;
811          if ( fSqrDist0 < fSqrDist )
812          {
813            fSqrDist = fSqrDist0;
814            fR = fR0;
815            fS = fS0;
816            fT = fT0;
817          }
818          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
819                                           &fS0, &fT0 );
820          fR0 = REAL(0.0);
821          if ( fSqrDist0 < fSqrDist )
822          {
823            fSqrDist = fSqrDist0;
824            fR = fR0;
825            fS = fS0;
826            fT = fT0;
827          }
828        }
829        else if ( fT < REAL(0.0) )  // region 6m
830        {
831          // min on face t=0 or s+t=1 or r=0
832          Vector3Copy( triOrigin, kTriSegOrigin );
833          Vector3Copy( triEdge0, kTriSegDirection );
834          fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
835                                          kTriSegOrigin, kTriSegDirection,
836                                          &fR, &fS );
837          fT = REAL(0.0);
838          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
839          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
840          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
841                                           kTriSegOrigin, kTriSegDirection,
842                                           &fR0, &fT0 );
843          fS0 = REAL(1.0) - fT0;
844          if ( fSqrDist0 < fSqrDist )
845          {
846            fSqrDist = fSqrDist0;
847            fR = fR0;
848            fS = fS0;
849            fT = fT0;
850          }
851          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
852                                           &fS0, &fT0 );
853          fR0 = REAL(0.0);
854          if ( fSqrDist0 < fSqrDist )
855          {
856            fSqrDist = fSqrDist0;
857            fR = fR0;
858            fS = fS0;
859            fT = fT0;
860          }
861        }
862        else  // region 1m
863        {
864          // min on face s+t=1 or r=0
865          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
866          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
867          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
868                                          kTriSegOrigin, kTriSegDirection,
869                                          &fR, &fT );
870          fS = REAL(1.0) - fT;
871          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
872                                           &fS0, &fT0 );
873          fR0 = REAL(0.0);
874          if ( fSqrDist0 < fSqrDist )
875          {
876            fSqrDist = fSqrDist0;
877            fR = fR0;
878            fS = fS0;
879            fT = fT0;
880          }
881        }
882      }
883    }
884    else if ( fR <= REAL(1.0) )
885    {
886      if ( fS+fT <= REAL(1.0) )
887      {
888        if ( fS < REAL(0.0) )
889        {
890          if ( fT < REAL(0.0) )  // region 4
891          {
892            // min on face s=0 or t=0
893            Vector3Copy( triOrigin, kTriSegOrigin );
894            Vector3Copy( triEdge1, kTriSegDirection );
895            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
896                                            kTriSegOrigin, kTriSegDirection,
897                                            &fR, &fT );
898            fS = REAL(0.0);
899            Vector3Copy( triOrigin, kTriSegOrigin );
900            Vector3Copy( triEdge0, kTriSegDirection );
901            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
902                                             kTriSegOrigin, kTriSegDirection,
903                                             &fR0, &fS0 );
904            fT0 = REAL(0.0);
905            if ( fSqrDist0 < fSqrDist )
906            {
907              fSqrDist = fSqrDist0;
908              fR = fR0;
909              fS = fS0;
910              fT = fT0;
911            }
912          }
913          else  // region 3
914          {
915            // min on face s=0
916            Vector3Copy( triOrigin, kTriSegOrigin );
917            Vector3Copy( triEdge1, kTriSegDirection );
918            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
919                                            kTriSegOrigin, kTriSegDirection,
920                                            &fR, &fT );
921            fS = REAL(0.0);
922          }
923        }
924        else if ( fT < REAL(0.0) )  // region 5
925        {
926          // min on face t=0
927          Vector3Copy( triOrigin, kTriSegOrigin );
928          Vector3Copy( triEdge0, kTriSegDirection );
929          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
930                                          kTriSegOrigin, kTriSegDirection,
931                                          &fR, &fS );
932          fT = REAL(0.0);
933        }
934        else  // region 0
935        {
936          // global minimum is interior, done
937          fSqrDist = REAL(0.0);
938        }
939      }
940      else
941      {
942        if ( fS < REAL(0.0) )  // region 2
943        {
944          // min on face s=0 or s+t=1
945          Vector3Copy( triOrigin, kTriSegOrigin );
946          Vector3Copy( triEdge1, kTriSegDirection );
947          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
948                                          kTriSegOrigin, kTriSegDirection,
949                                          &fR, &fT );
950          fS = REAL(0.0);
951          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
952          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
953          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
954                                           kTriSegOrigin, kTriSegDirection,
955                                           &fR0, &fT0 );
956          fS0 = REAL(1.0) - fT0;
957          if ( fSqrDist0 < fSqrDist )
958          {
959            fSqrDist = fSqrDist0;
960            fR = fR0;
961            fS = fS0;
962            fT = fT0;
963          }
964        }
965        else if ( fT < REAL(0.0) )  // region 6
966        {
967          // min on face t=0 or s+t=1
968          Vector3Copy( triOrigin, kTriSegOrigin );
969          Vector3Copy( triEdge0, kTriSegDirection );
970          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
971                                          kTriSegOrigin, kTriSegDirection,
972                                          &fR, &fS );
973          fT = REAL(0.0);
974          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
975          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
976          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
977                                           kTriSegOrigin, kTriSegDirection,
978                                           &fR0, &fT0 );
979          fS0 = REAL(1.0) - fT0;
980          if ( fSqrDist0 < fSqrDist )
981          {
982            fSqrDist = fSqrDist0;
983            fR = fR0;
984            fS = fS0;
985            fT = fT0;
986          }
987        }
988        else  // region 1
989        {
990          // min on face s+t=1
991          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
992          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
993          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
994                                          kTriSegOrigin, kTriSegDirection,
995                                          &fR, &fT );
996          fS = REAL(1.0) - fT;
997        }
998      }
999    }
1000    else  // fR > 1
1001    {
1002      if ( fS+fT <= REAL(1.0) )
1003      {
1004        if ( fS < REAL(0.0) )
1005        {
1006          if ( fT < REAL(0.0) )  // region 4p
1007          {
1008            // min on face s=0 or t=0 or r=1
1009            Vector3Copy( triOrigin, kTriSegOrigin );
1010            Vector3Copy( triEdge1, kTriSegDirection );
1011            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1012                                            kTriSegOrigin, kTriSegDirection,
1013                                            &fR, &fT );
1014            fS = REAL(0.0);
1015            Vector3Copy( triOrigin, kTriSegOrigin );
1016            Vector3Copy( triEdge0, kTriSegDirection );
1017            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1018                                             kTriSegOrigin, kTriSegDirection,
1019                                             &fR0, &fS0 );
1020            fT0 = REAL(0.0);
1021            if ( fSqrDist0 < fSqrDist )
1022            {
1023              fSqrDist = fSqrDist0;
1024              fR = fR0;
1025              fS = fS0;
1026              fT = fT0;
1027            }
1028            Vector3Add( segOrigin, segDirection, kPt );
1029            fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1030                                             &fS0, &fT0 );
1031            fR0 = REAL(1.0);
1032            if ( fSqrDist0 < fSqrDist )
1033            {
1034              fSqrDist = fSqrDist0;
1035              fR = fR0;
1036              fS = fS0;
1037              fT = fT0;
1038            }
1039          }
1040          else  // region 3p
1041          {
1042            // min on face s=0 or r=1
1043            Vector3Copy( triOrigin, kTriSegOrigin );
1044            Vector3Copy( triEdge1, kTriSegDirection );
1045            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1046                                            kTriSegOrigin, kTriSegDirection,
1047                                            &fR, &fT );
1048            fS = REAL(0.0);
1049            Vector3Add( segOrigin, segDirection, kPt );
1050            fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1051                                             &fS0, &fT0 );
1052            fR0 = REAL(1.0);
1053            if ( fSqrDist0 < fSqrDist )
1054            {
1055              fSqrDist = fSqrDist0;
1056              fR = fR0;
1057              fS = fS0;
1058              fT = fT0;
1059            }
1060          }
1061        }
1062        else if ( fT < REAL(0.0) )  // region 5p
1063        {
1064          // min on face t=0 or r=1
1065          Vector3Copy( triOrigin, kTriSegOrigin );
1066          Vector3Copy( triEdge0, kTriSegDirection );
1067          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1068                                          kTriSegOrigin, kTriSegDirection,
1069                                          &fR, &fS );
1070          fT = REAL(0.0);
1071          Vector3Add( segOrigin, segDirection, kPt );
1072          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1073                                           &fS0, &fT0 );
1074          fR0 = REAL(1.0);
1075          if ( fSqrDist0 < fSqrDist )
1076          {
1077            fSqrDist = fSqrDist0;
1078            fR = fR0;
1079            fS = fS0;
1080            fT = fT0;
1081          }
1082        }
1083        else  // region 0p
1084        {
1085          // min face on r=1
1086          Vector3Add( segOrigin, segDirection, kPt );
1087          fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1088                                          &fS, &fT );
1089          fR = REAL(1.0);
1090        }
1091      }
1092      else
1093      {
1094        if ( fS < REAL(0.0) )  // region 2p
1095        {
1096          // min on face s=0 or s+t=1 or r=1
1097          Vector3Copy( triOrigin, kTriSegOrigin );
1098          Vector3Copy( triEdge1, kTriSegDirection );
1099          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1100                                          kTriSegOrigin, kTriSegDirection,
1101                                          &fR, &fT );
1102          fS = REAL(0.0);
1103          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1104          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1105          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1106                                           kTriSegOrigin, kTriSegDirection,
1107                                           &fR0, &fT0 );
1108          fS0 = REAL(1.0) - fT0;
1109          if ( fSqrDist0 < fSqrDist )
1110          {
1111            fSqrDist = fSqrDist0;
1112            fR = fR0;
1113            fS = fS0;
1114            fT = fT0;
1115          }
1116          Vector3Add( segOrigin, segDirection, kPt );
1117          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1118                                           &fS0, &fT0 );
1119          fR0 = REAL(1.0);
1120          if ( fSqrDist0 < fSqrDist )
1121          {
1122            fSqrDist = fSqrDist0;
1123            fR = fR0;
1124            fS = fS0;
1125            fT = fT0;
1126          }
1127        }
1128        else if ( fT < REAL(0.0) )  // region 6p
1129        {
1130          // min on face t=0 or s+t=1 or r=1
1131          Vector3Copy( triOrigin, kTriSegOrigin );
1132          Vector3Copy( triEdge0, kTriSegDirection );
1133          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1134                                          kTriSegOrigin, kTriSegDirection,
1135                                          &fR, &fS );
1136          fT = REAL(0.0);
1137          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1138          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1139          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1140                                           kTriSegOrigin, kTriSegDirection,
1141                                           &fR0, &fT0 );
1142          fS0 = REAL(1.0) - fT0;
1143          if ( fSqrDist0 < fSqrDist )
1144          {
1145            fSqrDist = fSqrDist0;
1146            fR = fR0;
1147            fS = fS0;
1148            fT = fT0;
1149          }
1150          Vector3Add( segOrigin, segDirection, kPt );
1151          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1152                                           &fS0, &fT0 );
1153          fR0 = REAL(1.0);
1154          if ( fSqrDist0 < fSqrDist )
1155          {
1156            fSqrDist = fSqrDist0;
1157            fR = fR0;
1158            fS = fS0;
1159            fT = fT0;
1160          }
1161        }
1162        else  // region 1p
1163        {
1164          // min on face s+t=1 or r=1
1165          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1166          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1167          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1168                                          kTriSegOrigin, kTriSegDirection,
1169                                          &fR, &fT );
1170          fS = REAL(1.0) - fT;
1171          Vector3Add( segOrigin, segDirection, kPt );
1172          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, 
1173                                           &fS0, &fT0 );
1174          fR0 = REAL(1.0);
1175          if ( fSqrDist0 < fSqrDist )
1176          {
1177            fSqrDist = fSqrDist0;
1178            fR = fR0;
1179            fS = fS0;
1180            fT = fT0;
1181          }
1182        }
1183      }
1184    }
1185  }
1186  else
1187  {
1188    // segment and triangle are parallel
1189    Vector3Copy( triOrigin, kTriSegOrigin );
1190    Vector3Copy( triEdge0, kTriSegDirection );
1191    fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1192                                    kTriSegOrigin, kTriSegDirection, &fR, &fS );
1193    fT = REAL(0.0);
1194
1195    Vector3Copy( triEdge1, kTriSegDirection );
1196    fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1197                                     kTriSegOrigin, kTriSegDirection,
1198                                     &fR0, &fT0 );
1199    fS0 = REAL(0.0);
1200    if ( fSqrDist0 < fSqrDist )
1201    {
1202      fSqrDist = fSqrDist0;
1203      fR = fR0;
1204      fS = fS0;
1205      fT = fT0;
1206    }
1207
1208    Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1209    Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1210    fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1211                                     kTriSegOrigin, kTriSegDirection, &fR0, &fT0 );
1212    fS0 = REAL(1.0) - fT0;
1213    if ( fSqrDist0 < fSqrDist )
1214    {
1215      fSqrDist = fSqrDist0;
1216      fR = fR0;
1217      fS = fS0;
1218      fT = fT0;
1219    }
1220
1221    fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
1222                                     &fS0, &fT0 );
1223    fR0 = REAL(0.0);
1224    if ( fSqrDist0 < fSqrDist )
1225    {
1226      fSqrDist = fSqrDist0;
1227      fR = fR0;
1228      fS = fS0;
1229      fT = fT0;
1230    }
1231
1232    Vector3Add( segOrigin, segDirection, kPt );
1233    fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, 
1234                                     &fS0, &fT0 );
1235    fR0 = REAL(1.0);
1236    if ( fSqrDist0 < fSqrDist )
1237    {
1238      fSqrDist = fSqrDist0;
1239      fR = fR0;
1240      fS = fS0;
1241      fT = fT0;
1242    }
1243  }
1244
1245  if ( pfSegP )
1246    *pfSegP = fR;
1247
1248  if ( pfTriP0 )
1249    *pfTriP0 = fS;
1250
1251  if ( pfTriP1 )
1252    *pfTriP1 = fT;
1253
1254  return fSqrDist;
1255}
Note: See TracBrowser for help on using the repository browser.