function DY=top3d_augm_ode_ver5(t,Y);
% function DY=top3d_augm_ode_ver5(t,Y);
%
%  ■　(g) 微分方程式の定義
%
% 数値積分ルーチン ode45 を用いて、数値積分を行う微分方程式を定義する 
% 式(4.14)(4.30)(4.32)(4.33) 参照
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 井上 剛志, 竹原 昭一郎 All Rights Reserved.

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

global fa c M invM Jd invJd rGCdtilde rGCd Alpha Beta;
%
               % オイラー角は3-1-3（Z'X'Z'）変換
rG=Y(1:3);     % 状態変数から　質量中心位置（x,y,z）     を抽出
vG=Y(4:6);     % 状態変数から　質量中心速度（Vx,Vy,Vz）  を抽出
thOG=Y(7:9);   % 状態変数から　オイラー角(φ,θ,ψ)      を抽出
omOGd=Y(10:12);% 状態変数から　角速度（ωx,ωy,ωz）     を抽出
C=cos(thOG);   % cos(φ),cos(θ),cos(ψ)
S=sin(thOG);   % sin(φ),sin(θ),sin(ψ)
%
nd=-c*omOGd;   % 回転運動に対する外トルク（この場合は減衰による外トルク）
%
%  回転マトリックス(式(4.35), 基式(4.50))
%
AOG=[	C(1)*C(3)-S(1)*C(2)*S(3)  -C(1)*S(3)-S(1)*C(2)*C(3)   S(1)*S(2);
	S(1)*C(3)+C(1)*C(2)*S(3)  -S(1)*S(3)+C(1)*C(2)*C(3)  -C(1)*S(2);
	S(2)*S(3)                  S(2)*C(3)                  C(2)];
%
%  オイラー角の時間微分を求めるための係数マトリックス    式(4.14)
%
invGed=1/S(2)*[  S(3)        C(3)          0;
		 S(2)*C(3)  -S(2)*S(3)     0;
		-C(2)*S(3)  -C(2)*C(3)     S(2)];
%
omOGdtilde=tilde(omOGd);      % 角速度omOGdから作った歪み対称マトリックス
CC=rG+AOG*rGCd;               % 拘束条件式                    式(4.17)
DC=vG-AOG*rGCdtilde*omOGd;    % 拘束条件の1階微分式           式(4.18)
%
Cr=eye(3);                    % 拘束条件の変分（並進変位）    式(4.19)
Cthetad=-AOG*rGCdtilde;       % 拘束条件の変分（回転変位）    式(4.19)
%
gamma=AOG*omOGdtilde*rGCdtilde*omOGd-2*Alpha*DC-Beta^2*CC;  % 式(4.23)
Lambda=inv(Cr*invM*Cr'+Cthetad*invJd*Cthetad')*(Cr*invM*fa+Cthetad*invJd*(-omOGdtilde*Jd*omOGd)-gamma);                                     % 式(4.29)
%
DrG=vG;                       %並進変位の微分DrGと並進速度vGの関係
%								式(4.26)
DvG=invM*(fa-Cr'*Lambda);     %並進速度の微分DvG（式(13.30)*より）
%                                                             式(4.32)第1式
DthOG=invGed*omOGd;           % オイラー角の微分と角速度の関係
%				（基式(5.52b),基式(5.54)より)   式(4.14)
DomOGd=invJd*(nd-omOGdtilde*Jd*omOGd-Cthetad'*Lambda);
				% 角速度の時間微分について（基式(13.31)より）
								% 式(4.32)第2式
%
% 状態変数の時間微分
DY=[DrG; DvG; DthOG; DomOGd]; % ode45で数値積分する微分方程式（拡大系）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
