【Apollo Common代码解析-LineSegment2d类】

基本定义

直线(straight line)由无数个点构成，点动成线。直线是面的组成成分，并继而组成体。没有端点，向两端无限延伸，长度无法度量。线段（segment），意思是指直线上两点间的有限部分（包括两个端点）。
直线的平面方程

• 一般式
  适用于所有直线的方程：
  AX+BY+C=0；
  假设已知两点坐标为(X1,Y1),(X2,Y2)
  A=Y2-Y1
  B=X1-X2
  C=X2Y1-X1Y2
  其中A、B不能同时为0
• 点斜式
  知道直线上一点(X1,Y1)，并且直线的斜率k存在，则直线可表示为：
  Y-Y1=k(X-X1);
  即Y=KX+B;
  点斜式转一般式：
  A = K;
  B = -1;
  C = B;

标题LineSegment2d类

直线-线段类如下所示，两个构造函数，共有五个成员变量：

Vec2d start_;
  Vec2d end_;
  Vec2d unit_direction_;
  double heading_ = 0.0;
  double length_ = 0.0;

其中unit_direction_是指xy方向的基向量，也是单位向量，其xy是向量在x和y轴上的单位方向向量。

 unit_direction_ =
            (length_ <= kMathEpsilon ? Vec2d(0, 0)
                                     : Vec2d(dx / length_, dy / length_));

/**
       * @class LineSegment2d
       * @brief Line segment in 2-D.
       */
      class LineSegment2d
      {
      public:
        /**
         * @brief Empty constructor.
         */
        LineSegment2d();

        /**
         * @brief Constructor with start point and end point.
         * @param start The start point of the line segment.
         * @param end The end point of the line segment.
         */
        LineSegment2d(const Vec2d &start, const Vec2d &end);

      private:
        Vec2d start_;
        Vec2d end_;
        Vec2d unit_direction_;
        double heading_ = 0.0;
        double length_ = 0.0;
      };

标题计算点到直线的距离

点到直线的距离公式很简单，apollo里面并没有此函数。

 double PointToLineDis(Point3d p, Point3d p1, Point3d p2)
    {
        double A = p2.y - p1.y;
        double B = p1.x - p2.x;
        double C = p2.x * p1.y - p1.x * p2.y;
        double dis = (A * p.x + B * p.y + C) / sqrt(A * A + B * B);
        return dis;
    }

判断点在不在线段上IsPointIn

该算法涉及到math中的CrossProd函数。

double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1,
const Vec2d &end_point_2)
{
return (end_point_1 - start_point).CrossProd(end_point_2 - start_point);
}

该函数就是计算三个点构成的平行四边形的面积，分正负，如果面积为0，那么三点共线。

上面的函数看懂了，下面的意思就明显了，如果面积为0那么就共线，否则三点共线，然后再判断该点的x,y在不在start和end两点之间即可。

 bool LineSegment2d::IsPointIn(const Vec2d &point) const
      {
        if (length_ <= kMathEpsilon)
        {
          return std::abs(point.x() - start_.x()) <= kMathEpsilon &&
                 std::abs(point.y() - start_.y()) <= kMathEpsilon;
        }
        const double prod = CrossProd(point, start_, end_);
        if (std::abs(prod) > kMathEpsilon)
        {
          return false;
        }
        return IsWithin(point.x(), start_.x(), end_.x()) &&
               IsWithin(point.y(), start_.y(), end_.y());
      }

判断点在线段的哪一侧

判断原理为叉乘，向量A叉乘向量B，若大于0说明向量B在向量A的逆时针方向。如下图所示，计算向量p0p1和向量p0p的叉乘结果，如果大于零说明向量p0p在p0p1的逆时针方向即左侧，如果小于零说明在顺时针方向右侧。

 bool LineSegment2d::LeftOfLine(const Vec2d &p0, const Vec2d &p1,
                                     const Vec2d &p)
      {
        Vec2d p0p1 = p1 - p0;
        Vec2d p0p = p - p0;
        double temp = p0p1.CrossProd(p0p);
        if (temp >= 0)
        {
          return true;
        }

        return false;
      }

计算一个点沿着线段移动一定距离得到新的点的坐标

实际上是线性插值

 void LineSegment2d::PointAlongLineSegmentMoveDis(Vec2d &point,
                                                   const double &dis)
  {
    point = start_ + (end_ - start_) * (dis / length_);
  }

判断两个线段是否相交及交点坐标

首先判断四个端点在不在对方线段上，如果在对方线段上那么返回对应的交点即可。叉乘的几何意义是计算平行四边形的面积，但是这个面积是存在正负之分的，cc1和cc2是判断一个线段是否在另外一个线段的一侧，如果同时为正或者同时为负那么肯定是在同一侧的，否则就可能相交但也不能完全确定（丁字状，不相连的情况），如果cc3和cc4不同时为正或者负值，那么可以确定线段肯定是相交的，其交点的计算如图所示。

 bool LineSegment2d::GetIntersect(const LineSegment2d &other_segment,
                                       Vec2d *const point) const
      {
        // CHECK_NOTNULL(point);
        if (IsPointIn(other_segment.start()))
        {
          *point = other_segment.start();
          return true;
        }
        if (IsPointIn(other_segment.end()))
        {
          *point = other_segment.end();
          return true;
        }
        if (other_segment.IsPointIn(start_))
        {
          *point = start_;
          return true;
        }
        if (other_segment.IsPointIn(end_))
        {
          *point = end_;
          return true;
        }
        if (length_ <= kMathEpsilon || other_segment.length() <= kMathEpsilon)
        {
          return false;
        }
        const double cc1 = CrossProd(start_, end_, other_segment.start());
        const double cc2 = CrossProd(start_, end_, other_segment.end());
        if (cc1 * cc2 >= -kMathEpsilon)
        {
          return false;
        }
        const double cc3 =
            CrossProd(other_segment.start(), other_segment.end(), start_);
        const double cc4 =
            CrossProd(other_segment.start(), other_segment.end(), end_);
        if (cc3 * cc4 >= -kMathEpsilon)
        {
          return false;
        }
        const double ratio = cc4 / (cc4 - cc3);
        *point = Vec2d(start_.x() * ratio + end_.x() * (1.0 - ratio),
                       start_.y() * ratio + end_.y() * (1.0 - ratio));
        return true;
      }

假设s1e2和s2e2是两条线段，那么cc3表示的是平行四边形s1e1s2左侧的那个，cc4表示的是s1e2e1的右侧的平行四边形，两者的符号是相反的，这两个平行四边形是同底边的，假设高分别是s2p1和e2p2,则ratio意思是s1e1e2p2/(s1e1s2p1+s1e1*e2p2)=h2/(h1+h2);三角形os2p1和三角形oe2p2是相似的，所以ratio=e2o/(s2o+oe2);
同理大的三角形也是相似的。
假设ratio=k；
$h_2/(h_1+h_2)=k$
$s_1{p}_1=L\ast (1-k)$
可以得到中心点的坐标

double os1=segment.length()(1-ratio);
point =Vec2d(segment.start().x() + os1cos(segment.heading() ) ,
segment.start().y() + os1sin(segment.heading() ));

点到线段的距离在：https://blog.csdn.net/weixin_41766123/article/details/136231734#comments_31379867已经说明过。

线段类基本介绍完了。
以上为个人学习使用，请批评指正，如有侵权，请联系删除。

参考文献

【1】百度apollo源码
[2]百度百科