%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample Matlab program for Homework 3.
%
% A Matlab script file to integrate the equations 
% of motion for a uniform, quadratic chain.  The
% following functions are used:
%
% cayley.m, invCayley.m, strCayley.m, crossmat.m
% elasLoads.m, extrLoads.m
% plotBox.m
%
% Here's how to get started: 
%        1) Copy myprogram3B.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 "myprogram3B" in the Matlab window
%           to run this file.
% Note: the program may take a few minutes to run.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

orig = [0 0 0]' ;    %define origin of fixed frame
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


NB = 10 ; %number of bodies

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

M = 0.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  = diag([20 20 20]) ;    %translational stiff matrix (Newton/meter)
C  = diag([0 0 0])    ;    %coupling matrix (Newton)
K  = diag([3 3 3])    ;    %rotational stiff matrix (Newton*meter)
etahat = [0 0 0.5]'   ;    %config parameter eta-hat (meters)
uhat   = [0 0 0]'     ;    %config parameter u-hat (dimensionless)

Q0(:,:,1) = cayley(uhat)                      ; %set initial conds (begin)
q0(:,1)   = etahat                            ; %the body variables are
v0(:,1)   = [0 0 0]'                          ; %stored in large arrays:
w0(:,1)   = [0 0 0]'                          ; %Q0 = 3x3xNB array 
for aa=2:NB                                   ; %q0 = 3xNB array
Q0(:,:,aa) = Q0(:,:,aa-1)*cayley(uhat)        ; %v0 = 3xNB array
q0(:,aa)   = q0(:,aa-1) + Q0(:,:,aa-1)*etahat ; %w0 = 3xNB array
v0(:,aa)   = [0 0 0]'                         ;  
w0(:,aa)   = [0 0 0]'                         ; 
end                                           ; %set initial conds (end)

figure(1); clf ;                  %create fig #1 and clear
plot3(0,0,0)   ;                  %initialize 3D plot
plotBox(orig,e1,e2,e3,a,b,c) ;    %plot body "B_0" for reference
for aa=1:NB                       %loop over bodies aa=1...NB
  cntr = q0(:,aa)   ;             %get mass center
  d1   = Q0(:,1,aa) ;             %get d1-vec
  d2   = Q0(:,2,aa) ;             %get d2-vec
  d3   = Q0(:,3,aa) ;             %get d3-vec
  plotBox(cntr,d1,d2,d3,a,b,c) ;  %plot body B_aa
end
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.01 ; T = 7 ; N = round(T/dt) ;

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

[fen, tauen] = elasLoads(NB,qn,Qn,G,C,K,etahat,uhat) ; %elas loads at t_n
[fxn, tauxn] = extrLoads(NB,qn,Qn,tn)                ; %extr loads at t_n

fn   = fen   + fxn   ; %total net force  at t_n
taun = tauen + tauxn ; %total net torque at t_n


for i=1:N

   for aa=1:NB
     vbar(:,aa) = vn(:,aa) + 0.5*dt*(1/M)*fn(:,aa) ;   %compute vbar
     wbar(:,aa) = wn(:,aa) +...
       0.5*dt*GamInv*(crossmat(Gam*wn(:,aa))*...
            wn(:,aa) + taun(:,aa)) ;                   %compute wbar

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

   tnn = tn + dt  ; %time t_n+1
   [fenn, tauenn] = elasLoads(NB,qnn,Qnn,G,C,K,etahat,uhat) ; %elas loads at t_n+1
   [fxnn, tauxnn] = extrLoads(NB,qnn,Qnn,tnn)               ; %extr loads at t_n+1
   fnn   = fenn   + fxnn    ; %total net force  at t_n+1 
   taunn = tauenn + tauxnn  ; %total net torque at t_n+1
   fnh   = 0.5*(fn+fnn)     ; %compute f_n+1/2
   taunh = 0.5*(taun+taunn) ; %compute tau_n+1/2

   for aa=1:NB
     vnn(:,aa) = vn(:,aa) + dt*(1/M)*fnh(:,aa)  ;   %compute v_n+1
     wnn(:,aa) = wn(:,aa) +...
        dt*GamInv*(crossmat(Gam*wbar(:,aa))*...
	wbar(:,aa) + taunh(:,aa)) ;                 %compute w_n+1
   end

   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

   clf;                              %clear figure for next timestep
   plotBox(orig,e1,e2,e3,a,b,c) ;    %plot body "B_0" for reference
   for aa=1:NB                       %loop over bodies aa=1...NB
     cntr = qn(:,aa)   ;             %get mass center
     d1   = Qn(:,1,aa) ;             %get d1-vec
     d2   = Qn(:,2,aa) ;             %get d2-vec
     d3   = Qn(:,3,aa) ;             %get d3-vec
     plotBox(cntr,d1,d2,d3,a,b,c) ;  %plot body B_aa
   end

   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

