function [Fq, RHS] = tran(Body, Joint, q, dq, flag)
% function [Fq, RHS] = tran(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(2*Joint.TJ.Number, 3*Body.Number);
RHS = zeros(2*Joint.TJ.Number, 1);

for k=1:Joint.TJ.Number
	
	i = Joint.TJ.Data(k,1);
	j = Joint.TJ.Data(k,2);
	
	xpi_xi = Joint.TJ.Data(k,3)*cos(q(i,3)) - Joint.TJ.Data(k,4)*sin(q(i,3));
	ypi_yi = Joint.TJ.Data(k,3)*sin(q(i,3)) + Joint.TJ.Data(k,4)*cos(q(i,3));
	xpj_xj = Joint.TJ.Data(k,7)*cos(q(j,3)) - Joint.TJ.Data(k,8)*sin(q(j,3));
	ypj_yj = Joint.TJ.Data(k,7)*sin(q(j,3)) + Joint.TJ.Data(k,8)*cos(q(j,3));
	xpi_xqi= (Joint.TJ.Data(k,3)-Joint.TJ.Data(k,5))*cos(q(i,3)) ...
			- (Joint.TJ.Data(k,4)-Joint.TJ.Data(k,6))*sin(q(i,3));
	ypi_yqi= (Joint.TJ.Data(k,3)-Joint.TJ.Data(k,5))*sin(q(i,3)) ...
			+ (Joint.TJ.Data(k,4)-Joint.TJ.Data(k,6))*cos(q(i,3));
	
	% ===========================================================================
	% Jacobian matrix nonzero entries
	
	Fq(2*(k-1)+1, 3*(i-1)+1) =  ypi_yqi;
	Fq(2*(k-1)+1, 3*(i-1)+2) = -xpi_xqi;
	Fq(2*(k-1)+1, 3*(i-1)+3) = -(q(j,1) + xpj_xj - q(i,1))*xpi_xqi - (q(j,2)+ypj_yj-q(i,2))*ypi_yqi;
	Fq(2*(k-1)+1, 3*(j-1)+1) = -ypi_yqi;
	Fq(2*(k-1)+1, 3*(j-1)+2) =  xpi_xqi;
	Fq(2*(k-1)+1, 3*(j-1)+3) =  xpj_xj*xpi_xqi+ypj_yj*ypi_yqi;
	Fq(2*(k-1)+2, 3*(i-1)+3) =  1;
	Fq(2*(k-1)+2, 3*(j-1)+3) = -1;
	
	% ===========================================================================
	% Constraint equations
	if flag==1
		RHS(2*(k-1)+1)= xpi_xqi*(q(j,2) + ypj_yj -q(i,2) -ypi_yi) ...
			-ypi_yqi*(q(j,1) + xpj_xj -q(i,1) -xpi_xi);
		RHS(2*(k-1)+2)= q(i,3) - q(j,3) - (Body.InitPos(i,3) - Body.InitPos(j,3));
	end
	
	% ===========================================================================
	% Right-hand-side of velocity equations
	if flag==2
		RHS(2*(k-1)+1)= 0;
		RHS(2*(k-1)+2)= 0;
	end
	
	% ===========================================================================
	% Right-hand-side of acceleration equations
	if flag==3
		RHS(2*(k-1)+1)= -dq(i,3)*(2*(xpi_xqi*(dq(i,1)-dq(j,1)) + ypi_yqi*(dq(i,2)-dq(j,2))) ...
			+ dq(i,3)*(xpi_xqi*(q(i,2)-q(j,2)) - ypi_yqi*(q(i,1)-q(j,1))));
		RHS(2*(k-1)+2)= 0;
 	end
end

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