bht_gravity_center(array_fishes_solids,Ms,mm,dT,q,dq)

function [Q,dQ,JJ] = bht_gravity_center(array_fishes_solids,Ms,mm,dT,q,dq)
% BHT_GRAVITY_CENTER
% 
% Return the positions, velocities and momenta of inertia of the
% articulated bodies.
% 
% Syntax
% 
%     [Q,dQ,JJ] = BHT_GRAVITY_CENTER(array_fishes_solids,Ms,mm,dT,q,dq)
% 
% Description
% 
%     The function is ran by BHT_TRAJECT_COMPUTE. BhT provides with the
%     user several ways of locating the bodies (see Articulated body's
%     degrees of freedom in the dox). The function BHT_GRAVITY_CENTER 
%     returns
% 
%           o the coordinates of the bodies' centers of mass and the
%             bodies' orientations (stored in the variable Q) . 
%           o the related bodies' velocities (stored in the variable dQ). 
%           o the row vector JJ, whose elements are the bodies' momenta 
%             of inertia.
% 
%     In the computations, the following quantities are required:
% 
%     * the coordinates, orientations and velocities of the solids (stored
%       in respectively q and dq). 
%     * the row vector array_fishes_solids whose
%       element k gives the number of solids composing the body k. 
%     * the mass matrix Ms containing the mass and momenta of inertia of 
%       the solids. 
%     * the row vector mm  whose elements are the bodies' mass. 
%     * the time step dT used to compute bodies' orientations by means 
%       of Newton's second law (balance of angular momentum) by 
%       integrating bodies' angular momenta.
% 
% See also BHT_TRAJECT_COMPUTE

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
%                      BIOHYDRODYNAMICS MATLAB TOOLBOX                    %
%                                                                         %
%                         A. MUNNIER and B. PINCON                        %
%                                                                         %
% [e-mail]        [e-mail]   %
% http://www.iecn.u-nancy.fr/~munnier  http://www.iecn.u-nancy.fr/~pincon %
%                                                                         %
%                                                                         %
%                      INSTITUT ELIE CARTAN, NANCY 1                      %
%                       http://www.iecn.u-nancy.fr/                       %
%                                                                         %
%                      INRIA Lorraine, Projet CORIDA                      %
%                    http://www.iecn.u-nancy.fr/~corida/                  %
%                                                                         %  
%                                                                         %  
%                                                                         %
%                                                                         %
%                            August 15th 2008                             %
%                                                                         %
%                                               GNU GPL v3 licence        %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%##########################################################################
% preallocation of variables 
nb_fishes = length(array_fishes_solids);
nb_solids_cumul = [0,cumsum(array_fishes_solids)];

%-------------------------------------------------
nb_time_step = size(q,2);
Q = zeros(3*nb_fishes,nb_time_step);
dQ = zeros(3*nb_fishes,nb_time_step);
QQ = zeros(3,nb_time_step);
dQQ = zeros(3,nb_time_step);
JJ = zeros(nb_fishes,nb_time_step);
%==========================================================================

for k = 1:nb_fishes
    
    nb_solids = array_fishes_solids(k);
    % =====================================================================
    main_diag = repmat([0,0,1],1,nb_solids);
    sub_diag = repmat([1,0,0],1,nb_solids);
    sub_diag(end) = [];
    over_diag = repmat([-1,0,0],1,nb_solids);
    over_diag(end) = [];
    S = diag(main_diag)+diag(sub_diag,-1)+diag(over_diag,1);
    
    % Ms_loc : submatrix of Ms corresponding to fish k ====================
    dim = 3*nb_solids_cumul(k)+1:3*nb_solids_cumul(k+1);
    
    Ms_loc = Ms(dim,dim);
    
    %qq = generalized variables relating to fish k ========================
    qq = q(dim,:);
    dqq = dq(dim,:);
    
    qq(3:3:3*nb_solids,:) = zeros(nb_solids,nb_time_step);
    
    % ggr = coordinates of center of mass of fish k =======================
    QQ(1,:) = repmat([1,0,0],1,array_fishes_solids(k))*Ms_loc*qq./mm(k);
    QQ(2,:) = repmat([0,1,0],1,array_fishes_solids(k))*Ms_loc*qq./mm(k);
    
    % dggr = velocity of center of mass of fish k =========================
    dQQ(1,:) = repmat([1,0,0],1,array_fishes_solids(k))*Ms_loc*dqq./mm(k);
    dQQ(2,:) = repmat([0,1,0],1,array_fishes_solids(k))*Ms_loc*dqq./mm(k);
    % gr = vector obtained by concatening vectors grr =====================
    Q(3*(k-1)+1:3*(k-1)+2,:) = QQ(1:2,:);
    dQ(3*(k-1)+1:3*(k-1)+2,:) = dQQ(1:2,:);
    
    % ------------------------------
    % center of mass of the fish
    hh = [QQ(1:2,:);ones(1,nb_time_step)];
    % nb_solids copies of the center of mass
    hh = repmat(hh,nb_solids,1);
    % center of mass of each solid in the local frame
    dx = (qq-hh);
    % perp vector of the preceeding one
    dxperp = S*dx;
    % square of the distance from the center of mass of the fish to 
    % the center of mass of each solid.
    dx2 = dx.^2;
    % scalar product
    dxx = (dqq-repmat(dQQ,nb_solids,1)).*dxperp;
    
    % JJ(k,i) = moment of intertia of fish k at time t = T(i) =============
    JJ(k,:) = repmat([1,1,1],1,array_fishes_solids(k))*Ms_loc*dx2;
    
    dQQ(3,:) = repmat([1,1,-1],1,array_fishes_solids(k))*Ms_loc*dxx./JJ(k,:);
    
    %theta(k,i) = orientation of fish k at time t = T(i) ==================
    dQ(3*k,:) = dQQ(3,:);
    Q(3*k,:) = cumsum(dQQ(3,:))*dT;
    
end