function [q, dq, ddq] = kin(Body, Joint, q, dq, t)
% function [q, dq, ddq] = kin(Body, Joint, q, dq, t)
% 
% キネマティクス解析を実行するプログラム
% 
% 呼び出されるプログラム 
%  const.m : 拘束条件式とヤコビ行列の評価
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% Newton-Raphson法における最大繰り返し回数を設定
NRMAX = 10;
% 収束判定値を設定
eps = 0.001;

iteration = 1;

% ==================================================
% 位置解析
% ==================================================

% Newton-Raphson iteration

while iteration <= NRMAX
	
	for i=1:Body.Number
		for j=1:3
			q_column(3*(i-1)+j,1) = q(i,j);
		end
	end
	
	[Fq, phi] = const(Body, Joint, q, dq, t, 1);
	
	q_column = q_column - Fq\phi;
	
	for i=1:Body.Number
		for j=1:3
			q(i,j) = q_column(3*(i-1)+j,1);
		end
	end
	
	if (sqrt(phi'*phi) < eps)
		break;
	end
	
	iteration = iteration + 1;
end

% ==================================================
% 速度解析
% ==================================================

[Fq, nu]  = const(Body, Joint, q, dq, t, 2);

dq_column = Fq\nu;

for i=1:Body.Number
	for j=1:3
		dq(i,j) = dq_column(3*(i-1)+j,1);
	end
end


% ==================================================
% 加速度解析
% ==================================================

[Fq, gmm]  = const(Body, Joint, q, dq, t, 3);

ddq_column = Fq\gmm;

for i=1:Body.Number
	for j=1:3
		ddq(i,j) = ddq_column(3*(i-1)+j,1);
	end
end

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