%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample Matlab program for Homework 2.
%
% A Matlab script file that uses the function cayley.m, 
% crossmat.m and plotBox.m to simulate the spinning book 
% problem
%
% Here's how to get started: 
%        1) Copy myprogram2B.m (this file), cayley.m,
%           plotBox.m and crossmat.m into your working 
%           directory.
%        2) Invoke Matlab by typing "matlab" at
%           the unix prompt. 
%        3) Type "myprogram2B" in the Matlab window
%           to run this file.
% Note: the program may take a few seconds to run.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

e1 = [1 0 0]' ;    %define e1-coord vector (column) 
e2 = [0 1 0]' ;    %define e2-coord vector (column) 
e3 = [0 0 1]' ;    %define e3-coord vector (column) 

Id = eye(3,3) ;    %initialize 3x3 identity matrix

figure(1); clf ;   %create figure window #1 and clear its contents

a=0.16 ; b=0.24 ; c=0.03 ;  %set dimensions of box (meters)
origin=[0 0 0]'   ;         %set origin for plotBox
lim = 1.5*max([a,b,c]) ;    %set limits for x,y,z axes

plotlimits = [-lim lim -lim lim -lim lim]; 

plot3(0,0,0) ;                    %initialize 3D plot
plotBox(origin,e1,e2,e3,a,b,c) ;  %plot box defined by e-vecs
axis(plotlimits) ;                %set limits for plot axes
axis square  ;                    %set scales for plot axes 
axis manual  ;                    %freeze scales in plot
pause(1.0)   ;                    %pause to draw and see plot

M = 1 ;                                        %mass (kg)
Gam = (M/12)*diag([b^2+c^2,a^2+c^2,a^2+b^2]) ; %intertia matrix (kg-m^2)
GamInv = inv(Gam) ;                            %compute inverse
Ohm = 12 ;                                     %spin rate (rad/sec)

T = 3 ; N = 1000 ; dt = T/N ; 

time  = zeros(N+1,1) ; %define array for time values
wdata = zeros(N+1,3) ; %define array for w-vecs

t = 0;               %initial time
Qn = Id ;            %initial condition for Q
wn = [Ohm 0 0]' ;    %initial condition for w

time(1) = t ;     %store initial t value
wdata(1,:)=wn' ;  %store initial w vec

for i=1:N
   t = t + dt;                       %increment time

   wbar = wn + 0.5*dt*GamInv*(crossmat(Gam*wn)*wn) ; %compute wbar
   Qn = Qn*cayley(dt*wbar) ;                         %compute Q_n+1
   wn = wn + dt*GamInv*(crossmat(Gam*wbar)*wbar) ;   %compute w_n+1

   d1=Qn*e1 ; d2=Qn*e2 ; d3=Qn*e3 ;  %compute d-vecs for plot
   clf;                              %clear figure for next timestep
   plotBox(origin,d1,d2,d3,a,b,c) ;  %plot box defined by d-vecs
   axis(plotlimits) ;                %set limits for plot axes
   axis square ;                     %set scales for plot axes 
   axis manual ;                     %freeze scales in plot
   pause(0.01) ;                     %pause to draw and see plot

   time(i+1) = t ;                   %store t value
   wdata(i+1,:)=wn' ;                %store w vec
end

figure(2) ;clf; plot(time,wdata(:,1),'g',...  %plot w1 vs t in green
                     time,wdata(:,2),'r',...  %plot w2 vs t in red
		     time,wdata(:,3),'b') ;   %plot w3 vs t in blue
axis([0 T -1.2*Ohm 1.2*Ohm]) ;                %set limits for plot axes
xlabel('time: t') ;                           %set label for x axis
ylabel('ang vel: w1, w2, w3') ;               %set label for y axis

