function FRC = force(Body, Force, q, dq)
% function FRC = force(Body, Force, q, dq)
% 
% ボディに作用する外力の評価
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% FRCのサイズを宣言
FRC = zeros(3*Body.Number,1);

% Body forces
for i=1:Body.Number
	FRC(3*(i-1)+1,1) = Force.Body(i,1);
	FRC(3*(i-1)+2,1) = Force.Body(i,2) - Body.Mass(i,1)*9.81; % Gravity force calculation
	FRC(3*(i-1)+3,1) = Force.Body(i,3);
end

% Translational force elements
for k = 1:Force.Tran.Number
	i = Force.Tran.Data(k,1);
	j = Force.Tran.Data(k,2);
	
	% Calculate position
	xpi_xi = Force.Tran.Data(k,3)*cos(q(i,3)) - Force.Tran.Data(k,4)*sin(q(i,3));
	ypi_yi = Force.Tran.Data(k,3)*sin(q(i,3)) + Force.Tran.Data(k,4)*cos(q(i,3));
	xpj_xj = Force.Tran.Data(k,5)*cos(q(j,3)) - Force.Tran.Data(k,6)*sin(q(j,3));
	ypj_yj = Force.Tran.Data(k,5)*sin(q(j,3)) + Force.Tran.Data(k,6)*cos(q(j,3));
	
	% Current Spring length and change of length
	Lx = q(j,1) + xpj_xj - q(i,1) - xpi_xi;
	Ly = q(j,2) + ypj_yj - q(i,2) - ypi_yi;
	
	Length = sqrt(Lx^2+Ly^2);
	DifL = Length - Force.Tran.Data(k,10);
	
	if abs(Length)<1.0e-10 Length = 1.0e-10; end
	
	% Unit vector
	Ux = Lx / Length;
	Uy = Ly / Length;
	
	% Velocity of Translational Elements
	dLx = dq(j,1) - ypj_yj*dq(j,3) - dq(i,1) + ypi_yi*dq(i,3);
	dLy = dq(j,2) + xpj_xj*dq(j,3) - dq(i,2) - xpi_xi*dq(i,3);
	dLength = (Lx*dLx+Ly*dLy)/Length;
	
	% Element forces
	FF =   Force.Tran.Data(k,7) * DifL ...
	     + Force.Tran.Data(k,8) * dLength ...
	     + Force.Tran.Data(k,9);
	
	Fx = FF * Ux;
	Fy = FF * Uy;
	
	% Convert to force vector
	FRC(3*(i-1)+1,1) = FRC(3*(i-1)+1,1) + Fx;
	FRC(3*(i-1)+2,1) = FRC(3*(i-1)+2,1) + Fy;
	FRC(3*(i-1)+3,1) = FRC(3*(i-1)+3,1) - ypi_yi*Fx + xpi_xi*Fy;
	FRC(3*(j-1)+1,1) = FRC(3*(j-1)+1,1) - Fx;
	FRC(3*(j-1)+2,1) = FRC(3*(j-1)+2,1) - Fy;
	FRC(3*(j-1)+3,1) = FRC(3*(j-1)+3,1) + ypj_yj*Fx - xpj_xj*Fy;
end

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