function HW3_2018

close all

%% Problem 2: Trajectory of a particle in a trap
alpha = 10^(-39); 
U1 = 8000;   % Static voltage (V)
U30 = 4000;  % Oscillating voltage (V)
omega = 900*2*pi; % Frequency (rad/s)
z0 = 1.e-03;    % Trap dimension (m)
m = 1.15e-26;   % Atom mass (kg)
c1 = 3*alpha*U30/(4*z0^4*m);
c2 = 3*U30/(2*z0^2);

initial_w = [0; 0.1e-03; 0.1e-03; 0.01; 0; 0.];
[times,sols] = ode45(@(t,w) trap(t,w,c1,c2,U1,omega),[0,0.1],initial_w);

 figure
 plot3(sols(:,1),sols(:,2),sols(:,3));
 grid('on');
 ylabel('y (mm)');
 xlabel('x (mm)');
 zlabel('z (mm)');
 title('Trajectory of a particle in an electrostatic trap');


initial_w = [0; 0.1e-03; 0.1e-03; 0.1; 0; 0.];
[times,sols] = ode45(@(t,w) trap(t,w,c1,c2,U1,omega),[0,0.1],initial_w);

figure
 plot3(sols(:,1),sols(:,2),sols(:,3));
 grid('on');
 ylabel('y (mm)');
 xlabel('x (mm)');
 zlabel('z (mm)');
 title('Escape of a particle with high velocity');

 
 omega = 550*2*pi; % Frequency (rad/s)
 initial_w = [0; 0.1e-03; 0.1e-03; 0.01; 0; 0.];
[times,sols] = ode45(@(t,w) trap(t,w,c1,c2,U1,omega),[0,0.1],initial_w);

 figure
 plot3(sols(:,1),sols(:,2),sols(:,3));
 grid('on');
 ylabel('y (mm)');
 xlabel('x (mm)');
 zlabel('z (mm)');
 title('Trap with 550 Hz oscillating voltage');
 
%%
L = 2;    % Beam length, m
g = 9.81; % Gravitational acceleration
alpha = 5*g/L; % Beam angular acceleration, rad/s^2
d = 1.1;    % Sling length, m
t_end = sqrt(3*pi/alpha);  % Time at end of simulation
initial_w = [0;0];    % Initial conditions

[times,sols] = ode45(@(t,w) trebuchet(t,w,L,d,alpha,g),[0:t_end/100:t_end],initial_w);
figure
plot(times,sols(:,1),'Color',[1,0,0],'LineWidth',1);
 grid('on');
 ylabel('Angle \theta (radians)');
 xlabel('Time (sec)');
 title('Rotation of trebuchet sling');

%animate_trebuchet(times,sols,alpha,L,d);



for i = 1:20

    d = 0.5 + (i-1)*1.25/19;   
    options = odeset('Events',@(t,w) trebuchet_event(t,w,L,d,alpha) ); 
    [times,sols] = ode45(@(t,w) trebuchet(t,w,L,d,alpha,g),[0:t_end/100:t_end],initial_w,options);

    t = times(end);
    theta = sols(end,1);
    omega = sols(end,2);
    vx = d*cos(theta)*omega - L*alpha*t*sin(-pi/3+alpha*t^2/2);
    vy = d*sin(theta)*omega + L*alpha*t*cos(-pi/3+alpha*t^2/2);
    vmag(i) = sqrt(vx^2 + vy^2);
    dval(i) = d;


end

figure
plot(dval,vmag,'Color',[1,0,0],'LineWidth',1);
 grid('on');
 ylabel('Launch velocity (m/s)');
 xlabel('Sling length d (m)');
 title('Influence of sling length on launch velocity');

% Solve again with the optimal design (not required) 
 [vmax,i] = max(vmag);
  d = dval(i);
 [times,sols] = ode45(@(t,w) trebuchet(t,w,L,d,alpha,g),[0:t_end/100:t_end],initial_w,options); 
%animate_trebuchet(times,sols,alpha,L,d);



end

function dwdt = trap(t,w,c1,c2,U1,omega)
   x = w(1); y = w(2); z = w(3);
   v = w(4:6);

   Fx = c1*sin(omega*t)*( c2*sin(omega*t)*(x^2+y^2) - U1)*x;
   Fy = c1*sin(omega*t)*( c2*sin(omega*t)*(x^2+y^2) - U1)*y;
   Fz = 2*c1*sin(omega*t)*( 2*c2*sin(omega*t)*z^2 + 3*U1)*z;
  
   dwdt = [v;Fx;Fy;Fz];
   
end

function dwdt = trebuchet(t,w,L,d,alpha,g)
   theta = w(1); omega = w(2);
   phi = -pi/3 + alpha*t^2/2;
   dphidt = alpha*t;
   domegadt = -g/d*sin(theta) ...
            + (L/d)*dphidt^2*cos(theta-phi) ...
            - (L/d)*alpha*sin(theta-phi);
   dthetadt = omega;
   
   dwdt = [dthetadt;domegadt];
   
end

function [ev,stop,dir] = trebuchet_event(t,w,L,d,alpha)
  theta = w(1); omega = w(2);
  
vx = d*cos(theta)*omega - L*alpha*t*sin(-pi/3+alpha*t^2/2);
vy = d*sin(theta)*omega + L*alpha*t*cos(-pi/3+alpha*t^2/2);

ev = vy+vx;
dir = -1;
stop = 1;

end


function animate_trebuchet(times,sols,alpha,L,d)
figure('Color',[1 1 1])
x = [];
y = [];
for i = 1:length(times)
   t = times(i);
   r = [L*cos(-pi/3+alpha*t^2/2),L*sin(-pi/3+alpha*t^2/2)];
   xbar = [-r(1)/10,r(1)];
   ybar = [-r(2)/10,r(2)];
   clf
   plot(xbar,ybar,'LineWidth',4,'Color',[0,0,0]);
   xlim([-1.5*L 2.5*L])
   ylim([-2.*L 2.*L])
   axis off
   axis square
   hold on
   xsling = [r(1),r(1) + d*sin(sols(i,1))];
   ysling = [r(2),r(2) - d*cos(sols(i,1))];
   plot(xsling,ysling,'LineWidth',2,'Color',[0,1,0])
   plot(xsling(2),ysling(2),'ro','MarkerSize',11,'MarkerFaceColor',[1,0,0])
   x = [x,xsling(2)];
   y = [y,ysling(2)];
   plot(x,y,'Color',[0.5,0.5,0],'LineStyle','--');
   pause(0.05)  
end

end