function poisson_top

% -------------------------------------------------------------------------
%   Poisson Top Animation
%   Andreas Hansen
%   UC Berkeley
%   ME 175 (Intermediate Dynamics) Fall 2014
%
%   Revised by Daniel Kawano (12/22/2017)
% -------------------------------------------------------------------------

clear all;
close all;
clc;

%--------------------------------------------------------------------------

% Physical parameters, simulation parameters, and settings:

% Plot figures separately or together?

singleplot = false;
multiplot = true;

% Physical parameters and top properties:

P.g = 9.81;                             % m/s^2
P.m = 8/1000;                           % kg
P.radius1 = 20/1000;                    % m
P.lambdat = 1/4*P.m*P.radius1^2;       % kg-m^2    
P.lambdaa = 1/2*P.m*P.radius1^2;       % kg-m^2  
P.l_3 = 20/1000;                        % m  

P.faces = 20;
P.radius2 = P.radius1/20;                   % m
P.l_1 = P.l_3/50;                           % m
P.l_21 = 0.8*P.l_3;                         % m
P.l_22 = P.l_21+10*1/4*(P.l_3-P.l_21);      % m

top_color = [1, 0.8, 0.6];

% Initial conditions:

psi0 = 0;                   % rad
theta0 = 10*(pi/180);       % rad
phi0 = 0;                   % rad
x10 = 0;                    % m
x20 = 0;                    % m
psidot0 = 0;                % rad/s
thetadot0 = 0;              % rad/s
phidot0 = 90;               % rad/s
x1dot0 = 0;                 % m/s
x2dot0 = 0;                 % m/s

Y0 = [psi0, theta0, phi0, x10, x20, psidot0, thetadot0, phidot0, x1dot0, x2dot0]'; 

% Simulation parameters:

tf = 1;                 % s
dt = 0.001;             % s
T = (0 : dt : tf)';     % s

tol = 1e-8;
options = odeset('abstol', tol, 'reltol', tol);

% Plot display times:

wait_time = 0; 	
plot_time = 5;

%--------------------------------------------------------------------------

% Numerically integrate the equations of motion:

[T, Y] = ode45(@(t, Y) fun(t, Y, P), T, Y0, options);

% Extract the results:

psi = Y(:,1)';          % rad
theta = Y(:,2)';        % rad
phi = Y(:,3)';          % rad
x1 = Y(:,4)';           % m
x2 = Y(:,5)';           % m
psidot = Y(:,6)';       % rad/s
thetadot = Y(:,7)';     % rad/s
phidot = Y(:,8)';       % rad/s
x1dot = Y(:,9)';        % m/s
x2dot = Y(:,10)';       % m/s

% Compute the evolution of the corotational basis vectors and the angular 
% velocity vector:

e1 = zeros(3,length(T));
e2 = zeros(3,length(T));
e3 = zeros(3,length(T));
omega = zeros(3,length(T));

for i = 1:length(T)
    
    % Corotational basis vectors:
    
    e1(:,i) = [ cos(phi(i))*cos(theta(i))*cos(psi(i))-sin(phi(i))*sin(psi(i)); ...
                cos(phi(i))*cos(theta(i))*sin(psi(i))+sin(phi(i))*cos(psi(i)); ...
               -cos(phi(i))*sin(theta(i))];
    e2(:,i) = [-sin(phi(i))*cos(theta(i))*cos(psi(i))-cos(phi(i))*sin(psi(i)); ...
               -sin(phi(i))*cos(theta(i))*sin(psi(i))+cos(phi(i))*cos(psi(i)); ...
                sin(phi(i))*sin(theta(i))];
    e3(:,i) = [sin(theta(i))*cos(psi(i)); ...
               sin(theta(i))*sin(psi(i)); ...
               cos(theta(i))];
           
    % Angular velocity vector in terms of the corotational basis:
    
    omega(:,i) = [-cos(phi(i))*csc(theta(i)), sin(phi(i))*csc(theta(i)), 0; ...
                   sin(phi(i)), cos(phi(i)), 0; ...
                   cos(phi(i))*cot(theta(i)), -sin(phi(i))*cot(theta(i)), 1]\ ...
                 [psidot(i); thetadot(i); phidot(i)];   % rad/s

end

% Compute the displacement of the contact point P and the position/velocity 
% of the top's center of mass:

xP = [x1-P.l_3*e3(1,:); x2-P.l_3*e3(2,:); zeros(size(x1))];     % m
xCOM = xP + P.l_3*e3;                                           % m
vCOM = [x1dot; x2dot; -P.l_3*thetadot.*sin(theta)];             % m/s

% Compute the total mechanical energy:

Trot = zeros(1,length(T));
Ttrans = zeros(1,length(T));
U = zeros(1,length(T));

for i = 1:length(T)    
    Trot(i) = 1/2*(P.lambdat*omega(1,i)^2 + P.lambdat*omega(2,i)^2 + P.lambdaa*omega(3,i)^2);
    Ttrans(i) = 1/2*P.m*vCOM(:,i)'*vCOM(:,i);
    U(i) = P.m*P.g*xCOM(3,i);
end

E = Trot + Ttrans + U;      % J

%--------------------------------------------------------------------------

% Plot the results separately:

if singleplot == true  
    
    % (1) Top animation:
    
    % Set up the figure:
    
    figure('color', 'w', 'name', 'Poisson top animation'); 
    hold on; grid on; view([45 30]); camlight;
    title('Poisson top animation');
    axis('equal');
    xlabel('\itx\rm_1 (cm)');
    ylabel('\itx\rm_2 (cm)');
    zlabel('\itx\rm_3 (cm)              ', 'rotation', 0);
    xlim([-3,3]);
    ylim([-3,3]);
    zlim([0,6]);
    
    % Initialize the top geometry:
    
    for j = 1:P.faces
        top_surf1{j} = patch([0; 0; 0; 0], [0; 0; 0; 0], [0; 0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf2{j} = patch([0; 0; 0; 0], [0; 0; 0; 0], [0; 0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
        top_surf3{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
        top_surf4{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf5{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf6{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
    end
    
    vertices1_x = zeros(4,P.faces);
    vertices1_y = zeros(4,P.faces);
    vertices1_z = zeros(4,P.faces);
    vertices2_x = zeros(4,P.faces);
    vertices2_y = zeros(4,P.faces);
    vertices2_z = zeros(4,P.faces);
    vertices3_x = zeros(3,P.faces);
    vertices3_y = zeros(3,P.faces);
    vertices3_z = zeros(3,P.faces);
    vertices4_x = zeros(3,P.faces);
    vertices4_y = zeros(3,P.faces);
    vertices4_z = zeros(3,P.faces);
    vertices5_x = zeros(3,P.faces);
    vertices5_y = zeros(3,P.faces);
    vertices5_z = zeros(3,P.faces);
    vertices6_x = zeros(3,P.faces);
    vertices6_y = zeros(3,P.faces);
    vertices6_z = zeros(3,P.faces);
    
    % Animate the top:
    
    for i = 1:length(T)
        
        for j = 1:P.faces 
            
            % Vertices for the middle ring patch:
            
            v1_1 = xCOM(:,i) + P.radius1*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) + P.l_1*e3(:,i);
            v1_2 = xCOM(:,i) + P.radius1*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) - P.l_1*e3(:,i);
            v1_3 = xCOM(:,i) + P.radius1*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) - P.l_1*e3(:,i);
            v1_4 = xCOM(:,i) + P.radius1*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) + P.l_1*e3(:,i);
            vertices1_x(:,j) = [v1_1(1); v1_2(1); v1_3(1); v1_4(1)];
            vertices1_y(:,j) = [v1_1(2); v1_2(2); v1_3(2); v1_4(2)];
            vertices1_z(:,j) = [v1_1(3); v1_2(3); v1_3(3); v1_4(3)];
            
            % Vertices for the pin patch:
            
            v2_1 = xCOM(:,i) + P.radius2*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) + P.l_22*e3(:,i);
            v2_2 = xCOM(:,i) + P.radius2*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) - P.l_21*e3(:,i);
            v2_3 = xCOM(:,i) + P.radius2*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) - P.l_21*e3(:,i);
            v2_4 = xCOM(:,i) + P.radius2*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) + P.l_22*e3(:,i);
            vertices2_x(:,j) = [v2_1(1); v2_2(1); v2_3(1); v2_4(1)];
            vertices2_y(:,j) = [v2_1(2); v2_2(2); v2_3(2); v2_4(2)];
            vertices2_z(:,j) = [v2_1(3); v2_2(3); v2_3(3); v2_4(3)];
            
            % Vertices for the tip patch:
           
            v3_1 = v2_2;
            v3_2 = v2_3;
            v3_3 = xP(:,i);
            vertices3_x(:,j) = [v3_1(1); v3_2(1); v3_3(1)];
            vertices3_y(:,j) = [v3_1(2); v3_2(2); v3_3(2)];
            vertices3_z(:,j) = [v3_1(3); v3_2(3); v3_3(3)];
            
            % Vertices for the middle ring top patch:
            
            v4_1 = v1_1;
            v4_2 = v1_4;
            v4_3 = xCOM(:,i) + P.l_1*e3(:,i);
            vertices4_x(:,j) = [v4_1(1); v4_2(1); v4_3(1)];
            vertices4_y(:,j) = [v4_1(2); v4_2(2); v4_3(2)];
            vertices4_z(:,j) = [v4_1(3); v4_2(3); v4_3(3)];
            
            % Vertices for the middle ring bottom patch:
            
            v5_1 = v1_2;
            v5_2 = v1_3;
            v5_3 = xCOM(:,i) - P.l_1*e3(:,i);
            vertices5_x(:,j) = [v5_1(1); v5_2(1); v5_3(1)];
            vertices5_y(:,j) = [v5_1(2); v5_2(2); v5_3(2)];
            vertices5_z(:,j) = [v5_1(3); v5_2(3); v5_3(3)];
            
            % Vertices for the pin top patch:
            
            v6_1 = v2_1;
            v6_2 = v2_4;
            v6_3 = xCOM(:,i) + P.l_22*e3(:,i);
            vertices6_x(:,j) = [v6_1(1); v6_2(1); v6_3(1)];
            vertices6_y(:,j) = [v6_1(2); v6_2(2); v6_3(2)];
            vertices6_z(:,j) = [v6_1(3); v6_2(3); v6_3(3)];
            
            % Update the patches:
            
            set(top_surf1{j}, 'xdata', vertices1_x(:,j)*100, 'ydata', vertices1_y(:,j)*100, 'zdata', vertices1_z(:,j)*100);
            set(top_surf2{j}, 'xdata', vertices2_x(:,j)*100, 'ydata', vertices2_y(:,j)*100, 'zdata', vertices2_z(:,j)*100);
            set(top_surf3{j}, 'xdata', vertices3_x(:,j)*100, 'ydata', vertices3_y(:,j)*100, 'zdata', vertices3_z(:,j)*100);
            set(top_surf4{j}, 'xdata', vertices4_x(:,j)*100, 'ydata', vertices4_y(:,j)*100, 'zdata', vertices4_z(:,j)*100);
            set(top_surf5{j}, 'xdata', vertices5_x(:,j)*100, 'ydata', vertices5_y(:,j)*100, 'zdata', vertices5_z(:,j)*100);
            set(top_surf6{j}, 'xdata', vertices6_x(:,j)*100, 'ydata', vertices6_y(:,j)*100, 'zdata', vertices6_z(:,j)*100);
        
        end
        
        drawnow;
        pause(wait_time);
    
    end
    
    % (2) Corotational basis vectors animation:
    
    % Set up the figure:
    
    figure('color', 'w', 'name', 'Evolution of the corotational basis');
    hold on; grid on; view([45 30]);
    title({['Evolution of the corotational basis:']; ['e_1 = red,  e_2 = green,  e_3 = blue']});
    axis('equal');
    xlabel('\bfE\rm_1');
    ylabel('\bfE\rm_2');
    zlabel('\bfE\rm_3       ', 'rotation', 0);
    xlim([-1,1]);
    ylim([-1,1]);
    zlim([-1,1]);
    
    % Initialize the corotational basis vectors and their trajectories:
    
    corot_basis1 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'r');
    corot_basis2 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'g');
    corot_basis3 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'b');
    corot_basis_trace1 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'r', 'LineWidth', 0.5);
    corot_basis_trace2 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'g', 'LineWidth', 0.5);
    corot_basis_trace3 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'b', 'LineWidth', 0.5);
    
    % Animate the basis vectors:
    
    for i = 1:length(T)
        set(corot_basis1, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e1(1,i), 'vdata', e1(2,i), 'wdata', e1(3,i), 'LineWidth', 2, 'Color', 'r');
        set(corot_basis2, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e2(1,i), 'vdata', e2(2,i), 'wdata', e2(3,i), 'LineWidth', 2, 'Color', 'g');
        set(corot_basis3, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e3(1,i), 'vdata', e3(2,i), 'wdata', e3(3,i), 'LineWidth', 2, 'Color', 'b');
        set(corot_basis_trace1, 'xdata', e1(1,1:i), 'ydata', e1(2,1:i), 'zdata', e1(3,1:i));
        set(corot_basis_trace2, 'xdata', e2(1,1:i), 'ydata', e2(2,1:i), 'zdata', e2(3,1:i));
        set(corot_basis_trace3, 'xdata', e3(1,1:i), 'ydata', e3(2,1:i), 'zdata', e3(3,1:i));
        drawnow;
        pause(wait_time);
    end
    
    % (3) Display the trajectory of the top's contact point P:
    
    figure('color', 'w', 'name', 'Trajectory of the contact point');
    hold on;
    title('Trajectory of the contact point \itP\rm\bf on the horizontal plane');
    axis('equal');
    xlabel('\itx\rm_1 (cm)');
    ylabel('\itx\rm_2 (cm)          ', 'rotation', 0);
    xlim([-1,1]);
    ylim([-1,1]);
    plot(xP(1,:)*100, xP(2,:)*100, 'Color', 'b', 'LineWidth', 2);
    pause(plot_time);
    
    % (4) Confirm conservation of the total mechanical energy E:
    
    figure('color', 'w', 'name', 'Total mechanical energy over time'); 
    hold on;
    title('Total mechanical energy over time');
    axis('square');
    plot(T, E*1000, 'Color', 'b', 'LineWidth', 2);
    ylim([0.9*max(E*1000), 1.1*max(E*1000)]);
    xlabel('Time (s)');
    ylabel('Energy (mJ)');
    pause(plot_time);

end

%--------------------------------------------------------------------------

% Plot the results together:

if multiplot == true
    
    figure('color', 'w', 'name', 'Motion of a Poisson top');
    
   % (1) Top animation:
    
    % Set up the subplot figure:
    
    subplot(2,2,1); hold on; grid on; view([45 30]); camlight;
    title('Poisson top animation');
    axis('equal');
    xlabel('\itx\rm_1 (cm)');
    ylabel('\itx\rm_2 (cm)');
    zlabel('\itx\rm_3 (cm)              ', 'rotation', 0);
    xlim([-3,3]);
    ylim([-3,3]);
    zlim([0,6]);
    
    % Initialize the top geometry:
    
    for j = 1:P.faces
        top_surf1{j} = patch([0; 0; 0; 0], [0; 0; 0; 0], [0; 0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf2{j} = patch([0; 0; 0; 0], [0; 0; 0; 0], [0; 0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
        top_surf3{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
        top_surf4{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf5{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', top_color, 'FaceAlpha', 1);
        top_surf6{j} = patch([0; 0; 0], [0; 0; 0], [0; 0; 0], 'FaceColor', 'k', 'FaceAlpha', 1);
    end
    
    vertices1_x = zeros(4,P.faces);
    vertices1_y = zeros(4,P.faces);
    vertices1_z = zeros(4,P.faces);
    vertices2_x = zeros(4,P.faces);
    vertices2_y = zeros(4,P.faces);
    vertices2_z = zeros(4,P.faces);
    vertices3_x = zeros(3,P.faces);
    vertices3_y = zeros(3,P.faces);
    vertices3_z = zeros(3,P.faces);
    vertices4_x = zeros(3,P.faces);
    vertices4_y = zeros(3,P.faces);
    vertices4_z = zeros(3,P.faces);
    vertices5_x = zeros(3,P.faces);
    vertices5_y = zeros(3,P.faces);
    vertices5_z = zeros(3,P.faces);
    vertices6_x = zeros(3,P.faces);
    vertices6_y = zeros(3,P.faces);
    vertices6_z = zeros(3,P.faces);
    
    % (2) Corotational basis vectors animation:
    
    % Set up the subplot figure:
    
    subplot(2,2,2); hold on; grid on; view([45 30]);
    title({['Evolution of the corotational basis:']; ['e_1 = red,  e_2 = green,  e_3 = blue']});
    axis('equal');
    xlabel('\bfE\rm_1');
    ylabel('\bfE\rm_2');
    zlabel('\bfE\rm_3       ', 'rotation', 0);
    xlim([-1,1]);
    ylim([-1,1]);
    zlim([-1,1]);
    
    % Initialize the corotational basis vectors and their trajectories:
    
    corot_basis1 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'r');
    corot_basis2 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'g');
    corot_basis3 = quiver3(0, 0, 0, 0, 0, 0, 'LineWidth', 2, 'Color', 'b');
    corot_basis_trace1 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'r', 'LineWidth', 0.5);
    corot_basis_trace2 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'g', 'LineWidth', 0.5);
    corot_basis_trace3 = line(0, 0, 0, 'LineStyle', '-', 'Color', 'b', 'LineWidth', 0.5);     

    % (3) Trace the trajectory of the top's contact point P:
    
    % Set up the subplot figure:
    
    subplot(2,2,3); hold on;
    title({['Trajectory of the contact point']; ['\itP\rm\bf on the horizontal plane']});
    axis('equal');
    xlabel('\itx\rm_1 (cm)');
    ylabel('\itx\rm_2 (cm)          ', 'rotation', 0);
    xlim([-1,1]);
    ylim([-1,1]);
    
    % Initialize the trajectory:
    
    top_contact = line(0, 0, 0, 'LineStyle', '-', 'Color', 'b', 'LineWidth', 2);
    
    % (4) Confirm conservation of the total mechanical energy E:
    
    % Set up the subplot figure:
    
    subplot(2,2,4); hold on;
    title({['Total mechanical energy']; ['over time']});
    axis('square');
    xlim([0, tf]);
    ylim([0.9*max(E*1000), 1.1*max(E*1000)]);
    xlabel('Time (s)');
    ylabel('Energy (mJ)');
    
    % Initialize the energy curve:
    
    top_energy = line(0, 0, 'LineStyle', '-', 'Color', 'b', 'LineWidth', 2);
    
    % Animate the subplots:
    
    for i = 1:length(T)
        
        % (1) Top:
        
        for j = 1:P.faces 
            
            % Vertices for the middle ring patch:
            
            v1_1 = xCOM(:,i) + P.radius1*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) + P.l_1*e3(:,i);
            v1_2 = xCOM(:,i) + P.radius1*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) - P.l_1*e3(:,i);
            v1_3 = xCOM(:,i) + P.radius1*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) - P.l_1*e3(:,i);
            v1_4 = xCOM(:,i) + P.radius1*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) + P.l_1*e3(:,i);
            vertices1_x(:,j) = [v1_1(1); v1_2(1); v1_3(1); v1_4(1)];
            vertices1_y(:,j) = [v1_1(2); v1_2(2); v1_3(2); v1_4(2)];
            vertices1_z(:,j) = [v1_1(3); v1_2(3); v1_3(3); v1_4(3)];
            
            % Vertices for the pin patch:
            
            v2_1 = xCOM(:,i) + P.radius2*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) + P.l_22*e3(:,i);
            v2_2 = xCOM(:,i) + P.radius2*(cos((j-1)*2*pi/P.faces)*e1(:,i) + sin((j-1)*2*pi/P.faces)*e2(:,i)) - P.l_21*e3(:,i);
            v2_3 = xCOM(:,i) + P.radius2*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) - P.l_21*e3(:,i);
            v2_4 = xCOM(:,i) + P.radius2*(cos(j*2*pi/P.faces)*e1(:,i) + sin(j*2*pi/P.faces)*e2(:,i)) + P.l_22*e3(:,i);
            vertices2_x(:,j) = [v2_1(1); v2_2(1); v2_3(1); v2_4(1)];
            vertices2_y(:,j) = [v2_1(2); v2_2(2); v2_3(2); v2_4(2)];
            vertices2_z(:,j) = [v2_1(3); v2_2(3); v2_3(3); v2_4(3)];
            
            % Vertices for the tip patch:
           
            v3_1 = v2_2;
            v3_2 = v2_3;
            v3_3 = xP(:,i);
            vertices3_x(:,j) = [v3_1(1); v3_2(1); v3_3(1)];
            vertices3_y(:,j) = [v3_1(2); v3_2(2); v3_3(2)];
            vertices3_z(:,j) = [v3_1(3); v3_2(3); v3_3(3)];
            
            % Vertices for the middle ring top patch:
            
            v4_1 = v1_1;
            v4_2 = v1_4;
            v4_3 = xCOM(:,i) + P.l_1*e3(:,i);
            vertices4_x(:,j) = [v4_1(1); v4_2(1); v4_3(1)];
            vertices4_y(:,j) = [v4_1(2); v4_2(2); v4_3(2)];
            vertices4_z(:,j) = [v4_1(3); v4_2(3); v4_3(3)];
            
            % Vertices for the middle ring bottom patch:
            
            v5_1 = v1_2;
            v5_2 = v1_3;
            v5_3 = xCOM(:,i) - P.l_1*e3(:,i);
            vertices5_x(:,j) = [v5_1(1); v5_2(1); v5_3(1)];
            vertices5_y(:,j) = [v5_1(2); v5_2(2); v5_3(2)];
            vertices5_z(:,j) = [v5_1(3); v5_2(3); v5_3(3)];
            
            % Vertices for the pin top patch:
            
            v6_1 = v2_1;
            v6_2 = v2_4;
            v6_3 = xCOM(:,i) + P.l_22*e3(:,i);
            vertices6_x(:,j) = [v6_1(1); v6_2(1); v6_3(1)];
            vertices6_y(:,j) = [v6_1(2); v6_2(2); v6_3(2)];
            vertices6_z(:,j) = [v6_1(3); v6_2(3); v6_3(3)];
            
            % Update the patches:
            
            set(top_surf1{j}, 'xdata', vertices1_x(:,j)*100, 'ydata', vertices1_y(:,j)*100, 'zdata', vertices1_z(:,j)*100);
            set(top_surf2{j}, 'xdata', vertices2_x(:,j)*100, 'ydata', vertices2_y(:,j)*100, 'zdata', vertices2_z(:,j)*100);
            set(top_surf3{j}, 'xdata', vertices3_x(:,j)*100, 'ydata', vertices3_y(:,j)*100, 'zdata', vertices3_z(:,j)*100);
            set(top_surf4{j}, 'xdata', vertices4_x(:,j)*100, 'ydata', vertices4_y(:,j)*100, 'zdata', vertices4_z(:,j)*100);
            set(top_surf5{j}, 'xdata', vertices5_x(:,j)*100, 'ydata', vertices5_y(:,j)*100, 'zdata', vertices5_z(:,j)*100);
            set(top_surf6{j}, 'xdata', vertices6_x(:,j)*100, 'ydata', vertices6_y(:,j)*100, 'zdata', vertices6_z(:,j)*100);
        
        end
        
        % (2) Corotational basis:
        
        set(corot_basis1, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e1(1,i), 'vdata', e1(2,i), 'wdata', e1(3,i), 'LineWidth', 2, 'Color', 'r');
        set(corot_basis2, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e2(1,i), 'vdata', e2(2,i), 'wdata', e2(3,i), 'LineWidth', 2, 'Color', 'g');
        set(corot_basis3, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', e3(1,i), 'vdata', e3(2,i), 'wdata', e3(3,i), 'LineWidth', 2, 'Color', 'b');
        set(corot_basis_trace1, 'xdata', e1(1,1:i), 'ydata', e1(2,1:i), 'zdata', e1(3,1:i));
        set(corot_basis_trace2, 'xdata', e2(1,1:i), 'ydata', e2(2,1:i), 'zdata', e2(3,1:i));
        set(corot_basis_trace3, 'xdata', e3(1,1:i), 'ydata', e3(2,1:i), 'zdata', e3(3,1:i));
        
        % (3) Contact point:
        
        set(top_contact, 'xdata', xP(1,1:i)*100, 'ydata', xP(2,1:i)*100, 'zdata', -xP(3,1:i)*100);
        
        % (4) Energy:
        
        set(top_energy, 'xdata', T(1:i), 'ydata', E(1:i)*1000);
        
        drawnow;
             
    end
    
end

%==========================================================================

% Specify the equations of motion:

function Ydot = fun(t, Y, P);

Ydot = zeros(10,1);

psi = Y(1);
theta = Y(2);
phi = Y(3);
x1 = Y(4);
x2 = Y(5);
psidot = Y(6);
thetadot = Y(7);
phidot = Y(8);
x1dot = Y(9);
x2dot = Y(10);

% Define the generalized mass matrix M(t, u = Y(1:5)):

M = [P.lambdaa*cos(theta)^2 + P.lambdat*sin(theta)^2, 0, P.lambdaa*cos(theta), 0, 0;
     0, P.lambdat + P.m*P.l_3^2*sin(theta)^2, 0, 0, 0;
     P.lambdaa*cos(theta), 0, P.lambdaa, 0, 0;
     0, 0, 0, P.m, 0;
     0, 0, 0, 0, P.m];

% Define the column vector alpha(t, u = Y(1:5), udot = Y(6:10)):

alpha = [2*(P.lambdat-P.lambdaa)*psidot*thetadot*cos(theta)*sin(theta) - P.lambdaa*phidot*thetadot*sin(theta);  
         (P.lambdaa-P.lambdat)*psidot^2*cos(theta)*sin(theta) + P.lambdaa*phidot*psidot*sin(theta) + P.m*P.l_3^2*thetadot^2*cos(theta)*sin(theta); 
         -P.lambdaa*psidot*thetadot*sin(theta);  
         0;
         0];

% Define the right-hand side of the second-order equations of motion,
% f(t, u = Y(1:5)):

f = [0; 
     P.m*P.g*P.l_3*sin(theta); 
     0; 
     0; 
     0];

% Establish the equations of motion in first-order form:

Ydot = [psidot;
        thetadot;
        phidot;
        x1dot;
        x2dot;
        M\(f - alpha)];