% 16.07 Lab 3 solution code
% Chris Sequeira (csequeir@mit.edu), Fall 2004
% Last updated November 9th, 2004

% Coordinate systems: 
% Inertial:
%   +x: to the computer screen right
%   +y: to the computer screen top
% Body:
%   +t: to the aircraft nose parallel to the flight direction
%   +n: to the aircraft top perpendicular to the flight direction
% Aerodynamic: angle of attack = angle of incidence
%   +Lift:   to the aircraft top perpendicular to the flight direction
%   +Drag:   to the aircraft tail parallel to the flight direction
%   +Thrust: to the aircraft nose parallel to the flight direction
%
% State vector and state derivative vector:
% X' and X are vectors in --column format--!!!!!!
% X  = [ x
%        y
%        x_dot
%        y_dot     ]
% X' = [ x_dot
%        y_dot
%        x_dot_dot
%        y_dot_dot ]

% integrator options
options = odeset('RelTol', 1e-4, 'AbsTol', 1e-4,'MaxStep', .01);

% set initial state vector
X0 = [ 0.0                          % horizontal position [m]
       10000.0                      % vertical position [m]
       185.0                        % horizontal speed [m/s]
       0.0     ];                   % vertical speed [m/s]

% how long do we run for?
tspan = [0, 100];                   % starting and ending time [s]


% simulate the circular loop
[time, X] = ode45(@getDerivs, tspan, X0, options);


% get r_B, v_B, a_B in column format
r_B = zeros(3, length(time)); v_B = zeros(3, length(time)); a_B = zeros(3, length(time));
r_B(1, :) = X(:, 1); r_B(2, :) = X(:, 2);
v_B(1, :) = derivative(time, X(:, 1)); v_B(2, :) = derivative(time, X(:, 2));
a_B(1, :) = derivative(time, X(:, 3)); a_B(2, :) = derivative(time, X(:, 4));

% get r_A/B in column format
r_AB = zeros(3, length(time));
for trans = 1:length(time)
    r_AB(:, trans) = -r_B(:, trans) + [1000; 0; 0];
end

% get aircraft tangential vector t in column format
t = zeros(3, length(time));
for i = 1:length(time)
    t(:, i) = v_B(:,i)/norm(v_B(:, i));
end

% get aircraft normal vector n in column format
n = zeros(3, length(time));
for i = 1:length(time)
    n(:, i) = cross([0;0;1], t(:, i));
end

% get (r_A/B)_tn in column format by coord transformation
r_AB_tn = zeros(3, length(r_AB));
for conv = 1:length(time)
    % direction cosine matrix
    T = [ t(1, conv),  t(2, conv),  0;
          n(1, conv),  n(2, conv),  0;
          0           ,  0           ,  1  ];
      
    % transformation
    r_AB_tn(:, conv) = T*r_AB(:, conv);
end

% get (v_A/B)_tn and (a_A/B)_tn in column format by differentiation
v_AB_tn = zeros(3, length(time)); a_AB_tn = zeros(3, length(time));
v_AB_tn(1, :) = derivative(time, r_AB_tn(1, :));
v_AB_tn(2, :) = derivative(time, r_AB_tn(2, :));
a_AB_tn(1, :) = derivative(time, v_AB_tn(1, :)); 
a_AB_tn(2, :) = derivative(time, v_AB_tn(2, :));

% get (v_A/B)_tn and (a_A/B)_tn in inertial coordinates
v_AB_tn_i = zeros(3, length(time));
a_AB_tn_i = zeros(3, length(time));
for conv = 1:length(time)
    % direction cosine matrix... in 3D
    T = transpose([ t(1, conv),  t(2, conv),  0;
                    n(1, conv),  n(2, conv),  0;
                    0           ,  0           ,  1  ]);

    % transformation
    v_AB_tn_i(:, conv) = T*v_AB_tn(:, conv);
    a_AB_tn_i(:, conv) = T*a_AB_tn(:, conv);
end

% plot (v_A/B)_tn,i and (a_A/B)_tn,i
figure;
plot(v_AB_tn_i(1, :), v_AB_tn_i(2, :),'x');
title('(v_{A/B})_{tn,i}, y vs. x');
xlabel('x');
ylabel('y');
grid on;
figure;
plot(a_AB_tn_i(1, :), a_AB_tn_i(2, :),'x');
title('(a_{A/B})_{tn,i}, y vs. x');
xlabel('x');
ylabel('y');
grid on;

% get aircraft angular velocity and accel as column vectors
omega_B = zeros(3, length(time)); omega_dot_B = zeros(3, length(time));
for count = 1:length(time)
    omega_B(3, count) = dot(a_B(:, count), n(:, count))/norm(v_B(:, count));
end
omega_dot_B(3, :) = derivative(time, omega_B(3, :));

% compute omega_B x (r_A/B) and plot the y vs. x coords of it
cross_or = zeros(3, length(time));
cross_or = cross(omega_B, r_AB);
figure;
plot(cross_or(1, :), cross_or(2, :),'x');
title('\Omega_B x r, y vs. x');
xlabel('x');
ylabel('y');
grid on;

% plot v_B + v_AB_tn_i + cross(omega_B, r_AB), y vs. x
v_sum = zeros(3, length(time));
v_sum = v_B + v_AB_tn_i + cross_or;
figure;
plot(v_sum(1, :), v_sum(2, :),'o');
title('v_{B/A} + (v_{A/B})_{tn,i} + \Omega_B x r_{A/B}, y vs. x');
xlabel('x');
ylabel('y');
grid on;

% get centrifugal term in inertial frame and plot y vs. x
centrif = zeros(3, length(time));
centrif = cross(omega_B, cross(omega_B, r_AB));
figure;
subplot(3,1,1);
plot(centrif(1, :), centrif(2, :),'x');
title('\Omega_B x (\Omega_B x r_{A/B}), y vs. x');
xlabel('x');
ylabel('y');
grid on;
subplot(3,1,2);
plot(time, centrif(1, :),'x');
title('\Omega_B x (\Omega_B x r_{A/B}), x vs. time');
xlabel('time');
ylabel('x');
grid on;
subplot(3,1,3);
plot(time, centrif(2, :),'x');
title('\Omega_B x (\Omega_B x r_{A/B}), y vs. time');
xlabel('time');
ylabel('y');
grid on;

% get Coriolis accel in inertial frame and plot y vs. x
coriolis = zeros(3, length(time));
coriolis = cross(2*omega_B, v_AB_tn_i);
figure;
subplot(3,1,1);
plot(coriolis(1, :), coriolis(2, :),'x');
title('Coriolis acceleration, y vs. x');
xlabel('x');
ylabel('y');
grid on;
subplot(3,1,2);
plot(time, coriolis(1, :),'x');
title('Coriolis acceleration, x vs. time');
xlabel('time');
ylabel('x');
grid on;
subplot(3,1,3);
plot(time, coriolis(2, :),'x');
title('Coriolis acceleration, y vs. time');
xlabel('time');
ylabel('y');
grid on;

% angular accel term
ang_accel = zeros(3, length(time));
ang_accel = cross(omega_dot_B, r_AB);
figure;
%plot(ang_accel(1, :), ang_accel(2, :),'x');
%title('d\Omega_B/dt x r_{A/B}, y vs. x');
%xlabel('x');
%ylabel('y');
%grid on;
subplot(2,1,1);
plot(time, ang_accel(1, :), 'x');
title('Angular Acceleration, x vs. time');
xlabel('time');
ylabel('x');
grid on;
subplot(2,1,2);
plot(time, ang_accel(2, :), 'x');
title('Angular Acceleration, y vs. time');
xlabel('time');
ylabel('y');
grid on;

% acceleration comparison
accel_right = zeros(3, length(time));
accel_right = a_B + a_AB_tn_i + ang_accel + coriolis + centrif;
figure;
%plot(accel_right(1, :), accel_right(2, :),'x');
%title('Sum of accel components, y vs. x');
%xlabel('x');
%ylabel('y');
%grid on;
subplot(2,1,1);
plot(time, accel_right(1, :),'x');
title('Sum of accel components, x vs. time');
xlabel('time');
ylabel('x');
grid on;
subplot(2,1,2);
plot(time, accel_right(2, :),'x');
title('Sum of accel components, y vs. time');
xlabel('time');
ylabel('y');
grid on;
