一、簡介

基於matlab求解三維裝箱優化問題

二、原始碼

close all;
clear;
clc;

%% set data
Dbox = [10, 10, 10];
Dobj = [1, 1.5, 1;
        1, 1.5, 1];

PobjI = [7, 5, 5, 0, 0, 0;
         3, 5, 5, 0, 0, 0];

PobjD = [3, 3, -3, 0, 0, 0;
         8, 8, 13, 0, 0, 0];

objN = size(Dobj, 1);
Rcost = 10;



%% shape & bounding box & start point



[x, f] = genFunction(Rcost, PobjD);

Ar = [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1];
syms vx vy vz;
Vcoor = [vx; vy; vz];
for i=1:objN
    br = [Dobj(i,1); Dobj(i,1); Dobj(i,2); Dobj(i,2); Dobj(i,3); Dobj(i,3)];
    Gc{i} = Ar * Vcoor - br;
end

G = zeros(0);
for i=1:objN
    G = [G; genGi(i, Dbox, Dobj, x)];
end

%%
tic;
[X0,~] = zoutendijk1(f, x, G, Gc, Dbox, Dobj, PobjI, PobjD);
toc
figure;
plotResult(X0, Dbox, Dobj, PobjI, PobjD);

 












function [ f ] = genCollision( Gc, X, x0, Dobj, o1, o2 )

syms vx vy vz;
x = [vx vy vz];

Gc1 = Gc{o1};
Gc2 = Gc{o2};
a1 = Dobj(o1,1);
b1 = Dobj(o1,2);
c1 = Dobj(o1,3);
a2 = Dobj(o2,1);
b2 = Dobj(o2,2);
c2 = Dobj(o2,3);

R1V = [a1 b1 c1; -a1 -b1 c1; -a1 b1 c1; a1 -b1 c1;
       a1 b1 -c1; -a1 -b1 -c1; -a1 b1 -c1; a1 -b1 -c1];
 
R2V = [a2 b2 c2; -a2 -b2 c2; -a2 b2 c2; a2 -b2 c2;
       a2 b2 -c2; -a2 -b2 -c2; -a2 b2 -c2; a2 -b2 -c2];
 
% R1V(1,:) = [-a1, -b1];
% R1V(2,:) = [a1, b1];
% R1V(3,:) = [-a1, b1];
% R1V(4,:) = [a1, -b1];
% 
% R2V(1,:) = [-a2, -b2];
% R2V(2,:) = [a2, b2];
% R2V(3,:) = [-a2, b2];
% R2V(4,:) = [a2, -b2];

newR1V = R1V;
newR2V = R2V;
for i=1:size(R1V,1)
    newR1V(i,:) = rt(R1V(i,:), transpose(x0(6*(o1-1)+1:6*(o1-1)+3)), transpose(x0(6*(o2-1)+1:6*(o2-1)+3)), x0(6*(o1-1)+4:6*(o1-1)+6), x0(6*(o2-1)+4:6*(o2-1)+6));
    newR2V(i,:) = rt(R2V(i,:), transpose(x0(6*(o2-1)+1:6*(o2-1)+3)), transpose(x0(6*(o1-1)+1:6*(o1-1)+3)), x0(6*(o2-1)+4:6*(o2-1)+6), x0(6*(o1-1)+4:6*(o1-1)+6));
end

temp = zeros(size(newR2V,1),3);
minEdged = zeros(0,3);
for i=1:size(Gc1,1)
    for j=1:size(newR2V,1)
        temp(j,:) = [subs(Gc1(i), x, newR2V(j,:)), i, j];
    end
    [~, vec] = sort(temp);
    temp1 = temp(vec(:,1),:);
    for k=1:size(newR2V,1)
        if abs(temp1(k,1)-temp1(k+1,1)) < 0.1
            continue
        else
            break
        end
    end
    minEdged = [minEdged; temp1(1:k,:)];
end
[~, vec] = sort(minEdged, 'descend');
temp1 = minEdged(vec(:,1),:);
for k=1:size(temp1,1)
    if abs(temp1(k,1)-temp1(k+1,1)) < 0.1
        continue
    else
        break
    end
end
maxEdged1 = temp1(1:k,:);

minEdged = zeros(0,3);        % value % edge num % v num
for i=1:size(Gc1,1)
    for j=1:size(newR2V,1)
        temp(j,:) = [subs(Gc2(i), x, newR1V(j,:)), i, j];
    end
    [~, vec] = sort(temp);
    temp1 = temp(vec(:,1),:);
    for k=1:size(temp1,1)
        if abs(temp1(k,1)-temp1(k+1,1)) < 0.1
            continue
        else
            break
        end
    end
    minEdged = [minEdged; temp1(1:k,:)];   
end       
[~, vec] = sort(minEdged, 'descend');
temp1 = minEdged(vec(:,1),:);
for k=1:size(temp1,1)
    if abs(temp1(k,1)-temp1(k+1,1)) < 0.1
        continue
    else
        break
    end
end
maxEdged2 = temp1(1:k,:);
      

f = zeros(0,0);
if abs(maxEdged1(1,1)-maxEdged2(1,1)) < 0.1
    for i=1:size(maxEdged1,1)
        f = [f; -subs(Gc1(maxEdged1(i,2)), x, rt(R2V(maxEdged1(i,3),:), X(6*(o2-1)+1:6*(o2-1)+3), X(6*(o1-1)+1:6*(o1-1)+3), X(6*(o2-1)+4:6*(o2-1)+6), X(6*(o1-1)+4:6*(o1-1)+6)))];
    end
    for i=1:size(maxEdged2,1)
        f = [f; -subs(Gc2(maxEdged2(i,2)), x, rt(R1V(maxEdged2(i,3),:), X(6*(o1-1)+1:6*(o1-1)+3), X(6*(o2-1)+1:6*(o2-1)+3), X(6*(o1-1)+4:6*(o1-1)+6), X(6*(o2-1)+4:6*(o2-1)+6)))];
    end
elseif (maxEdged1(1,1) < maxEdged2(1,1))
    for i=1:size(maxEdged2,1)
        f = [f; -subs(Gc2(maxEdged2(i,2)), x, rt(R1V(maxEdged2(i,3),:), X(6*(o1-1)+1:6*(o1-1)+3), X(6*(o2-1)+1:6*(o2-1)+3), X(6*(o1-1)+4:6*(o1-1)+6), X(6*(o2-1)+4:6*(o2-1)+6)))];
    end
else
    for i=1:size(maxEdged1,1)
        f = [f; -subs(Gc1(maxEdged1(i,2)), x, rt(R2V(maxEdged1(i,3),:), X(6*(o2-1)+1:6*(o2-1)+3), X(6*(o1-1)+1:6*(o1-1)+3), X(6*(o2-1)+4:6*(o2-1)+6), X(6*(o1-1)+4:6*(o1-1)+6)))];
    end
end

return

end

 

三、執行結果

四、備註

版本:2014a