image thumbnail

Determination of the minimum distance between two SuperEllipsoids surfaces. Using Optimization

by

 

Optimization method to determine the minimum distance (or max overlap) between two SuperEllipsoids?

MAIN

Contents

function ContactOptim
% =========================================================================
% This function determines the minimum distance between two superellipsoids
% surfaces(or the maximum interference distance), using optimization
% techniques. The initial estimate for the contact points is determined
% so that the contact points are located on the surfaces.
% The visualization of the surfaces, the initial estimate and the
% optimization solution for the minimum distance is provided.
%
% Credits:
% Ricardo Fontes Portal / Luis Sousa
% IDMEC - Instituto Superior Tecnico - Universidade Técnica de Lisboa
% ricardo.portal(at)dem(.)ist(.)utl(.)pt
%
% April 2009 original version
% March 2013 updated version
%
% VERSION 1.0
% All rights reserved.
%
% Authors Acknowledgment:
% If you use SuperELLIPSOIDs-OPT in any program or publication, please
% acknowledge % the authors by refering The Archive of Mechanical
% Engineering paper:
% Portal, Ricardo. Sousa, Luís. Dias, João. “Contact Detection between
% Convex Superquadric Surfaces”. The Archive of Mechanical Engineering.
% Versita, Warsaw. LVII(2), pp. 165-186, DOI 10.2478/v10180-010-0009-8. 2010.
%

=========================================================================

close all; clear all; clc;
global p1 p2 p3 p4 p5 p6 a1 a2 a3 a4 a5 a6 e1 e2 e3 e4 ConSet;
global n1n n2n TRa TRb xlb xub x0;

INPUT SUPERELLIPSOIDS DATA ==============================================

p1=1.0; p2=3.2; p3=1.0;           % position (x,y,z) Surf 1
p4=2.0; p5=-1.0; p6=1.3;          % position (x,y,z) Surf 2
a1=2.0; a2=1.25; a3=1.6;          % superellipsoid 1 semi-axes
a4=2.0; a5=1.1; a6=0.6;           % superellipsoid 2 semi-axes
e1=4.0; e2=2.2;                   % implicit indexes Surf 1 (roundness)
e3=3.2; e4=2.5;                   % implicit indexes Surf 2 (roundness)
euler1A=0.0; euler2A=0.1; euler3A=-0.2;   % Euler param. Surf 1 (orientation)
euler1B=0.3; euler2B=0.3; euler3B=-0.1;   % Euler param. Surf 2 (orientation)
%==========================================================================

= Show options and constraints set ======================================

Constraints set

ConSet='normals_dot';
% 'surfaces'          % surfaces only       a)
% 'normals_cross'     % n1 x n2             b)
% 'normals_cross+n1'  % n1 x n2 + n2.r12    c)
% 'normals_cross+n2'  % n1 x n2 + n2.r12    d)
% 'normals_dot'       % n1.n2               e)
% 'normals_dot+n1'    % n1.n2   + n1.r12    f)
% 'normals_dot+n2'    % n1.n2   + n2.r12    g)
% 'dist_n1_n2'        % n1.r12  + n2.r12    h)

Calculation of euler e0 e inv(e1,e2,e3,e4)

euler0A=sqrt(1.0 - euler1A^2 - euler2A^2 - euler3A^2);
euler0B=sqrt(1.0 - euler1B^2 - euler2B^2 - euler3B^2);
e1(1)=2.0/e1(1);  e2(1)=2.0/e2(1);  e3(1)=2.0/e3(1);  e4(1)=2.0/e4(1);

% Calculation of the Transformation Matrices A and B
TRa(1,1)= euler0A^2 + euler1A^2 - euler2A^2 - euler3A^2; % 1st column = TRA(1)
TRa(2,1)= 2*(euler1A*euler2A + euler0A*euler3A);         %            = TRA(2)
TRa(3,1)= 2*(euler1A*euler3A - euler0A*euler2A);         %            = TRA(3)
TRa(1,2)= 2*(euler1A*euler2A - euler0A*euler3A);         % 2nd column = TRA(4)
TRa(2,2)= euler0A^2 - euler1A^2 + euler2A^2 - euler3A^2; %            = TRA(5)
TRa(3,2)= 2*(euler2A*euler3A + euler0A*euler1A);         %            = TRA(6)
TRa(1,3)= 2*(euler1A*euler3A + euler0A*euler2A);         % 3rd column = TRA(7)
TRa(2,3)= 2*(euler2A*euler3A - euler0A*euler1A);         %            = TRA(8)
TRa(3,3)= euler0A^2 - euler1A^2 - euler2A^2 + euler3A^2; %            = TRA(9)

TRb(1,1)= euler0B^2 + euler1B^2 - euler2B^2 - euler3B^2; % 1st column = TRA(1)
TRb(2,1)= 2*(euler1B*euler2B + euler0B*euler3B);         %            = TRA(2)
TRb(3,1)= 2*(euler1B*euler3B - euler0B*euler2B);         %            = TRA(3)
TRb(1,2)= 2*(euler1B*euler2B - euler0B*euler3B);         % 2nd column = TRA(4)
TRb(2,2)= euler0B^2 - euler1B^2 + euler2B^2 - euler3B^2; %            = TRA(5)
TRb(3,2)= 2*(euler2B*euler3B + euler0B*euler1B);         %            = TRA(6)
TRb(1,3)= 2*(euler1B*euler3B + euler0B*euler2B);         % 3rd column = TRA(7)
TRb(2,3)= 2*(euler2B*euler3B - euler0B*euler1B);         %            = TRA(8)
TRb(3,3)= euler0B^2 - euler1B^2 - euler2B^2 + euler3B^2; %            = TRA(9)

Create figure

figure1 = figure('PaperSize',[20.98 29.68],'Color',[1 1 1],...
    'NumberTitle','off','ToolBar','figure','MenuBar','none','Units','pixels',...
    'Name','Minimum Distance between Superellipsoids Surfaces');
annotation(gcf,'textbox',[0.5 0.99 0 0],...
    'String',{'Determination of the Minimum Distance between Superellipsoids Surfaces'},...
    'HorizontalAlignment','center','FontWeight','bold','FontSize',14,...
    'FontName','Calibri','FitBoxToText','on','EdgeColor','none');

% Create axes
axes1 = axes('Parent',gcf,'DataAspectRatio',[1 1 1],'TickDir','in',...
    'FontName','Calibri','FontWeight','light','FontSize',12);

set(gcf,'color','white', 'Position',[20 50 1000 750],...
    'nextplot','replacechildren');
view([120 20]);
hold('all');
lighting GOURAUD % FLAT, GOURAUD, PHONG, NONE
camlight headlight

Create Surfaces (with the parametric equations of the superellipsoids)

Surface 1

nxy = 20; nxz = nxy;
i=1;
for w=-pi : 2*pi/nxy : pi;
    j=1;
    for n=-pi/2 : pi/nxz : pi/2;
        xA(j,i)=a1*sign(cos(n))*abs(cos(n)).^e1*sign(cos(w))*abs(cos(w)).^e2;
        yA(j,i)=a2*sign(cos(n))*abs(cos(n)).^e1*sign(sin(w))*abs(sin(w)).^e2;
        zA(j,i)=a3*sign(sin(n))*abs(sin(n)).^e1;
        j=j+1;
    end
    i=i+1;
end
% Surface 2
i=1;
for w=-pi : 2*pi/nxy : pi;
    j=1;
    for n=-pi/2 : pi/nxz : pi/2;
        xB(j,i)=a4*sign(cos(n))*abs(cos(n)).^e3*sign(cos(w))*abs(cos(w)).^e4;
        yB(j,i)=a5*sign(cos(n))*abs(cos(n)).^e3*sign(sin(w))*abs(sin(w)).^e4;
        zB(j,i)=a6*sign(sin(n))*abs(sin(n)).^e3;
        j=j+1;
    end
    i=i+1;
end

% Global coordinates
xA_global= p1 + xA*TRa(1,1) + yA*TRa(1,2) + zA*TRa(1,3);
yA_global= p2 + xA*TRa(2,1) + yA*TRa(2,2) + zA*TRa(2,3);
zA_global= p3 + xA*TRa(3,1) + yA*TRa(3,2) + zA*TRa(3,3);
xB_global= p4 + xB*TRb(1,1) + yB*TRb(1,2) + zB*TRb(1,3);
yB_global= p5 + xB*TRb(2,1) + yB*TRb(2,2) + zB*TRb(2,3);
zB_global= p6 + xB*TRb(3,1) + yB*TRb(3,2) + zB*TRb(3,3);

% Global Reference Frame
w=4; h=2*w; LWidth=1.5; arrowlenght=2;
arrowXX = arrow3([0 0 0],[arrowlenght 0 0],'_K',w,h); set(arrowXX,'LineWidth',LWidth);
arrowYY = arrow3([0 0 0],[0 arrowlenght 0],'_K',w,h); set(arrowYY,'LineWidth',LWidth);
arrowZZ = arrow3([0 0 0],[0 0 arrowlenght],'_K',w,h); set(arrowZZ,'LineWidth',LWidth);
text(1.1*arrowlenght,0,0,'X','FontName','Calibri','FontWeight','bold','FontSize',16);
text(0,1.1*arrowlenght,0,'Y','FontName','Calibri','FontWeight','bold','FontSize',16);
text(0,0,1.1*arrowlenght,'Z','FontName','Calibri','FontWeight','bold','FontSize',16);

% Local Reference Frames
w=4; h=2*w; LWidth=1.5; delta=1.5;
arrowxxA = arrow3([p1 p2 p3],[p1+a1*delta*TRa(1,1) p2+a1*delta*TRa(2,1) p3+a1*delta*TRa(3,1)],'_K',w,h);
arrowyyA = arrow3([p1 p2 p3],[p1+a2*delta*TRa(1,2) p2+a2*delta*TRa(2,2) p3+a2*delta*TRa(3,2)],'_K',w,h);
arrowzzA = arrow3([p1 p2 p3],[p1+a3*delta*TRa(1,3) p2+a3*delta*TRa(2,3) p3+a3*delta*TRa(3,3)],'_K',w,h);
set(arrowxxA,'LineWidth',LWidth);set(arrowyyA,'LineWidth',LWidth);set(arrowzzA,'LineWidth',LWidth);

arrowxxB = arrow3([p4 p5 p6],[p4+a4*delta*TRb(1,1) p5+a4*delta*TRb(2,1) p6+a4*delta*TRb(3,1)],'_K',w,h);
arrowyyB = arrow3([p4 p5 p6],[p4+a5*delta*TRb(1,2) p5+a5*delta*TRb(2,2) p6+a5*delta*TRb(3,2)],'_K',w,h);
arrowzzB = arrow3([p4 p5 p6],[p4+a6*delta*TRb(1,3) p5+a6*delta*TRb(2,3) p6+a6*delta*TRb(3,3)],'_K',w,h);
set(arrowxxB,'LineWidth',LWidth);set(arrowyyB,'LineWidth',LWidth);set(arrowzzB,'LineWidth',LWidth);

surfaceA = surf(xA_global,yA_global,zA_global,'Parent',gca,...
    'EdgeLighting','gouraud',...
    'FaceLighting','gouraud',...
    'LineWidth',1.5,...
    'FaceColor',[0.68 0.92 1],...
    'FaceAlpha',0.6,...
    'EdgeColor',[0.04 0.14 0.42]);

surfaceB = surf(xB_global,yB_global,zB_global,'Parent',gca,...
    'EdgeLighting','gouraud',...
    'FaceLighting','gouraud',...
    'LineWidth',1.5,...
    'FaceColor',[0.6 1.0 0.6],...
    'FaceAlpha',0.6,...
    'EdgeColor',[0.2 0.2 0.2]);

axis equal; axis on; grid on;

Starting point estimate (initial points on the surfaces)

miu0=0.05;
% Start with the default options
options = optimset;
% Modify options setting
options = optimset(options,'Display','off',... % iter none final
    'MaxFunEvals' ,50,...
    'MaxIter'     ,20,...
    'TolFun'      ,1e-3,...
    'TolX'        ,1e-3,...
    'Jacobian'    ,'off',...
    'LargeScale'  ,'off');
tic
miuA = fsolve(@startpoint_estimate,miu0,options,a1,a2,a3,e1,e2,TRa);
miuB = fsolve(@startpoint_estimate,miu0,options,a4,a5,a6,e3,e4,TRb);
timeStartPoint= toc;

x0(1) = p1 + miuA*(p4-p1); x0(4) = p4 + miuB*(p1-p4);
x0(2) = p2 + miuA*(p5-p2); x0(5) = p5 + miuB*(p2-p5);
x0(3) = p3 + miuA*(p6-p3); x0(6) = p6 + miuB*(p3-p6);

Sol1 =line([x0(1) x0(4)],[x0(2) x0(5)],[x0(3) x0(6)],...
    'Parent',gca,'MarkerFaceColor',[1 1 1], 'MarkerEdgeColor',[0 0 0],...
    'Marker','o','MarkerSize',6.0,'LineWidth',1.5, 'Color',[0 0 0]);

lower and upper bounds

aaux1= [a1 a2 a3]; xbound1=max(aaux1);
aaux2= [a4 a5 a6]; xbound2=max(aaux2);

gap2=1.1;
xlb(1)=p1-(gap2)*xbound1;   xlb(2)=p2-(gap2)*xbound1;
xlb(3)=p3-(gap2)*xbound1;   xlb(4)=p4-(gap2)*xbound2;
xlb(5)=p5-(gap2)*xbound2;   xlb(6)=p6-(gap2)*xbound2;
xub(1)=p1+(gap2)*xbound1;   xub(2)=p2+(gap2)*xbound1;
xub(3)=p3+(gap2)*xbound1;   xub(4)=p4+(gap2)*xbound2;
xub(5)=p5+(gap2)*xbound2;   xub(6)=p6+(gap2)*xbound2;

OPTIMIZATION to determine the contact points

options = optimset;
options = optimset(options,'Display','off',... 'off',... 'iter',... 'notify',... 'final',...
    'Algorithm','interior-point',...'Trust-Region-Reflective',... 'interior-point',...'Active-Set',... 'interior-point',...
    'SubproblemAlgorithm' ,'cg',... (ldl-factorization,cg,lu-eig-factorization,cg-lu)
    'AlwaysHonorConstraints','none',...'bounds',...
    'MaxIter',100,...
    'MaxFunEvals',1000,...
    'DerivativeCheck','off',...
    'Diagnostics','off',...
    'GradObj','on',...      % if on -> consider user obj function gradient
    'GradConstr','off',...  % if on -> consider user contraints function gradient
    'FinDiffType','forward',... 'central',...
    'Hessian','bfgs',... 'lbfgs',...
    'TolX',  1e-6,...       % default 1e-10
    'TolCon',1e-6,...       % default 1e-06
    'TolFun',1e-6);         % default 1e-06
%   'PlotFcns' ,{@plotfval @plotconstrviolation});
%                          @optimplotx
%                          @optimplotfunccount
%                          @optimplotstepsize
%                          @optimplotfirstorderopt
tic % starts a stopwatch timer
[x,OBJopt,exitflag,output] = fmincon(@objfun,x0,[],[],[],[],xlb,xub,@constraints,options);
timeoptMATLAB=timeStartPoint + toc;
lastiter = output.iterations;
%ConViol  = output.constrviolation;
Fcalls = output.funcCount;
x1opt=x(1);x2opt=x(2); x3opt=x(3); x4opt=x(4); x5opt=x(5); x6opt=x(6);

[c ceq]=constraints(x); % to show the nonlinear constraints values ceq

Test for distance or penetration

Compute the dot product between n1 and (x4,x5,x6)-(x1,x2,x3)

ContactTest = n1n(1)*(x4opt-x1opt)+n1n(2)*(x5opt-x2opt)+n1n(3)*(x6opt-x3opt);
if (ContactTest <= 0)
    OBJreport = -sqrt(OBJopt);
    fprintf('Contact detected with : %10.6f m of penetration \n',-OBJreport);
else
    OBJreport = sqrt(OBJopt);
    fprintf('Minimum distance between bodies: %10.6f m \n',OBJreport);
end
Minimum distance between bodies:   1.731128 m 

Display Information

fprintf('Point on 1st superellipsoid: %10.6f %10.6f %10.6f \n', x1opt,x2opt,x3opt);
fprintf('Point on 2nd superellipsoid: %10.6f %10.6f %10.6f \n\n', x4opt,x5opt,x6opt);
fprintf('Initial estimate and optimization runs in: %6.3f seconds \n',timeoptMATLAB);
fprintf('Opt. Iterations: %5i \n',lastiter);
fprintf('Opt. Fun calls:  %5i \n',Fcalls);
G1opt = ceq(1); G2opt = ceq(2);
if (strcmp(ConSet,'surfaces'))
    fprintf('Nonlinear Constraints: %10.4g %10.4g\n',ceq);
elseif (strcmp(ConSet,'normals_dot'))
    G3opt = ceq(3);
    fprintf('Nonlinear Constraints: %10.4g %10.4g %10.4g\n',ceq);
elseif (strcmp(ConSet,'normals_dot+n1') || strcmp(ConSet,'normals_dot+n2') || strcmp(ConSet,'dist_n1_n2'))
    G3opt = ceq(3); G4opt = ceq(4);
    fprintf('Nonlinear Constraints: %10.4g %10.4g %10.4g %10.4g\n',ceq);
elseif (strcmp(ConSet,'normals_cross'))
    G3opt = ceq(3); G4opt = ceq(4); G5opt = ceq(5);
    fprintf('Nonlinear Constraints: %10.4g %10.4g %10.4g %10.4g %10.4g\n',ceq);
elseif (strcmp(ConSet,'normals_cross+n1') || strcmp(ConSet,'normals_cross+n2'))
    G3opt = ceq(3); G4opt = ceq(4); G5opt = ceq(5); G6opt = ceq(6);
    fprintf('Nonlinear Constraints: %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g\n',ceq);
end
Point on 1st superellipsoid:   1.912502   1.761475   1.548071 
Point on 2nd superellipsoid:   2.016728   0.036250   1.645742 

Initial estimate and optimization runs in:  0.742 seconds 
Opt. Iterations:    20 
Opt. Fun calls:    142 
Nonlinear Constraints: 3.078e-012  2.48e-011  2.95e-011

Show Solutions

figure(1); % Make the figure h current, visible, and
% displayed on top of other figures
% set(0,'CurrentFigure',1); % Make the figure h current, but do not
% change its visibility or stacking with respect to other figures

% Show normals of the initial estimate
ScaleNorm=2;
NormalA =line([x0(1) (x0(1)+(n1n(1))/ScaleNorm)],...
    [x0(2) (x0(2)+(n1n(2))/ScaleNorm)],...
    [x0(3) (x0(3)+(n1n(3))/ScaleNorm)],...
    'Parent',gca,'LineWidth',1.75, 'Color',[0 0 0],'DisplayName','Normal A');
NormalB =line([x0(4) (x0(4)+(n2n(1))/ScaleNorm)],...
    [x0(5) (x0(5)+(n2n(2))/ScaleNorm)],...
    [x0(6) (x0(6)+(n2n(3))/ScaleNorm)],...
    'Parent',gca,'LineWidth',1.75, 'Color',[0 0 0],'DisplayName','Normal B');
disp(' ')
disp('Press any key to continue.')
pause

% show Final Solution
Solution=line([x1opt x4opt], [x2opt x5opt], [x3opt x6opt],...
    'Parent',gca,'MarkerFaceColor',[1 1 0], 'MarkerEdgeColor',[0 0 0],...
    'Marker','o','MarkerSize',8.0,'LineWidth',2, 'Color',[0 0 0]);
disp('END.')
fclose('all');
 
Press any key to continue.
END.
end % Function Optim

Contact us