Commit 84d22963 authored by Adrien Matta's avatar Adrien Matta
Browse files

* Progess on min distance

parent fc4a0675
......@@ -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;
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment