function out=dpend()
%
% Animation of a double pendulum
%
% Often crashes after some 50 time units.... not found the bug yet.
% maybe the DEs are wrong. or problem is with numerics not preserving
% energy.......
%
% Matthias Kawski. http://math.la.asu.edu/~kawski
% Original version: July 31, 2005.
if nargin < 5
   n=3;
end
global m1 m2 L1 L2 g;
m1 = 2;
m2 = 1;
g  = 1;
L1 = 1;
L2 = sqrt(3);
dt = 0.1;
y0 = [-pi 0 -pi 2*rand(1,1)-1];
t0 = 0;
twin=30;
r1 = (L1+L2)/20*sqrt(m1/(m1+m2));
r2 = (L1+L2)/20*sqrt(m2/(m1+m2));
%% used for sizing the animation window
LL = 1.2 * ( L1 + L2 );
%% reused data everytime when plotting a disk
bx = cos(pi*[0:0.1:2]);
by = sin(pi*[0:0.1:2]);

myfig=figure;        
set(myfig,'Units','normalized')
set(myfig,'Position',[0.02,0.05,0.40,0.90]);
% set(myfig,'Renderer','painters')
set(myfig,'DoubleBuffer','on')
fig2=subplot('position',[0.02,0.25,0.9,0.15]);
plot([0 100],[0 0],'k-')
     hold on     
fig3=subplot('position',[0.02,0.05,0.9,0.15]);
plot([0 100],[0 0],'k-');
     hold on     
fig1=subplot('position',[0.02,0.45,0.9,0.50]);

%% initial positions etc. 
%% assign graphics handles so we can later delete the previous 
%% image of the pendulum
xx1= L1*sin(y0(1));
yy1=-L1*cos(y0(1));
xx2=xx1+L2*sin(y0(3));
yy2=yy1-L2*cos(y0(3));
bbb4=plot([0,xx1],[0,yy1],'k-','LineWidth',2);
hold on
bbb3=plot([xx1,xx2],[yy1,yy2],'k-','LineWidth',2);
bbb1=fill(xx1+r1*bx,yy1+r1*by,'b');
bbb2=fill(xx2+r2*bx,yy2+r2*by,'r');
%% scale the window so that double pendulum fits nicely
axis([-LL LL -LL LL])
     
for N=[1:1000]
     %% save the old angles and positions so we can draw line segments
     %% from old to new positions
     posold=[xx1 yy1 xx2 yy2]';
     yold=y0;
     %% solve the systems of odes until next frame
     [t,y]=ode45(@dblpend,[t0,t0+dt],y0);   
     %% keep only the last (new) angles, delete the old ones
     y0=y(size(y,1),:);
     clear y
     clear t
     %% for plotting the graphs translate angles into [-pi,pi]
     y0(1)=mod(y0(1)+pi,2*pi)-pi;
     y0(3)=mod(y0(3)+pi,2*pi)-pi;
     t0=t0+dt;
     
     subplot(fig2)
     %% plot graph of angle and angular velocity of first pendulum
     %% for last time interval [t,t+dt]. Keep thr old one
     %% slide the plot window to show last 30 time units
     if abs(yold(1)-y0(1)) < 2.8
        plot([t0-dt,t0],[yold(1),y0(1)],'b-');
     end
     plot([t0-dt,t0],[yold(2),y0(2)],'c-.');
     axis([max(0,t0+1-twin) max(twin,t0+1) -3.5 3.5]);
     
     subplot(fig3)
     %% plot graph of angle and angular velocity of second pendulum
     %% for last time interval [t,t+dt]. Keep the old one
     %% slide the plot window to show last 30 time units    
     if abs(yold(3)-y0(3)) < 2.8
        plot([t0-dt,t0],[yold(3),y0(3)],'r-');
     end
     plot([t0-dt,t0],[yold(4),y0(4)],'m-.');
     axis([max(0,t0+1-twin) max(twin,t0+1) -3.5 3.5]);

     subplot(fig1)
     %% delete the old pictures of the pendulum 
     %% calculate the new positions
     xx1 =       L1*sin(y0(1));
     yy1 =     - L1*cos(y0(1));
     xx2 = xx1 + L2*sin(y0(3));
     yy2 = yy1 - L2*cos(y0(3));
     %% plot the path of the pendulumsin last time interval (overlay)
     plot([posold(1) xx1],[posold(2) yy1],'Color',[0.7,0.7,1]);
     plot([posold(3) xx2],[posold(4) yy2],'Color',[1,0.8,0.8]);
     %% plot the images of the pendulums at new positions (overlay)
     set(bbb4,'xdata',[0,xx1],'ydata',[0,yy1]);
     set(bbb3,'xdata',[xx1,xx2],'ydata',[yy1,yy2]);
     set(bbb1,'xdata',xx1+r1*bx,'ydata',yy1+r1*by);
     set(bbb2,'xdata',xx2+r2*bx,'ydata',yy2+r2*by);
     drawnow
     pause(0.01)
end
 
figure(myfig);
pause
close(myfig)

%--------------------------
% Subfunctions
%--------------------------

function dydt=dblpend(t,y)
global m1 m2 L1 L2 g;
%% possible type-ohs -- not triple checked to be the physically correct 
%% eqns of motions
dydt=[y(2),...
    (-m2*L1*y(2)^2*sin(-2*y(1)+2*y(3))-g*m2*sin(2*y(3)-y(1))+...
    g*sin(y(1))*m2+2*m2*L2*y(4)^2*sin(-y(1)+y(3))+2*g*sin(y(1))*m1)/...
    (-L1*m2+L1*m2*cos(-2*y(1)+2*y(3))-2*L1*m1),...
    y(4),...
    (g*m2*sin(y(3))+2*m2*L1*y(4)^2*sin(-y(1)+y(3))+...
    g*sin(y(3))*m1-m2*L2*y(4)^2*sin(-2*y(1)+2*y(3))+...
    2*L1*y(4)^2*m1*sin(-y(1)+y(3))+g*m1*sin(y(3)-2*y(1))+g*m2*sin(y(3)-2*y(1)))/...
    (-m2*L2+L2*m2*cos(-2*y(1)+2*y(3))-2*L2*m1)]';
return