
function [f, tau] = elasLoads(NB,q,Q,G,C,K,etahat,uhat)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Computes net elastic force components f and 
% torque components tau for each body in a
% uniform chain with quadratic interactions.  
% The parameters are
%
%       NB    = number of bodies in chain
%       q,Q   = absolute config variales for chain
%       G     = translational stiffness matrix
%       C     = coupling matrix
%       K     = rotational stiffness matrix
%       etahat, uhat  = config parameters
%
% Notice that the force and torque on the first 
% and last body are treated separately in the code 
% below.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


Lam(:,:,1) = Q(:,:,1)                              ; %compute relative
u(:,1)     = invCayley(Lam(:,:,1))                 ; %config variables
eta(:,1)   = q(:,1)                                ; %for entire chain
for aa=2:NB                                        ;
  Lam(:,:,aa) = Q(:,:,aa-1)'*Q(:,:,aa)             ; 
  u(:,aa)     = invCayley(Lam(:,:,aa))             ; 
  eta(:,aa)   = Q(:,:,aa-1)'*(q(:,aa) - q(:,aa-1)) ;
end


dUdeta   = G*(eta(:,1)-etahat)  + C*(u(:,1)-uhat) ; %dU^a / d(eta)^a
dUdeta_p = G*(eta(:,2)-etahat)  + C*(u(:,2)-uhat) ; %dU^a+1 / d(eta)^a+1
dUdu     = C'*(eta(:,1)-etahat) + K*(u(:,1)-uhat) ; %dU^a / du^a
dUdu_p   = C'*(eta(:,2)-etahat) + K*(u(:,2)-uhat) ; %dU^a+1 / du^a+1
Y        = strCayley(u(:,1))                      ; %Y(u^a)
Y_p      = strCayley(u(:,2))                      ; %Y(u^a+1)
f(:,1)   = -dUdeta + Q(:,:,1)*dUdeta_p            ; %net force on B_1
tau(:,1) = -Y*dUdu + Lam(:,:,2)*Y_p*dUdu_p ...
                 + crossmat(eta(:,2))*dUdeta_p    ; %net torque on B_1 

for aa=2:NB-1
 dUdeta   = G*(eta(:,aa)-etahat)    + C*(u(:,aa)-uhat)   ; %dU^a / d(eta)^a
 dUdeta_p = G*(eta(:,aa+1)-etahat)  + C*(u(:,aa+1)-uhat) ; %dU^a+1 / d(eta)^a+1
 dUdu     = C'*(eta(:,aa)-etahat)   + K*(u(:,aa)-uhat)   ; %dU^a / du^a
 dUdu_p   = C'*(eta(:,aa+1)-etahat) + K*(u(:,aa+1)-uhat) ; %dU^a+1 / du^a+1
 Y        = strCayley(u(:,aa))                           ; %Y(u^a)
 Y_p      = strCayley(u(:,aa+1))                         ; %Y(u^a+1)
 f(:,aa)   = -Q(:,:,aa-1)*dUdeta + Q(:,:,aa)*dUdeta_p    ; %net force on B_aa
 tau(:,aa) = -Y*dUdu + Lam(:,:,aa+1)*Y_p*dUdu_p ...
                 + crossmat(eta(:,aa+1))*dUdeta_p        ; %net torque on B_aa
end


dUdeta    = G*(eta(:,NB)-etahat)  + C*(u(:,NB)-uhat) ; %dU^a / d(eta)^a
dUdu      = C'*(eta(:,NB)-etahat) + K*(u(:,NB)-uhat) ; %dU^a / du^a
Y         = strCayley(u(:,NB))                       ; %Y(u^a)
f(:,NB)   = -Q(:,:,NB-1)*dUdeta                      ; %net force on B_n
tau(:,NB) = -Y*dUdu                                  ; %net torque on B_n


