* Progess on min distance

NPLib/TrackReconstruction/NPTrackingUtility.h

@@ -18,14 +18,25 @@
  *****************************************************************************/
 
 namespace NPL{
-  double MinimunDistance(TVector3 v1,TVector3 v2, TVector3 w1, TVector3 w2){
-  TVector3 v=v1-v2;
+  // return the minimum distance between v and w defined respectively by points 
+  // v1,v2 and w1 w1
+  // Also compute the best crossing position BestPosition, i.e. average position
+  // at the minimum distance.
+  double MinimumDistance(const TVector3& v1,const TVector3& v2, const TVector3& w1, const TVector3& w2, TVector3& BestPosition){
+  TVector3 v = v2-v1;
   TVector3 w = w2-w1;
-  // Minimum distance
   // let be n perpendicular to both line
   TVector3 n = v.Cross(w);
 
-  double d = n.Dot(v1-w1)/n.Mag();
+  // Finding best position
+  TVector3 e = v2-w2;
+  double A = -(v.Mag2()*w.Mag2()-(v.Dot(w)*v.Dot(w)));
+  double s = (-v.Mag2()*(w.Dot(e))+(v.Dot(e))*(w.Dot(v)))/A;
+  double t = (w.Mag2()*(v.Dot(e))-(w.Dot(e)*w.Dot(v)))/A;
+  double d = sqrt((e+v*t-w*s).Mag2());
+  
+  BestPosition = 0.5*(v1+t*v+w1+s*w);
+  
   return d;
   }
 }