function trajectory
% Mass moves on plane with various forces
% See TAM 2030 lectures from 2/21/2013
clear all
%time span
 t0 = 0;    h = .005;  t1 = 80*pi;  
 n = floor((t1-t0)/h);         % number of time steps
 tspan = linspace(t0,t1,n);    % set of all times

%Initial conditions: 2 comps of position and 2 comps of velocity 
 r0 = [3 0]';    v0 = [ 0 .6]'; 

%Parameters
 p.m = 1; p.k = 1; 
 p.L0 = 1;   % rest length of spring
 p.g = 1;    % near earth gravity
 p.GM = 1;   % far from earth gravity
 p.c = 1;    % drag constan

tnew=t0; z0 = [r0; v0]; % the four initial conditions
 
%The ODE solution is all done on the next line.
[tarray zarray] = solveode(tspan, z0, p);

%Unpack the positions for plotting
xarray = zarray(:,1); yarray = zarray(:,2);

figure(1)
plot(xarray,yarray, 'b-' )
xlabel('x'); ylabel('y');
title('Andy''s plot','fontsize', 20)
axis('equal')

end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%THIS IS THE RIGHT HAND SIDE FUNCTION FOR THE ODEs
function  zdot=rhs(t,z,p)
% Each line of code is readable.
r=z(1:2);  v=z(3:4);      % Unpack z into variables with good names

rdot = v;
%SOME FAMOUS PARTICLE DYNAMICS PROBLEMS 
%     (uncomment only one of these)
%Trajectory with viscous drag
vdot = -p.c*v/p.m  - p.g*[0 1]';  % gravity part = [0 -m*g]/m

%Trajectory with quadratic drag
%vdot = -p.c*v*norm(v)/p.m  - p.g*[0 1]';  

%Gravity:  Sattelite around Earth; %r/norm(r) is unit vec. in r dir.
%vdot  = - p.GM* (1/norm(r))^2 * (r/norm(r));  

% Mass held in place by spring anchored at origin
vdot   = -p.k*(norm(r) - p.L0)*  (r/norm(r)); 

zdot = [rdot; vdot];  % put together rdot and vdot
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%SOLVE ODES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [tarray zarray] = solveode(tspan,z0,p)
%Midpoint method of solving any number of 1st order ODEs
znew = z0;              % get things started
ntimes = length(tspan)  % Figure out how many times to evaluate
n_odes=length(z0);      % Figure out how many 1st order ODEs

zarray = zeros(ntimes,n_odes);   % initialize z
zarray(1,:) = znew'; 

%The main loop
for i = 2:ntimes
 t    = tspan(i-1);  % the present 'old' time  
 z    = znew;         % the new old is the old new
 h    = tspan(i) - tspan(i-1);   % size of full time step
 
 %Midpoint method is just the next 4 lines
 zdottemp =  rhs(t,z,p);            % Use equations of motion and kinematics
 znewtemp =  z   + zdottemp*h/2;    % Half step ...
 zdot     =  rhs(t+h/2,znewtemp,p); % ... to get approx. ave. of zdoteva
 znew     =  z   + zdot    *h;      % Take full step using best guess at zdot
 
 zarray(i,:) = znew';  % store the new values as row of array
end
 
tarray=tspan;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


