Function PointToLineDistance

roborts_common/math/math.h:101–132

}

inline double PointToLineDistance (const double px, const double py,
                                   const double x0, const double y0,
                                   const double x1, const double y1) {
  double A = px - x0;
  double B = py - y0;
  double C = x1 - x0;
  double D = y1 - y0;

  double dot = A * C + B * D;
  double len_sq = C * C + D * D;
  double param = dot / len_sq;

  double xx, yy;

  if (param < 0)
  {
    xx = x0;
    yy = y0;
  }
  else if (param > 1)
  {
    xx = x1;
    yy = y1;
  }
  else
  {
    xx = x0 + param * C;
    yy = y0 + param * D;
  }

  return PointDistance(px, py, xx, yy);
}

inline double PointToLineDistance (const geometry::Point2D &point,
                                   const geometry::LineSegment2D &line) {