function [q, dq, ddq] = dyn(Body, Joint, Force, SimInfo, q, dq, t)
% function [q, dq, ddq] = dyn(Body, Joint, Force, SimInfo, q, dq, t)
% 
% 4次のRunge-Kutta法を用いてダイナミクス解析を実行するプログラム
% 
% 呼び出されるプログラム 
%  difeqn.m : DAE(微分代数方程式)を解くプログラム
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% Runge-Kutta法
[k1_q, k1_dq, lambda] = ...
	difeqn(Body, Joint, Force, q, dq, t );

[k2_q, k2_dq, lambda] = ...
	difeqn(Body, Joint, Force, q + k1_q*SimInfo.dt/2.0, ...
	dq + k1_dq*SimInfo.dt/2.0, t + SimInfo.dt/2.0 );

[k3_q, k3_dq, lambda] = ...
	difeqn(Body, Joint, Force, q + k2_q*SimInfo.dt/2.0, ...
	dq + k2_dq*SimInfo.dt/2.0, t + SimInfo.dt/2.0 );

[k4_q, k4_dq, lambda] = ...
	difeqn(Body, Joint, Force, q + k3_q*SimInfo.dt, ...
	dq + k3_dq*SimInfo.dt, t + SimInfo.dt );

ddq = (k1_dq + 2*k2_dq + 2*k3_dq + k4_dq) / 6.0;
dq  = dq +                                      ddq * SimInfo.dt;
q   = q  + (k1_q  + 2*k2_q  + 2*k3_q  + k4_q) / 6.0 * SimInfo.dt;

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