%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Mar 30, 2016

clear all
close all
clc

%  (1)  Load and process the crash test data:

load honda-cr-v-driver-data

%  The angular velocity data are reported in deg/s, so convert the data 
%  into rad/s.  Also, it appears that the sign of the data is opposite of
%  what it should be, based on the SAE standard for dummy coordinate 
%  systems, so fix this problem:

omega1 = -omega1*(pi/180);          %  rad/s
omega2 = -omega2*(pi/180);          %  rad/s
omega3 = -omega3*(pi/180);          %  rad/s

%  The acceleration data are reported in g's, so convert the data into
%  m/s^2:

g = 9.81;           %  m/s^2

a1 = a1*g;          %  m/s^2
a2 = a2*g;          %  m/s^2
a3 = a3*g;          %  m/s^2

%  (2)  Use ode45 to numerically integrate the coupled differential 
%       equations governing the head's orientation.

%  Simulation parameters:

tol = 1e-6;

psi0 = 0;           %  rad
theta0 = 0;         %  rad
phi0 = 0;           %  rad

Y0 = [psi0, theta0, phi0]';

%  Numerically integrate the state equations in mass-matrix form,
%  M(t,Y)*Y'(t) = F(t,Y):

options = odeset('abstol', tol, 'reltol', tol, 'mass', @M);

[t, Y] = ode45(@F, tdata, Y0, options, omega1, omega2, omega3, tdata);

%  Extract the results:

psi = Y(:,1);           %  rad
theta = Y(:,2);         %  rad
phi = Y(:,3);           %  rad

%  (3)  Use the trapezoidal rule to numerically integrate the acceleration
%       data to determine the displacement of the head.

%  Express the acceleration data in terms of the space-fixed frame by using
%  the computed Euler angles to determine the evolution of the corotational 
%  basis {e1,e2,e3}:

for k = 1:length(psi)
    R1 = [cos(psi(k)), sin(psi(k)), 0;              %  3-2-1 set of               
          -sin(psi(k)), cos(psi(k)), 0;             %  Euler angles
          0, 0, 1];
    R2 = [cos(theta(k)), 0, -sin(theta(k));       
          0, 1, 0;
          sin(theta(k)), 0, cos(theta(k))];    
    R3 = [1, 0, 0;                               
          0, cos(phi(k)), sin(phi(k));       
          0, -sin(phi(k)), cos(phi(k))];
    e1(:,k) = ([1, 0, 0]*(R3*R2*R1))';                      %  e1
    e2(:,k) = ([0, 1, 0]*(R3*R2*R1))';                      %  e2        
    e3(:,k) = ([0, 0, 1]*(R3*R2*R1))';                      %  e3
    accel(:,k) = a1(k)*e1(:,k) + a2(k)*e2(:,k) + ...        %  m/s^2
                 a3(k)*e3(:,k);
end

%  Extract the space-fixed components of acceleration:

x1ddot = accel(1,:)';           %  m/s^2
x2ddot = accel(2,:)';           %  m/s^2
x3ddot = accel(3,:)';           %  m/s^2

%  Numerically integrate the acceleration to obtain the head's velocity:

x1dot0 = 35*(0.44704);          %  m/s
x2dot0 = 0;                     %  m/s
x3dot0 = 0;                     %  m/s

x1dot = cumtrapz(tdata, x1ddot) + x1dot0;           %  m/s
x2dot = cumtrapz(tdata, x2ddot) + x2dot0;           %  m/s
x3dot = cumtrapz(tdata, x3ddot) + x3dot0;           %  m/s

%  Numerically integrate the velocity to obtain the head's displacement:

x10 = 0;        %  m
x20 = 0;        %  m
x30 = 0;        %  m

x1 = cumtrapz(tdata, x1dot) + x10;          %  m
x2 = cumtrapz(tdata, x2dot) + x20;          %  m
x3 = cumtrapz(tdata, x3dot) + x30;          %  m

%  (4)  Plot and animate the motion of the head.

%  Plot the Euler angles over time:

figure
set(gcf, 'color', 'w')
subplot(311)
plot(t*1000, psi*(180/pi), '-b', 'linewidth', 2)
ylim(90*[-1, 1])
xlabel('Time (ms)')
ylabel('\it\psi\rm (deg)')
subplot(312)
plot(t*1000, theta*(180/pi), '-r', 'linewidth', 2)
ylim(90*[-1, 1])
xlabel('Time (ms)')
ylabel('\it\theta\rm (deg)')
subplot(313)
plot(t*1000, phi*(180/pi), '-k', 'linewidth', 2)
ylim(90*[-1, 1])
xlabel('Time (ms)')
ylabel('\it\phi\rm (deg)')

%  Plot the displacement of the head's mass center over time:

figure
set(gcf, 'color', 'w')
subplot(311)
plot(t*1000, x1, '-b', 'linewidth', 2)
ylim([-0.5, 2])
xlabel('Time (ms)')
ylabel('\itx\rm_{1} (m)')
subplot(312)
plot(t*1000, x2, '-r', 'linewidth', 2)
ylim([-0.5, 2])
xlabel('Time (ms)')
ylabel('\itx\rm_{2} (m)')
subplot(313)
plot(t*1000, x3, '-k', 'linewidth', 2)
ylim([-0.5, 2])
xlabel('Time (ms)')
ylabel('\itx\rm_{3} (m)')

%  Animate the head's motion:

animate_head_motion(x1, x2, x3, e1, e2, e3);