%jose David Falon Castillo

%Trayectoria de una linea recta

a=2.5;
b=1;
c=2.5;
d=2;
ap=0.5;

theta2_ini = acosd((a^2+d^2-b^2-c^2)/(2*a*d)+(b*c)/(a*d));
theta2_fin = acosd((a^2+d^2-b^2-c^2)/(2*a*d)-(b*c)/(a*d))

K1 = d/a;
K2 = d/c;
K3 = (a^2-b^2+c^2+d^2)/(2*a*c);
K4 = d/b;
K5 = (c^2-d^2-a^2-b^2)/(2*a*b);
cont = 1;
for theta2 = theta2_ini:theta2_fin
THETA2(cont) = theta2;
A = cosd(theta2)-K1-K2*cosd(theta2)+K3;
B = -2*sind(theta2);
C = K1-(K2+1)*cosd(theta2)+K3;
D = cosd(theta2)-K1+K4*cosd(theta2)+K5;
E = -2*sind(theta2);
F = K1+(K4-1)*cosd(theta2)+K5;

theta4_ab = 2*atand((-B-sqrt(B^2-4*A*C))/(2*A));
theta4_cr = 2*atand((-B+sqrt(B^2-4*A*C))/(2*A));

theta3_ab = 2*atand((-E-sqrt(E^2-4*D*F))/(2*D));
theta3_ac = 2*atand((-E+sqrt(E^2-4*D*F))/(2*D));

THETA3(cont) = theta3_ab;
THETA4(cont) = theta4_ab;

% Dibujo en la config. Abierta

Z2 = a*(cosd(theta2)+1i*sind(theta2));
Z3 = b*(cosd(theta3_ab)+1i*sind(theta3_ab));
Z4 = c*(cosd(theta4_ab)+1i*sind(theta4_ab));
AP = ap*(cosd(theta3_ab)+1i*sind(theta3_ab));
Tray(cont) = Z2+AP;

cont = cont+1;
Z = [0 Z2 Z2+Z3 Z2+AP Z2+Z3 Z2+Z3-Z4];
plot(Z,'or-','LineWidth',1.2)
hold on
plot(Tray,'b*')

pause(0.1)
hold off
end