碰撞检测算法

wenjie included in Planning

2024-03-04 2177 words 5 minutes

Contents

粗检测 – 筛选明显不相交的元素

包围型法

包围形方法的基本思想是用一个简单的包围形状（圆、矩形）将复杂不规则的物体包围住，当两个包围形不相交时，则两个物体肯定不相交。

包围形可分为：

外接圆形
轴对齐包围矩形（Axis Aligned Bounding Box，AABB）

外接圆

判断依据：只要两个圆形的圆心距离 > 两个圆的半径之和，则这两个圆不相交。

但是只用一个圆的精度是不够的，可以用多个圆来覆盖，注意：尽可能减少同一个物体上圆的覆盖面积

AABB

轴对齐包围矩形（Axis Aligned Bounding Box，AABB），**即用一个矩形包围车体，且矩形的边与某个坐标轴平行，**只要在 X 轴或 Y 轴上没有重合，则这两个矩形不相交。

判断依据：

细检测 – 检测是否相交

转向包围矩形（Oriented Bounding Box，OBB）就是找一个最小的包围物体的矩形，这在自动驾驶系统中也是最常用的，感知模块给出物体的轮廓通常就是此形状

分离轴定理（Separating Axis Theorem，SAT）

适用于 bounding box 和 polygon，在实际应用中，为了提高效率，通常先使用基于轴对齐包围矩形（AABB）的方法进行粗略的碰撞检测，然后再使用分离轴定理（SAT）做精细碰撞检测。

概念：通过判断任意两个凸多边形在任意角度下的投影是否均存在重叠，来判断是否发生碰撞。若在某一角度光源下，两物体的投影存在间隙，则为不碰撞，否则为发生碰撞。

注意：

分离轴定理只适合凸多边形，所以如果是凹多边形的话需要转换成多个**凸多边形,**具体转换算法参考git上https://github.com/schteppe/poly-decomp.js 用于将二维多边形分解为凸块的库。

上图中的黑线为分离线（Seperating line），与之垂直的绿线为分离轴（Separating axis），图中虚线表示的是多边形在分离轴上的投影。

实际应用中，遍历所有角度的分离轴是不现实的，受益于多边形的性质，对于两个都是多边形的物体，只需要依次在每条边的垂直线做投影即可。我的理解是，如果两个多边形有重合的话，那么必然在重合边的垂线上的投影是重合的

以下图中的两个多边形 A 和 B 为例，分离轴定理的具体步骤为：

首先根据边1的两个顶点位置坐标，计算出边1的向量，设为（x，y）；
进而求出边1的法向量，作为分离轴，为（y, -x）或（-y，x）。若需要求两个多边形的最小分离距离，这里的法向量还需要化为单位向量；若只需判断两个多边形是否相交，则不需要化为单位向量；
依次将多边形 A 和 B 的所有顶点与原点组成的向量投影到这个分离轴上，并记录两个多边形顶点投影到分离轴上的最小值和最大值（Pmin，Pmax），形成一个投影线段；
判断这两个投影线段是否发生重叠，若不重叠，则有（PAmax < PBmin）||（PAmin > PBmax）；
若两个投影线段不重叠，则代表存在这样一条直线将两个多边形分开，两个多边形不相交，可以直接退出循环；
若两个投影线段重叠，则回到步骤1，继续以边2的法向量作为分离轴，进行投影计算；
当两个多边形的所有边都检查完之后，找不到这样一条分离的直线，则意味着两个多边形相交。

GJK（Gilbert–Johnson–Keerthi）

点在三角形内部

面积求和

如果点在三角形内部，那么三个小三角形的面积一定＝大三角形的面积

可以用海伦公式解

先求出三条边的长度abc，然后分别计算面积，判断是否相等

向量叉乘

若点P在三角形内部时，沿逆时针方向，则点P一直在向量AB、BC、CA的左侧；
若点P在三角形外部时，则点P必然在AB、BC、CA某一向量的右侧。

若是三角形三点是顺时针方向，则若点P在三角形内部时，点P一直在向量AB、BC、CA的右侧。

对于点P在三角形的某一边上或与某一顶点重合的特殊情况，上述两种方法同样适用，需要注意一下**临界条件（即叉乘=0）**的设置。

点在多边形内部PIP（Point in Polygon）

https://www.toutiao.com/article/6643743002114130436/?group_id=6643743002114130436&wid=1709463411493

从该点做一条射线，计算它跟多边形边界的交点个数，如果交点个数为奇数，那么点在多边形内部，否则点在多边形外部

两条线段是否相交

step1. 首先使用AABB粗筛

step2. 然后通过叉乘进行判断

其次，如下图。AB与CD相交必然有A、B在线段CD两边，C、D在线段AB两边（两个要同时满足！）。

根据上面的公式和右手螺旋法则，如果相交，AB X AC与AB X AD必然异号；同样的，DC X DA与DC X DB也必然异号。

特别的，如果B在CD上时，求得的z坐标值是0。所以只要同时满足(AB X AC) * (AB X AD)≤ 0，(DC X DA) * (DC X DB) ≤ 0，就能保证必然相交

class line{
public:
	int xa;
	int ya;
	int xb;
	int yb;
	line(){}
	line(int xa, int ya, int xb, int yb){
		this->xa = xa;
		this->ya = ya;
		this->xb = xb;
		this->yb = yb;
	}
	int get_max_x(){
		return xa > xb ? xa : xb;
	}
	int get_min_x(){
		return xa > xb ? xb : xa;
	}
	int get_max_y(){
		return ya > yb ? ya : yb;
	}
	int get_min_y(){
		return ya > yb ? yb : ya;
	}
};

bool is_intersect(line myline1, line myline2){
	if(myline1.get_max_x() < myline2.get_min_x() || 
	   myline2.get_max_x() < myline1.get_min_x() ||
	   myline1.get_max_y() < myline2.get_min_y() || 
	   myline2.get_max_y() < myline1.get_min_y())   return false;
	int res1 = (myline1.xa - myline1.xb) * (myline2.ya - myline1.yb) - (myline1.ya - myline1.yb) * (myline2.xa - myline1.xb);
	int res2 = (myline1.xa - myline1.xb) * (myline2.yb - myline1.yb) - (myline1.ya - myline1.yb) * (myline2.xb - myline1.xb);
	
	int res3 = (myline2.xa - myline2.xb) * (myline1.ya - myline2.yb) - (myline2.ya - myline2.yb) * (myline1.xa - myline2.xb);
	int res4 = (myline2.xa - myline2.xb) * (myline1.yb - myline2.yb) - (myline2.ya - myline2.yb) * (myline1.xb - myline2.xb);
	if(res1 * res2 <= 0 && res3 * res4 <= 0) return true;
	else return false;
}