%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Feb 06, 2016

clear all
close all
clc

%  Specify symbolic parameters and symbolic variables that are functions of
%  time:

syms psi(t) theta(t) phi(t) Dpsi(t) Dtheta(t) Dphi(t)
syms m g lambdat lambdaa L3
assume((m > 0) & (g > 0) & (lambdat > 0) & (lambdaa > 0) & (L3 > 0))

%  (1)  Angular velocity kinematics:

%  Relate the space-fixed basis {E1,E2,E3} to the corotational basis
%  {e1,e2,e3} using a 3-1-3 set of Euler angles:

R1 = [cos(psi), sin(psi), 0;        
      -sin(psi), cos(psi), 0;
      0, 0, 1]; 
  
R2 = [1, 0, 0;                    	
      0, cos(theta), sin(theta);
      0, -sin(theta), cos(theta)];

R3 = [cos(phi), sin(phi), 0;        
      -sin(phi), cos(phi), 0;
      0, 0, 1];  

%  Express the angular velocity vector in terms of {e1,e2,e3}:  
  
omega = simplify((R3*R2*R1)*[0; 0; diff(psi)] + ...
                 (R3*R2)*[diff(theta); 0; 0] + R3*[0; 0; diff(phi)]);

%  (2)  Kinematic constraints:

%  The top's point of contact P with the ground has zero velocity.

%  Integrable constraints:

vCOM = simplify([0; 0; 0] + transpose(R3*R2*R1)*cross(omega, [0; 0; L3]));

Dx1 = [1, 0, 0]*vCOM;
Dx2 = [0, 1, 0]*vCOM;
Dx3 = [0, 0, 1]*vCOM;

x1 = simplify(int(Dx1));
x2 = simplify(int(Dx2));
x3 = simplify(int(Dx3));

%  Simplify the kinematic equations for future manipulation: 

omega = subs(omega, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, ...
             Dphi]);

Dx1 = subs(Dx1, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]);
Dx2 = subs(Dx2, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]);
Dx3 = subs(Dx3, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]);
         
%  (3)  Balance of linear momentum:

%  Solve for the normal (constraint) force, as well as the lateral
%  (constraint) forces required to keep the top's contact point P fixed:

F1 = m*diff(Dx1);
F2 = m*diff(Dx2);
N = m*diff(Dx3) + m*g;

%  (4)  Balance of angular momentum with respect to the top's mass center:

%  Taking the top to be axisymmetric, evaluate the absolute time derivative 
%  of the angular momentum about the mass center:

H = diag([lambdat, lambdat, lambdaa])*omega;
omegaRF = omega;

DH = diff(H) + cross(omegaRF, H);

%  Sum moments about the mass center:

sumM = cross([0; 0; -L3], (R3*R2*R1)*[F1; F2; N]);

%  Construct the second-order ODEs for rotational motion of the top:

ODEsRot = DH == sumM;

%  (5)  Manipulate the system of ODEs into a form suitable for numerical 
%       integration:

%  Express the second-order ODEs in first-order form:

ODEsRot1 = simplify(subs(ODEsRot, [diff(psi), diff(theta), diff(phi)], ...
                         [Dpsi, Dtheta, Dphi]));

%  Manipulating the first two ODEs ultimately yields a cleaner form for the
%  state equations:                       

ODEsRot1 = [sin(phi), -cos(phi), 0;
            cos(phi), sin(phi), 0;
            0, 0, 1]*ODEsRot1;        
        
%  Relate the state variables in first-order form:

ODEsRot2 = [diff(psi) == Dpsi; diff(theta) == Dtheta; diff(phi) == Dphi];

%  Compile the state equations and arrange the state variables:
     
StateEqns = simplify([ODEsRot1; ODEsRot2]);

StateVars = [Dpsi; Dtheta; Dphi; psi; theta; phi];

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

[Msym, Fsym] = massMatrixForm(StateEqns, StateVars);

Msym = simplify(Msym)
Fsym = simplify(Fsym)

%  Convert M(t,Y) and F(t,Y) to symbolic function handles with the input 
%  parameters specified:

M = odeFunction(Msym, StateVars, m, g, lambdat, lambdaa, L3);  
F = odeFunction(Fsym, StateVars, m, g, lambdat, lambdaa, L3); 

%  Save M(t,Y) and F(t,Y):

save lagrange_top_ODEs.mat Msym Fsym StateVars M F