function [k_q, k_dq, lambda] = difeqn(Body, Joint, Force, q, dq, t)
% function [k_q, k_dq, lambda] = difeqn(Body, Joint, Force, q, dq, t)
% 
% DAE(微分代数方程式)を解くプログラム
% 
% 呼び出されるプログラム 
%  const.m : 拘束条件式とヤコビ行列の評価
%  baumgarte.m : Baumgarteの安定化法
%  force.m : ボディに作用する外力の評価
%  mass.m : 質量行列の評価
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% Evaluate right hand of acceleration eqs.
[Fq, gmm] = const(Body, Joint, q, dq, t, 3);

% Stabilization method by Baumgarte
gmm = baumgarte(Body, Joint, q, dq, t, gmm);

% Evaluate force
FRC = force(Body, Force, q, dq);

% Evaluate mass matrix
EM = mass(Body, Joint, Fq);

% Solve for accelerations and Lagrange multipliers
W = EM\([FRC; gmm]);

% Store results

k_q = dq;

k_dq = zeros(Body.Number, 3);

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

lambda = W(3*Body.Number+1:3*Body.Number+Joint.m);

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