%Trajectory in 2D  -A Ruina, from lecture on March 12, 2021
% Comments added after lecture.

% New things in this file for 2030 students this semester
% * Various forces on a particle, in the rhs file below.
%      e.g.,  springs, gravity, Gravity, linear and quadratic drag.
% * Putting solution to ODE in interpolating file: soln = ode45(...
% * Using  deval(soln, tarray) to  interpolate at desired times
% * 'Events' option in ODE45 and related function myevents  (far below)
% * Using soln.xe to find time of the event

clear all;

%parameters (for all manner of force laws)
p.m = 1; p.g = 10; p.G=1; p.M=1; p.c1 = 1; p.c2 = 1; p.k = 1; p.L0 = 5;

%ICs:
launchangle = 0;  launchspeed  = .2;  % initial angle and speed
x0 = 0;  y0 = 5;      
     r0 = [x0;y0];
vx0 = launchspeed * cos(launchangle); vy0 = launchspeed * sin(launchangle);
     v0 = [vx0;vy0];
z0 = [r0;v0];  % Initial state: position and velocity

eqn   = @(t,z)  myrhs(t,z,p);     % use anonymous functions to pass parameters
tend = 200;  tspan = [0,tend];    % Set this time to be big enough for the event
small = 1e-3;                     % 'small' is the estimated error in ODE45
                                  % i.e. small = 1e-13 is super accurate
                                  
% ODE45 has lots of options, type 'help odeset' to read about them
options = odeset('AbsTol', small, 'RelTol', small, 'Events', @myevents);


%  type  'help ode45' to read about various ways to invoke ode45
soln = ode45(eqn, tspan, z0, options);  %solve ODE

%Set end time to be at event, if there was an event, 
if isnumeric(soln.xe);  % If event is not reached, soln.xe = [] which is not numeric
   tend = soln.xe;  % 'x' at event (for us x is time)
end  %If this 'if' is not evoked, tend keeps its value from above

   %Note: In ode45 accuracy and number of points you want solution are
   %      unrelated.
   tarray = linspace(0, tend, 101);   % Times at which we want solution
   
   % Type 'help deval' to read about interpolating ode45 solutions
   zarray = deval(soln, tarray);      % Differential Equation solution Evaluator

xarray= zarray(1,:);  yarray = zarray(2,:);  % x and y  components

plot(xarray, yarray); axis equal; grid on; title('trajectory'); shg

%%%%%%%%%%%%%%%%%%%%%%
%%
function zdot = myrhs(t,z,p);
r= z(1:2);  v = z(3:4);  % unpack state variables

%Unpack parameters
m = p.m; g = p.g; G = p.G; M = p.M; c1 = p.c1; c2 = p.c2; k = p.k; L0 = p.L0;

rdot = v;  % first two of 4 ODES

%Various forces, pick and choose
Fg      = -m*g*[0;1];                     % Near-earth gravity
FG      = -G*m*M*r/(norm(r))^3;           % Inverse-square gravity
FDL     = -c1*v;                          % Linear drag
FDQ     = -c2*norm(v)*v;                  % Quadratic drag
Fspring = -k*(norm(r) - L0)* r/norm(r);   % Spring

% Use one or more of the forces above or any other forces
Ftot    = FG;

% LMB a.k.a. F = ma
vdot    = Ftot/m;   % second two of 4 ODEs


zdot = [rdot;vdot];
end % end of rhs function
%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%
%%
% Calculates when to stop integrating ODE
function [value,isterminal,direction] = myevents(t,z)
  y          = z(2);
  value      =  y;  % termination is when value = 0
  direction  =  1;  % 1 = increasing, -1 = decreasing, 0 = doesn't matter
  isterminal =  1;  % 1 = stop integration at the event
end  % end of events function
%%%%%%%%%%%%%%%%%%%%%%
