% Name: pathgenanim.M
% To animate any four-bar linkage given its dimensional parameters.
% Dimensional Parameters:
clear
clc
format long
%echo off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ao= [1.569386172, -.5516232500e-1]%ao=[1.568064266, -.59674332e-1];
b=[1.377627993, -1.785748385]%b=[1.374896000, -1.788217855];
aos=[1.430457599, -1.944701801]%aos=[1.42700120, -1.944492568];
bs=[2.622598020, -3.213988330]%bs=[2.618640075, -3.211433286];
%%%%%%%%%%%%%%%%%%%%%%%%
a1=norm(b-bs);
a2=norm(b-ao);
a3=norm(ao-aos);
a4=norm(bs-aos);

f=norm(ao);
e1=(aos-ao)'/a3;
E=[0 -1;1 0];
e2=E*e1;
alpha=atan2(-ao*e2/norm(ao),-ao*e1/norm(ao));
psiov=[(ao-b)*(bs-b)',(ao-b)*E*(bs-b)']/(a1*a2);
psio=atan2(psiov(2),psiov(1));

phiov=[(aos-bs)*(bs-b)',(aos-bs)*E*(bs-b)']/(a1*a4);
phio=atan2(phiov(2),phiov(1));
%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%
k1=(a1^2+a2^2-a3^2+a4^2)/(2*a2*a4);
k2=a1/a2;
k3=a1/a4;
fig=figure;
set(fig,'color','k');

p= line( ...
        'color','r', ...
        'linestyle','-', ...
        'linewidth',5, ...
        'erase','xor', ...
        'xdata',[],'ydata',[]);
%h=plot(0,0,'g','EraseMode','background','LineWidth',3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%
psi=psio;
A=k1-k2+(1-k3)*cos(psi);
B=-2*sin(psi);
C=k1+k2-(1+k3)*cos(psi);
T=roots([A B C]);
phi=2*atan(T(1));
O1=[0 0];
O2=a2*[cos(psi) sin(psi)];
O4=a1*[1 0];
O3=O4+a4*[cos(phi) sin(phi)];
u1=(O3-O2)'/norm(O3-O2);
u2=[0 -1;1 0]*u1;
R=O2+f*cos(alpha)*u1'+f*sin(alpha)*u2';
%linkage=[O1;O2;O3;O4];
link=[O1;O2;R;O3;O2;O3;O4];
%break


%%%%%%%%%%%%%%%%
%body = line( ...
 %       'color','y', ...
  %      'LineStyle','-', ...
   %     'erase','none', ...
    %    'xdata',link(:,1),'ydata',link(:,2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
poses=[-1 -1 90;
      -1.239 -.5529 77.3621
       -1.4204 .3232 55.0347
       -1.1668 1.2858 30.1974
       -.5657 1.8871 10.0210
       -.02927 1.9547 1.712
       .2632 1.5598 10.03
       .5679 .9339 30.1974
       1.0621 .3645 55.0346
       1.6311 .0632 77.3620
       2 0 90];
pose_j=poses([2:11],:)-ones(10,1)*poses(1,:);
r_j=[0 -1;1 0]'*pose_j(:,[1 2])';
phi_j=pose_j(:,3)*pi/180;
for kk=1:10
Q_j=[cos(phi_j(kk)) -sin(phi_j(kk));sin(phi_j(kk)) cos(phi_j(kk))];
error_j(kk)=norm(r_j(:,kk)-b'+Q_j*ao')-norm(ao-b);
errors(kk)=norm(r_j(:,kk)-bs'+Q_j*aos')-norm(aos-bs);
end
%position=poses(:,[1 2])+ones(11,1)*R;
pos = line( ...
        'color','y', ...
        'LineStyle','.', ...
        'erase','none', ...
        'MarkerSize',3,...
        'xdata',R(1),'ydata',R(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v2=[1 0]
v1=[bs-b]/a1
sina=v1*E'*v2'
cosa=v1*v2'
rotang=atan2(sina,cosa)
Q=[cos(rotang) -sin(rotang);sin(rotang) cos(rotang)]
Ro=-Q*b'
Co=Ro-Q*E'*[-1 -1]'

position=poses(:,[1 2])'
Rj=Co*ones(1,11)+Q*E'*poses(:,[1 2])'
%Rj(:,[5 6])=[];
RRj=Q*(-b'*ones(1,10)+r_j);

body = line( ...
      'color','y', ...
        'LineStyle','o', ...
        'erase','none', ...
  'xdata',Rj(1,:),'ydata',Rj(2,:));
%      'xdata',position(1,1),'ydata',position(1,2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%



axis([-3.5 4.5 -2.5 2.5])
axis equal
axis off
%hold on
E=[0 -1;1 0];
psiv=linspace(psio,-2*pi+.3*psio,10000);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%angle=linspace(0,2*pi,100)';
%Joints=.1/3*[cos(angle) sin(angle)];
%JOINTS=line( ...
%      'color','g', ...
%        'LineStyle','.', ...
%        'erase','none', ...
%  'xdata',Joints(1,:),'ydata',Joints(2,:));
%fill(Joints(:,1)+aos(1),Joints(:,2)+aos(2),'w')
%%%%%%%%%%%%%%%%%%%%%%%%%%

for jj=1:length(psiv)
psi=psiv(jj);    
A=k1-k2+(1-k3)*cos(psi);
B=-2*sin(psi);
C=k1+k2-(1+k3)*cos(psi);
T=roots([A B C]);

O1=[0 0];
O2=a2*[cos(psi) sin(psi)];
O4=a1*[1 0];
for nn=1:2
phi=2*atan(T(nn));
O3=O4+a4*[cos(phi) sin(phi)];
e1=(O2-O3)'/norm(O3-O2);
e2=(O4-O3)'/norm(O3-O2);

mu=180/pi*atan2(e1'*E'*e2,e1'*e2);
signmu(nn)=sign(sin(mu*pi/180));
end
if signmu==1
   phi=2*atan(T(2));
else
    phi=2*atan(T(1));
end
O3=O4+a4*[cos(phi) sin(phi)];
e1=(O2-O3)'/norm(O3-O2);
e2=(O4-O3)'/norm(O3-O2);
%pause
u1=(O3-O2)'/norm(O3-O2);
u2=[0 -1;1 0]*u1;
R=O2+f*cos(alpha)*u1'+f*sin(alpha)*u2';
linkage=[O1;O2;R;O3;O2;O3;O4];
%linkage=[O1;O2;O3;O4];

set(p,'XData',linkage(:,1),'YData',linkage(:,2))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(pos,'XData',R(1),'YData',R(2))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




drawnow
%plot(linkage(:,1),linkage(:,2))
%M(:,j)=getframe;
end
%movie2avi(M,'animlink','Quality',100)
