%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% doublepend.m
% Animation program for double pendulum 
% using IVP solver
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function doublepend(int,ic,h,p, friction)

% Inputs: int = [a b] time interval,

% ic = [y(1,1) y(1,2)], initialize

% h = stepsize, p = steps per point plotted

% Calls a one-step method such as trapstep.m

% Example usage: pend([0 10],[pi/2 0],.05,1)

clf                       % clear figure window

a=int(1);b=int(2);n=ceil((b-a)/(h*p)); % plot n points in total

y(1,:)=ic;                % enter initial conds in y

t(1)=a;xmin=10;

set(gca,'XLim',[-3.2 3.2],'YLim',[-3.2 3.2], ...

  'XTick',[-1 0 1],'YTick',[-1 0 1], ...

  'Drawmode','fast','Visible','on','NextPlot','add');

cla;            % clear screen

plot(0,0,'ks')  % pivot where rod attached

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

  for i=1:p

    t(i+1) = t(i)+h;

    y(i+1,:) = doubletrapstep(t(i),y(i,:),h, friction);

  end

  y(1,:) = y(p+1,:);t(1)=t(p+1);

  xbob = cos(y(1,1)-pi/2); ybob = sin(y(1,1)-pi/2);xmin=min(xbob,xmin);

  xrod = [0 xbob]; yrod = [0 ybob];

  set(rod,'xdata',xrod,'ydata',yrod)

  set(bob,'xdata',xbob,'ydata',ybob)

  

  xbob2 = cos(y(1,3)-pi/2)+xbob; ybob2 = sin(y(1,3)-pi/2)+ybob;xmin=min(xbob2,xmin);

  xrod2 = [xbob xbob2]; yrod2 = [ybob ybob2];

  set(rod2,'xdata',xrod2,'ydata',yrod2)

  set(bob2,'xdata',xbob2,'ydata',ybob2)

  

  drawnow; pause(h)

end

xmin



function y = eulerstep(t,x,h)

%one step of the Euler method

y = x+h*ydot(t,x);



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 ydot=ydot(t,y)

g=9.8;length=1;

ydot(1) = y(2);

ydot(2) = -(g/length)*sin(y(1));