function [Fq, RHS] = grnd(Body, Joint, q, dq, flag)
% function [Fq, RHS] = grnd(Body, Joint, q, dq, flag)
% 
% グランド拘束条件の評価
% 
% 引数の説明：
%  flag==1: Constraint equation
%  flag==2: Right-hand-side of velocity equations
%  flag==3: Right-hand-side of acceleration equations
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% FqとRHSのサイズを宣言
Fq  = zeros(3*Joint.GR.Number, 3*Body.Number);
RHS = zeros(3*Joint.GR.Number, 1);

for k=1:Joint.GR.Number

	i = Joint.GR.Data(k,1);
	
	% Jacobian matrix nonzero entry	
	
	Fq(3*(k-1)+1, 3*(i-1)+1) = 1;
	Fq(3*(k-1)+2, 3*(i-1)+2) = 1;
	Fq(3*(k-1)+3, 3*(i-1)+3) = 1;
	
	% Constraint equation
	if flag==1
		RHS(3*(k-1)+1) = q(i,1) - Body.InitPos(i,1);
		RHS(3*(k-1)+2) = q(i,2) - Body.InitPos(i,2);
		RHS(3*(k-1)+3) = q(i,3) - Body.InitPos(i,3);
	end
	
	% Right-hand-side of velocity/acceleration equation
	if (flag==2 | flag==3)
		RHS(3*(k-1)+1) = 0;
		RHS(3*(k-1)+2) = 0;
		RHS(3*(k-1)+3) = 0;
	end

end

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