%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample Matlab program for Homework 3.
%
% A Matlab script file to simulate a rigid body
% subject to gravity and Hookean spring loads.
% The following functions are used:
%
% cayley.m, crossmat.m
% springLoads.m
% plotVector.m, plotBox.m
%
% Here's how to get started: 
%        1) Copy myprogram3A.m (this file) and the
%           above list of files into your working 
%           directory.
%        2) Invoke Matlab by typing "matlab" at
%           the unix prompt. 
%        3) Type "myprogram3A" 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


a=0.16 ; b=0.24 ; c=0.10 ;  %set dimensions of box (meters)
lim = 4*max([a,b,c]) ;      %set limits for axes in plots
plotlimits = [-lim lim -lim lim -lim 0]; 

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
g = 10 ;                                       %grav accel (m/s^2)

K   = 20   ;             %spring stiffness (Newton/m)
len = 0.25 ;             %spring relaxed length (meter)
xi    = [a/2 b/2 c/2]' ; %spring body pt (d frame)
xstar = [a/2 b/2 3*c]' ; %spring anchor pt (e frame)

q0 = [0 0 0]'  ;  %initial position of mass center
v0 = [0 0 0]'  ;  %initial velocity of mass center
Q0 = Id        ;  %initial rotation matrix
w0 = [0 0 0]'  ;  %initial angular velocity

figure(1); clf ;                  %create fig #1 and clear
plot3(0,0,0) ;                    %initialize 3D plot
plotBox(q0,e1,e2,e3,a,b,c) ;      %plot initial config
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

dt = 0.010 ; T = 10 ; N = round(T/dt) ;

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

tn = 0  ;            %initial time
qn = q0 ;            %initial condition for q
Qn = Q0 ;            %initial condition for Q
vn = v0 ;            %initial condition for v
wn = w0 ;            %initial condition for w

[fn, taun] = springLoads(qn,Qn,xi,xstar,K,len) ; %spring loads at t_n

time(1)    = tn  ;    %store initial t value
qdata(1,:) = qn' ;    %store initial q vec

for i=1:N
 
   vbar = vn + 0.5*dt*(1/M)*(fn-M*g*e3) ;           %compute vbar
   wbar = wn + 0.5*dt*GamInv*(...
                  crossmat(Gam*wn)*wn + taun) ;     %compute wbar

   qnn = qn + dt*vbar       ;                       %compute q_n+1
   Qnn = Qn*cayley(dt*wbar) ;                       %compute Q_n+1

   tnn = tn + dt  ;                                   %time t_n+1 
   [fnn, taunn]=springLoads(qnn,Qnn,xi,xstar,K,len) ; %spring loads at t_n+1
   fnh   = 0.5*(fn+fnn) ;                             %compute f_n+1/2
   taunh = 0.5*(taun+taunn) ;                         %compute tau_n+1/2

   vnn = vn + dt*(1/M)*(fnh-M*g*e3)   ;              %compute v_n+1
   wnn = wn + dt*GamInv*(...
                 crossmat(Gam*wbar)*wbar + taunh) ;  %compute w_n+1


   tn = tnn     ;  %update time
   qn = qnn     ;  %update q
   Qn = Qnn     ;  %update Q
   vn = vnn     ;  %update v
   wn = wnn     ;  %update w
   fn = fnn     ;  %update f
   taun = taunn ;  %update tau

   time(i+1)    = tn   ;  %save t value
   qdata(i+1,:) = qn'  ;  %save q vec

   d1=Qn*e1 ; d2=Qn*e2 ; d3=Qn*e3 ;  %compute d-vecs for plot
   x = qn + Qn*xi ;                  %compute posn of spring attach pt
   clf;                              %clear figure for next timestep
   plotBox(qn,d1,d2,d3,a,b,c) ;      %plot box defined by d-vecs
   plotVector(xstar,x,'c') ;         %plot spring as vector (cyan)
   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

end

figure(2) ;clf; plot(time,qdata(:,1),'g',...  %plot q1 vs t in green
                     time,qdata(:,2),'r',...  %plot q2 vs t in red
		     time,qdata(:,3),'b') ;   %plot q3 vs t in blue
xlabel('time: t') ;                           %set label for x axis
ylabel('position: q1, q2, q3') ;              %set label for y axis

