%script 4-R
function linkage()
clear; close all

a1=10;a2=6;a3=7;a4=2;
psi0=-30;
A=[0,0]
B=[a1,0]
axis equal
for i = 1:300
    phi(i)=i*pi/30;
    psi(i) = fzero(@bar4,psi0,[],phi(i),a1,a2,a3,a4);
    C=[a1+a2*cos(psi(i)),a2*sin(psi(i))];
    D=[a4*cos(phi(i)),a4*sin(phi(i))];
    XX=[A;B;C;D;A]
    plot(XX(:,1),XX(:,2),'k-')
    hold on
    x = (a1+a2*cos(psi)+a4*cos(phi))/2;
    y =    (a2*sin(psi)+a4*sin(phi))/2;
    if i<=60
       plot(x,y,'ro')
    else if i<=120
            plot(x(61:i),y(61:i),'g-')
         else if i<=180
                 plot(x(121:i),y(121:i),'b*')
              else if i<=240
                      psi = 30;
                      plot(x(181:i),y(181:i),'k+')
                  else
                       psi0=60;
                      plot(x(241:i),y(241:i),'go')
                   end
              end
         end
     end
    pause(.1)
    plot(XX(:,1),XX(:,2),'y-')
end
    plot(XX(:,1),XX(:,2),'k-')

function z = bar4(psi,phi,a1,a2,a3,a4)
z=a1^2+a2^2-a3^2+a4^2-2*a2*a4*cos(phi-psi)+...
    2*a1*(a2*cos(psi)-a4*cos(phi));
