在线生成网站地图,食品网络营销策划方案,定制麻将app软件多少钱,企业支付的网站开发费如何入帐目录 0 专栏介绍1 凸多边形碰撞检测2 多边形判凸算法3 分离轴定理(SAT)4 算法仿真与可视化4.1 核心算法4.2 仿真实验 0 专栏介绍
#x1f525;课设、毕设、创新竞赛必备#xff01;#x1f525;本专栏涉及更高阶的运动规划算法轨迹优化实战#xff0c;包括#xff1a;曲线… 目录 0 专栏介绍1 凸多边形碰撞检测2 多边形判凸算法3 分离轴定理(SAT)4 算法仿真与可视化4.1 核心算法4.2 仿真实验 0 专栏介绍
课设、毕设、创新竞赛必备本专栏涉及更高阶的运动规划算法轨迹优化实战包括曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)每个算法都包含代码实现加深理解
详情运动规划实战进阶轨迹优化篇 本期实现如下的碰撞检测效果 1 凸多边形碰撞检测
在计算机图形学、游戏开发和机器人运动规划中碰撞检测是保证物体交互真实性的核心技术。凸多边形因其独特的几何特性任意两点连线均位于图形内部成为碰撞检测的高效研究对象。与凹多边形相比凸多边形的碰撞判定可通过分离轴定理Separating Axis Theorem, SAT在多项式时间内完成且无需复杂的三角剖分。本节将通过几何投影原理揭示如何通过极值投影快速判断两个凸多边形是否相交
2 多边形判凸算法
并非所有多边形都天生为“凸”。判凸算法是碰撞检测的前置关卡其任务是判断给定多边形的顶点序列是否满足凸性条件只有凸多边形才能应用分离轴定理进行碰撞检测。
通过向量积可以判断向量的旋转方向。如图所示由于 P 1 P 2 → × P 2 P 3 → 0 \overrightarrow{P_1P_2}\times \overrightarrow{P_2P_3}0 P1P2 ×P2P3 0
说明从 P 1 P 2 → \overrightarrow{P_1P_2} P1P2 到 P 2 P 3 → \overrightarrow{P_2P_3} P2P3 是向左转由于 P 2 P 3 → × P 3 P 4 → 0 \overrightarrow{P_2P_3}\times \overrightarrow{P_3P_4}0 P2P3 ×P3P4 0
说明从 P 2 P 3 → \overrightarrow{P_2P_3} P2P3 到 P 3 P 4 → \overrightarrow{P_3P_4} P3P4 是向右转。若多边形是凸多边形则向量的选择方向始终同向——逆时针遍历则总是向左转、顺时针遍历则总是向右转。所以在逆时针遍历多边形顶点的过程中若存在 P i − 1 P i → × P i P i 1 → 0 \overrightarrow{P_{i-1}P_i}\times \overrightarrow{P_iP_{i1}}0 Pi−1Pi ×PiPi1 0
则表明多边形非凸否则为凸多边形。 3 分离轴定理(SAT)
分离轴定理的核心思想直击几何本质若存在一条直线能将两图形投影分隔则二者不相交反之则碰撞。直观地如下图所示若两个凸多边形没有发生碰撞则必存在某角度的光源使两物体的投影存在间隙也即必存在一条直线使得两个多边形在这条直线上的投影不重叠这条直线被称为分离轴 如下图所示称凸多边形的某条边为边缘向量平行于边缘向量法向的直线称为投影轴。所有投影轴组成投影轴集合 P P P ∣ P ∣ |P| ∣P∣等于两个凸多边形的边数之和。遍历 P P P中的每条投影轴 p i \boldsymbol{p}_i pi将两个多边形分别投影到 p i \boldsymbol{p}_i pi上得到两个投影线段其重叠区域的长度称为重叠深度 d i o v e r l a p d_{i}^{\mathrm{overlap}} dioverlap。定义穿透深度 d p min i { d i o v e r l a p } d^p\min _i\left\{ d_{i}^{\mathrm{overlap}} \right\} dpimin{dioverlap}
若 d p 0 d^p0 dp0则两个凸多边形没有发生碰撞若 d p 0 d^p0 dp0则两凸多边形存在碰撞其中 d p d^p dp所在的投影轴称为穿透向量或分离向量将其中一个多边形沿分离向量运动 d p d^p dp个单位可以最快消除碰撞。 分离轴定理的算法流程如下所示 4 算法仿真与可视化
4.1 核心算法
首先找到两个待检测多边形的分离轴。分离轴平行于边缘法向量其位置不限因为其长度是无限的该轴的方向才是关键
std::vectorOgre::Vector3 axes;
for (int i 0; i size(); i)
{const auto pt1 points_[i];const auto pt2 points_[next(i)];const auto edge pt2 - pt1;Ogre::Vector3 nor(edge.y, -edge.x, 0.0);nor.normalise();axes.emplace_back(std::move(nor));
}for (int i 0; i other-size(); i)
{const auto pt1 other-points()[i];const auto pt2 other-points()[other-next(i)];const auto edge pt2 - pt1;Ogre::Vector3 nor(edge.y, -edge.x, 0.0);nor.normalise();axes.emplace_back(std::move(nor));
}接着对每一条分离轴计算两个多边形在该轴的投影。通过将一个多边形上的每个顶点向量与选定的投影轴进行点积然后保留该多边形在该投影轴上所有投影中的最大值和最小值即可表示一个多边形在某投影轴上的投影
double proj_1_min, proj_1_max;
double proj_2_min, proj_2_max;
project(points_, axis, proj_1_min, proj_1_max);
project(other-points(), axis, proj_2_min, proj_2_max);只要存在一条分离轴使两个多边形的投影不重合即表明不发生碰撞
if (!(proj_1_min proj_2_max proj_2_min proj_1_max))
{return false;
}4.2 仿真实验
通过Rviz-Add New Tool添加Polygon Simulation插件 开启碰撞检测功能后验证凸多边形的相交检测功能
未相交情形 相交情形 完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
