% subCoin.m
%************************************************************************
%                     Rolling Coin   (SUB)     拡大法
%************************************************************************
% Copyright (C) 2007 清水 信行, 曽我部 潔 All Rights Reserved.

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

function [hsdot]=subCoin(t,q, r, m, g, rho, k );
%-----------------------------------------------
% 転がり運動するコインのDAE(微分代数方程式)の定義
%-----------------------------------------------

dq(1)=q(6); dq(2)=q(7); dq(3)=q(8); dq(4)=q(9); dq(5)=q(10);
       
%--- ヤコビアンマトリックス ---
a=[1  0  -r*cos(q(3))*cos(q(4))  r*sin(q(3))*sin(q(4))  -r*cos(q(3))
   0  1  -r*sin(q(3))*cos(q(4))  -r*cos(q(3))*sin(q(4))  -r*sin(q(3))]; 

%--- ヤコビアンの時間微分 ---       
da=r*[0  0  (dq(3)*sin(q(3))*cos(q(4))+dq(4)*cos(q(3))*sin(q(4))) ...     
      (dq(3)*cos(q(3))*sin(q(4))+dq(4)*sin(q(3))*cos(q(4)))  (dq(3)*sin(q(3)))
      0  0  (-dq(3)*cos(q(3))*cos(q(4))+dq(4)*sin(q(3))*sin(q(4))) ...
    (dq(3)*sin(q(3))*sin(q(4))-dq(4)*cos(q(3))*cos(q(4)))  (-dq(3)*cos(q(3))) ];                                                           

%--- bベクトルとその時間微分 --- 
b = [0; 0 ];
db= [0; 0 ];
       
%--- 慣性マトリックス ---
lm = m*[1  0  0  0  0;
        0  1  0  0  0;
        0  0  k^2*(1+(cos(q(4)))^2)/2  0  k^2*cos(q(4));                                
        0  0  0  (k^2/2+r^2*(cos(q(4)))^2)  0;
        0  0  k^2*cos(q(4))  0  k^2 ];
                
%--- 外力 ---
f = m*[0; 
       0;
       k^2*(dq(3)*dq(4)*sin(q(4))*cos(q(4))+dq(4)*dq(5)*sin(q(4)));
       (r^2*(dq(4))^2-k^2*(dq(3))^2/2)*sin(q(4))*cos(q(4))-k^2*dq(3)*dq(5)*sin(q(4))-g*r*cos(q(4));      
       k^2*dq(3)*dq(4)*sin(q(4)) ];  
   
%--- 零マトリックス ---
rei55=zeros(5,5);                                                      %5行5列
rei22=zeros(2,2);                                                      %2行2列
rei52=zeros(5,2);                                                      %5行2列
       
%--- 単位マトリックス ---
iti= eye(5,5);                                                         %5行5列
       
%--- 速度ベクトル ---
skd=[dq(1); dq(2); dq(3); dq(4); dq(5)];
vv=skd;
       
%--- 計算結果 ---
atai=[iti rei55 rei52; rei55 lm -a.'; -da -a rei22 ]\[vv; f; db];
       
for i= 1:10;
  hsdot(i, 1)= atai(i, 1);
end;