%roll_the_loop
% Simulation where a cylinder or sphere of radius R is released from rest
% from height h on a circular track of radius h+k, rough enough such that
% no slip occurs. The rolling object then attempts a loop around a smaller
% circle or radius r. Depending on the ratio h/(r-k), the object will either
% complete a loop of the smaller circle, or fall off and become a
% projectile.
%
% LAST UPDATED by Andy French Jan 2019

function roll_the_loop

%% INPUTS %%
r = 1;              %Radius of smaller circle /m
k = r/7;            %Radius of rolling object /m
m = 1.2;            %Mass of rolling object /kg
g = 9.81;           %Strength of gravity (N/kg)
dt = 0.001;         %Timestep dt /s
make_movie = 1;     %Make movie?
FPS = 30;           %Set frames per second for video
speed = 0.2;        %Set speed up factor for video
fsize = 18;         %Font size for plot

%Moment of inertia of rolling object (about axis through centre)
I = (2/5)*m*k^2;    %Sphere
I = (1/2)*m*k^2;    %Cylinder

%Radius of larger circle /m
h =0.7*( 5/2 +I/(2*m*k^2) )*(r-k);

%

%% SIMULATION %%

%Initialize parameters
n = 1; t = 0; theta = -pi/2; phi = 0;
w = 0; v = 0;
E = m*g*h;
x = -h; y = h+k;
R = NaN; F = NaN; mu_min = NaN;

%Calculate dynamics for initial quarter-circular slope ...
while theta(end) < 0     
    %Rate of change of angle
    theta_dot = v(n)/( h+k-k);
    
    %Update rotation angle of object /radians
    phi(n+1) = phi(n) + w(n)*dt;
    
    %Update theta
    theta(n+1) = theta(n) + theta_dot*dt;
    
    %Update time /s
    t(n+1) = t(n) + dt;
    
    %Increment time step counter
    n = n+1;
    
    %Angular speed (rads^-1)
    w(n) = sqrt( ( m*g*h -m*g*(h+k-k)*(1-cos(theta(n))) )/( 0.5*m*k^2 +0.5*I) );
    
    %Speed (m/s)
    v(n) = k*w(n);
    
    %Update x,y coordinates of centre of rolling object
    x(n) = h*sin( theta(n) );
    y(n) = h + k - h*cos( theta(n) );
    
    %Total energy /J
    E(n) = 0.5*m*v(n)^2 + 0.5*I*w(n)^2 + m*g*y(n);
    
    %Normal contact force R (radially inwards) (N)
    R(n) = m*( v(n)^2 )/(h+k-k) + m*g*cos( theta(n) );
    
    %Friction force (opposing motion, tangentially) (N)
    F(n) = m*g*sin(theta(n))/( (m*k^2)/I - 1 );
    
    %Compute minimum coefficient of friction to prevent slip
    mu_min(n) = F(n)/R(n);    
end

%Calculate parameters of smaller loop of radius r ...
projectile = 0;
while ( projectile==0 ) && ( theta(end) < 2*pi )
    
    %Rate of change of angle
    theta_dot = v(n)/( r-k);
    
    %Update rotation angle of object /radians
    phi(n+1) = phi(n) + w(n)*dt;
    
    %Update theta
    theta(n+1) = theta(n) + theta_dot*dt;
    
    %Update x,y coordinates of centre of rolling object
    x(n+1) = (r-k)*sin( theta(n+1) );
    y(n+1) = r - (r-k)*cos( theta(n+1) );
    
    %Update time /s
    t(n+1) = t(n) + dt;
    
    %Increment time step counter
    n = n+1;
    
    %Angular speed (rads^-1)
    w(n) = sqrt( ( m*g*h -m*g*(r-k)*(1-cos(theta(n))) )/( 0.5*m*k^2 +0.5*I) );
    
    %Speed (m/s)
    v(n) = k*w(n);
    
    %Normal contact force R (radially inwards) (N)
    R(n) = m*( v(n)^2 )/(r-k) + m*g*cos( theta(n) );
    
    %Total energy /J
    E(n) = 0.5*m*v(n)^2 + 0.5*I*w(n)^2 + m*g*y(n);
    
    %Has object entered projectile phase?
    if R(n) < 0
        projectile = 1;
        %Friction force (opposing motion, tangentially) (N)
        F(n) = 0;
        
        %Compute minimum coefficient of friction to prevent slip
        mu_min(n) = NaN;
    else
        %Friction force (opposing motion, tangentially) (N)
        F(n) = m*g*sin(theta(n))/( (m*k^2)/I - 1 );
        
        %Compute minimum coefficient of friction to prevent slip
        mu_min(n) = F(n)/R(n);
    end   
end

%Determine final angular speed (preserved if object enters projectile
%phase)
ww = w(n);

%Determine x,y velocities at end of r loop phase
vx = v(n)*cos( theta(n) );
vy = v(n)*sin( theta(n) );

%Calculate projectile phase ...
while projectile == 1
    
    %Angular speed (rads^-1)
    w(n+1) = ww;
    
    %Normal contact force R (radially inwards) (N)
    R(n+1) = 0;
    
    %Friction force (opposing motion, tangentially) (N)
    F(n+1) = 0;
    
    %Compute minimum coefficient of friction to prevent slip
    mu_min(n+1) = NaN;
    
    %Update rotation angle of object /radians
    phi(n+1) = phi(n) + w(n)*dt;
    
    %Update x,y coordinates of centre of projectile
    x(n+1) = x(n) + vx*dt;
    vy = vy - g*dt;
    y(n+1) = y(n) + vy*dt -0.5*g*dt^2;
    
    %Total energy /J
    E(n+1) = 0.5*m*( vx^2 + vy^2 ) + 0.5*I*w(n)^2 + m*g*y(n);
    
    %Determine deparation between edge of circle and loop of radius r
    a = r/sqrt( 1 + ( ( y(n+1) - r )/x(n+1) )^2 );
    bplus = (y(n+1) - r)*a/x(n+1) + r;
    bminus = -(y(n+1) - r)*a/x(n+1) + r;
    delta_plus = sqrt( (x(n+1) - a)^2 + (y(n+1) - bplus)^2 ) - k;
    delta_minus = sqrt( (x(n+1) + a)^2 + (y(n+1) - bminus)^2 ) - k;
    delta = min( [delta_plus,delta_minus] );
    if delta < 0
        %Object has struck circle - we shall assume a 'crash stop' rather
        %than an elastic bounce here!
        projectile = 0;
    end
    
    %Update time /s
    t(n+1) = t(n) + dt;
    
    %Increment time step counter
    n = n+1;
end

%Determine coordinate of a fixed position on the surface of the rolling
%object
xx = x + k*sin(phi);
yy = y + k*cos(phi);

%

%% PLOTS %%

%Filename string
fname =[ strrep( ['h=',num2str(h,3),' r=',num2str(r,3),...
    ' k=',num2str(k,3)],'.','p' )];
mkdir(fname);

%Plot minimum mu vs time
figure('color',[1 1 1],'name','minimum mu');
plot(t,mu_min);
ylim([0,2])
set( gca,'fontsize',fsize);
xlabel('t /s','fontsize',fsize);
ylabel('\mu_{min}','fontsize',fsize);
set(gca,'fontsize',fsize,'Box','on','Xgrid','on','Ygrid','on',...
    'Xminorgrid','on','Yminorgrid','on','GridLineStyle','-');
title('Minimum \mu to roll without sliding','fontsize',fsize);
print( gcf, [fname,'\mu min ',fname],'-dpng','-r300');
close(gcf);

%Plot total energy vs time
figure('color',[1 1 1],'name','total energy')
plot(t,E);
set( gca,'fontsize',fsize);
xlabel('t /s','fontsize',fsize);
ylabel('Total energy /J','fontsize',fsize);
set(gca,'fontsize',fsize,'Box','on','Xgrid','on','Ygrid','on',...
    'Xminorgrid','on','Yminorgrid','on','GridLineStyle','-');
title('Total energy /J','fontsize',fsize);
print( gcf, [fname,'\Energy ',fname],'-dpng','-r300');
close(gcf);

%Plot total angular speed vs time
figure('color',[1 1 1],'name','angular speed')
plot(t,w);
set( gca,'fontsize',fsize);
xlabel('t /s','fontsize',fsize);
ylabel('Angular speed \omega (rads^{-1})','fontsize',fsize);
set(gca,'fontsize',fsize,'Box','on','Xgrid','on','Ygrid','on',...
    'Xminorgrid','on','Yminorgrid','on','GridLineStyle','-');
title('Angular speed \omega','fontsize',fsize);
print( gcf, [fname,'\omega ',fname],'-dpng','-r300');
close(gcf);

%Plot figure and loop animation of rolling object
fig = figure('renderer','painters','color',[1 1 1],'name',...
    'roll the loop by Andy French Jan 2019','keypressfcn',@keypressfunc);
global d
if make_movie==1
    set(fig,'name',...
        'roll the loop by Andy French Jan 2019. MAKING MOVIE FRAMES. COULD BE SLOW!')
end

%Plot outer ramp
a = linspace(-pi/2,0,500);
plot( (h+k)*sin(a),-(h+k)*cos(a)+h+k ,'g','linewidth',2 );
hold on;

%Plot smaller circle
a = linspace(0,2*pi,500);
plot( r*sin(a),r*cos(a)+r ,'g','linewidth',2 );
axis equal
axis manual
axis tight
set( gca,'fontsize',fsize);
xlabel('x /m','fontsize',fsize);
ylabel('y /m','fontsize',fsize);
tit = title(['h=',num2str(h,2),', r=',num2str(r,2),...
    ', k=',num2str(k,2),', t=',num2str(t(n),2)],'fontsize',fsize);

%Define rolling circle
a = linspace(0,2*pi,500);
c = plot( x(1) + k*cos(a), y(1) + k*sin(a) ,'r','linewidth',2);

%Define blue star as a fixed point on the circumference of the rolling
%circle
bs = plot(xx(1),yy(1),'b*');

%Initialise blue star trajectory
bst = plot( xx(1),yy(1),'b-');

%Initialize rolling object centre of mass trajectory
cmt = plot( x(1),y(1),'r--');

%Animate rolling trajectory
d.stop = 0;
cycle = 1;
tt = 0;
video = [];
while d.stop ==0
    if (make_movie==1) && (cycle==1)
        %Open video file and set frame rate.
        video= VideoWriter( [fname,'\',fname] );
        video.FrameRate = FPS;
        open(video);
    end
    for n=1:length(t);
        %Update x,y coordinates of objects in scene
        set( c,'xdata',x(n) + k*cos(a), 'ydata',y(n) + k*sin(a) );
        set( bs,'xdata',xx(n), 'ydata',yy(n) );
        set( bst,'xdata',xx(1:n), 'ydata',yy(1:n) );
        set( cmt,'xdata',x(1:n), 'ydata',y(1:n) );
        set( tit, 'string', ['h=',num2str(h,2),', r=',num2str(r,2),...
            ', k=',num2str(k,2),', t=',num2str(t(n),2)] )
        drawnow;
        
        %Update time which resets every video frame
        tt = tt + dt;
        if tt > speed/FPS
            tt = 0;
            if ( cycle==1 ) && ( make_movie == 1)
                %Write video frame
                print( gcf, [fname,'\roll the loop ',fname],'-dpng','-r200');
                [image,map] = imread([fname,'\roll the loop ',fname],'png');
                writeVideo(video,image);
            end
        end
    end
    
    %Print a snapshot of final state and close video
    if cycle==1
        print( gcf, [fname,'\roll the loop ',fname],'-dpng','-r300');
        if make_movie == 1
            close(video);
        end
    end
    
    %Pause for two seconds then loop again
    pause(2);
    cycle = cycle + 1;
    set( fig, 'name',...
        ['roll the loop by Andy French Jan 2019. Repeat #',num2str(cycle)] );
end

%If d.stop == 1, save final frame, close any open video files and close
%figure
print( fig, [fname,'\roll the loop ',fname],'-dpng','-r300');
close(video);
delete(fig);

%%

% Key press handler
function keypressfunc( fig,evnt )
global d
if strcmp(get(fig,'currentkey'),'q')==1
    d.stop = 1;
end

%End of code