【Apollo Common代码解析-计算点到线段的距离】

计算点到线段的最短距离，在路径规划领域使用较多，是为基础函数，降低其耗时，对整个规划的效率有益。
先以apollo中使用DistanceTo函数实现为例。

 double LineSegment2d::DistanceTo(const Vec2d &point) const
      {
        if (length_ <= kMathEpsilon)
        {
          return point.DistanceTo(start_);
        }
        const double x0 = point.x() - start_.x();
        const double y0 = point.y() - start_.y();
        const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
        if (proj <= 0.0)
        {
          return hypot(x0, y0);
        }
        if (proj >= length_)
        {
          return point.DistanceTo(end_);
        }
        return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
      }

如果长度很小那么该线段只是一个点，计算两点之间的距离即可。
x0,y0组成的向量即为向量SP,unit_direction_为向量SE的单位向量图中红色的箭头，
那么proj的结果是向量SP和向量unit_direction_的内积，内积的大小是指向量SP在单位向量unit_direction_的投影，如果这个投影距离大于了线段的长度，那么说明该点point距离end_点是最近的，只需要返回这两个点之间的距离即可。
std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
上面这一句明显是叉乘的结果，叉乘的含义是两个向量组成的平行四边形的面积，由于unit_direction_是单位向量，那么该面积实际上就是垂线。

下面是另外求线段到点的最短距离的算法
假设线段是ab,求点c到线段的距离，其原理是算出三个点的相互距离，找出线段ac.bc中最大的一个max_cab，求出c点到直线ab的距离c_ab，然后用max_cab的平方-c_ab的平方得到的实际上就是ad的距离，如果ad长度小于线段长度，那么垂足就在线段上，如果大于ab那么垂足就在线段之外，只需要取ac,bc的最小值即可。

  double Point2LineSegment(const Point3d &c, const Point3d &a, const Point3d &b)
    {
        auto dis_between2points = [](const Point3d &Point1, const Point3d &Point2) -> double
        {
            return sqrt((Point1.x - Point2.x) * (Point1.x - Point2.x) + (Point1.y - Point2.y) * (Point1.y - Point2.y));
        };
        double min_dis = 0;
        // 先求出三个点之间的距离
        double ac = dis_between2points(a, c);
        double ab = dis_between2points(a, b);
        double bc = dis_between2points(c, b);
        double max_cab = max(ac, bc);
        // 计算点c到直线AB的距离
        // 根据两个点的坐标求直线方程
        double A = b.y - a.y;
        double B = a.x - b.x;
        double C = b.x * a.y - a.x * b.y;
        auto distance2Line = [](const double x, const double y, const double A, const double B, const double C) -> double
        {
            return std::abs(A * x + B * y + C) / std::sqrt(A * A + B * B);
        };
        double c_ab = distance2Line(c.x, c.y, A, B, C);
        double delt = pow(max_cab, 2) - pow(c_ab, 2);
        if (delt > ab * ab)
        {
            // 说明垂足在线段之外
            min_dis = min(ac, bc);
        }
        else
        { // 说明垂足在线段之内
            min_dis = c_ab;
        }
        return min_dis;
    }

以下是测试用例：

 planner::common::math::Vec2d p0(2, 3);
    planner::common::math::Vec2d p1(5, 4);
    planner::common::math::Vec2d p2(3, 7);

    planner::common::math::LineSegment2d line_segment2d_(p0, p1);
    double dis1 = line_segment2d_.DistanceTo(p2);

    //
    planner::common::math::Vec2d unit_direction_;
    const double dx = p1.x() - p0.x();
    const double dy = p1.y() - p0.y();
    double length_ = hypot(dx, dy);
    unit_direction_ =
        (planner::common::math::Vec2d(dx / length_, dy / length_));
    const double x0 = p2.x() - p0.x();
    const double y0 = p2.y() - p0.y();
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    if (proj <= 0.0)
    {
        // return hypot(x0, y0);
    }
    if (proj >= length_)
    {
        // return p2.DistanceTo(p1);
    }
    double dis4 = std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
    //
    planner::Point3d O;
    planner::Point3d q0(p0.x(), p0.y(), 0);
    planner::Point3d q1(p1.x(), p1.y(), 0);
    planner::Point3d q2(p2.x(), p2.y(), 0);
    planner::Point3d unit_direction(unit_direction_.x(), unit_direction_.y(), 0);
    planner::Point3d p02(x0, y0, 0);
    planner::Point3d p01(p1.x() - p0.x(), p1.y() - p0.y(), 0);
    double dis2 = planner::Point2LineSegment(q2, q0, q1);
    MatplotVisualization_.PlotLineSegmentArrow(O, q0, planner::plot_black_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(O, q1, planner::plot_black_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(O, q2, planner::plot_black_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(O, unit_direction, planner::plot_red_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(O, p02, planner::plot_blue_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(O, p01, planner::plot_green_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(q0, q1, planner::plot_magenta_arrow);
    // MatplotVisualization_.PlotLineSegmentArrow(q1, q2, planner::plot_magenta_arrow);
    MatplotVisualization_.PlotLineSegmentArrow(q0, q2, planner::plot_magenta_arrow);
    MatplotVisualization_.PlotOnePoint(q0, planner::plot_red_point);
    MatplotVisualization_.PlotOnePoint(q1, planner::plot_red_point);
    MatplotVisualization_.PlotOnePoint(q2, planner::plot_red_point);
    MatplotVisualization_.PlotOnePoint(unit_direction, planner::plot_black_point);
    MatplotVisualization_.PlotOnePoint(p02, planner::plot_green_point);
    MatplotVisualization_.PlotOnePoint(p02, planner::plot_blue_point);

    MatplotVisualization_.PlotPltPause(0.1);
    MatplotVisualization_.PlotShow();
    int break_point = 0;

下面是耗时比较：

 planner::common::math::Vec2d p0(2, 3);
    planner::common::math::Vec2d p1(5, 4);
    planner::common::math::Vec2d p2(3, 7);
    auto start_time1 = system_clock::now();
    for (size_t i = 0; i < 10000; i++)
    {
        planner::common::math::LineSegment2d line_segment2d_(p0, p1);
        double dis1 = line_segment2d_.DistanceTo(p2);
    }
    duration<double> cost_time1 = system_clock::now() - start_time1;
    cout << "LineSegment2d time: " << cost_time1.count() << std::endl;
    auto start_time3 = system_clock::now();
    for (size_t i = 0; i < 10000; i++)
    {
        double dis1 =  planner::common::math::LineSegment2d::DistanceToLineSegment(p2,p0,p1) ;
    }
    duration<double> cost_time3 = system_clock::now() - start_time3;
    cout << "DistanceToLineSegment time: " << cost_time3.count() << std::endl;
    planner::Point3d q0(p0.x(), p0.y(), 0);
    planner::Point3d q1(p1.x(), p1.y(), 0);
    planner::Point3d q2(p2.x(), p2.y(), 0);
    auto start_time2 = system_clock::now();
    for (size_t i = 0; i < 10000; i++)
    {
        double dis2 = planner::Point2LineSegment(q2, q0, q1);
    }
    duration<double> cost_time2 = system_clock::now() - start_time2;
    cout << "Point2LineSegment time: " << cost_time2.count() << std::endl;

耗时的结果（单位：s）如下所示，类的构建是很耗时的，采用静态函数，可以提高计算速度。
LineSegment2d time: 0.00124451
DistanceToLineSegment time: 0.0002523
Point2LineSegment time: 0.00047673

以上为个人学习使用，请批评指正，如有侵权，请联系删除。

参考文献
【1】百度apollo源码