% bodysim2.m
% Rigid Body Animation and Dynamics 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 = 5   ;      % Max Y dimension
%l = 8   ;      % 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 I A1 A2 A3
I1 = (1/12)*m*(b^2 + l^2) ; % Moment of inertia in X direction Ixx
I2 = (1/12)*m*(a^2 + l^2) ; % Moment of inertia in Y direction Iyy
I3 = (1/12)*m*(a^2 + b^2) ; % Moment of inertia in Z direction Izz
I = [I1  0  0 ; ... 
      0 I2  0 ; ...
      0  0 I3 ] ;           % Moment of inertia matrix in principal axes
A1 = (I2-I3)/(I2*I3) ;
A2 = (I3-I1)/(I3*I1) ;
A3 = (I1-I2)/(I1*I2) ;

% Initial configuration of rigid body
r11 = 1 ; r12 = 0 ; r13 = 0 ;
r21 = 0 ; r22 = 1 ; r23 = 0 ;
r31 = 0 ; r32 = 0 ; r33 = 1 ;

% Define rotation matrix "vector"
r0 = [ r11 r12 r13   r21 r22 r23   r31 r32 r33 ] ;


% 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;

% Choose an initial body angular velocity vector

%  w0 = [sqrt(3)/3 -sqrt(3)/3 sqrt(3)/3 ] ;
 w0 = [1 .01 0];  % Major Axis Rotation 
% w0 = [0 1 0];  % Intermediate Axis Rotoation
% w0 = 10*
% w0 = [0 1 .01]; % Intermediate Axis Rotation is Unstable
% w0 = [0 0 1];  % Minor Axis Rotation
% i = 2 ;
% j = 3 ;
% w0 = [ cos(phi(i))*cos(theta(j)) ...
%        cos(phi(i))*sin(theta(j)) ...
%        sin(phi(i))              ] ;

% Define initial condition of complete state vector
S0 = [r0 w0];

% Choose simulation start and end times
t0 = 0; tf = 75 ;         

% Integrate the Euler RBB equations to find R(t) and w(t)
[t,S] = ode45(@sdot,[t0,tf],S0);
%[t,S] = ode45('sdot',t0,tf,S0);

% Close any current figures
close ; close

% Rigid Body Dynamics Figures
tframe = figure('Color','black','Name','Rigid Body Dynamics', ...
                'NumberTitle','off','Position',[25,25,600,300]) ;

%tframe = figure('Color','black','Name','Rigid Body Dynamics', ...
%                'NumberTitle','off','Position',[25,375,600,300]) ;

% Calculate Body Motion Axis Limits
limit = 7;
%limit = max([a/2 b/2 l/2]);
tbody = axes('View',[37 30],'Box','off','Color','none', ...
             'DrawMode','normal','NextPlot','add',         ...
             'Position',[.1 .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,'Body Inertial Motion',...
         'HorizontalAlignment','center',      ...
         'VerticalAlignment'  ,'bottom',      ...
         'Color','y' );
btitle2 = text(1.2*limit,-1.2*limit,-.9*limit,'Rigid Body',...
         'HorizontalAlignment','center',      ...
         'VerticalAlignment'  ,'bottom',      ...
         'Color','y' );
xtext = text(1.3*limit,0,0,'X-Axis');
ytext = text(0,1.3*limit,0,'Y-Axis');
ztext = text(0,0,1.3*limit,'Z-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');

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

% Movie
% aviobj = avifile('bodysim.avi');

% Define Rigid Body Vertices
x10 = [ a/2 -b/2  l/2 ]' ;
x20 = [-a/2 -b/2  l/2 ]' ;
x30 = [-a/2  b/2  l/2 ]' ;
x40 = [ a/2  b/2  l/2 ]' ;
x50 = [ a/2 -b/2 -l/2 ]' ;
x60 = [-a/2 -b/2 -l/2 ]' ;
x70 = [-a/2  b/2 -l/2 ]' ;
x80 = [ a/2  b/2 -l/2 ]' ;

% Create Body Dyanmics Movie
for i = 1:size(S,1),

 % Choose rotation matrix
 R = [ S(i,1) S(i,2) S(i,3) ; ... 
       S(i,4) S(i,5) S(i,6) ; ...
       S(i,7) S(i,8) S(i,9) ] ;

 % Calculate rotated vertices
 x1 = R*x10 ; 
 x2 = R*x20 ; 
 x3 = R*x30 ; 
 x4 = R*x40 ; 
 x5 = R*x50 ; 
 x6 = R*x60 ; 
 x7 = R*x70 ; 
 x8 = R*x80 ; 

 % 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 Velocity Vector
 set(tframe,'CurrentAxes',tomeg)
 x = [ 0 S(i,10) ];
 y = [ 0 S(i,11) ];
 z = [ 0 S(i,12) ];
 omega = line(x,y,z,'Color','y') ;

 % Draw Path of Body Angular Velocity Vector
 if i > 1
  xo = [ S(i-1,10) S(i,10) ] ;
  yo = [ S(i-1,11) S(i,11) ] ;
  zo = [ S(i-1,12) S(i,12) ] ;
  orbit = line(xo,yo,zo,'Color','m') ; 
 end 

 % Save Movie Frame
 % aviobj= addframe(aviobj,getframe(tframe));
 
 % Draw Last Movie Frame
 %if i == size(S,1)
 % set(tframe,'InvertHardCopy','off')
 % print tpicture -dpsc
 %end 


 % Delete Rigid Body Faces
 set(tframe,'CurrentAxes',tbody)
 delete(face1) ; delete(face2) ; delete(face3) ;
 delete(face4) ; delete(face5) ; delete(face6) ;

 % Delete Body Angular Velocity Vector
 set(tframe,'CurrentAxes',tomeg)
 delete(omega)
end

% Close Movie
%aviobj=close(aviobj);

break
                        

