发布时间:2023-05-16 19:30
在笛卡尔坐标系下,检查两个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对应的橙色虚线框内。
判断条件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对应的橙色虚线框内。
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();
}
【光剑藏书轩2021】《表象与本质:类比,思考之源和思维之火》
动手学习深度学习2.0-线性回归的从零开始实现&pytorch框架实现
Pytorch笔记:诡异的索引操作 + too many indices for tensor of dimension 1的一种解决方法
官宣!Apache Doris 从 Apache 孵化器毕业,正式成为 Apache 顶级项目
Vue+SpringBoot上传图片至Gitee并获取网页访问图片的url
【小程序自动化Minium】三、元素定位- WXSS 选择器的使用
pandas 作图 统计_绘图和可视化 Matplotlib,Pandas,Sseaborn, Pyecharts(数据蛙视频课摘录)...