%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Feb 10, 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 x1(t) x2(t) x3(t) Dx1(t) Dx2(t) Dx3(t)
syms m g lambdat lambdaa L3 muk
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)  Kinematics of the top's point of contact P with the ground:

%  The top's contact point slides laterally with friction, but it cannot 
%  leave the surface.

%  Lateral components of the contact point's velocity:

vP = simplify([Dx1; Dx2; Dx3] - transpose(R3*R2*R1)*cross(omega, ...
                                                          [0; 0; L3]));

vP1 = [1, 0, 0]*vP;
vP2 = [0, 1, 0]*vP;

%  Integrable constraint:

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

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

x3 = simplify(int(Dx3));

%  Simplify the kinematic equations for future manipulation: 

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

vP1 = subs(vP1, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]);
vP2 = subs(vP2, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]);

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

%  Magnitude of the contact point's velocity:

vP = simplify(sqrt(vP1^2 + vP2^2));

%  (3)  Balance of linear momentum:

%  Solve for the normal (constraint) force, and use it to form expressions 
%  for the lateral sliding friction force components:

N = m*diff(Dx3) + m*g;
F1 = -muk*N*vP1/vP;
F2 = -muk*N*vP2/vP;

%  Specify the second-order ODEs for lateral motion of the top's mass 
%  center:

ODEsTrans = [m*diff(Dx1) == F1;
             m*diff(Dx2) == F2];

%  (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:

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

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

%  Manipulating the first two ODEs for rotational motion ultimately yields 
%  a cleaner form for the state equations when friction is ignored:                       

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

ODEsTrans2 = [diff(x1) == Dx1; diff(x2) == Dx2];

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

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

StateVars = [Dpsi; Dtheta; Dphi; psi; theta; phi; Dx1; Dx2; x1; x2];

%  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, muk);  
F = odeFunction(Fsym, StateVars, m, g, lambdat, lambdaa, L3, muk); 

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

save poisson_top_ODEs.mat Msym Fsym StateVars M F