% initialize.m
% 
% 初期化及びモデルファイルのチェック
%  モデルファイルの定義に矛盾がないかをチェックし、問題がない場合は変数の
%  初期化を行う
% 
% 呼び出されるプログラム 
%  errormsg.m : エラーメッセージを返すプログラム
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 椎葉 太一 All Rights Reserved.

% ======================================================================
% モデルファイルの読み込み
% ======================================================================
eval(ModelFileName);

% ======================================================================
% Body.Mass のチェック
% ======================================================================
if strcmp(AnalysisType,'DYNAMICS')
	[m, n] = size(Body.Mass);
	
	if (m ~= Body.Number) | (n ~= 2)
		errormsg('Check Body Definition');
		break;
	end
end

% ======================================================================
% Body.InitPos のチェック
% ======================================================================
[m, n] = size(Body.InitPos);
	
if (m ~= Body.Number) | (n ~= 6)
	errormsg('Check Body Definition');
	break;
end

% ======================================================================
% Joint.RJ のチェック
% ======================================================================
if (Joint.RJ.Number ~= 0)
	[m, n] = size(Joint.RJ.Data);
	
	if (m~= Joint.RJ.Number) | (n ~= 6)
		errormsg('Check Revolute Joint Definition');
		break;
	end
end

% ======================================================================
% Joint.TJ のチェック
% ======================================================================
if (Joint.TJ.Number ~= 0)
	[m, n] = size(Joint.TJ.Data);
	
	if (m ~= Joint.TJ.Number) | (n ~= 8)
		errormsg('Check Translational Joint Definition');
		break;
	end
end

% ======================================================================
% Joint.GR のチェック
% ======================================================================
if (Joint.GR.Number ~= 0)
	[m, n] = size(Joint.GR.Data);
	
	if (m~= Joint.GR.Number) | (n ~= 1)
		errormsg('Check Ground Constraint Definition');
		break;
	end
end

% ======================================================================
% Joint.SM のチェック
% ======================================================================
if (Joint.SM.Number ~= 0)
	[m, n] = size(Joint.SM.Data);
	
	if (m~= Joint.SM.Number) | (n ~= 2)
		errormsg('Check Simple Constraints Definition');
		break;
	end
end

% ======================================================================
% Joint.DR のチェック
% ======================================================================
if (Joint.DR.Number ~= 0)
	[m, n] = size(Joint.DR.Data);
	
	if (m~= Joint.DR.Number) | (n ~= 5)
		errormsg('Check Driving Constraints Definition');
		break;
	end
end

% ======================================================================
% 一時変数を消去
% ======================================================================
clear m n;

% ======================================================================
% 拘束条件の次数計算と過拘束のチェック
% ======================================================================
Joint.m =   Joint.RJ.Number *2 ...
          + Joint.TJ.Number *2 ...
          + Joint.GR.Number *3 ...
          + Joint.SM.Number    ...
          + Joint.DR.Number;

switch AnalysisType
	
	case 'KINEMATICS',
		if 3*Body.Number ~= Joint.m
			errormsg('N must be equal to M');
			break;
		end
	
	case 'DYNAMICS',
		if 3*Body.Number < Joint.m
			errormsg('N must be greater than M');
			break;
		end
	
	otherwise,
		errormsg('Check AnalysisType parameter');
		break;
end

% ======================================================================
% ステップ数計算と時間に関する条件のチェック
% ======================================================================
SimInfo.TotalStep = ...
	ceil((SimInfo.TimeEnd - SimInfo.TimeStart) / SimInfo.dt);

if SimInfo.TotalStep < 0
	errormsg('Check SimInfo parameter');
	break;
end

% ======================================================================
% 時刻の設定
% ======================================================================
time = SimInfo.TimeStart;

% ======================================================================
% 初期条件の格納
% ======================================================================
q  = Body.InitPos(:,1:3);
dq = Body.InitPos(:,4:6);
ddq = zeros(Body.Number, 3);

% ======================================================================
% シミュレーション結果を格納する変数の宣言
% ======================================================================
result_q = zeros(Body.Number,3,SimInfo.TotalStep+1);
result_q(:,:,1) = q;

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