第四十三篇，点线关系

一、缘起

故事的起源是做AD的插值预瞄发现一个bug，首先放上一维线性插值的代码如下：

template <typename T>
T lerp(const T &x0, const double t0, const T &x1, const double t1,
       const double t) {
  if (std::abs(t1 - t0) <= 1.0e-6) {
    return x0;
  }
  const double r = (t - t0) / (t1 - t0);
  const T x = x0 + r * (x1 - x0);
  return x;
}

比如在两个轨迹点a和b之间做插值预瞄，正常来说插值点应当落在ab之间，一开始没发现什么问题，只是轨迹跟踪误差有点儿大，就找优化的手段。

二、现象与初步分析

找来找去最后还是通过MATLAB plot做了个动画发现了端倪，有的时段插值后的预瞄距离会莫名其妙地逐渐缩短，换句话说，插值预瞄点会逐渐靠近自车甚至飘到自车后头去。

分析后发现，当自车在线段ab之外时正常，如下面左图所示场景；当自车落在线段ab之间时问题就出现了，如下面右图所示场景（P是期望的插值位置）：

进一步分析发现，当自车落在线段ab之间时，按照插值原理，自车与后方点即a点的距离应置为负值才能保证插值结果的正确性，而代码中未做处理，执行std::hypot()之后就不管了，所以无论何种场景，与任何点的距离都是正值，这种没头脑的不符合数学物理常识的方法是有隐患的。

三、数学抽象

到这里也就明确了，需要对自车与轨迹点连线ab的相对位置做检查，并根据检查结果对自车与a点的距离做正负处理，抽象为数学问题即点线关系。

在本例中，为点在线段区间内还是区间外的问题，如下图所示：

四、方法一

从几何学的角度可知，可以用构造直线方程的方法来做。

首先，我们有ab两点的坐标，很容易构造出它俩所在直线的方程y=kx+m；

然后，经过点a且与线ab垂直的直线Line1、经过点b且与线ab垂直的直线Line2，Line1与Line2所夹形成一个空间，如果自车在此空间外，则与a点的距离为正；如果自车落在此空间内部，则需将与a点的距离置为负；

根据初中数学，很容易可知Line1与Line2的斜率都为-1/k，它们分别经过点a、b，则可以得出完整的直线方程；

关键来了，Line1与Line2的直线方程有了，怎么利用这些信息检查自车是否在区间内？

方法是这样的：将自车坐标的x代入Line1方程，得到位于Line1上的一个y，如果此y大于点a的y则自车在区间外，否则自车在区间内，查看上述示意图可知，代码如下。

double dfDistWith_a = 1; // 假定先前已算出自车与a点的距离绝对值为1m
double dfX, dfY; // 假定自车坐标
double dfX_a, dfY_a; // 假定轨迹点a的坐标
double dfX_b, dfY_b; // 假定轨迹点b的坐标

if (std::abs(dfX_a-dfX_b) > 1E-4)
{
    // 这里的处理直接取了倒数，省去一步检查除数为0的操作
    double k_perpendicular = (dfY_b-dfY_a) / (dfX_b-dfX_a);
    k_perpendicular *= -1.0; // x = k*y + b

    double b_perpendicular_a = dfX_a - k_perpendicular * dfY_a;
    double b_perpendicular_b = dfX_b - k_perpendicular * dfY_b;
    double pseudo_x_a = k_perpendicular * dfY + b_perpendicular_a;
    double pseudo_x_b = k_perpendicular * dfY + b_perpendicular_b;
    if ((dfX>pseudo_x_a && dfX<pseudo_x_b) || 
        (dfX<pseudo_x_a && dfX>pseudo_x_b))
    {
        dfDistWith_a *= -1.0F;
    }
}
else
{
    if ((dfY>dfY_a && dfY<dfY_b) || 
        (dfY<dfY_a && dfY>dfY_b))
    {
        dfDistWith_a *= -1.0F;
    }
}

五、更优解

另外，还有一个更简便代码量也更少的方法，即向量夹角法。

重新观察最上面的左右两个图可以发现这样一个规律，当自车在区间外时，自车与a点所成向量，与向量ab的夹角为钝角，否则为直角或锐角。

有了这一信息，可以想到，将上述两个向量构造出来，做向量点乘（也叫内积），判断点乘结果的正负（钝角为负，否则为0或正），即可得知自车与a点的距离该正还是该负，方便快捷！

代码如下，代码量和逻辑都简洁了很多，并且少了除法、ab是横线竖线等特殊情况的判断，优雅！

如果使用二维向量库Vec2d的话代码量会更少。

double dfDistWith_a = 1; // 假定先前已算出自车与a点的距离绝对值为1m
double dfX, dfY; // 假定自车坐标
double dfX_a, dfY_a; // 假定轨迹点a的坐标
double dfX_b, dfY_b; // 假定轨迹点b的坐标

double dfVector_a_x = dfX - dfX_a; // 构造一个向量
double dfVector_a_y = dfY - dfY_a;
double dfVector_b_x = dfX_b - dfX_a; // 构造另一个向量
double dfVector_b_y = dfY_b - dfY_a;

if (dfVector_a_x*dfVector_b_x + dfVector_a_y*dfVector_b_y > 0) // 向量点乘
{
    dfDistWith_a *= -1.0F; // over
}