% Control3_1.m
% Solution to problem 1 of control 3
% JMS, May.2024

% Set parameters
clear
me = 140e-3;       % ball mass (kg)
Ka = 1.3e-3;       % drag force coefficient (kg/m)
KM = 0.2e-3;       % Magnus force coefficient (kg)
g = 9.8;           % gravitational acceleration (m/s^2)
h = 1.5;           % initial height (m)
v0x = 40;          % initial velocity (m/s)
w0 = 1800/60*2*pi; % rotational velocity (rad/s)
dt = 1e-2;         % integration time step (s)
tmax = 100;        % integration time (s)

% Set force
mg = me*g;
ballForce = @(v,w) KM*cross(v,w) ...   % Magnus force
                 - Ka*norm(v)*v ...    % drag force
                 - mg*[0;0;1];         % gravitational force

% Integrate trajectories
r0 = [0;0;h];
v0 = [v0x;0;0];
mystop = @(r,v,t)(r(3)<0);

myforce = @(r,v,t)ballForce(v,[0;0;0]);
[r1,v1,a1] = newtonRK( myforce, me, r0, v0, dt, tmax, mystop );

myforce = @(r,v,t)ballForce(v,[0;0;w0]);
[r2,v2,a2] = newtonRK( myforce, me, r0, v0, dt, tmax, mystop );

myforce = @(r,v,t)ballForce(v,[0;w0;0]);
[r3,v3,a3] = newtonRK( myforce, me, r0, v0, dt, tmax, mystop );

% Plot trajectories
figure(1)
it = find(r3(3,:)==max(r3(3,:)),1);  % highest point of r3
plot3( r1(1,:),r1(2,:),r1(3,:), ...
       r2(1,:),r2(2,:),r2(3,:), ...
       r3(1,:),r3(2,:),r3(3,:), ...
       r3(1,it),r3(2,it),r3(3,it),'o', 'LineWidth',2 )
legend('r_1','r_2','r_3')
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
% axis equal
grid on
set(gca,'FontSize',15)

% Find energy and work for second trajectory
nt = size(r2,2); 
t = [0:dt:dt*(nt-1)];
WM = zeros(1,nt);                     % Magnus force work (since force
                                      % is orthogonal to velocity)
Wa = integral(t,-Ka*vecnorm(v2).^3);  % drag work
Wg = integral(t,-mg*v2(3,:));         % gravitation work
Ec = 0.5*me*vecnorm(v2).^2;           % kinetic energy
Et = Ec-WM-Wa-Wg;                     % "total energy"

% Plot energy and work
figure(2)
plot( t,Ec, t,WM, t,Wa, t,Wg, t,Et, 'LineWidth',2 )
legend('E_c', 'W_M', 'W_a', 'W_g', 'E_{tot}')
xlabel('time (s)')
ylabel('energy (J)')
grid on
set(gca,'FontSize',15)

