performance tuning - Distance between two line segments in 3-space

I need to compute the distance between two line segments in a project. After googling, I found this algorithm and used it to implement a Mathematica version:

distSegToSeg[{p1_, p2_}, {q1_, q2_}] :=
  Module[{small = 10^(-6), u, v, w,
          a, b, c, d, e, D,
          sc, sN, sD,
          tc, tN, tD},

    u = p2 - p1;
    v = q2 - q1;
    w = p1 - q1;
    a = u.u;
    b = u.v;
    c = v.v;
    d = u.w;
    e = v.w;
    D = a*c - b*b;
    sD = D;

    tD = D;
    If[D < small, 
      sN = 0; sD = 1; tN = e; tD = c,
      sN = b*e - c*d;
      tN = (a*e - b*d);
      If[sN < 0, 
        sN = 0.0; tN = e; tD = c,
        If[sN > sD, sN = sD; tN = e + b; tD = c;]]];
    If[tN < 0, 
      tN = 0;

      If[-d < 0, 
        sN = 0,
        If[-d > a, 
          sN = sD, 
          sN = -d; sD = a]],
      If[tN > tD, 
        tN = tD; 
        If[-d + b < 0, 
          sN = 0,
          If[-d + b > a, 

            sN = sD, 
            sN = -d + b; sD = a]]]];
    sc = If[Norm[sN] < small, 0, sN/sD];
    tc = If[Norm[tN] < small, 0, tN/tD];
    N[Norm[w + sc*u - tc*v]]
]

The code is not in good Mathematica style, and it's very slow. Do you have any idea to make this faster? Do you have a better way to compute the distance between two line segments in 3D?

Answer

You can easily see that if the nearest approach of the two infinite lines does not fall within both segments then you must calculate the nearest distance of all four end points to the other segment.

 pointsegdis[{seg_, pointlist_}] := 
       Module[{u = Subtract @@ seg, mean = Plus @@ seg/2},
       {#, (mean - u  Sign@# Min[1, Abs@#]/2) &@(-(2 ( # - mean).u )/u.u) } & 
        /@ pointlist ]
 segsegdis[s1_, s2_] := Module[{u, v, w, d, t1, t2},
   {u, v, w} = Subtract @@ # & /@ {Sequence @@ #, First /@ #} &@{s1, s2};
   If[(d = u.u v.v - (v.u)^2) != 0 &&
       Abs[t2 = 2 (u.u v.w - u.w v.u)/d + 1] <= 1 &&
       Abs[t1 = 2 (v.u v.w - v.v u.w)/d + 1] <= 1,
    Plus @@ (#[[1]] (1 + {1, -1} #[[2]] )) /2 & /@ {{s1, t1}, {s2, t2}} ,

    First@SortBy[Flatten[pointsegdis /@ {{s1, s2}, {s2, s1}}, 1], 
                       Total@((Subtract @@ #)^2) &]
       ] ] ;

 {s1, s2} = RandomReal[{-1, 1}, {2, 2, 3}];
 connect = segsegdis[s1, s2];
 Graphics3D[{Line /@ {s1, s2}, {Red, Line[connect]}}]

This is several orders of magnitude faster than Minimise or FindMinimum

 testcases = RandomReal[{-1, 1}, {100, 2, 2, 3}];
 (aa = Norm[ Subtract @@ segsegdis[#[[1]], #[[2]]] ] & /@ testcases) //Timing // First
 (bb = First@
      Minimize[{  Norm[(#[[1, 1]] (1 - t1)/2 + #[[1, 2]] (1 + t1)/2) -
         (#[[2, 1]] (1 - t2)/2 + #[[2, 2]] (1 + t2)/2) ]  , 
            {-1<=t1 <= 1 , -1 <= t2 <= 1}}, {t1, t2}] & /@ testcases) // Timing //First
 (cc = First@
       FindMinimum[{  
        Norm[(#[[1, 1]] (1 - t1)/2 + #[[1, 2]] (1 + t1)/2) -
         (#[[2, 1]] (1 - t2)/2 + #[[2, 2]] (1 + t2)/2) ]  , 

               {-1 <= t1 <= 1 , -1 <= t2 <= 1}}, {t1, t2}] & /@ testcases) // Timing // First

{0.015600, 12.620481, 2.714417}

verify all three give the same result to reasonable numerical precision

 Max[Abs[aa - bb]] -> 5.82532*10^-10
 Max[Abs[aa - cc]] -> 2.25578*10^-6