/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
Segment3D get_shortest_segment(const Segment3D &sa,
const Segment3D &sb) {
const double eps= .0001;
algebra::Vector3D va= sa.get_point(1) - sa.get_point(0);
algebra::Vector3D vb= sb.get_point(1) - sb.get_point(0);
double ma= va*va;
double mb= vb*vb;
// if one of them is too short to have a well defined direction
// just look at an endpoint
if (ma < eps && mb < eps) {
return Segment3D(sa.get_point(0), sb.get_point(0));
} else if (ma < eps) {
return get_reversed(get_shortest_segment(sb, sa.get_point(0)));
} else if (mb < eps) {
return get_shortest_segment(sa, sb.get_point(0));
}
algebra::Vector3D vfirst = sa.get_point(0)- sb.get_point(0);
IMP_LOG_VERBOSE( vfirst << " | " << va << " | " << vb << std::endl);
double dab = vb*va;
double denom = ma * mb - dab * dab;
// they are parallel or anti-parallel
if (std::abs(denom) < eps) {
return get_shortest_segment_parallel(sa, sb);
}
double dfb = vfirst*vb;
double dfa = vfirst*va;
double numer = dfb * dab - dfa * mb;
double fa = numer / denom;
double fb = (dfb + dab * fa) / mb;
/*
denom = d2121 * d4343 - d4321 * d4321;
numer = d1343 * d4321 - d1321 * d4343;
*mua = numer / denom;
*mub = (d1343 + d4321 * (*mua)) / d4343;
pa->x = p1.x + *mua * p21.x;
pa->y = p1.y + *mua * p21.y;
pa->z = p1.z + *mua * p21.z;
pb->x = p3.x + *mub * p43.x;
pb->y = p3.y + *mub * p43.y;
pb->z = p3.z + *mub * p43.z;
*/
algebra::Vector3D ra=get_clipped_point(sa, fa);
algebra::Vector3D rb=get_clipped_point(sb, fb);
IMP_LOG_VERBOSE( fa << " " << fb << std::endl);
return Segment3D(ra, rb);
}
double get_distance(const Segment3D &a, const Segment3D &b) {
Segment3D s = get_shortest_segment(a, b);
return (s.get_point(0) - s.get_point(1)).get_magnitude();
}
double get_distance(const Segment3D &s, const Vector3D &p) {
Segment3D ss = get_shortest_segment(s, p);
return (ss.get_point(0) - ss.get_point(1)).get_magnitude();
}