% orbitsim.m
% Rigid Body Co-Adjoint Orbit Simulator
% C. P. Coleman Tue 2-May-06

% Specify Physical Dimensions of Rigid Body
% Using Meriam (1978), p499, notation for Rectangular Parallelepiped

global a b l m
%a = 3   ;      % Max X dimension 
%b = 6   ;      % Max Y dimension
%l = 10  ;      % Max Z dimension
%a = 3; b = 3; c = 10;  % Prolate
%a = 3; b = 4; c = 10; % Triaxial
%a = 3; b = 5; c = 10;
a = 3; b = 6, c = 10;
%a = 3; b = 8; c = 10;
%a = 3; b = 9.9; c = 10;
%a = 3; b = 10; c = 10; % Oblate

m = 10  ;      % Mass

% Define and Calculate Principal Moments of Inertia Meriam (1978), p 499.
global I1 I2 I3 
I1 = (1/12)*m*(b^2 + l^2) ;
I2 = (1/12)*m*(a^2 + l^2) ;
I3 = (1/12)*m*(a^2 + b^2) ;

% Calculate constants defined in Marsden and Ratiu (1993), p404.
A1 = (I2-I3)/(I2*I3) ;
A2 = (I3-I1)/(I3*I1) ;
A3 = (I1-I2)/(I1*I2) ;

% Close any current figures
  close ; close

% Orbit Dynamics Figure
tframe = figure('Color','black','Name','Angular Momentum Dynamics', ...
               'NumberTitle','off','Position',[25,325,600,300]) ;

% Calculate Body Motion Axis Limits
limit = max([a/2 b/2 l/2]);
tbody = axes('View',[115 30],'Box','off','Color','none', ...
             'DrawMode','normal','NextPlot','add',         ...
             'Position',[.06 .1 .4 .9],                     ...
             'XColor','w','XGrid','on',                    ...
             'Xlim',[-1.3*limit 1.3*limit],'XLimMode','manual',          ...
             'YColor','w','YGrid','on',                    ...
             'Ylim',[-1.3*limit 1.3*limit],'YLimMode','manual',          ...
             'ZColor','w','ZGrid','on',                    ...
             'Zlim',[-1.3*limit 1.3*limit],'ZLimMode','manual') ;
btitle = text(-1.2*limit,-1.2*limit,.9*limit,'Rigid Body Configuration',...
         'HorizontalAlignment','center',      ...
         'VerticalAlignment'  ,'bottom',      ...
         'Color','y' );
% -1.2*limit
btitle2 = text(1.2*limit,0,-1.2*limit,'Generic Rigid Body',...
         'HorizontalAlignment','center',      ...
         'VerticalAlignment'  ,'bottom',      ...
         'Color','y' );
xtext = text(1.3*limit,0,0,'Major Axis');
ytext = text(0,1.3*limit,0,'Inter Axis');
ztext = text(0,0,1.3*limit,'Minor Axis');
xa = line('xdata',[0 1.1*limit],'ydata',[0 0],'zdata',[0 0],'color','w');
ya = line('xdata',[0 0],'ydata',[0 1.1*limit],'zdata',[0 0],'color','w');
za = line('xdata',[0 0],'ydata',[0 0],'zdata',[0 1.1*limit],'color','w');

tmomen = axes('View',[115 30],'Box','off','Color','none', ...
             'DrawMode','normal','NextPlot','add',         ...
             'Position',[.55 .1 .4 .9],                    ...
             'XColor','w','XGrid','on',                    ...
             'Xlim',[-1.3 1.3],'XLimMode','manual',        ...
             'YColor','w','YGrid','on',                    ...
             'Ylim',[-1.3 1.3],'YLimMode','manual',        ...
             'ZColor','w','ZGrid','on',                    ...
             'Zlim',[-1.3 1.3],'ZLimMode','manual',        ...
             'ZTick',[-1.3 0 1.3],'ZTickMode','manual') ;
otitle = text(-1.3,-1.3,.9,'Body Angular Momentum Sphere',...
         'HorizontalAlignment','center',           ...
         'VerticalAlignment'  ,'bottom',           ...
         'Color','y' );
matext = text(3, 0  , 0  ,'Major Axis');
intext = text( 0  ,1.5, 0  ,'Inter Axis');
mitext = text( 0  , 0  ,1.5,'Minor Axis');
major = line('xdata',[1 2.5],'ydata',[0 0],'zdata',[0 0],'color','w');
inter = line('xdata',[0 0],'ydata',[1 1.25],'zdata',[0 0],'color','w');
minor = line('xdata',[0 0],'ydata',[0 0],'zdata',[1 1.25],'color','w');

% Test Movie
% momentummovie  = zeros(length(getframe(tframe)),size(S,1));

% Define Rigid Body Vertices
x1 = [ a/2 -b/2  l/2 ]' ;
x2 = [-a/2 -b/2  l/2 ]' ;
x3 = [-a/2  b/2  l/2 ]' ;
x4 = [ a/2  b/2  l/2 ]' ;
x5 = [ a/2 -b/2 -l/2 ]' ;
x6 = [-a/2 -b/2 -l/2 ]' ;
x7 = [-a/2  b/2 -l/2 ]' ;
x8 = [ a/2  b/2 -l/2 ]' ;

% Draw Rigid Body
set(tframe,'CurrentAxes',tbody) 
 
X = [ x1(1) x4(1) ; x5(1) x8(1) ];
Y = [ x1(2) x4(2) ; x5(2) x8(2) ];
Z = [ x1(3) x4(3) ; x5(3) x8(3) ];
face1 = surface(X,Y,Z,'EdgeColor','k','FaceColor','magenta');

X = [ x2(1) x3(1) ; x6(1) x7(1) ];
Y = [ x2(2) x3(2) ; x6(2) x7(2) ];
Z = [ x2(3) x3(3) ; x6(3) x7(3) ];
face2 = surface(X,Y,Z,'EdgeColor','k','FaceColor','cyan');

X = [ x1(1) x2(1) ; x5(1) x6(1) ];
Y = [ x1(2) x2(2) ; x5(2) x6(2) ];
Z = [ x1(3) x2(3) ; x5(3) x6(3) ];
face3 = surface(X,Y,Z,'EdgeColor','k','FaceColor','yellow');

X = [ x4(1) x3(1) ; x8(1) x7(1) ];
Y = [ x4(2) x3(2) ; x8(2) x7(2) ];
Z = [ x4(3) x3(3) ; x8(3) x7(3) ];
face4 = surface(X,Y,Z,'EdgeColor','k','FaceColor','green');

X = [ x5(1) x6(1) ; x8(1) x7(1) ];
Y = [ x5(2) x6(2) ; x8(2) x7(2) ];
Z = [ x5(3) x6(3) ; x8(3) x7(3) ];
face5 = surface(X,Y,Z,'EdgeColor','k','FaceColor','red');

X = [ x1(1) x2(1) ; x4(1) x3(1) ];
Y = [ x1(2) x2(2) ; x4(2) x3(2) ];
Z = [ x1(3) x2(3) ; x4(3) x3(3) ];
face6 = surface(X,Y,Z,'EdgeColor','k','FaceColor','blue');

drawnow

% Draw Body Angular Momentum Trajectories

% Set Current Axis to Angular Momentum Axis
set(tframe,'CurrentAxes',tmomen) 

% Rigid Body Dynamics Simulator

% Initialize theta and phi coordinates for coadjoint orbit plots
k=4;
n=2^k-1;
theta = pi*[-n:2:n]/n;
phi = (pi/2)*[-n:2:n]'/n;

% Plot different curves
for i = 1:n,
        % Integrate the Euler RBB equations
        t0 = 0; tf = 1000 ;         % Initial and final time
        Pi0 = [ cos(phi(i)) ...   % Initial condition
                0 ...
                sin(phi(i)) ];
        [t,x] = ode45('pidot',[t0,tf],Pi0);
        % Check to see if magnitude is begin preserved!
%       for j=1:size(x,1),
%         magPi(j) = [x(j,1) x(j,2) x(j,3)]*[x(j,1) x(j,2) x(j,3)]';
%	end
        % Plot trajectory in 3D
        plot3(x(:,1),x(:,2),x(:,3));
	% Show the trajectory now
        drawnow
	% Hold on 
        hold on
end

for i = 1:n,
        % Integrate the Euler RBB equations
        t0 = 0; tf = 1000 ;         % Initial and final time
        Pi0 = [ cos(phi(i) + pi) ...   % Initial condition
                0 ...
                sin(phi(i) + pi) ];
        [t,x] = ode45('pidot',[t0,tf],Pi0);
        % Plot trajectory in 3D
        plot3(x(:,1),x(:,2),x(:,3));
	% Show the trajectory now
        drawnow
	% Hold on 
        hold on
end

% Define time vector
t = [-1000:.1:1000 ];

% Calculate components of heteroclinic orbits using equations
% given in Marsden and Ratiu (1993), p 408, Eq (15.8.10)
% Plot the four heteroclinic orbits.

Pi1 = sqrt(A1/(-A2))*sech(-sqrt(A1*A3)*t);
Pi2 =                tanh(-sqrt(A1*A3)*t);
Pi3 = sqrt(A3/(-A2))*sech(-sqrt(A1*A3)*t);

plot3(Pi1,Pi2,Pi3,'m')

Pi1 = sqrt(A1/(-A2))*sech(-sqrt(A1*A3)*t);
Pi2 = -              tanh(-sqrt(A1*A3)*t);
Pi3 = sqrt(A3/(-A2))*sech(-sqrt(A1*A3)*t);

plot3(Pi1,Pi2,Pi3,'m')

Pi1 = -sqrt(A1/(-A2))*sech(-sqrt(A1*A3)*t);
Pi2 =                 tanh(-sqrt(A1*A3)*t);
Pi3 =  sqrt(A3/(-A2))*sech(-sqrt(A1*A3)*t);

plot3(Pi1,Pi2,Pi3,'m')

Pi1 =  sqrt(A1/(-A2))*sech(-sqrt(A1*A3)*t);
Pi2 =                 tanh(-sqrt(A1*A3)*t);
Pi3 = -sqrt(A3/(-A2))*sech(-sqrt(A1*A3)*t);

plot3(Pi1,Pi2,Pi3,'m')


