2D to 3D Bouncing ball conversion

Hello everyone,

I am Jacob and I am a studend trying to learn Matlab. I found online a 2D Bauncing Ball simulation writen by "Matthew Kelly". I have tried for about a month now to make it a 3D simulation and have had no progress. If anyone out there could help me covert it or tell me exactly what to change, I will be most grateful. Thanks in advance.

Here is the code:

%MAIN  --  Bouncing Ball Simulator

%% This script uses ode45 with events to simulate a ball bouncing over an
%% uneven surface.


clc; clear;

%Get all of the user defined parameters
P = Set_Parameters();

%% Run the simulation

    %The results of the simulation are stored here. Each cell contains a
    %singel ode45 output struct
    Trajectory = cell(P.maxBounce,1);
    
    %Initialize the loop:
        IC = P.initCond;            %State at the start of each trajectory
        tNow = 0;                   %time at the start of each trajectory
        tEnd = P.maxTime;           %end simulation at this time
        maxBounce = P.maxBounce;    %end simulation if this many bounces
        dynFunc = @(t,X)Ball_Dynamics(t,X,P);   %Handle for the dynamics function
        TermCond = 'Termination Condition: Max Bounces';  %For display only
        
    %% Loop through each section of the trajectory
    for bounceNum=1:maxBounce
        %% Run the dynamics until bounce or out of time
        Tspan = [tNow,tEnd];         
        sol = ode45(dynFunc,Tspan,IC,P.Options); %events defined in Options
        
        %% Store the state immediately before the collision
        impactState = sol.y(:,end);
        
        %% Solve the collision 
        [IC, ballRolling]= impactMap(impactState,P);   %This becomes the new start state 
        
        tNow = sol.x(end);  %Grab the last time
        Trajectory{bounceNum}=sol;  %Store the output of ode45
        
        %% Check exit condition
        if tEnd <= tNow
            TermCond = 'Termination Condition: Max Time';
            break
        elseif ballRolling
            TermCond = 'Termination Condition: Ball started rolling';
            break
        end
    end
    disp(TermCond);  %Display the termination condition

    %Remove any extra cells in the trajectory
    Trajectory = Trajectory(1:bounceNum);

    
%% Prepare the data for plotting and animation:
% This section interpolates the solution returned by ode45 to a much finer
% grid that is used for plotting and animation.
   
    %It is typically bad practice to allocate memory by just appending new
    %blocks to the arrays. Here it does not cost us too much time, and it
    %makes the code a bit easier to read and understand.
        timeAll = [];   %Stores the interpolated solution for plotting
        stateAll = [];
        timeOde = [];   %Stores the original ode45 solution, stitched
        stateOde = [];
    %Loop through data, interpolating and then stitching:
    for i=1:length(Trajectory)

        %Figure out where we want to interpolate the data
        dt = P.plotTimeStep;
        tStart = Trajectory{i}.x(1);
        tFinal = Trajectory{i}.x(end);
        time = linspace(tStart,tFinal,round((tFinal-tStart)/dt));

        if ~isempty(time)
            %Now use fancy interpolation to create evenly spaced points
            state = deval(Trajectory{i},time);
        else
            disp('Something funny happened - only one grid point in this trajectory section')
            time = Trajectory{i}.x;
            state = Trajectory{i}.y;
        end
        
        %Make sure that the ball didn't pass through the ground and print a
        %warning and suggested fix if it does.
        heightList = EventFunction([],state);
        if min(heightList)<-P.Options.AbsTol
            %Then the ball passed through the ground
            disp('WARNING - the ball passed through the ground!')
            disp('    Suggested fix - reduce the MaxStep size in P.Options by editing Set_Parameters.m')
        end

        %Append new data to the matricies
        timeAll = [timeAll, time]; %#ok<AGROW>
        stateAll = [stateAll, state]; %#ok<AGROW>
        timeOde = [timeOde, Trajectory{i}.x]; %#ok<AGROW>
        stateOde = [stateOde, Trajectory{i}.y]; %#ok<AGROW>
    
    end

%Plot States
%figure(1);
%PlotStates(timeAll, stateAll, P);

%Plot Energy
% figure(2);
% PlotEnergy(timeAll, stateAll, P);

%Animate the solution
%figure(3);
Animate(timeAll, stateAll, stateOde, P);

% 
%figure(2);
%%bb();

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Ball_Dynamics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function dZ = Ball_Dynamics(~,Z,P)

%This function computes the dynamics of the ball as it travels through the
%air. The ball is treated as a point mass, acting under the uniform
%acceleration of gravity and quadratic drag from the air.
%
% INPUTS:
%   (~) = time (not used)
%   Z = a [4xN] matrix of states, where each column is a state vector
%   P = a parameter struct created by Set_Parameters.m
%
% OUTPUTS:
%   dZ = a [4xN] matrix giving the derivative of Z with respect to time
%

%Unpack the physical parameters
    g = P.gravity;  %(m/s^2) gravitational acceleration
    m = P.mass;     %(kg) mass of the ball
    c = P.drag;     %(N*s^2/m^2) quadratic drag coefficient
    
%Get the velocity vector
    % pos = Z(1:2,:);   %Position vector is not used
    vel = Z(3:4,:);   %Velocity vector
    
% Compute gravity and drag accelerations
    speed = sqrt(vel(1,:).^2+vel(2,:).^2);      %(m/s) scalar speed
    Drag = -c*vel.*speed/m;  %(m/s^2) quadratic drag
    Gravity = [0;-g]*ones(size(speed));       %(m/s^2) gravitational acceleration
    
% Return derivative of the state
    acc = Drag + Gravity;
    dZ = [vel;acc];
    
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Event Function %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [value,isterminal,direction] = EventFunction(~,X)

x = X(1,:);
y = X(2,:);
ground = groundHeight(x);

value = y-ground;

%Stop if the hit the ground
isterminal = true;

%Should only be coming to the ground from above
direction = -1;

end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Animation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Animate(timeAll,stateAll,stateOde,P)
%
% This function is used to animate the simulation data
%
% INPUTS:
%   timeAll = [1xN] vector of monotonically increasing time stamps
%   stateAll = [4xN] matrix of state vectors at each time in timeAll
%   stateOde = [4xM] matrix of state vectors returned by ode45
%
% OUTPUTS:
%   A plot and animation displayed on the current figure
%

clf;

%break out the data:
    xPos = stateAll(1,:);
    yPos = stateAll(2,:);
    Pos = [xPos;yPos];

%Get the ground shape
    xGround = linspace(min(xPos),max(xPos),1000);   
    yGround = groundHeight(xGround);

%Get the extents for plotting:
    Extents = [min(xPos),max(xPos),min(yGround),max(yPos)];

    
tic; %Start a timer for synchronizing the animation   

endTime = timeAll(end);
SlowMo = P.slowMotionFactor;
cpuTime=toc/SlowMo;
counter = 1;  %Keeps track of the current plotting index
while (cpuTime<endTime)  
    
    cpuTime = toc/SlowMo;
    while (timeAll(counter)<cpuTime) && counter<length(timeAll)
        %Advance the counter to catch up with real time
        counter=counter+1;
    end
    PosNow = Pos(:,counter);  %Get the current position, based on counter
    
    %Check if the ball has left the screen:
    if PosNow(2) < Extents(3)
       %if the ball height is less than the lower part of the plot
       disp('The ball left the field of view!')
       break;
    end
    
    %clear the figure
    clf; hold on; axis(Extents); axis equal; axis manual;
    
    %Plot the ball
    plot(PosNow(1),PosNow(2),'b.','MarkerSize',P.PlotBallSize); 
    
    %Plot the trace
    plot(Pos(1,1:counter),Pos(2,1:counter),'b--','LineWidth',max(P.CurveLineWidth-1,1)); 
    
    %Plot the ground
    plot(xGround,yGround,'k-','LineWidth',P.CurveLineWidth);

    %Annotations
    ylabel('Vertical Position (m)','FontSize',P.LabelFontSize)
    xlabel('Horizontal Position (m)','FontSize',P.LabelFontSize)
    title('Path taken by the ball','fontsize',P.TitleFontSize)
    set(gca,'fontsize',P.AxisFontSize);    
    
    %Force the plot to appear:
    drawnow; 
    pause(0.001);
    
end

%Plot the points that ode45 returned
%%plot(stateOde(1,:),stateOde(2,:),'.r','MarkerSize',ceil(P.PlotBallSize/3));

ylabel('Vertical Position (m)','FontSize',P.LabelFontSize)
xlabel('Horizontal Position (m)','FontSize',P.LabelFontSize)
title('Path taken by the ball','fontsize',P.TitleFontSize)
hLegend = legend('ball','trajectory','ground','ode45 gridpoints');
set(gca,'fontsize',P.AxisFontSize);
set(hLegend,'fontsize',P.LegendFontSize);

end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Ground Height %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [y, slope] = groundHeight(x)
%
% This function returns the height (y) and slope (Dy) of the ground as a
% function of the horizontal position. An interesting ground profile is
% generated by summing three sine waves. This also makes the derivative
% (slope) easy to compute.
%

%Parameters for each of the three sine ewaves:
    Amp1 = 0.3;
    Freq1 = 1;
    Phase1 = 1.58;

    Amp2 = 0.1;
    Freq2 = 6;
    Phase2 = 1.9;

    Amp3 = 0.2;
    Freq3 = 3.7;
    Phase3 = 1.58;

%Compute the ground height
y = Amp1*sin(Freq1*x+Phase1)+...
    Amp2*sin(Freq2*x+Phase2)+...
    Amp3*sin(Freq3*x+Phase3);

%Compute the ground slope
slope = Amp1*Freq1*cos(Freq1*x+Phase1)+...
        Amp2*Freq2*cos(Freq2*x+Phase2)+...
        Amp3*Freq3*cos(Freq3*x+Phase3);

end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Impact Map %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [stateOut, ballRolling] = impactMap(stateIn,P)
%
% This function computes the impact map for the collision that occurs when
% the ball hits the ground.
%
% It is assumed that the collision occurs instantaneously, and that the
% coefficient of restitution is applied normal to the ground at the
% point of impact. The tangential component of the velocity is
% unaffected by the collision. 
% 
% INPUTS:
%   stateIn = the state immediately before the collision
%   P = parameter struct, defined in Set_Paramters.m
%
% OUTPUTS:
%   stateOut = the state immediately after the collision
%   ballRolling = did the ball start rolling?
%

posOut = stateIn(1:2,:);   %The position is not affected by the collision

velIn = stateIn(3:4,:);   %The velocity before the collision

horizPos = stateIn(1,:);  %The horizontal position of the collision

%Get the slope of the ground at the collision location
[~,groundSlope] = groundHeight(horizPos);   

%Rename the coefficient of restitution
e = P.coeff_restitution; 

%Get the angle between the horizontal axis and the tangent to the ground
theta = atan2(groundSlope,1);

%Get the rotation matrix that rotates the velIn vector to be in the
%coordinate frame that is orientated with the slope of the ground
R = [...
    cos(theta)  sin(theta);
    -sin(theta) cos(theta)];

%Get the collision map for a simple collision, that occurs when the axis
%are aligned nicely (only the vertical component is affected)
E = [1 0;
     0 -e];

% Compute the collision
%   This matrix calculation does the following, in order:
%       1) rotate the input velocity
%       2) compute the impact map in rotated coordinates
%       3) rotate the output velocity back to the original coordinates

%Normal and Tangential velocity components
velNT = E*(R*velIn);

%Rotate the velocity back to the inertial coordinates
velOut = R\velNT;

%Pack up the position and velocity and then return
stateOut = [posOut;velOut];

%Check if the ball should start rolling:
if velNT(1)/velNT(2) > P.rollingThreshold
   %The ball will start rolling, because its velocity is nearly tangent
   %to the collision surface:
   disp('The ball is assumed to start rolling, due to the small normal velocity after the collision.');
    ballRolling = true;
else
    ballRolling = false;
end
    

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Set Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function P = Set_Parameters()

%This function is used to collect all of the user-specified parameters and
%return them as a single struct, P.

%% Physical parameters
    P.gravity = 9.81;    %(m/s^2) gravitational acceleration
    P.mass = 0.3;        %(kg) mass of the ball
    P.drag = 0.02;       %(N*s^2/m^2) quadratic drag coefficient

    %NOTE - if the drag is set to negative, then the speed will
    %exponentially increase throughout the simulation, eventually  reaching
    %infinity. This is not suggested.
    if P.drag < 0
        disp('WARNING - air drag is negative - not advised!');
    end
    
    %It is assumed that the collision occurs instantaneously, and that the
    %coefficient of restitution is applied normal to the ground at the
    %point of impact. The tangential component of the velocity is
    %unaffected by the collision. 
    P.coeff_restitution = 0.95;  
    
    %If the ratio between the tangential and normal components of the ball
    %after the collision is larger than this number, then assume that the
    %ball will start rolling. Note- normal and tangential components are
    %measured with respect to the collision surface.
    P.rollingThreshold = 1e3;

%% Exit conditions
% Run until either the maximum number of bounces or the maximum time 
    P.maxBounce = 25;  %Number of impact events before exit
    P.maxTime = 25;   %Maximum simulation time


%% Initial Conditions:

    Pos_X = 0;    %(m) Initial horizontal position of the ball
    Pos_Y = 1.5;  %(m) Initial vertical position of the ball
    Vel_X = 0;    %(m/s) Initial horizontal speed of the ball
    Vel_Y = -1;   %(m/s) Initial vertical speed of the ball

    P.initCond = [Pos_X; Pos_Y; Vel_X; Vel_Y];


%% ode45 option struct
    P.Options = odeset(  'RelTol',1e-6,...
                        'AbsTol', 1e-6,...
                        'Events', @EventFunction,...
                        'Vectorized',true,...
                        'MaxStep',0.1);    
          %NOTE: We should not need to set MaxStep, but it seems that the
          %event detection in ode45 misses some events when it takes large
          %time steps, if the system dynamics are simple, as is the case
          %here, especially when there is negligable drag.
                
%% Display parameters (plotting + animation)

    %ode45 returns points that are widely spaced and not uniformly spaced.
    %Thus, for better plotting I use the function deval to accurately
    %interpolate the solution to a more useful grid spacing, while
    %preserving the endpoints of each trajectory.
    P.plotTimeStep = 0.005;


    %Sometimes it is useful to run the animation either faster or slower
    %than real time. The simulation time tracks the current cpu time,
    %divided by this number. Thus a value of 2 means play a 1 second
    %simulation over the course of 2 seconds of cpu time.
    P.slowMotionFactor = 1;
    
    %Set the font and line sizes for the figures:
    P.TitleFontSize = 18;
    P.LabelFontSize = 16;
    P.LegendFontSize = 14;
    P.AxisFontSize = 14;
    P.CurveLineWidth = 3;
    P.PlotBallSize = 50;

end