2018-11-13 14:17:15 -08:00
|
|
|
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
|
|
|
|
% ##### # # # %
|
|
|
|
|
% #### ##### ###### # # # # # # # %
|
|
|
|
|
% # # # # # ## # # # # # %
|
|
|
|
|
% # # # # ##### # # # # #### # ### %
|
|
|
|
|
% # # ##### # # # # # # # # # # %
|
|
|
|
|
% # # # # # ## # # # # # # %
|
|
|
|
|
% #### # ###### # # ##### ##### # # %
|
|
|
|
|
% %
|
2019-07-29 04:44:15 -07:00
|
|
|
% This file is part of openGJK. %
|
|
|
|
|
% %
|
|
|
|
|
% openGJK is free software: you can redistribute it and/or modify %
|
|
|
|
|
% it under the terms of the GNU General Public License as published by %
|
|
|
|
|
% the Free Software Foundation, either version 3 of the License, or %
|
|
|
|
|
% any later version. %
|
|
|
|
|
% %
|
|
|
|
|
% openGJK is distributed in the hope that it will be useful, %
|
|
|
|
|
% but WITHOUT ANY WARRANTY; without even the implied warranty of %
|
|
|
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The %
|
|
|
|
|
% GNU General Public License for more details. %
|
|
|
|
|
% %
|
|
|
|
|
% You should have received a copy of the GNU General Public License %
|
|
|
|
|
% along with Foobar. If not, see <https://www.gnu.org/licenses/>. %
|
|
|
|
|
% %
|
|
|
|
|
% openGJK: open-source Gilbert-Johnson-Keerthi algorithm %
|
|
|
|
|
% Copyright (C) Mattia Montanari 2018 - 2019 %
|
|
|
|
|
% http://iel.eng.ox.ac.uk/?page_id=504 %
|
|
|
|
|
% %
|
2018-11-13 14:17:15 -08:00
|
|
|
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
|
|
|
|
% %
|
2019-07-29 04:44:15 -07:00
|
|
|
% This file runs an example to illustrate how to cll the openGJK library %
|
|
|
|
|
% withing Matlab. It assumes that a mex file openGJK is availalbe, see %
|
2018-11-13 14:17:15 -08:00
|
|
|
% the runme.m script for information on how to compile it. %
|
|
|
|
|
% The example computes the minimum distance between two polytopes in 3D, %
|
|
|
|
|
% A and B, both defined as a list of points. %
|
|
|
|
|
% %
|
|
|
|
|
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
|
|
|
|
|
|
|
|
|
|
% DEFINE BODY A AS 3xN MATRIX, WHERE N IS THE NUMBER OF VERTICES OF BODY A
|
|
|
|
|
A = [ 0.0 2.3 8.1 4.3 2.5 7.1 1.0 3.3 6.0
|
|
|
|
|
5.5 1.0 4.0 5.0 1.0 1.0 1.5 0.5 1.4
|
|
|
|
|
0.0 -2.0 2.4 2.2 2.3 2.4 0.3 0.3 0.2];
|
|
|
|
|
|
|
|
|
|
% DEFINE BODY B IN THE OPPOSITE QUADRANT OF BODY A
|
|
|
|
|
B = -A;
|
|
|
|
|
|
|
|
|
|
% COMPUTE MINIMUM DISTANCE AND RETURN VALUE
|
|
|
|
|
dist = openGJK( A, B );
|
|
|
|
|
fprintf('The minimum distance between A and B is %.2f\n',dist);
|
|
|
|
|
|
|
|
|
|
% VISUALISE RESULTS
|
|
|
|
|
% .. create new figure
|
|
|
|
|
figure('units','centimeters', 'WindowStyle','normal', 'color','w',...
|
|
|
|
|
'Position',[0 8.5 9 6],'defaultAxesColorOrder',parula,...
|
|
|
|
|
'Renderer','opengl')
|
|
|
|
|
% .. adjust properties
|
|
|
|
|
axis equal tight off; hold all;
|
|
|
|
|
% .. display body A
|
|
|
|
|
DT = delaunayTriangulation(A');
|
|
|
|
|
[K,~] = convexHull(DT);
|
|
|
|
|
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
|
|
|
|
'EdgeColor','none','FaceColor',[.4 1 .9 ],...
|
|
|
|
|
'FaceLighting','flat' )
|
|
|
|
|
% .. display body B
|
|
|
|
|
DT = delaunayTriangulation(B');
|
|
|
|
|
[K,~] = convexHull(DT);
|
|
|
|
|
trisurf(K,DT.Points(:,1),DT.Points(:,2),DT.Points(:,3),...
|
|
|
|
|
'EdgeColor','none','FaceColor',[.4 1 .8 ],...
|
|
|
|
|
'FaceLighting','flat' )
|
|
|
|
|
% .. represent the computed distance as a sphere
|
|
|
|
|
[x,y,z] = sphere(100);
|
|
|
|
|
surf(x.*dist/2,y.*dist/2,z.*dist/2,'facecolor',[.9 .9 .9],...
|
|
|
|
|
'EdgeColor','none','FaceLighting','flat','SpecularColorReflectance',0,...
|
|
|
|
|
'SpecularStrength',1,'SpecularExponent',10,'facealpha',.7)
|
|
|
|
|
% ... adjust point of view
|
|
|
|
|
view(42,21)
|
|
|
|
|
% ... add light
|
|
|
|
|
light('Position',[5 -10 20],'Style','local');
|
