% Simulation of a bouncing lacrosse ball.
% Allen B. Downey

% The ball is modeled as a thin shell around a core, connected
% by a torsional spring.  When the ball is in contact with the
% ground, the shell stops rotating and the core rotates relative
% shell, causing a displacement between the angle of the core
% and the shell, which generates a restoring torque.  This torque
% creates a force that acts beween the ball and the floor,
% accelerating the ball laterally.

function res = lacrosse()
    % run a simulation
    speedup = 2;

    % compute the initial unit vectors
    ihat = [1 0]';
    jhat = [0 1]';
    g = 9.8;          % acceleration of gravity in m/s^2

    m = 0.145;        % mass of lacrosse ball in kg
    radius = 0.00325;       % radius of lacrosse ball in m

    % initial conditions
    IP = [0 10]';
    IV = [3 0]';
    spin = 400;                        % revolutions per minute
    spin = spin * 2 * pi / 60;          % now angular velocity is in rad/s

    % the shell and the core start at the same angle and spin
    init = [IP; 0; 0; IV; spin; spin];

    % run the simulation, then show the results
    tend = 10;
    options = odeset('Events', @events_func);
    options = odeset();
    [T, M] = ode45(@slope_func, [0:0.05:tend], init, options);
    animate_func(T,M);

    % return the final time
    res = T(end);

    % plot the difference between the angles
    theta = M(:,3);
    phi = M(:,4);
    thdot = M(:,7);
    phdot = M(:,8);

    clf
    hold on
    plot(T, theta-phi, 'b')
    plot(T, thdot-phdot, 'r')
    hold off

    function animate_func(T,M)
        % show an animation of the data in M in real time (if possible)
        X = M(:,1);
        Y = M(:,2);
        TH = M(:,3);
        Z = M(:,4);

        for i=1:length(T)-1
            clf;
            hold on;
            unit = 10;
            axis([-unit, unit, -unit, unit]);

            x = X(i);
            y = Y(i);

            % draw the ball
            plot(x, y, 'y.', 'MarkerSize', 50);

            % draw a dot that shows the rotation of the ball
            th = TH(i);
            dot_func(th, radius*100, 'b.')

            z = Z(i);
            dot_func(z, radius*70, 'r.')

            left = [-unit, -unit];
            right = [unit, -unit];
            line_func(left, right);

            hold off;
            drawnow;
            dt = T(i+1) - T(i);
            pause(dt / speedup);
        end

        function dot_func(angle, r, options)
            % draw a dot that shows the rotation of the ball
            x1 = x + r * cos(angle);
            y1 = y + r * sin(angle);
            plot(x1, y1, options, 'MarkerSize', 10);
        end

        function line_func(A, B)
            % draw a blue line from point A to point B
            plot([A(1), B(1)], [A(2), B(2)], 'b-', 'LineWidth', 5)
        end
    end

    function res = slope_func(t, W)
        % this is a slope function invoked by ode45
        n = length(W)/2;
        P = W(1:n);
        V = W(n+1:end);

        res = acceleration_func(t, P, V);
    end

    function res = acceleration_func(t, P, V)
        % unpack P and V
        x = P(1);
        y = P(2);
        th = P(3);
        ph = P(4);
        xdot = V(1);
        ydot = V(2);
        thdot = V(3);
        phdot = V(4);

        k_tor = 0.05;                  % torsional spring constant
        tau = k_tor * (th - ph);      % torque generated by torsion spring

        % add some damping proportional to the relative velocity
        % of the shell and core
        k_damp = 0.001;
        tau = tau + k_damp * (thdot - phdot);

        % convert torque to angular accelerations
        Ishell = 0.0001;
        Icore = 1;
        thddot = -tau / Ishell;
        phddot = tau / Icore;

        % when the ball is in the air, there is no spring force,
        % no torsion and no friction
        Fr = 0 * ihat;
        Ft = 0 * jhat;
        Ff = 0 * ihat;

        y_floor = -10.0;          %  position of the floor in m
        height = y - y_floor;   % distance from center of mass to floor

        % check whether we are in contact with the floor
        if height < radius
           dx = height - radius;
           k_res = 1000.0;            % restoring spring constant
           Fr = -k_res * dx * jhat;
           Ft = tau / radius * ihat;      % force of the wall on the ball

           % if the ball is in contact with the wall, theta stops
           thdot = 0;
           thddot = 0;

           % add a little friction
           k_fric = 0.1;
           Ff = - k_fric * xdot * ihat - k_fric * ydot * jhat;
        end

        % compute the total acceleration
        Ag = -g * jhat;
        F = Fr + Ft + Ff;
        A = Ag + F/m;

        % pack the acceleration vector
        dRdt = [xdot; ydot; thdot; phdot];
        dVdt = [A; thddot; phddot];
        res = [dRdt; dVdt];
    end

    function [value,isterminal,direction] = events_func(t,W)
        % stop the simulation if the rider leaves the ramp
        % in either direction
        y = W(2);
        value = y + 10;
        isterminal = 1;
        direction = -1;
    end

end
