参考链接：
【规划】Box2d::HasOverlap() 碰撞检测接口详解_lemon_zy的博客-CSDN博客_box2d碰撞检测
一个常用方法即为超平面分离定理（Hyperplane Separation Theorem），用于检测N维欧几里得空间内的凸集是否存在交集，严格的定义可以参考维基百科。其二维情形被称为分离轴定理（Separating Axis Theorem），简要描述如下：
两个平面凸集不相交的充要条件是，存在某条直线（即分离轴），使得两平面在直线上的投影不相交，类似的可以写出否命题。显然，要证明相交我们需要穷尽所有的直线，这是不可能的。幸运的是，对于凸多边形，我们只需要检查其边所在的直线是否为分离线（不是分离轴）即可。（大家可以思考一下对于凹多边形和圆怎么实现）
在自动驾驶的范畴内，对于障碍物一般用平面矩形表示（更高精度的会用平面凸多边形），对于两个矩形来说，我们只需要遍历4条边（每个矩形各自相邻的两条边）即可，只要这四条边有一条为分离轴那么两个矩形就不相交。相反，若两个矩形相交，则四条边都不是分离轴（比较自车和目标轮廓在分离轴上投影长度之和的一半，与投影距离的大小，投影距离全部小于投影及有重叠hasOverLap），即他们在上面的投影都有重合。
为了高效地表示平面矩形，我们一般会这样定义：
-
struct Box2d {
-
float center_x_;
-
float center_y_;
-
float length_;
-
float width_;
-
float half_length_;
-
float half_width_;
-
float heading_;
-
float cos_heading_;
-
float sin_heading_;
-
float min_x_;
-
float max_x_;
-
float min_y_;
-
float max_y_;
-
struct Vec2d corners_
[4];
-
};
投影原理：针对自车两条相邻边和障碍物相邻的两条边做投影
我们只需要比较自车和目标轮廓在分离轴上投影长度之和的一半，与投影距离的大小，即可判断二者投影有没有相交。显然，自车在分离轴上的投影长度为length_。
以下代码是对应Vehicle相邻的两条边上的投影计算
-
std::
abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
-
std::
abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
-
std::
abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
-
half_length_ &&
-
std::
abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
-
std::
abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
-
std::
abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
-
half_width_
即vehicle任意一点不在obstacle对应的橙色虚线框内。
以下代码是对应Obstacle相邻的两条边上的投影计算
-
std::
abs(shift_x * box.
cos_heading() + shift_y * box.
sin_heading()) <=
-
std::
abs(dx1 * box.
cos_heading() + dy1 * box.
sin_heading()) +
-
std::
abs(dx2 * box.
cos_heading() + dy2 * box.
sin_heading()) +
-
box.
half_length() &&
-
std::
abs(shift_x * box.
sin_heading() - shift_y * box.
cos_heading()) <=
-
std::
abs(dx1 * box.
sin_heading() - dy1 * box.
cos_heading()) +
-
std::
abs(dx2 * box.
sin_heading() - dy2 * box.
cos_heading()) +
-
box.
half_width()
Box2d::HasOverlap(const Box2d &box)
-
bool Box2d::
HasOverlap(
const Box2d &
box)
const {
-
if (
box.
max_x() <
min_x() ||
box.
min_x() >
max_x() ||
box.
max_y() <
min_y() ||
-
box.
min_y() >
max_y()) {
-
return
false;
-
}
-
-
const double shift_x =
box.
center_x() - center_.
x();
-
const double shift_y =
box.
center_y() - center_.
y();
-
-
const double dx1 = cos_heading_ * half_length_;
-
const double dy1 = sin_heading_ * half_length_;
-
const double dx2 = sin_heading_ * half_width_;
-
const double dy2 = -cos_heading_ * half_width_;
-
const double dx3 =
box.
cos_heading() *
box.
half_length();
-
const double dy3 =
box.
sin_heading() *
box.
half_length();
-
const double dx4 =
box.
sin_heading() *
box.
half_width();
-
const double dy4 = -
box.
cos_heading() *
box.
half_width();
-
-
return std::
abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
-
std::
abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
-
std::
abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
-
half_length_ &&
-
std::
abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
-
std::
abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
-
std::
abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
-
half_width_ &&
-
std::
abs(shift_x *
box.
cos_heading() + shift_y *
box.
sin_heading()) <=
-
std::
abs(dx1 *
box.
cos_heading() + dy1 *
box.
sin_heading()) +
-
std::
abs(dx2 *
box.
cos_heading() + dy2 *
box.
sin_heading()) +
-
box.
half_length() &&
-
std::
abs(shift_x *
box.
sin_heading() - shift_y *
box.
cos_heading()) <=
-
std::
abs(dx1 *
box.
sin_heading() - dy1 *
box.
cos_heading()) +
-
std::
abs(dx2 *
box.
sin_heading() - dy2 *
box.
cos_heading()) +
-
box.
half_width();
-
}
转载：https://blog.csdn.net/zhengshifeng123/article/details/125659950
查看评论