Пример #1
0
/*
  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);
}
Пример #2
0
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();
}
Пример #3
0
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();
}