function EM = mass(Body, Joint, Fq)
% function EM = mass(Body, Joint, Fq)
% 
% 拡大法において用いる質量行列を計算する
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% Initialize
EMM = zeros(3*Body.Number);

% Mass and moment of inertia
for i=1:Body.Number
	j=3*(i-1)+1;
	EMM(j,  j  )=Body.Mass(i,1);
	EMM(j+1,j+1)=Body.Mass(i,1);
	EMM(j+2,j+2)=Body.Mass(i,2);
end

EM = [ EMM Fq'
       Fq  zeros(Joint.m)];

% =================================================================================
% プログラムの使用条件
% 1. 本プログラムおよびデータを商用で，頒布してはならない．
% 2. 本プログラムおよびデータを著作者の許可なく営利目的で利用してはならない．
% 3. 本プログラムおよびデータの改変は，営利目的でない限り自由である．
% 4. 本プログラムおよびデータを使用することにより生じた損害などについては，著者，
%    プロクラム著作者，(社)日本機械学会，および(株)コロナ社は一切の責任を負わない．
% 5. 本プログラムの著作権は，本書編著者ならびにプログラム著作者に帰す．
% =================================================================================