% mainCoin.m
%**********************************************************
%                Rolling Coin の main       
%**********************************************************
% 転がり運動するコインの解析
% 呼び出される関数
% gillCoin.m : ルンゲ・クッタ・ギルによる数値積分計算
%
% Copyright (C) 2007 清水 信行, 曽我部 潔 All Rights Reserved.

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

%---------------------------------------------------------- 
%               initial  setting
%----------------------------------------------------------
clear all;
clear functions;

%-- generalized coordinates --
% qi=[qi(1); qi(2); qi(3); qi(4); qi(5)]=[X; Y; psai; theta; phai];
%-- generalized velocities --
% dqi=[dqi(1); dqi(2); dqi(3); dqi(4); dqi(5)]=[dX; dY; dpsai; dtheta; dphai];


%-- physical parameters --
r= 0.25;                                                     %ディスク半径
m= 1.0;                                                      %質量
g= 9.8;                                                      %重力加速度
rho= 5*r;                                                    %中心が描く半径
k= r/2^(1/2);                                                %回転半径 
                                                       
%----- time ------
t0=0.0;                                                      %開始時間
tf=30.0;                                                     %終了時間
h=0.001;                                                     %刻み時間

     
%--- initial value --- 
% テキスト4.4節の[計算例題1]から[計算例題3] の初期条件のみを下記のように決定して
% いる。その他の問題に対しては、その都度、初期条件を決定する必要がある。ユーザー
% はテキストを参考に別途導出してセットしてください。
% 
                                
theta=input('theta=  ');                                     %初期章動角を入力
if theta==pi/2-0.001;
  qi=[0; 0; 0; theta; 0];     % ケース3 ; [x方向; y方向; 歳差角; 章動角; スピン角]
        
  %-- initial generalized velocity --
  dqi(3)=0.0;                                                %歳差角速度
  dqi(4)=0.0;                                                %章動角速度
  dqi(5)=(0.2*(g*r/3)^(1/2))/r;                              %スピン角速度
else   
  qi= [0; -5*r; 0; theta; 0];  % ケース1,2 ; 
  
  %-- initial generalized velocity --
  dqi(3)= ((4*g*cot(pi/3))/(r*cos(pi/3)+6*rho))^(1/2);        
  if qi(4)== pi/3;                                            
    dqi(4)= 0.0;              % ケース1                      %定常歳差 
  else
    dqi(4)= 0.5*dqi(3);       % ケース2                      %非定常歳差 
  end; 
  dqi(5)= -((rho/r)+cos(pi/3))*dqi(3);                        
end;
  
dqi(1)=r*(dqi(3)*cos(qi(4))+dqi(5))*cos(qi(3))...
       -r*dqi(4)*sin(qi(3))*sin(qi(4));                      %X方向の速度
dqi(2)=r*(dqi(3)*cos(qi(4))+dqi(5))*sin(qi(3))...
       +r*dqi(4)*cos(qi(3))*sin(qi(4));                      %Y方向の速度

fqi=[qi(1); qi(2); qi(3); qi(4); qi(5);...
     dqi(1); dqi(2); dqi(3); dqi(4); dqi(5)]  ;              %初期値

disp(' Initial genaralized displacement ')
disp(qi)
disp(' Initial genaralized velocity ')
disp(dqi)
     
t=t0;
tic
%----------------------------------------------------------
%                  Runge-Kutta-Gill calculation
%----------------------------------------------------------
hold on
n=1: tf/h+1;                                                 %解析回数
icount=0;
for j=1:10;
  cq0(j)=0.0;                                                %補助量
  cq4(j)=0.0;
  xa(j)=0.0;                                                 %初期条件
  x0(j)=xa(j);
  xa(j)=0.0;
end;
%
xxx=zeros(1,length(n));
yyy=zeros(1,length(n));
phi3=zeros(1,length(n));                  %
theeta4=zeros(1,length(n));
for ii=1:tf/h+1;
  t=t0+(ii-1)*h; 
  for j=1:10;
    cq0(j)=cq4(j);
    x0(j)=xa(j);
    xa(j)=0.0;
  end;
  if ii==1;
    for j=1: 10;
      x0(j)=fqi(j);
    end;
  end;
  [xa,cq4]=gillCoin(t, x0, r, m, g, rho, k, h, cq0);          %Runge-Kutta-Gill

   
  %***** data save *****  
  xxx(1,ii)=xa(:,1);
  yyy(1,ii)=xa(:,2);
  phi3(1,ii)=xa(:,3);                    %
  theeta4(1,ii)=xa(:,4);

end;
toc

%----------------------------------------------------------
%                  Output figures of result
%----------------------------------------------------------
%
fs=18;                                                         %フォントサイズ指定
figure(1)
%-- Position of the center of the disk as a function of time --%重心の位置
%
plot(n*h, xxx, '-', n*h, yyy, '-.');                           %t-x,yプロット
ax1=gca;
set(ax1,'XColor','k','YColor','k','fontsize',fs-2);            %軸の線の色、フォントサイズ
xlim([0,max(h*n)]);                                            %x軸の幅
legend('x','y',2);                                             %凡例
grid on                                                        %グリットライン表示
title('Position of the center of the disk as a function of time','fontsize',fs); %タイトル
xlabel('Time (s)','fontsize',fs);                              %x軸のラベル
ylabel('Position (m)','fontsize',fs);                          %y軸のラベル

figure(2)
%--- Nutation and precession angles as a function of time --- %章動角と歳差角のプロット
%
hl1=plot(n*h, theeta4,'-.');                                  %Nutationプロット
ax1= gca;
set(ax1,'XColor','k','YColor','k','fontsize',fs-2);
xlim([0,max(h*n)]);
title('Nutation and precession angles as a function of time','fontsize',fs);
grid on
xlabel('Time (s)','fontsize',fs);   
ylabel('Nutation, \theta','fontsize',fs);

ax2= axes('Position',get(ax1,'Position'), ...
'YaxisLocation','right', ...
'Color','none', ...
'XColor','k','YColor','k');
hl2= line(n*h, phi3, 'Parent',ax2);            %             %Precessionプロット
set(ax2,'XColor','k','YColor','k','fontsize',fs-2);
xlim([0,max(h*n)]);
legend([hl1,hl2],'\theta','\phi',2);
ylabel('Precession, \phi','fontsize',fs);

figure(3)
%--- Path of the center of the disk ---                      %中心点の経路のプロット
plot(xxx, yyy)                                               %中心点プロット
grid on
title('Path of the center of the disk','fontsize',fs);
xlabel('X coordinate (m)','fontsize',fs);  
ylabel('Y coordinate (m)','fontsize',fs);
axis square;


%
%  ■　コインの運動のまとめのプロット
%
figure(4)
subplot(221);
plot(n*h, xxx, n*h, yyy);

subplot(222);
plot(n*h, theeta4);

subplot(223);
plot(n*h, phi3);               %

subplot(224);
plot(xxx, yyy);


