Epsilon中碰撞检测算法

1. 首先预测一段时间内自车与周围车辆的行驶轨迹，之后检测每一时刻自车与周边车辆的轨迹是否会发生碰撞，代码如下：

（1）首先将自车与周围车辆的长宽都加一个Offset值，代码中为0.1。

（2）基于当前的状态检查是否会发生碰撞

2.使用OBB算法检测两障碍物是否会发生碰撞，对自车与障碍物使用如下矩形box包络。

（1）车辆的坐标通常表示车辆后轴中心点处的坐标，首先需要计算自车与目标车质心的坐标，如下图所示。

代码如下：

2）分别计算自车与目标车四个角点的位置坐标，代码如下：

以自车右前方角点为例，如下图所示：

    O为车辆质心的位置（x,y），计算C点的坐标为：(x+OD+BC，y+AD-AB)

    OD = 0.5*obb.Length*cos_theta

    BC = 0.5*obb.Width*sin_theta

    AD = 0.5*obb.Length*sin_theta

    AB = 0.5*obb.Width*cos_theta

    其他角点的坐标算法同理。

（3）分别计算自车与目标车x轴，y轴方向的法向量

    代码如下：

（4）分别计算自车与目标车的四个顶点在四条轴上的投影，如下图所示：

    如上图所示，A,B分别为自车的四个顶点在自车在法向量上的投影点，C，D，E，F分别为目标车在法向量上的投影点，筛选出最小与最大的投影点，假设自车最小的投影点为B，最大的投影点为A，则目标车最小的投影点为F，最大的投影点为C，计算两障碍物最大投影点与最小投影点区间的重叠长度，若存在一侧重叠长度为0，则该目标不会发生碰撞，上图重叠长度为AF，所以该投影算出的结果会发生碰撞。

    如上图所示，A,B分别为自车的四个顶点在自车在法向量上的投影点，C，D，E，F分别为目标车在法向量上的投影点，筛选出最小与最大的投影点，假设自车最小的投影点为B，最大的投影点为A，则目标车最小的投影点为F，最大的投影点为C，计算两障碍物最大投影点与最小投影点区间的重叠长度，上图AB与AF之间没有重叠，所以直接返回在该时刻两目标不会发生碰撞。

    如上图所示，A，B，C，D分别为自车的四个顶点在自车在法向量上的投影点，E，F分别为目标车在法向量上的投影点，筛选出最小与最大的投影点，假设自车最小的投影点为D，最大的投影点为A，则目标车最小的投影点为F，最大的投影点为E，计算两障碍物最大投影点与最小投影点区间的重叠长度，上图AD与EF之间的重叠长度为ED，所以该投影算出的结果会发生碰撞。

    如上图所示，A，B，C，D分别为自车的四个顶点在自车在法向量上的投影点，E，F分别为目标车在法向量上的投影点，筛选出最小与最大的投影点，假设自车最小的投影点为D，最大的投影点为A，则目标车最小的投影点为F，最大的投影点为E，计算两障碍物最大投影点与最小投影点区间的重叠长度，上图AD与EF之间没有重叠，所以直接返回在该时刻两目标不会发生碰撞。

（5）综上所述，该时刻两目标不会发生碰撞，代码如下：