碰撞检测 | 详解矩形AABB与OBB碰撞检测算法(附ROS C++可视化)

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

矩形包围盒碰撞检测的核心原理是用矩形近似待检测物体轮廓，通过计算两个矩形间是否有重叠区域来进行碰撞判断。常用的包围形式有轴对齐包围盒(Axis-Aligned Bounding Box, AABB)以及更精细化具有角度的方向包围盒(Oriented Bounding Box)

在这里插入图片描述

本文的最终效果如下所示

在这里插入图片描述

1 AABB碰撞检测

如图所示为AABB碰撞检测的原理，其通过计算两个矩形在对齐的若干条轴上的投影来判碰，当且仅当所有轴的投影都发生重叠时两个AABB产生碰撞。

在这里插入图片描述

算法原理如表所示，其中矩形用左上角坐标和长宽来表示

在这里插入图片描述

按照算法流程实现核心算法

bool VRectangle::_AABBCollisionDetection(const std::shared_ptr<VRectangle>& other)
{
  return lt_pt_.x < other->left_top_point().x + std::fabs(other->length()) &&
         other->left_top_point().x < lt_pt_.x + std::fabs(l_) &&
         lt_pt_.y < other->left_top_point().y + std::fabs(other->width()) &&
         other->left_top_point().y < lt_pt_.y + std::fabs(w_);
}

2 OBB碰撞检测

OBB碰撞检测的核心原理是应用分离轴定理，在矩形的长宽两个方向进行投影(共计4次)，判断投影是否重叠；只要存在一条投影轴不发生重叠，表明找到了至少一条分离轴，即两个OBB不发生碰撞

沿矩形长宽方向设置方向向量以及待投影的几何向量

$\begin{cases} \boldsymbol{k}_1=\left[ \begin{matrix} \cos \alpha& \sin \alpha\\\end{matrix} \right] ^T\\ \boldsymbol{k}_2=\left[ \begin{matrix} \sin \alpha& -\cos \alpha\\\end{matrix} \right] ^T\\ \boldsymbol{k}_3=\left[ \begin{matrix} \cos \beta& \sin \beta\\\end{matrix} \right] ^T\\ \boldsymbol{k}_4=\left[ \begin{matrix} \sin \beta& -\cos \beta\\\end{matrix} \right] ^T\\\end{cases}, \begin{cases} \boldsymbol{v}_1=l_{1}^{-}\boldsymbol{k}_1\\ \boldsymbol{v}_2=w_{1}^{-}\boldsymbol{k}_2\\ \boldsymbol{v}_3=l_{2}^{-}\boldsymbol{k}_3\\ \boldsymbol{v}_4=w_{2}^{-}\boldsymbol{k}_4\\\end{cases}$

其中 $l_{i}^{-}$ 、 $w_{i}^{-}$ 分别是矩形长、宽的一半

设置向量 $\boldsymbol{d}$ 连接两个矩形的中心，用于判断两个矩形的投影是否发生重叠，具体判据是

$d<\left( r_{1}^{-}+r_{2}^{-} \right)$

其中 $d$ 是向量 $\boldsymbol{d}$ 在给定投影轴的投影长度； $r_{i}^{-}$ 是矩形 $i$ 在给定投影轴的投影长度的一半

在这里插入图片描述
举个例子，对于下图所示的投影，计算

$\begin{cases} d=\left| \boldsymbol{d}^T\boldsymbol{k}_1 \right|\\ r_{1}^{-}=\left| \boldsymbol{v}_{1}^{T}\boldsymbol{k}_1 \right|\\ r_{2}^{-}=\left| \boldsymbol{v}_{3}^{T}\boldsymbol{k}_1 \right|+\left| \boldsymbol{v}_{4}^{T}\boldsymbol{k}_1 \right|\\\end{cases}$

再进行对比即可

在这里插入图片描述

遍历矩形的四条边即可得到算法流程

在这里插入图片描述

核心代码如下所示

bool VRectangle::_OBBCollisionDetection(const std::shared_ptr<VRectangle>& other)
{
  const float shift_x = other->center().x - center_.x;
  const float shift_y = other->center().y - center_.y;

  const float half_length = 0.5f * std::fabs(l_);
  const float half_width = 0.5f * std::fabs(w_);
  const float other_half_length = 0.5f * std::fabs(other->length());
  const float other_half_width = 0.5f * std::fabs(other->width());
  const float dx1 = cos_angle_ * half_length;
  const float dy1 = sin_angle_ * half_length;
  const float dx2 = sin_angle_ * half_width;
  const float dy2 = -cos_angle_ * half_width;
  const float dx3 = other->cos_angle() * other_half_length;
  const float dy3 = other->sin_angle() * other_half_length;
  const float dx4 = other->sin_angle() * other_half_width;
  const float dy4 = -other->cos_angle() * other_half_width;

  return std::abs(shift_x * cos_angle_ + shift_y * sin_angle_) <= std::abs(dx3 * cos_angle_ + dy3 * sin_angle_) +
                                                                      std::abs(dx4 * cos_angle_ + dy4 * sin_angle_) +
                                                                      half_length &&
         std::abs(shift_x * sin_angle_ - shift_y * cos_angle_) <= std::abs(dx3 * sin_angle_ - dy3 * cos_angle_) +
                                                                      std::abs(dx4 * sin_angle_ - dy4 * cos_angle_) +
                                                                      half_width &&
         std::abs(shift_x * other->cos_angle() + shift_y * other->sin_angle()) <=
             std::abs(dx1 * other->cos_angle() + dy1 * other->sin_angle()) +
                 std::abs(dx2 * other->cos_angle() + dy2 * other->sin_angle()) + other_half_length &&
         std::abs(shift_x * other->sin_angle() - shift_y * other->cos_angle()) <=
             std::abs(dx1 * other->sin_angle() - dy1 * other->cos_angle()) +
                 std::abs(dx2 * other->sin_angle() - dy2 * other->cos_angle()) + other_half_width;
}