% 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;