# segdist.h

```00001 #ifndef __SEG_DIST__
00002 #define __SEG_DIST__
00003
00004 // XXX watch out for this !!!
00005 #define float double
00006
00007 // This comes from David Eberly www.geometrictools.com
00008 // we basically minimize the quadratic function
00009 // Q(s,t) = as^2+2bst+ct^2+2ds+2et+f
00010
00011 float aux_edge(float tmp, float denum) {
00012   // F(t) = Q(1,t) = (a+2*d+f)+2*(b+e)*t+c*t^2
00013   // F'(T) = 0 when T = -(b+e)/c
00014   if (tmp>0.0)
00015     return 0.0;
00016   else {
00017     if (-tmp>denum)
00018       return 1.0;
00019     else
00020       return -tmp/denum;
00021   }
00022 }
00023
00024 float aux_corner(float s_in, float t_in, float Qs, float Qt,
00025                  float edge0, float denum0, float edge1,
00026                  float denum1, float &s, float &t) {
00027   if (Qs>0.0) { t=t_in;s=aux_edge(edge0,denum0); }
00028   else { s=s_in;
00029     if (Qt>0.0) t=aux_edge(edge1,denum1);
00030     else t = t_in;
00031   }
00032 }
00033
00034 template<class Vector>
00035 float min_seg_dist(const Vector &B0, const Vector &B0p, const Vector &B1,
00036                    const Vector &B1p,float &s,float &t) {
00037   const float tol = 1e-12;
00038   Vector D = B0-B1, M0 = B0p-B0, M1 = B1p-B1;
00039   float a = M0.dot(M0), b = -M0.dot(M1), c = M1.dot(M1);
00040   float d = M0.dot(D), e = -M1.dot(D), f = D.dot(D);
00041   float det = fabsf(a*c-b*b);
00042   float invDet, tmp;
00043   s = b*e-c*d; t = b*d-a*e;
00044
00045   if (det>=tol) {
00046     // non parallel case
00047     if (s>=0.0) {
00048       if (s<=det) {
00049         if (t>=0.0) {
00050           if (t<=det) { // region 0
00051             invDet = 1.0/det; s*= invDet; t*=invDet; }
00052           else { // region 3
00053             t=1.0; s=aux_edge(b+d,a);
00054           }
00055         }
00056         else { // region 7
00057           t=0.0; s=aux_edge(d,a);
00058         }
00059       }
00060       else {
00061         if (t>=0.0) {
00062           if (t<=det) { // region 1
00063             s = 1.0; t=aux_edge(b+e,c);
00064           }
00065           else { // region 2
00066             aux_corner(1.0,1.0,a+b+c,b+c+e,b+d,a,b+e,c,s,t);
00067           }
00068         }
00069         else { // region 8
00070           aux_corner(1.0,0.0,a+d,b+e,d,a,b+e,c,s,t);
00071         }
00072       }
00073     }
00074     else { // s<0
00075       if (t>=0.0) {
00076         if (t<=det) { // region 5
00077           s = 0.0; t=aux_edge(e,c);
00078         } else { // region 4
00079             aux_corner(0.0,1.0,-(b+d),c+e,b+d,a,e,c,s,t);
00080           }
00081       }
00082       else { // region 6
00083         aux_corner(0.0,0.0,-d,-e,d,a,e,c,s,t);
00084       }
00085     }
00086   }
00087   else {
00088     // parallel case
00089     // cerr << "Parallel case!\n";
00090     if (b>0) {
00091       if (d>=0) { s=0.0; t=0.0; }
00092       else {
00093         if (-d<=a) { s=-d/a; t=0.0; }
00094         else {
00095           s=1.0; tmp = a+d; if (-tmp>=b) t=1.0; else t = -tmp/b;
00096         }
00097       }
00098     }
00099     else {
00100       if (-d>=a) { s=1.0;t=0.0; }
00101       else { if (d<=0.0) { s=-d/a; t=0.0; }
00102       else { s=0.0; if (d>=-b) t=1.0; else t=-d/b;} }
00103     }
00104   }
00105   return ((B0+(B0p-B0)*s)-(B1+(B1p-B1)*t)).norm();
00106 }
00107
00108 #endif // __SEG_DIST__
```

Generated on Mon Feb 8 17:22:35 2010 for libbiarc by  1.5.6