
clc
clear all


r2=2;
r3=1.5;
r4=1.2;
dy=1.5;
dx=0.5;
r5=2;

for i=1:50
   teta=0+(i-1)*pi/25;
   a=2*r3*dx+2*r3*r4*cos(teta);
   b=2*r3*dy+2*r4*r3*sin(teta);
   c=-sin(teta)*2*r4*dy-cos(teta)*2*r4*dx-r4^2-r3^2+r2^2;
   fi1=atan2((b+sqrt(a^2+b^2-c^2)),(a+c));
   fi2=atan2((b-sqrt(a^2+b^2-c^2)),(a+c));
   X1(i)=r4*cos(teta);
	Y1(i)=r4*sin(teta);
   if abs(teta)<=pi 
   	X2(i)=X1(i)-r3*cos(fi2);
		Y2(i)=Y1(i)+r3*sin(fi2);
	else
   	X2(i)=X1(i)-r3*cos(fi1);
		Y2(i)=Y1(i)+r3*sin(fi1);
	end
   X3(i)=dx;
	Y3(i)=dy;
	X4(i)=X1(i)+r5*cos(fi1);
	Y4(i)=Y1(i)-r5*sin(fi1);
end

clf

for n=1:4
	for i=1:50
   	hold on
	   plot([0,X1(i)],[0,Y1(i)],'r.',[0,X1(i)],[0,Y1(i)],'b')
	   plot([X1(i),X2(i)],[Y1(i),Y2(i)],'r.',[X1(i),X2(i)],[Y1(i),Y2(i)],'b')
		plot([X2(i),X3(i)],[Y2(i),Y3(i)],'r.',[X2(i),X3(i)],[Y2(i),Y3(i)],'b')
	   plot([X1(i),X4(i)],[Y1(i),Y4(i)],'r.',[X1(i),X4(i)],[Y1(i),Y4(i)],'b')
		grid on
		axis('equal')
		axis([-4 4 -5 10])
		pause(0.001)
	   clf
	end
end