DoublePend.m

function DoublePend
format compact
c(1) = 'r';
c(2) = 'b';
c(3) = 'g';
c(4) = 'k';
c(5) = 'c';
c(6) = 'm';
c(7) = 'y';

h = colormap(lines);

E = 0.8;        %  1.0  1.2;    % Kinetic Energy

h2 = figure(2);
h2.Position = [11 -31 560 420];
dum = set(h2);

h3 = figure(3);
h3.Position = [10 381 560 420];
dum = set(h3);

h5 = figure(5);
h5.Position = [229 1 1008 804];
dum = set(h5);
clf
hold on


repnum = 128;                % 32
mulnum = 64/repnum;
for reploop = 1:repnum
    
    clear xvar yvar vy vx px py
    
    vx0 = 2*(0.5 - rand)*sqrt(E);
    vy0 = -vx0 + sign(randn)*sqrt(2*E - vx0.^2);
    
    v = (vx0 + vy0);
    KE = 0.5*(vx0.^2 + v.^2);
    
    x0 = 0.0;     % 0.1
    y0 = -0.0;    % -0.2
    
    y0 = [x0 y0 vx0 vy0];
    
    tspan = [1 3000];            % 500
    
    options = odeset('RelTol',1e-9);
    
    [t,y] = ode45(@f1,tspan,y0,options);
    
    
    siz = size(t);
%     y1 = mod(y(1:siz,1) + pi,2*pi)-pi;  % angle-1
%     y2 = mod(y(1:siz,2) + pi,2*pi)-pi;  % angle-2
%     y3 = mod(y(1:siz,3) + pi,2*pi)-pi;  % angular speed-1
%     y4 = mod(y(1:siz,4) + pi,2*pi)-pi;  % antular speed-2
    y1 = y(1:siz,1);  % angle-1
    y2 = y(1:siz,2);  % angle-2
    y3 = y(1:siz,3);
    y4 = y(1:siz,4);
    
    figure(1)
    plot(t(1:siz),y1,t(1:siz),y2)
    
    figure(2)
    plot(t(1:siz),y3,t(1:siz),y4)
    xlabel('Time')
    legend('speed')
    
    figure(3)
    plot(y1,y2)
    xlabel('x')
    ylabel('y')
    
    KinEnergy = 0.5*y3.^2 + 0.5*(y3.^2 + y4.^2 + 2*y3.*y4.*cos(y2-y1));
    PotEnergy = - 2*cos(y1) - cos(y2);
    Energy = KinEnergy + PotEnergy;
    figure(4)
    plot(t(1:siz),KinEnergy,t(1:siz),PotEnergy,t(1:siz),Energy)
    title('Energy')
    legend('Kin','Pot','E')
    
    % Power Spectrum
    Pow = 0;
    if Pow == 1
        yf = y(:,1);
        F = fft(yf);
        Pow = F.*conj(F);
        
        figure(4)
        plot(Pow)
    end
    
    trig = y2;  % Trigger for crossing Poincare section: try y1 and switch xvar/yvar and vx/vy
    
    %First Return Map
    Fst = 1;
    if Fst == 1
        cnt = 0;
        last = trig(1);
        for loop = 2:siz
            if (last < 0)&&(trig(loop) > 0)
                cnt = cnt+1;
                del1 = -trig(loop-1)./(trig(loop) - trig(loop-1));
                
                vx(cnt) = y3(loop-1) + del1*(y3(loop)-y3(loop-1));
                vy(cnt) = y4(loop-1) + del1*(y4(loop)-y4(loop-1));
                xvar(cnt) = y1(loop-1) + del1*(y1(loop)-y1(loop-1));
                yvar(cnt) = y2(loop-1) + del1*(y2(loop)-y2(loop-1));
                
                px(cnt) = 2*vx(cnt) + vy(cnt)*cos(yvar(cnt)-xvar(cnt));
                py(cnt) = vy(cnt) + vx(cnt)*cos(yvar(cnt)-xvar(cnt));
                
                last = trig(loop);
            else
                last = trig(loop);
            end
        end
        
        figure(5)
        plot(xvar,px,'o','MarkerSize',2,'Color',h(floor(0.9*mulnum*reploop)+1,:),'MarkerFaceColor',h(floor(0.9*mulnum*reploop)+1,:))
        xlabel('y')
        ylabel('vy')
    end
    
    pause(0.001)
    
end     % end reploop

figure(5)
hold off
print -dtiff DoublePendPrint


% Model function
    function dy = f1(t,y)
        
        A = y(4).^2.*sin(y(2)-y(1));
        B = -2*sin(y(1));
        C = y(3).^2.*sin(y(2)-y(1)).*cos(y(2)-y(1));
        D = sin(y(2)).*cos(y(2)-y(1));
        EE = 2 - (cos(y(2)-y(1))).^2;
        
        FF = y(4).^2.*sin(y(2)-y(1)).*cos(y(2)-y(1));
        G = -2*sin(y(1)).*cos(y(2)-y(1));
        H = 2*y(3).^2.*sin(y(2)-y(1));
        I = 2*sin(y(2));
        JJ = (cos(y(2)-y(1))).^2 - 2;
        
        dy = zeros(4,1);
        dy(1) = y(3);
        dy(2) = y(4);
        dy(3) = (A + B + C + D)./EE;
        dy(4) = (FF + G + H + I)./JJ;
        
    end     % end f5


end % end ltest