GJK碰撞检测（Matlab代码实现）

article/2025/9/25 0:26:01

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

计算几何算法经常用于机器人避碰运动规划等安全攸关领域，对这些算法进行正确性证明非常重要。用形式化方法对算法进行验证是一种十分有效的手段，尤其是定理证明的方法用严格的数学公理和定理推理证明逻辑模型的性质，对所验证的性质而言是完备的，基于GJK算法设计了计算空间两条线段间距离的算法，用定理证明器HOL4对其相关的定义和定理进行形式化定义和证明，进而基于霍尔逻辑完成形式化表示和证明，对该算法的正确性实现了形式化验证。最后，给出了这一经过验证的算法在双臂机器人无碰撞运动规划中的应用，碰撞检测是机器人、动画仿真与虚拟现实等领域中一个非常关键的问题，其基本任务是确定两个或多个物体彼此之间是否发生接触或穿透。尽管针对碰撞检测已有了大量有价值的研究成果，但随着诸如虚拟现实等新兴领域的涌现及随之而来的人们对交互实时性、场景真实性要求的不断提高，碰撞检测技术所面临的问题也日益突出，其中最核心的问题是如何有效地提高碰撞检测的速度。本文重点研究了基于距离跟踪的Gilbert-Johnson-Keerth(简称GJK)碰撞算法。GJK是通过支持映射来计算空间中两个凸体间距的渐进算法，虽然其数学模型比较复杂,且难以理解,但是基于GJK模型的算法有快速，易实施且适用于多种凸体的优点。

📚2 运行结果

GJK碰撞

部分代码：

% Example script for GJK function
% Animates two objects on a collision course and terminates animation
% when they hit each other. Loads vertex and face data from
% SampleShapeData.m. See the comments of GJK.m for more information
%
% Most of this script just sets up the animation and transformations of
% the shapes. The only key line is:
% collisionFlag = GJK(S1Obj,S2Obj,iterationsAllowed)
%
% Matthew Sheen, 2016
clc;clear all;close all

%How many iterations to allow for collision detection.
iterationsAllowed = 6;

% Make a figure
fig = figure;
hold on

% Load sample vertex and face data for two convex polyhedra
SampleShapeData;

% Make shape 1
S1.Vertices = V1;
S1.Faces = F1;
S1.FaceVertexCData = jet(size(V1,1));
S1.FaceColor = 'interp';
S1Obj = patch(S1);

% Make shape 2
S2.Vertices = V2;
S2.Faces = F2;
S2.FaceVertexCData = jet(size(V2,1));
S2.FaceColor = 'interp';
S2Obj = patch(S2);

hold off
axis equal
axis([-5 5 -5 5 -5 5])
fig.Children.Visible = 'off'; % Turn off the axis for more pleasant viewing.
fig.Color = [1 1 1];
rotate3d on;

%Move them through space arbitrarily.
S1Coords = S1Obj.Vertices;
S2Coords = S2Obj.Vertices;

S1Rot = eye(3,3); % Accumulate angle changes

% Make a random rotation matix to rotate shape 1 by every step
S1Angs = 0.1*rand(3,1); % Euler angles
sang1 = sin(S1Angs);
cang1 = cos(S1Angs);
cx = cang1(1); cy = cang1(2); cz = cang1(3);
sx = sang1(1); sy = sang1(2); sz = sang1(3);

S1RotDiff = ...
[ cy*cz, cy*sz, -sy
sy*sx*cz-sz*cx, sy*sx*sz+cz*cx, cy*sx
sy*cx*cz+sz*sx, sy*cx*sz-cz*sx, cy*cx];

S2Rot = eye(3,3);

% Make a random rotation matix to rotate shape 2 by every step
S2Angs = 0.1*rand(3,1); % Euler angles
sang2 = sin(S2Angs);
cang2 = cos(S2Angs);
cx = cang2(1); cy = cang2(2); cz = cang2(3);
sx = sang2(1); sy = sang2(2); sz = sang2(3);

S2RotDiff = ...
[ cy*cz, cy*sz, -sy
sy*sx*cz-sz*cx, sy*sx*sz+cz*cx, cy*sx
sy*cx*cz+sz*sx, sy*cx*sz-cz*sx, cy*cx];

% Animation loop. Terminates on collision.
for i = 3:-0.01:0.2;
S1Rot = S1RotDiff*S1Rot;
S2Rot = S2RotDiff*S2Rot;

S1Obj.Vertices = (S1Rot*S1Coords')' + i;
S2Obj.Vertices = (S2Rot*S2Coords')' + -i;

% Do collision detection
collisionFlag = GJK(S1Obj,S2Obj,iterationsAllowed);

drawnow;

if collisionFlag
t = text(3,3,3,'Collision!','FontSize',30);
break;
end
end