Apollo自动驾驶笔记|【规划】Box2d::HasOverlap() 碰撞检测接口详解百度|自动驾驶

Box2d::HasOverlap()

在笛卡尔坐标系下，检查两个box是否重叠
dx1~dx4, dy1~dy4, 以及部分判断条件如图（以车身坐标系为例，部分几何判断过程略过）

判断条件1：

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对应的橙色虚线框内。

Apollo自动驾驶笔记|【规划】Box2d::HasOverlap() 碰撞检测接口详解

文章图片

文章图片

判断条件2：

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()

即obstacle中的任意一点不在vehicle对应的橙色虚线框内。

文章图片

文章图片

【Apollo自动驾驶笔记|【规划】Box2d::HasOverlap() 碰撞检测接口详解】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(); }