function DY=barcol_augm_ode(t,Y,fg,c1,c2,MM,invMM,Jd,invJd,k,Lk0,a1d,a2d,b1xd,b2yd,rG2Dd,a1dtilde,a2dtilde,b1xdtilde,b2ydtilde,Alpha,Beta);
% function DY=barcol_augm_ode(t,Y,fg,c1,c2,MM,invMM,Jd,invJd,k,Lk0,a1d,a2d,b1xd,b2yd,rG2Dd,a1dtilde,a2dtilde,b1xdtilde,b2ydtilde,Alpha,Beta);
%
%  ■　(g) 微分方程式の定義
%
% 数値積分ルーチン ode45 を用いて、数値積分を行う微分方程式を定義する 
% 式(4.83)(4.84) 参照
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 井上 剛志 All Rights Reserved.

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

		%  オイラー角は3-1-3（Z'X'Z'）変換
rG1=Y(1:3);	% 状態変数から　質量中心位置（x1,y1,z1)      を抽出
rG2=Y(4:6);	% 状態変数から　質量中心位置（x2,y2,z2)      を抽出
%
vG1=Y(7:9);	% 状態変数から　質量中心速度（Vx1,Vy1,Vz1)   を抽出
vG2=Y(10:12);	% 状態変数から　質量中心速度（Vx2,Vy2,Vz2)   を抽出
vG =Y(7:12);
%
thOG1=Y(13:15); % 状態変数から　オイラー角(φ1,θ1,ψ1)  を抽出
thOG2=Y(16:18); % 状態変数から　オイラー角(φ2,θ2,ψ2)  を抽出
%
omOG1d=Y(19:21);% 状態変数から　角速度（ωx1,ωy1,ωz1)  を抽出
omOG2d=Y(22:24);% 状態変数から　角速度（ωx2,ωy2,ωz2)  を抽出
omOGd=Y(19:24);
%
C1=cos(thOG1);	% cos(φ1),cos(θ1),cos(ψ1)
S1=sin(thOG1);	% sin(φ1),sin(θ1),sin(ψ1)
C2=cos(thOG2);	% cos(φ2),cos(θ2),cos(ψ2)
S2=sin(thOG2);	% sin(φ2),sin(θ2),sin(ψ2)
%
nd1=(-c1*omOG1d);
nd2=(-c2*omOG2d);
nd=[nd1' nd2']';  % 回転運動に対する外トルク（この場合は減衰による外トルク）
%
%  回転マトリックス(式(4.35), 基式(4.50))
%
AOG1=[	C1(1)*C1(3)-S1(1)*C1(2)*S1(3) -C1(1)*S1(3)-S1(1)*C1(2)*C1(3)  S1(1)*S1(2);
	S1(1)*C1(3)+C1(1)*C1(2)*S1(3) -S1(1)*S1(3)+C1(1)*C1(2)*C1(3) -C1(1)*S1(2);
	S1(2)*S1(3)                    S1(2)*C1(3)                    C1(2)      ];
%
AOG2=[	C2(1)*C2(3)-S2(1)*C2(2)*S2(3) -C2(1)*S2(3)-S2(1)*C2(2)*C2(3)  S2(1)*S2(2);
	S2(1)*C2(3)+C2(1)*C2(2)*S2(3) -S2(1)*S2(3)+C2(1)*C2(2)*C2(3) -C2(1)*S2(2);
	S2(2)*S2(3)                    S2(2)*C2(3)                    C2(2)      ];
%
%  オイラー角の時間微分を求めるための係数マトリックス    式(4.14)
%
invGe1d=1/S1(2)*[	 S1(3)         C1(3)          0;
			 S1(2)*C1(3)  -S1(2)*S1(3)    0;
			-C1(2)*S1(3)  -C1(2)*C1(3)    S1(2)];
%
invGe2d=1/S2(2)*[	 S2(3)         C2(3)          0;
			 S2(2)*C2(3)  -S2(2)*S2(3)    0;
			-C2(2)*S2(3)  -C2(2)*C2(3)    S2(2)];
%
omOG1dtilde=tilde(omOG1d);      % 角速度omOG1dから作った歪み対称マトリックス
omOG2dtilde=tilde(omOG2d);      % 角速度omOG2dから作った歪み対称マトリックス
omOGdtilde=[omOG1dtilde zeros(3:3);zeros(3:3) omOG2dtilde];
%
a1=AOG1*a1d;
a2=AOG2*a2d;
%
AOG1T=AOG1';
AOG2T=AOG2';
%
b1x=AOG1*b1xd;
b2y=AOG2*b2yd;
%
d12=(rG2+a2)-(rG1+a1);
d12tilde=tilde(d12);
%
% 外力=ばね力＋重力
%
rDG1=rG1-(rG2+AOG2*rG2Dd);
Lk=sqrt(rDG1'*rDG1);
%
fs1d=[0; 0; k*(Lk-Lk0)];
%
fs2d=-fs1d;
fs1 =AOG1*fs1d;
fs2 =AOG2*fs2d;
fa  =fg+[fs1; fs2];
%
% 拘束方程式
%
CC1=rG1+AOG1*a1d;                                    % 式(4.57)
%
CC2=(AOG1*a1dtilde*AOG1T*AOG2*a2d);
CC2=CC2(1:2,:);                                      % 式(4.58) で上2行を残す
%
CC3=(AOG1*a1dtilde*AOG1T*((rG2+a2)-(rG1+a1)));
CC3=CC3(1:2,:);                                      % 式(4.59) で上2行を残す
%
CC4=b1x'*b2y;                                        % 式(4.60)
%
% ボディ1の並進に関する拘束式の変分（行は拘束順）
%
Cr11=eye(3);                  % 式(4.63)
Cr21=zeros(2:3);              % 式(4.66)
Cr31=-AOG1*a1dtilde*AOG1T;    % 式(4.69) 
Cr31=Cr31(1:2,:);             %上2行を残す
Cr41=[0 0 0];                 % 式(4.72)
%
%
% ボディ2の並進に関する拘束式の変分（行は拘束順）
%
Cr12=zeros(3:3);              % 式(4.63)
Cr22=zeros(2:3);              % 式(4.66)
Cr32=AOG1*a1dtilde*AOG1T;     % 式(4.69)
Cr32=Cr32(1:2,:);             %上2行を残す
Cr42=[0 0 0];                 % 式(4.72)
%
%
Cthetad11=-AOG1*a1dtilde;                                          % 式(4.63)
Cthetad21=AOG2*a2dtilde*AOG2T*AOG1*a1dtilde;                       % 式(4.66)
Cthetad21=Cthetad21(1:2,:);                                        %上2行を残す
Cthetad31=AOG1*a1dtilde*AOG1T*AOG1*a1dtilde-d12tilde*AOG1*a1dtilde;% 式(4.69)
Cthetad31=Cthetad31(1:2,:);                                        %上2行を残す
Cthetad41=-b2y'*AOG1*b1xdtilde;                                    % 式(4.72)
%
%
Cthetad12=zeros(3:3);                                    % 式(4.63)
Cthetad22=-AOG1*a1dtilde*AOG1T*AOG2*a2dtilde;            % 式(4.66)
Cthetad22=Cthetad22(1:2,:);                              %上2行を残す
Cthetad32=-AOG1*a1dtilde*AOG1T*AOG2*a2dtilde;            % 式(4.69)
Cthetad32=Cthetad32(1:2,:);                              %上2行を残す
Cthetad42=-b1x'*AOG2*b2ydtilde;                          % 式(4.72)
%
%
Cr1=[Cr11 ; Cr21 ; Cr31 ; Cr41 ];                        % 式(4.74)
Cr2=[Cr12 ; Cr22 ; Cr32 ; Cr42 ];                        % 
Cr=[Cr1 Cr2];                                            % 
%
Cthetad1=[Cthetad11 ;Cthetad21 ;Cthetad31 ;Cthetad41];   % 式(4.74)
Cthetad2=[Cthetad12 ;Cthetad22 ;Cthetad32 ;Cthetad42];   % 
Cthetad=[Cthetad1 Cthetad2];                             % 
%
Cq=[Cr Cthetad];                                         % 式(4.81)
%
%
DC=Cq*[vG; omOGd];          %  式(4.62),(4.65),(4.68),(4.71)
%
%
A12=AOG1T*AOG2;
A21=AOG2T*AOG1;
%
%  γ1  式(4.64)
gamma1=AOG1*omOG1dtilde*a1dtilde*omOG1d-2*Alpha*DC(1)-Beta^2*CC1;
%
%  γ2  式(4.67)
gamma2=-AOG2*(tilde(omOG2dtilde*a2dtilde)*A21+a2dtilde*A21*omOG1dtilde)*a1dtilde*omOG1d;
gamma2=gamma2+AOG1*(tilde(omOG1dtilde*a1dtilde)*A12+a1dtilde*A12*omOG2dtilde)*a2dtilde*omOG2d;
gamma2=gamma2(1:2,:);
gamma2=gamma2-2*Alpha*DC(2)-Beta^2*CC2;
%
%  γ3  式(4.70)
gamma3=2*(tilde(vG1)-tilde(vG2))*AOG1*a1dtilde*omOG1d;
gamma3=gamma3-(AOG1*omOG1dtilde*a1dtilde*a1dtilde+tilde(AOG2*omOG2dtilde*a2d-AOG1*omOG1dtilde*a1d)*AOG1*a1dtilde+d12tilde*AOG1*omOG1dtilde*a1dtilde)*omOG1d;
gamma3=gamma3+(AOG1*tilde(omOG1dtilde*a1d)*AOG1T*AOG2*a2dtilde+AOG1*a1dtilde*AOG1T*AOG2*omOG2dtilde*a2dtilde)*omOG2d;
gamma3=gamma3(1:2,:);
gamma3=gamma3-2*Alpha*DC(3)-Beta^2*CC3;
%
%  γ4  式(4.73)
gamma4=-b2yd'*(AOG2T*AOG1*omOG1dtilde*omOG1dtilde+omOG2dtilde*omOG2dtilde*AOG2T*AOG1)*b1xd;
gamma4=gamma4+2*omOG2d'*b2ydtilde*AOG2T*AOG1*b1xdtilde*omOG1d;
gamma4=gamma4-2*Alpha*DC(4)-Beta^2*CC4;
%
%  γ   式(4.77)
gamma=[gamma1; gamma2; gamma3; gamma4];
%
% 一般化外力 Q 式(4.81)
QQ=[fa;nd-omOGdtilde*Jd*omOGd];
%
%  λ   式(4.83)
Lambda=inv(Cq*invMM*Cq')*(Cq*invMM*QQ-gamma);
%
%
% 並進変位の微分DrGと並進速度vGの関係 式(4.76)
DrG=vG;
%
% オイラー角の微分と角速度の関係 式(4.78)（式(4.14)より)
DthOG=[invGe1d*omOG1d;invGe2d*omOG2d];  % 
%
% 一般化座標の2階微分について 式(4.84)
DDqG=invMM*(QQ-Cq'*Lambda);
%
% 一般化座標の2階微分から並進速度の微分DvG を抽出
DvG=DDqG(1:6); %
% 一般化座標の2階微分から角速度の時間微分を抽出
omOGd=DDqG(7:12); 
%
% 状態変数の時間微分
%
DY=[DrG; DvG; DthOG; omOGd]; % ode45で数値積分する微分方程式（拡大系）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
