clickon.m

Another inverse kinematic of two link manipulator, robots.

plot([0 0 1 1 0],[0.5 -0.5 -0.5 0.5 0.5],'--r','LineWidth',2);
axis(axs);

[xin, yin] = getpts(h);
xo=xin(1);
yo=yin(1);
% y2=yin(2);
% x2=xin(2);
x2=1;
y2=0;
L1=1;
L2=1;
x11=1/2*((xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*yo-(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*y2-xo^2-yo^2+L1+x2^2+y2^2-L2)/(-xo+x2);
% x12=1/2*((xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*yo-(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*y2-xo^2-yo^2+L1+x2^2+y2^2-L2)/(-xo+x2);
y11=1/2*(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2);
% y12=1/2*(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2);
theta1=atan2((y11-yo),(x11-xo));
beta1=atan2((y2-y11),(x2-x11));
xt1=xo+cos(theta1);
yt1=yo+sin(theta1);
xt2=xt1+cos(beta1);
yt2=yt1+sin(beta1);
% theta2=beta1;
% beta2=theta1;
% xtt1=xo+cos(theta2);
% ytt1=yo+sin(theta2);
% xtt2=xtt1+cos(beta2);
% ytt2=ytt1+sin(beta2);

X1=[xo xt1 xt2];Y1=[yo yt1 yt2];
plot(X1,Y1,'g','LineWidth',10);
hold on
% X1=[xo xtt1 xtt2];Y1=[yo ytt1 ytt2];
% plot(X1,Y1,'r','LineWidth',10);
% hold on

% X1=[xo x12 x2];Y1=[yo y12 y2];
% plot(X1,Y1,'--','LineWidth',10);hold on;
% X1=[xo x11 x2];Y1=[yo y11 y2];
% plot(X1,Y1,'--','LineWidth',10);hold on

x2=0;
y2=1;
L1=1;
L2=1;
x11=1/2*((xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*yo-(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*y2-xo^2-yo^2+L1+x2^2+y2^2-L2)/(-xo+x2);
x12=1/2*((xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*yo-(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2)*y2-xo^2-yo^2+L1+x2^2+y2^2-L2)/(-xo+x2);
y11=1/2*(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3+(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2);
y12=1/2*(xo^2*yo+xo^2*y2-yo^2*y2+yo*x2^2-yo*y2^2-2*x2*xo*yo+x2^2*y2-yo*L1+yo*L2+L1*y2-y2*L2-2*x2*xo*y2+yo^3+y2^3-(-6*xo^2*yo^2*y2^2-12*xo^2*yo^2*x2^2+4*xo^2*yo^3*y2+4*xo^4*yo*y2+4*yo*x2^2*y2^3+2*yo^2*x2^2*L2+2*yo^2*x2^2*L1+4*yo*x2^4*y2+8*yo^2*x2^3*xo-6*yo^2*y2^2*x2^2+4*yo^3*y2*x2^2+8*xo^3*y2^2*x2+2*xo^2*y2^2*L2+2*xo^2*y2^2*L1-12*xo^2*y2^2*x2^2+4*xo^2*yo*y2^3+2*xo^2*yo^2*L2+2*xo^2*yo^2*L1+8*xo^3*yo^2*x2+8*x2^3*y2^2*xo+2*x2^2*y2^2*L2+2*x2^2*y2^2*L1+2*x2*xo*yo^4+2*x2*xo*y2^4-16*xo^3*yo*x2*y2-4*xo^2*yo*y2*L2-4*xo^2*yo*L1*y2+24*xo^2*yo*x2^2*y2+2*xo^2*L1*L2+12*x2^2*xo^2*L2+12*x2^2*xo^2*L1-8*x2^3*xo*L2-8*x2^3*xo*L1+2*x2*xo*L1^2-8*x2*xo^3*L2-8*x2*xo^3*L1+2*x2*xo*L2^2-4*yo*x2^2*y2*L2-4*yo*x2^2*L1*y2+12*yo^2*y2^2*x2*xo+2*x2^2*L1*L2-8*yo^3*y2*x2*xo-8*yo*y2^3*x2*xo-16*yo*x2^3*xo*y2-4*x2*xo*yo^2*L2-4*x2*xo*yo^2*L1-4*y2^2*L2*x2*xo-4*L1*y2^2*x2*xo-x2^2*L1^2+2*x2^4*L1+2*x2^4*L2-xo^2*L2^2+2*xo^4*L1+2*xo^4*L2-4*x2*xo*L1*L2-xo^2*L1^2-x2^2*L2^2+20*x2^3*xo^3-15*x2^2*xo^4-15*x2^4*xo^2-2*yo^2*x2^4-yo^4*x2^2-2*xo^4*y2^2-xo^2*y2^4+8*x2*xo*yo*y2*L2-2*x2^4*y2^2+8*x2*xo*yo*L1*y2-x2^2*y2^4-xo^6-2*xo^4*yo^2-xo^2*yo^4-x2^6+6*x2*xo^5+6*x2^5*xo)^(1/2))/(yo^2+y2^2-2*yo*y2-2*x2*xo+xo^2+x2^2);
theta1=atan2((y11-yo),(x11-xo));
beta1=atan2((y2-y11),(x2-x11));
% xt1=xo+cos(theta1);
% yt1=yo+sin(theta1);
% xt2=xt1+cos(beta1);
% yt2=yt1+sin(beta1);
theta2=beta1;
beta2=theta1;
xtt1=xo+cos(theta2);
ytt1=yo+sin(theta2);
xtt2=xtt1+cos(beta2);
ytt2=ytt1+sin(beta2);

% X1=[xo xt1 xt2];Y1=[yo yt1 yt2];
% plot(X1,Y1,'g','LineWidth',10);
% hold on
X1=[xo xtt1 xtt2];Y1=[yo ytt1 ytt2];
plot(X1,Y1,'r','LineWidth',10);
hold on

% X1=[xo x12 x2];Y1=[yo y12 y2];
% plot(X1,Y1,'--','LineWidth',10);hold on;
% X1=[xo x11 x2];Y1=[yo y11 y2];
% plot(X1,Y1,'--','LineWidth',10);hold on



% --- If Enable == 'on', executes on mouse press in 5 pixel border.
% --- Otherwise, executes on mouse press in 5 pixel border or over pushbutton1.
function pushbutton1_ButtonDownFcn(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)