linepointclosesttopoint.cc

2007年机器人足球世界杯3D仿真组亚军

#include "math.hh"

namespace bats {

  /**
   *  Calculates the nearest point on a line to
   *  another point.
   *
   *  @param l0 the start point of the line.
   *  @param lVect the not normalized line vector.
   *  @param point the other point.
   */
  Vector3D Math::linePointClosestToPoint(Vector3D const &l0,
					 Vector3D const &lVect,
					 Vector3D const &point)
  {
    Vector3D v = lVect.normalize();
    Vector3D s = calcPerpend(v);

    // There probably is a more efficient formula.
    double u = ((s[0]/s[1])*(l0.getY() - point.getY()) + (point.getX() - l0.getX()))/(v[0] - (s[0]/s[1])*v[1]);

    // When no perpendicular line is posible within the segment, use
    // the closest endpoint.
    if (u > lVect.length()) {
      if ((l0 - point).length() < ((l0 + lVect) - point).length())
	return l0;
      else
	return l0+lVect;
    }

    return l0 + v*u;
  }


}