3D with rotation on matlab 三维装箱问题代码

% f = (x1-dx1)^2+(y1-dy1)^2 + (x2-dx2)^2+(y2-dy2)^2 + Rcost * ((theta1-dtheta1)^2 + (theta2-dtheta2)^2); % x=[x1, y1, x2, y2, sinthe1, sinthe2]; % f = (x1-dx1)^2+(y1-dy1)^2 + (x2-dx2)^2+(y2-dy2)^2; % x=[x1, y1, x2, y2]; % f = (x1-dx1)^2+(y1-dy1)^2 + (x2-dx2)^2+(y2-dy2)^2 + Rcost * ((sinthe1-sin(dtheta1))^2 + (sinthe2-sin(dtheta2))^2); % x=[x1, y1, x2, y2, sinthe1, sinthe2]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % rec1 % G(11,:) = -x1-a1*cos(theta1)-b1*sin(theta1); % G(12,:) = -x1+a1*cos(theta1)-b1*sin(theta1); % G(13,:) = -x1+a1*cos(theta1)+b1*sin(theta1); % G(14,:) = -x1-a1*cos(theta1)+b1*sin(theta1); % % G(15,:) = -y1-a1*sin(theta1)-b1*cos(theta1); % G(16,:) = -y1+a1*sin(theta1)-b1*cos(theta1); % G(17,:) = -y1+a1*sin(theta1)+b1*cos(theta1); % G(18,:) = -y1-a1*sin(theta1)+b1*cos(theta1); % % G(19,:) = -bbx+x1+a1*cos(theta1)+b1*sin(theta1); % G(20,:) = -bbx+x1+a1*cos(theta1)-b1*sin(theta1); % G(21,:) = -bbx+x1-a1*cos(theta1)-b1*sin(theta1); % G(22,:) = -bbx+x1-a1*cos(theta1)+b1*sin(theta1); % % G(23,:) = -bby+y1-a1*sin(theta1)-b1*cos(theta1); % G(24,:) = -bby+y1+a1*sin(theta1)+b1*cos(theta1); % G(25,:) = -bby+y1-a1*sin(theta1)+b1*cos(theta1); % G(26,:) = -bby+y1+a1*sin(theta1)-b1*cos(theta1); % % % rec2 % G(27,:) = -x2-a2*cos(theta2)-b2*sin(theta2); % G(28,:) = -x2+a2*cos(theta2)-b2*sin(theta2); % G(29,:) = -x2+a2*cos(theta2)+b2*sin(theta2); % G(30,:) = -x2-a2*cos(theta2)+b2*sin(theta2); % % G(31,:) = -y2-a2*sin(theta2)-b2*cos(theta2); % G(32,:) = -y2+a2*sin(theta2)-b2*cos(theta2); % G(33,:) = -y2+a2*sin(theta2)+b2*cos(theta2); % G(34,:) = -y2-a2*sin(theta2)+b2*cos(theta2); % % G(35,:) = -bbx+x2+a2*cos(theta2)+b2*sin(theta2); % G(36,:) = -bbx+x2+a2*cos(theta2)-b2*sin(theta2); % G(37,:) = -bbx+x2-a2*cos(theta2)-b2*sin(theta2); % G(38,:) = -bbx+x2-a2*cos(theta2)+b2*sin(theta2); % % G(39,:) = -bby+y2-a2*sin(theta2)-b2*cos(theta2); % G(40,:) = -bby+y2+a2*sin(theta2)+b2*cos(theta2); % G(41,:) = -bby+y2-a2*sin(theta2)+b2*cos(theta2); % G(42,:) = -bby+y2+a2*sin(theta2)-b2*cos(theta2); % G(43,:) = theta1-1.57; % G(44,:) = -theta1-1.57; % G(45,:) = theta2-1.57; % G(46,:) = -theta2-1.57; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % % rec1 % G(11,:) = -x1-a1*sqrt(1-sinthe1^2)-b1*sinthe1; % G(12,:) = -x1+a1*sqrt(1-sinthe1^2)-b1*sinthe1; % G(13,:) = -x1+a1*sqrt(1-sinthe1^2)+b1*sinthe1; % G(14,:) = -x1-a1*sqrt(1-sinthe1^2)+b1*sinthe1; % % G(15,:) = -y1-a1*sinthe1-b1*sqrt(1-sinthe1^2); % G(16,:) = -y1+a1*sinthe1-b1*sqrt(1-sinthe1^2); % G(17,:) = -y1+a1*sinthe1+b1*sqrt(1-sinthe1^2); % G(18,:) = -y1-a1*sinthe1+b1*sqrt(1-sinthe1^2); % % G(19,:) = -bbx+x1+a1*sqrt(1-sinthe1^2)+b1*sinthe1; % G(20,:) = -bbx+x1+a1*sqrt(1-sinthe1^2)-b1*sinthe1; % G(21,:) = -bbx+x1-a1*sqrt(1-sinthe1^2)-b1*sinthe1; % G(22,:) = -bbx+x1-a1*sqrt(1-sinthe1^2)+b1*sinthe1; % % G(23,:) = -bby+y1-a1*sinthe1-b1*sqrt(1-sinthe1^2); % G(24,:) = -bby+y1+a1*sinthe1+b1*sqrt(1-sinthe1^2); % G(25,:) = -bby+y1-a1*sinthe1+b1*sqrt(1-sinthe1^2); % G(26,:) = -bby+y1+a1*sinthe1-b1*sqrt(1-sinthe1^2); % % % rec2 % G(27,:) = -x2-a2*sqrt(1-sinthe2^2)-b2*sinthe2; % G(28,:) = -x2+a2*sqrt(1-sinthe2^2)-b2*sinthe2; % G(29,:) = -x2+a2*sqrt(1-sinthe2^2)+b2*sinthe2; % G(30,:) = -x2-a2*sqrt(1-sinthe2^2)+b2*sinthe2; % % G(31,:) = -y2-a2*sinthe2-b2*sqrt(1-sinthe2^2); % G(32,:) = -y2+a2*sinthe2-b2*sqrt(1-sinthe2^2); % G(33,:) = -y2+a2*sinthe2+b2*sqrt(1-sinthe2^2); % G(34,:) = -y2-a2*sinthe2+b2*sqrt(1-sinthe2^2); % % G(35,:) = -bbx+x2+a2*sqrt(1-sinthe2^2)+b2*sinthe2; % G(36,:) = -bbx+x2+a2*sqrt(1-sinthe2^2)-b2*sinthe2; % G(37,:) = -bbx+x2-a2*sqrt(1-sinthe2^2)-b2*sinthe2; % G(38,:) = -bbx+x2-a2*sqrt(1-sinthe2^2)+b2*sinthe2; % % G(39,:) = -bby+y2-a2*sinthe2-b2*sqrt(1-sinthe2^2); % G(40,:) = -bby+y2+a2*sinthe2+b2*sqrt(1-sinthe2^2); % G(41,:) = -bby+y2-a2*sinthe2+b2*sqrt(1-sinthe2^2); % G(42,:) = -bby+y2+a2*sinthe2-b2*sqrt(1-sinthe2^2); % % G(43,:) = sinthe1-1; % G(44,:) = -sinthe1-1; % G(45,:) = sinthe2-1; % G(46,:) = -sinthe2-1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % syms x1 x2; % f = 2*x1^2+2*x2^2-2*x1*x2-4*x1-6*x2; % x=[x1, x2]; %% % syms x1 y1 x2 y2; % f = (x1-1)^2+(y1+1)^2 + (11-x2)^2+(y2-8)^2; % x=[x1, y1, x2, y2]; %% % syms x1 x2 x3; % f = x1^2 + x1*x2 + 2*x2^2 - 6*x1 - 2*x2 - 12*x3; % x = [x1, x2, x3]; %% % syms x1 x2; % f = x1 + x2; % x = [x1, x2]; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % A=[1 1;1 5;-1 0;0 -1]; % b=[2 5 0 0]'; % G=[]; % x0=[0 0]; % Aeq=[]; % beq=[]; % label=1; %% % A=[-1 0 0 0; 0 -1 0 0; 1 0 0 0; 0 1 0 0; 0 0 -1 0; 0 0 0 -1; 0 0 1 0; 0 0 0 1]; % b=[-1 -1 9 9 -1 -1 9 9]'; % G=[]; % x0=[5 5 5 5]; % Aeq=[]; % beq=[]; % label=1; %% % A = []; % b = []; % G(1,:) = 2*x1^2 + x2^2 - 15; % G(2,:) = -x1 + 2*x2 + x3 - 3; % G(3,:) = -x1; % G(4,:) = -x2; % G(5,:) = -x3; % x0 = [0 0 0]; % Aeq = []; % beq = []; % label = 0; %% % A = []; % b = []; % G(1,:) = x1^2 + x2^2 - 1; % G(2,:) = x1^2 - x2; % x0 = [0 1]; % Aeq = []; % beq = []; % label = 0;