MATLAB手写GJK碰撞检测：从支撑函数到单纯形更新

简介MATLAB环境下的GJK碰撞检测算法实现源码包面向计算机图形学、物理模拟及机器人路径规划等方向的开发者和算法学习者。资源基于Gilbert-Johnson-Keerthi算法通过支撑向量计算、Minkowski差构造与迭代最近距离求解来判断凸体是否相交算法逐步收集支撑点并更新单纯形直至收敛得到碰撞结论。包内包含两个MATLAB主示例及SampleShapeData数据文件可直接运行观察结果README说明则辅助快速上手和二次开发。压缩包共6个文件其中4个为.m脚本、1个为Markdown文档、另有.gitignore整体仅6KB核心GJK.m保留关键实现适合逐行分析原理。目前已有226人学习该资源结合代码与示例可直观理解GJK如何逼近最近点、判断碰撞还能通过调试观察单纯形更新细节为扩展到碰撞响应、物理引擎模拟打下基础。整体是一份轻量完整的算法学习资料值得对照论文动手实践。1. 用MATLAB手写GJK碰撞检测之前先把几何直觉拉满GJK碰撞检测对做机器人路径规划和游戏物理的人来说不算陌生判断两个凸形状是否相交是仿真循环里每帧都会发生的操作。但很多人一开始会直接找现成函数调用结果遇到非标准形状或需要穿透深度时只能回头自己实现。GJK算法的核心思路是避免逐个比较顶点而是通过“差空间”和支撑点把问题降维——你不需要真正求出相交区域只需要判断原点是否落在两个物体的闵可夫斯基差里。MATLAB的优势在于向量化矩阵运算让支撑函数的实现非常短但劣势是动态数组扩展和循环会让实现显得慢。实际上对顶点数几十、调用次数几百次的离线场景纯MATLAB的GJK完全能跑进毫秒级。这篇内容从支撑函数讲起给你一个能直接运行的二维GJK碰撞检测实现并说明单纯形更新、终止条件和数值误差的处理方法。适合正在做凸体碰撞检测、又不想引入C MEX或外部库的MATLAB使用者。搞懂这套几何逻辑之后你还能自然过渡到EPA穿透深度和连续碰撞检测。2. GJK碰撞检测的数学核心支撑函数与闵可夫斯基差在MATLAB中的实现2.1 为什么GJK能绕开逐顶点比较传统方法判断两个凸多边形是否相交最直接的做法是遍历A的每条边和B的每条边做线段相交同时检查顶点是否被包含复杂度为O(n×m)。更麻烦的是如果两个物体恰好嵌入彼此你还得额外处理完全包含的情况。GJK通过构造闵可夫斯基差把这个判定问题转化成一个更简单的迭代问题。定义两个集合A和B的闵可夫斯基差为A⊖B {a - b | a∈A, b∈B}。如果原点0在A⊖B内部那么必然存在ab即A和B有接触。GJK的巧妙之处在于它不必显式构造整个差空间而只通过支撑函数在特定方向上的“最远点”来逐步逼近原点附近的形状。支撑函数定义为s_A(d) argmax_{x∈A} (x·d)。直观理解就是用一条法线方向去“顶”这个形状找到最远的那个点。对于凸体而言这个点一定在边界上而且计算起来非常快。一旦你有了支撑函数差空间的支撑点就是s_{A⊖B}(d) s_A(d) - s_B(-d)。这一条公式是整个GJK算法的基础后面所有迭代都围绕它展开。2.2 为不同形状写支撑函数MATLAB的switch结构在MATLAB里我习惯用结构体描述形状字段至少包含type然后通过switch分支返回支撑点。这样后续添加新形状只用加一个case不影响主循环。function p support(shape, d) % SUPPORT 返回 shape 在方向 d 上的支撑点 % shape 是结构体必须包含 type 字段 % d 是列向量不需要归一化但建议保持尺度一致 switch shape.type case circle % 圆形圆心 半径 * 单位方向 p shape.center shape.radius * d / norm(d); case polygon % 多边形顶点矩阵 vertices每行一个点 dots shape.vertices * d; [~, idx] max(dots); p shape.vertices(idx, :); case box % 二维轴对齐矩形center 为 [cx;cy]size 为 [w;h] p shape.center 0.5 * shape.size .* sign(d); otherwise error(GJK:support, 未知形状类型%s, shape.type); end end这段代码用到了几个关键点。圆形支撑点在d方向上的投影是圆心加上半径乘以单位向量注意如果d为零向量会出现除以零所以外层循环要保证方向向量不为零。多边形的支撑点通过矩阵乘法一次性算出所有顶点的点积再取最大值这比for循环逐个点判断快得多。矩形支撑点更简单沿着每个轴取符号函数对应的边界角点。支撑函数不要求d为单位向量因为max操作比较的是点乘结果缩放向量只会等比缩放所有点积最大点的索引不变。但后面终止条件和搜索方向计算时最好手动归一化方向向量避免数值误差累积。2.3 差空间支撑点一句代码封装有了单个形状的支撑函数两个形状之间的差空间支撑点就非常容易写function w supportDiff(A, B, d) w support(A, d) - support(B, -d); end注意第二个支撑函数传入的是-d因为我们要从B中减去对应点而B在差空间中的支撑方向是反的。这个函数在整个GJK主循环中被反复调用每次调用都需要两次支撑函数求值。如果形状是多边形时间复杂度为O(nAnB)所以当顶点数较多时优化支撑函数的意义大于优化迭代次数。2.4 各种形状支撑函数的复杂度对比下面是不同形状的支撑函数计算开销和适用场景。形状类型支撑函数复杂度是否依赖顶点数典型应用场景圆形/球体O(1)否机械臂末端、传感器范围轴对齐矩形O(1)否环境障碍包围盒凸多边形/多面体O(n)是连杆机构、打磨路径椭圆/超椭球O(1)否考虑各向异性的安全区域从表中可以看出圆形和矩形这类简单形状的支撑函数是常数时间而多边形需要线性扫描顶点。如果你要检测的对象是多个三角形的组合建议先做凸分解用一组凸体分别跑GJK比直接处理凹多边形简单得多。GJK本身只对凸体保证正确性这是算法前提不是实现缺陷。3. 用MATLAB实现GJK主循环最小可运行代码与单纯形更新3.1 搭建主循环一个二维GJK实现现在进入最核心的部分。我给出一个可用于二维凸形状的GJK碰撞检测函数它已经包含单纯形更新逻辑。function [collide, simplexOut] gjk2d(A, B, maxIter) % GJK2D 二维GJK碰撞检测 % 输入A,B为结构体形状maxIter为最大迭代次数 % 输出collide为布尔值simplexOut为最终单纯形 if nargin 3 maxIter 30; end % 初始搜索方向取两个物体中心连线方向 d A.center - B.center; if norm(d) 1e-12 d [1; 0]; end d d / norm(d); % 初始单纯形是一个点 simplex supportDiff(A, B, d); d -simplex / norm(simplex); for iter 1:maxIter % 计算新的支撑点 w supportDiff(A, B, d); % 终止条件新支撑点没有跨过原点 if dot(w, d) 0 collide false; simplexOut simplex; return; end % 把新点加入单纯形 simplex(:, end1) w; %#okAGROW % 更新单纯形和搜索方向 [simplex, d] solveSimplex(simplex); % 如果搜索方向消失说明原点在单纯形内部 if norm(d) 1e-8 collide true; simplexOut simplex; return; end d d / norm(d); end % 达到迭代上限保守判定为碰撞 collid true; simplexOut simplex; end上面代码中的终止条件dot(w, d) 0是整个算法的关键。在每一次迭代里d是我们猜测的“原点所在方向”。如果从差空间支撑点在d方向上的投影还是负的说明差空间完全没有跨越原点两个物体不可能相交。注意我在最后达到迭代上限时直接判定为碰撞。这是安全策略碰撞检测宁可多报误判也不能漏报。实际项目中如果频繁触发迭代上限应该提高maxIter或检查单纯形更新逻辑。3.2 单纯形更新从点到线段到三角形solveSimplex负责维护单纯形并生成新的搜索方向。二维空间中单纯形最多是三角形原点若被三角形包含则判定碰撞。下面是完整的更新函数。function [simplex, d] solveSimplex(simplex) if size(simplex, 2) 1 % 单点搜索方向指向原点 d -simplex; return; elseif size(simplex, 2) 2 % 线段判断原点投影是否在线段内部 a simplex(:, 1); b simplex(:, 2); ab b - a; ao -a; if dot(ab, ao) 0 % 原点投影在线段上方向为线段垂直方向 d perp(ab); if dot(d, ao) 0 d -d; end simplex [a, b]; else % 原点靠近A点单纯形退化为点 simplex a; d -a; end elseif size(simplex, 2) 3 % 三角形检查原点是否在内部 a simplex(:, 1); b simplex(:, 2); c simplex(:, 3); ab b - a; ac c - a; ao -a; % 计算三角形法向二维中退化为标量叉积 n cross2(ab, ac); % 如果ao指向与法向同侧则原点可能在三角形上方 if dot(n, ao) 0 % 检查边AB和AC abPerp perp(ab); if dot(abPerp, ao) 0 simplex [a, b]; d abPerp; if dot(d, ao) 0 d -d; end else acPerp perp(ac); if dot(acPerp, ao) 0 simplex [a, c]; d acPerp; if dot(d, ao) 0 d -d; end else % 原点在三角形内部 d [0; 0]; end end else % 原点在三角形下方等价于检查对边方向 bc c - b; bcPerp perp(bc); if dot(bcPerp, -b) 0 simplex [b, c]; d bcPerp; if dot(d, -b) 0 d -d; end else % 退化情况保留AB simplex [a, b]; d abPerp; if dot(d, ao) 0 d -d; end end end else error(GJK:simplex, 二维GJK单纯形最多三个点); end end function v perp(v) % 二维向量的逆时针垂直 v [-v(2); v(1)]; end function c cross2(a, b) % 二维叉积的z分量 c a(1)*b(2) - a(2)*b(1); end这段代码里最容易被忽略的是线段情况下的投影判断。dot(ab, ao) 0意思是原点在A点往B方向这一侧也就是原点对线段的投影确实落在线段AB内而不是落在A的后方。只有在这种情况下才需要搜索垂直方向否则保留A点作为新的单纯形让搜索方向直接指向原点。三角形分支的处理方法是把三角形拆成两条边分别判断原点是否在边的“内侧”。如果原点在两条边之间说明原点落在三角形内部此时搜索方向为零向量主循环会判定碰撞。如果不在内部则丢弃离原点最远的顶点保留对应的边作为新单纯形再重新计算搜索方向。3.3 在MATLAB中定义形状并测试创建两个圆形并运行上面的GJK函数A struct(type, circle, center, [0; 0], radius, 1); B struct(type, circle, center, [1.5; 0], radius, 1); [collide, simplex] gjk2d(A, B); disp(collide);两个圆心距离是1.5半径和为2所以一定会碰撞。输出为1。如果把B的圆心移到[2.1;0]则距离2.1大于半径和2输出为0。3.4 参数设定经验表参数名推荐值作用注意事项maxIter30限制最大迭代次数二维碰撞通常5-10次收敛30足够d初始化center差方向提供初始搜索方向两个圆心重合时加随机扰动终止阈值1e-8判断搜索方向是否消失太小可能陷入无限循环maxIter不要设太大因为GJK的收敛速度是线性的正常非接触场景几轮迭代就会因dot(w,d)0跳出。如果设成1000也不会提升准确率反而会掩盖单纯形更新里的逻辑错误。4. 测试、参数调整与可视化让MATLAB里的GJK结果可信4.1 用随机形状做批量回归测试手写几何算法最怕只在两三个例子上跑通。我一般在写完GJK后马上做两类测试一类是已知相交/分离的简单用例另一类是随机生成的多边形对与暴力线段相交法做对照。写一个简单的对照函数用于验证二维凸多边形是否相交function brute bruteIntersect(A, B) % 仅适用于多边形 vertsA A.vertices; vertsB B.vertices; brute false; % 检查所有边对 for i 1:size(vertsA,1) for j 1:size(vertsB,1) p1 vertsA(i,:); p2 vertsA(mod(i, size(vertsA,1)) 1, :); q1 vertsB(j,:); q2 vertsB(mod(j, size(vertsB,1)) 1, :); if segIntersect(p1, p2, q1, q2) brute true; return; end end end % 包含关系检查A的顶点是否在B内 for i 1:size(vertsA,1) if inpolygon(vertsA(i,1), vertsA(i,2), vertsB(:,1), vertsB(:,2)) brute true; return; end end end这个函数在调试时非常有用。虽然慢但它实现简单、正确性容易确认。随机生成上百个三角形和四边形对比GJK结果与暴力结果如果出现不一致就输出当前形状和单纯形缩小复现范围。4.2 可视化GJK迭代过程MATLAB的绘图功能是调试几何算法最好的帮手。在gjk2d主循环里加一个回调或保存历史数据就能画出单纯形如何从点变成三角形。history {}; % 在循环内每次更新simplex后执行 history{end1} simplex; figure; hold on; axis equal; plotShape(A, r); plotShape(B, b); for k 1:length(history) simplex history{k}; plot(simplex(1,:), simplex(2,:), k*-, LineWidth, 1.5); pause(0.3); end其中plotShape是你的形状绘制函数用patch或plot都行。观察单纯形每一个点是否都来自差空间边界如果某个点明显不在两个物体轮廓的可达范围内多半是支撑函数传错了方向。4.3 影响碰撞检测可靠性的三个参数除了maxIter还有两个参数容易被忽略。第一个是支撑函数的容差。对于多边形顶点可能因为数值误差产生微小凸起导致支撑点选取抖动。解决办法是对点积加上一个很小的正数比如max(dots - 1e-12)让max操作在相等时倾向于选择索引靠前的点。第二个是方向向量的归一化时机。我在主循环里每次更新d之后都做归一化这个习惯能避免两种问题一是dot(w,d)的阈值含义变得模糊二是搜索方向长度过小导致单纯形更新误判。还有一点值得注意GJK只适合凸体。如果你传入的是凹多边形算法会在某些方向返回错误的支撑点导致碰撞结果不可信。处理凹形状的常见做法是先用形式化凸分解拆成多个凸体或者用外接凸包替代但这样会损失精度。5. 进阶EPA穿透深度与连续碰撞检测在MATLAB里的扩展5.1 从GJK到EPA计算最小穿透向量GJK返回的只是是否碰撞但物理仿真和机械臂避障通常还需要“把物体推开的最近方向”。经典扩展是EPAExpanding Polytope Algorithm它的输入是GJK结束时包含原点的单纯形。EPA会不断扩展这个单纯形使其逼近闵可夫斯基差的实际边界最终找到距原点最近的那条边或面对应的距离就是穿透深度。在MATLAB里做二维EPA核心是维护一个凸多边形每次从所有边中找距原点最近的一条求出该边的法向再沿法向调用supportDiff得到新的支撑点如果新支撑点到边的距离没有改善就停止。代码结构比GJK略长但完全复用支撑函数。5.2 连续碰撞检测在时间轴上找首次接触点如果两个物体运动速度很快离散碰撞检测可能会漏掉一次穿透。常见解决方法是连续碰撞检测把运动拆成起始和结束两个状态然后二分查找第一次碰撞的时刻function t0 firstContactTime(A, B, vA, vB, dt, tol) lo 0; hi dt; A0 A; B0 B; for iter 1:30 mid (lo hi) / 2; A.center A0.center vA * mid; B.center B0.center vB * mid; if gjk2d(A, B) hi mid; else lo mid; end end t0 (lo hi) / 2; end这个二分法简单可靠最多30次迭代就能把误差压到1e-3量级配合GJK的支撑函数复用整体效率可以接受。如果你追求更高性能可以使用“保守推进”策略让每一步的时间步长自适应变化。5.3 利用GJK结果做空间粗测对于场景中大量物体不要直接调用GJK。我在MATLAB里做批量碰撞检测时会先用AABB包围盒做粗测function broadphase testAABB(A, B) minA A.center - A.size/2; maxA A.center A.size/2; minB B.center - B.size/2; maxB B.center B.size/2; broadphase all(minA maxB) all(minB maxA); end只有粗测通过的物体对才进入GJK。这一步能过滤掉八成以上的远距离组合让整体仿真在MATLAB里也可用。记住GJK判定的是精确碰撞而AABB提供的只是候选集合两者配合才是工程上常见的落地路径。本文还有配套的精品资源点击获取