% wc_animate.m - animate web cutter four-bar for ME 562
%   requires
%     wc_q.txt - npos x 9 matrix of {q}' at sequential positions
%     wc_link2.txt -  n2 x 2 matrix of local vertices for link 2
%     wc_link3.txt -  n3 x 2 matrix of local vertices for link 3
%     wc_link4.txt -  n4 x 2 matrix of local vertices for link 4
% HJSIII - 03.03.18

clear

% load q
all = load( 'wc_q.txt' );
[ npos, nc ] = size( all );

% load images of links
s2p = load( 'wc_link2.txt' )';
s3p = load( 'wc_link3.txt' )';
s4p = load( 'wc_link4.txt' )';

[ nr, n2 ] = size( s2p );
[ nr, n3 ] = size( s3p );
[ nr, n4 ] = size( s4p );

% initial image and handles
figure( 1 )
clf
h2 = plot( s2p(1,:),s2p(2,:) );
axis( [ -5 21  -5 21 ] )
axis square
hold on
h3 = plot( s3p(1,:),s3p(2,:) );
h4 = plot( s4p(1,:),s4p(2,:) );

% sequence through positions
for ipos = 1 : npos,
  q = all(ipos,:)';

% link 2
  r2 = q(1:2) * ones(1,n2);
  phi2 = q(3);
  A2 = [ cos(phi2) -sin(phi2) ; sin(phi2) cos(phi2) ];
  xy2 = r2 + A2*s2p;

% link 3
  r3 = q(4:5) * ones(1,n3);
  phi3 = q(6);
  A3 = [ cos(phi3) -sin(phi3) ; sin(phi3) cos(phi3) ];
  xy3 = r3 + A3*s3p;

% link 4
  r4 = q(7:8) * ones(1,n4);
  phi4 = q(9);
  A4 = [ cos(phi4) -sin(phi4) ; sin(phi4) cos(phi4) ];
  xy4 = r4 + A4*s4p;

% move images
  set( h2, 'XData', xy2(1,:) );
  set( h2, 'YData', xy2(2,:) );
  set( h3, 'XData', xy3(1,:) );
  set( h3, 'YData', xy3(2,:) );
  set( h4, 'XData', xy4(1,:) );
  set( h4, 'YData', xy4(2,:) );

pause( 0.1 )

% bottom of animation loop
end

