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

a1=10;a2=6;a3=7;a4=2;
psi0=pi/2;
A=[0,0];
B=[a1,0];
axis equal
for i = 1:60
    phi(i)=i*pi/30;
    %psi(i) = fzero(@bar4,psi0,[],phi(i),a1,a2,a3,a4);
    psi(i) = newton(@bar4,psi0,.000001,20,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;
    plot(x,y,'ro')
    pause(.1)
    plot(XX(:,1),XX(:,2),'y-')
end
    plot(XX(:,1),XX(:,2),'k-')



%function z = newton(func,z0,tol,maxiter,varargin)
%   hopt =  2*sqrt(eps);
%   z    =  z0;
%   val  =  feval(func,z,varargin{:});
%   iter = 1;
%while abs(val) >=tol
%   der  = (feval(func,z+hopt,varargin{:}) - val)/hopt;
%   z    = z - val/der;
%   val  = feval(func,z,varargin{:});
%   iter = iter+1;
%   if iter >= maxiter
%       fprintf('maxiter exceeded, no convergence')
%       break
%   end
%end


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));
