clear

clc
clf
a=7;
b=15;
c=16;
d=20;
for k= 1:1:360;
th2=k*pi/180;
axis([-30 30 -30 30]); hold on
cla
Ax= a*cos(th2);
Ay= a*sin(th2);
plot ([0 Ax],[0 Ay],'-ob','lineWidth',3);
s=(a^2-b^2+c^2-d^2)/(2*(Ax-d));
p=((Ay^2)/((Ax-d)^2))+1;
q=(2*Ay*d-2*Ay*s)/(Ax-d);
r=(d-s)^2-c^2;
By=(-q+(sqrt(q^2-4*p*r)))/(2*p);
Bx=s-((Ay*By)/(Ax-d));
th3= atan((By-Ay)/(Bx-Ax));
plot([Ax Bx],[Ay By],'-ob','linewidth',3);
plot([0 d],[0 0],'k','linewidth',4);
th4= atan(By/(Bx-d));
if th4<0;
cx=c*cos((th4)+pi);
cy=c*sin((th4)+pi);
else
cx=c*cos(th4);
cy=c*sin(th4);
end
plot([d cx+d],[0 cy],'-ob','linewidth',3)
% otra posicion
Bypri=(-q-(sqrt(q^2-4*p*r)))/(2*p)
Bxpri=s-((Ay*Bypri)/(Ax-d));
th3pri= atan((Bypri-Ay)/(Bx-Ax));
plot([Ax Bxpri],[Ay Bypri],':om','linewidth',2);
th4pri= atan(Bypri/(Bxpri-d));
if th4pri<0;
cxpri=c*cos((th4pri));
cypri=c*sin((th4pri));
else
cxpri=c*cos(th4pri-pi);
cypri=c*sin(th4pri-pi);
end
plot([d cxpri + d],[0 cypri],':og','linewidth',2)
mcxpri(k)=[cxpri + d];
mcypri(k)=[cypri];
plot(mcxpri,mcypri,'r')
mcx(k)=[cx+d];
mcy(k)=[cy];
plot(mcx,mcy,'r')
max(k)=[Ax];
may(k)=[Ay];
plot(max,may,'r')
drawnow
axis equal
 % datos graficas
mak(k)=[k]
math3(k)=[th3*180/pi];
math4(k)=[th4*180/pi];
end

%% grafias
hold off
plot (mak,math3,':r','lineWidth',3);
hold on
plot (mak,math4,'-g','lineWidth',3);
legend('theta3','theta4')
xlabel('theta2')
grid on
drawnow
pause (2)
%%
hold off
plot (mak,mcx,':r','lineWidth',3);
hold on
plot (mak,mcy,'-g','lineWidth',3);
hold on
plot (mak,max,':k','lineWidth',3);
hold on
plot (mak,may,'-m','lineWidth',3);
hold on
plot (mak,mcxpri,'-b','lineWidth',3);
hold on
plot (mak,mcypri,'-c','lineWidth',3);
legend('Bx','By','Ax','Ay','Bx*','By*','Location','southwest')
xlabel('theta2')
grid on
drawnow