% Program 6.3 Animation program for pendulum
% Inputs: time interval inter,
%  initial values ic = [y(1,1) y(1,2) y(1,3) y(1,4)], number of steps n
% Calls a one-step method such as trapstep.m
% Example usage: pend([0 10],[pi/2 0 pi 0],1000)
function doublePend(inter,ic,n)
h=(inter(2)-inter(1))/n;  % plot n points in total
y(1,:)=ic;                % enter initial conds in y
t(1)=inter(1);
set(gca,'xlim',[-2.2 2.2],'ylim',[-2.2 2.2], ...
  'XTick',[-2 0 2],'YTick',[-2 0 2], ...
  'Drawmode','fast','Visible','on','NextPlot','add');
cla;         
axis square               % make aspect ratio 1 - 1
bob = line('color','r','Marker','.','markersize',40,...
    'erase','xor','xdata',[],'ydata',[]);
rod = line('color','b','LineStyle','-','LineWidth',3,...
    'erase','xor','xdata',[],'ydata',[]);

bob2 = line('color','r','Marker','.','markersize',40,...
    'erase','xor','xdata',[],'ydata',[]);
rod2 = line('color','b','LineStyle','-','LineWidth',3,...
    'erase','xor','xdata',[],'ydata',[]);

for k=1:n
  t(k+1)=t(k)+h;
  y(k+1,:)=trapstep(t(k),y(k,:),h);
  xbob = sin(y(k+1,1)); ybob = -cos(y(k+1,1)); %bob1 coordinates
  xbob2 = xbob+sin(y(k+1,3)); ybob2 = ybob-cos(y(k+1,3)); %bob2 coordinates
  xrod = [0 xbob]; yrod = [0 ybob]; %rod1 coordinates
  xrod2 = [xbob xbob2]; yrod2 = [ybob ybob2]; %rod2 coordinates
  set(rod,'xdata',xrod,'ydata',yrod)
  set(rod2,'xdata',xrod2,'ydata',yrod2)
  set(bob,'xdata',xbob,'ydata',ybob)
  set(bob2,'xdata',xbob2,'ydata',ybob2)
  drawnow; pause(h)
end


function y = trapstep(t,x,h)
%one step of the Trapezoid Method
z1=ydot(t,x);
g=x+h*z1;
z2=ydot(t+h,g);
y=x+h*(z1+z2)/2;

function z=ydot(t,y)
d=0;  %d>0 is the damping coefficient representing friction at the pivot
A=12;    %A>0 is the coefficient of sinusoidal term A*sin(t) that represents outside forcing
g=9.81;length=1;
z(1) = y(2);
z(2) = ( (-3*g*sin(y(1)) -g*sin(y(1)-2*y(3)) -2*sin(y(1)-y(3))*( y(4)^2-( (y(2)^2)*cos(y(1)-y(3)) ) ))/(3-cos(2*y(1)-2*y(3))))-d*y(2);
z(3) = y(4);
z(4) = ( 2*sin(y(1)-y(3))*( (2*y(2)^2) +(2*g*cos(y(1))) +(y(4)^2*(cos(y(1)-y(3))) )))/ (3-cos(2*y(1)-2*y(3)));
