% top3d_augm_script_ver5.m
%
%
%  対称なこまの3次元回転運動の解析（拡大法）
%
% 呼び出される関数
% top3d_augm_ode_ver5.m
% tilde.m
%
%　　プログラム全体の説明
%
%  ・支点をボールジョイントで拘束されたこま
%  ・回転姿勢にオイラー角φθψを使用
%  ・SI単位(メートル、キログラム、秒)を使用
%  ・z軸の負の方向に重力が働く
%  ・こまの回転軸は局所 z 軸に取る．こまの支点位置は原点とする．
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 井上 剛志, 竹原 昭一郎 All Rights Reserved.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　　プログラム中の記号の説明
%
%  rG     :全体枠から見た質量中心の位置
%  thOG   :こまの姿勢を表すオイラー角φθψ（zxz変換）
%  AOG    :座標変換マトリックス
%  omOGd  :物体固定枠成分表示における角速度
%  DthOG  :thOGの時間微分（一般化速度）
%  DOmega :物体固定枠成分表示におけるomOGdの時間微分
%  M      :こまの質量マトリックス
%  Jd     :こまに固定された基準枠から見たこまの質量中心まわりの慣性マトリックス
%  invJd  :Jdの逆マトリックス
%  g      :重力加速度
%  c      :質量中心まわりの回転に関する減衰係数マトリックス
%  nd     :回転運動に対する減衰トルクに起因する外トルク
%  fa     :こまの質量中心に作用する外力（重力のみ）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(a) MATLABプログラムの前処理
%
%　　　ワークスペース内の関数や変数を全てクリア
%
clear all;
close all;
clear functions;
global fa c M invM Jd invJd rGCdtilde rGCd Alpha Beta;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(b) パラメータの入力
%
%  ★こまの形状パラメータ　　　　（入力）★
%
%
r=0.04;             % こまの質量部分の半径[m]
h=0.02;             % こまの質量部分の高さ[m]
m=r^2*pi*h;         % こまの質量[kg]
Jd=[m*(3*r^2+h^2)/12.0 0 0;0 m*(3*r^2+h^2)/12.0 0;0 0 m*r^2/2.0];
                    % 質量中心まわりの慣性マトリックス[kg m^2]
LGC=0.03;           % こまの長さ[m]%
%
%  ★こまの動的パラメータ　　　　（入力）★
%
cx=0;  %  x'''軸まわりの角速度成分に対する質量中心まわりの回転に関する減衰係数 [Nm/(rad/sec)]
cy=0;  %  y'''軸まわりの　　　〃
cz=0;  %  z'''軸まわりの　　　〃
%
%  ★こまの初期オイラー角，初期オイラー角の時間微分　（入力）★
%
fai0=0;             % 初期オイラー角φ0[rad]
theta0=pi/5;        % 　　　〃　　　θ0[rad]
psi0=0;             % 　　　〃　　　ψ0[rad]
%
dfai0=pi;           % 初期オイラー角の時間微分 dφ/dt[rad/sec]
dtheta0=0;          % 　　　〃　　　 　　　　　dθ/dt[rad/sec]
dpsi0=1000/60*2*pi; % 　　　〃　　　 　　　　　dψ/dt[rad/sec]
%
%  ★シミュレーション時間　　　　（入力）★
%
ts=0;               % 計算開始時間[sec]
dt=0.01;            % 計算結果出力刻み[sec]
te=4;               % 計算終了時間[sec]
%
%  ★バウムガルテの安定化法　　　（入力）★
%
Alpha=10;
Beta=10;
%
%  ★アニメーション用こまの形状設定（入力）★
%
Topshape1 =[0,0.5,0.5,3.0,3.0,0.5,0.5,0];%
[XXX,YYY,ZZZ]=cylinder(Topshape1,32);
ZZZ(2,:)=0.5;
ZZZ(3,:)=2.5;
ZZZ(4,:)=2.5;
ZZZ(5,:)=3.5;
ZZZ(6,:)=3.5;
ZZZ(7,:)=4.5;
ZZZ(8,:)=4.5;
%
%　こまの回転をわかりやすく示すようにこまに突起をつける
%
[iend,jend]=size(XXX);
XXX(4,1)=XXX(4,1)+0.3;
XXX(5,1)=XXX(5,1)+0.3;
XXX(4,end)=XXX(4,end)+0.3;
XXX(5,end)=XXX(5,end)+0.3;
%
%  パラメータの入力終了
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(c) 運動方程式中の定数の決定
%
g=9.81;                        % 重力加速度[m/s^2]
fa=[0 ; 0; -m*g];              % こまの質量中心に作用する外力（重力のみ）
M=m*eye(3);                    % こまの質量マトリックス
invM=inv(M);                   % Mの逆マトリックス
rGCd=[0; 0; -LGC];             % こまの座標系Gで見たこまの支点Cの位置
invJd=inv(Jd);                 % Jdの逆マトリックス
rGCdtilde=tilde(rGCd);         % rGCdから作った歪み対称マトリックス
c=[cx  0 0; 0 cy 0; 0  0 cz];  % 質量中心まわりの回転に関する減衰係数マトリックス
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%　■　(d) 状態変数の初期状態の計算
%
% 式(4.36)
rG=[LGC*sin(theta0)*sin(fai0); -LGC*sin(theta0)*cos(fai0); LGC*cos(theta0)];
%
% 式(4.36)
vG=[	LGC*cos(theta0)*sin(fai0)*dtheta0+LGC*sin(theta0)*cos(fai0)*dfai0;
	-LGC*cos(theta0)*cos(fai0)*dtheta0+LGC*sin(theta0)*sin(fai0)*dfai0;
	-LGC*sin(theta0)*dtheta0];
%
thOG=[fai0; theta0; psi0];     % 初期回転姿勢（オイラー角φθψ）
DthOG=[dfai0; dtheta0; dpsi0]; % オイラー角の時間微分
%
% 角速度をオイラー角の時間微分で表す．      基式(5.53)
%
Ged=[sin(theta0)*sin(psi0)  cos(psi0)   0;
     sin(theta0)*cos(psi0) -sin(psi0)   0;
     cos(theta0)            0           1];
%
omOGd=Ged*DthOG;                % 初期角速度
%
y0=[rG; vG; thOG; omOGd];     % 状態変数の初期値
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(f) 微分方程式の積分計算
%
[tt,YY]=ode45('top3d_augm_ode_ver5',ts:dt:te,y0);%
%
%  ・こまの運動の計算
%  ・出力ttは、tsからteまでdtキザミの時間が入った列マトリックス（n行1列）
%  ・出力YYは、各時間に対応したこまの質量中心位置，質量中心の速度，オイラー角,角速度を含むマトリックス（n行12列）
%
%  []にはオプションを入れることもできる．たとえば，相対誤差（1.0e-3）と絶対誤差（1.0e-6）
%　あるいは，イベント検出（なにか条件を与えると，それが発生した時点で終了）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(h) グラフ出力処理
%
ntt=length(tt);     % length：ベクトルの長さ　⇒　この場合は出力時間の数
%
%  計算結果出力（質量中心の時刻歴：図4.2）
%
figure(1);          %  ウィンドウ作成
set(1,'Position',[5 440 500 250]);
plot(tt,YY(:,1),'-k',tt,YY(:,2),':k',tt,YY(:,3),'--k');
                    %  時間－質量中心X,Y,Z座標を描画
xlabel('Time (s)')
ylabel('Displacement x,y,z (m)')
legend('x of center of mass','y of center of mass','z of center of mass')
%
%  計算結果出力（オイラー角θの時刻歴：図4.3）
%
figure(2);          %  ウィンドウ作成
set(2,'Position',[515 440 500 250]);
plot(tt,YY(:,8),'k');	%  時間－オイラー角θを描画
axis([0 4 0 pi]);
xlabel('Time (s)')
ylabel('Euler angle theta (rad)')
legend('Eular angle theta')
%
%  計算結果出力（オイラー角φ,ψの時刻歴：図4.4）
%
figure(3);          % ウィンドウ作成
set(3,'Position',[5 110 500 250]);
plot(tt,YY(:,7),'-k',tt,YY(:,9),':k');
                    %  時間－オイラー角φ,ψを描画
xlabel('Time (s)')
ylabel('Euler angle phi,psi (rad)')
legend('Eular angle phi','Eular angle psi')
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ■　(i) こまの運動のアニメーション
%
% アニメ用出力（図4.5），4つのウインドウの視点を表示する
%
figure(4);
set(4,'Position',[515 35 500 500]);
grid on;
%
subplot(221);
surf(XXX,YYY,ZZZ);
view([45 60 25]);
axis([-5 5 -5 5 0 10]);
xlabel('x');
ylabel('y');
zlabel('z');
%
subplot(222);
surf(XXX,YYY,ZZZ);
view([0 90 0]);
axis([-5 5 -5 5 0 10]);
xlabel('x');
ylabel('y');
zlabel('z');
%
subplot(223);
surf(XXX,YYY,ZZZ);
axis([-5 5 -5 5 0 10]);%
view([90 0 0]);
xlabel('x');
ylabel('y');
zlabel('z');
%
subplot(224);
surf(XXX,YYY,ZZZ);
axis([-5 5 -5 5 0 10]);%
xlabel('x');
ylabel('y');
zlabel('z');
%
view([180 90]);
%
%  アニメ用出力（図4.6），4つの視点から見たこまの運動を表示する
%
figure(5);
set(5,'Position',[225 55 600 600]);
axis([-5 5 -5 5 0 10]);%
grid on;%
%
for itt=1:ntt
    %  回転マトリックス(式(4.35), 基式(4.50))
    Cos1=cos(YY(itt,7));         % cos(φ)
    Cos2=cos(YY(itt,8));         % cos(θ)
    Cos3=cos(YY(itt,9));         % cos(ψ)
    Sin1=sin(YY(itt,7));         % sin(φ)
    Sin2=sin(YY(itt,8));         % sin(θ)
    Sin3=sin(YY(itt,9));         % sin(ψ)
    %
	AOG=[	Cos1*Cos3-Sin1*Cos2*Sin3  -Cos1*Sin3-Sin1*Cos2*Cos3   Sin1*Sin2;%
		Sin1*Cos3+Cos1*Cos2*Sin3  -Sin1*Sin3+Cos1*Cos2*Cos3  -Cos1*Sin2;%
		Sin2*Sin3                  Sin2*Cos3                  Cos2];
%
	for i=1:iend
		for j=1:jend
			RRR=(AOG*[XXX(i,j);YYY(i,j);ZZZ(i,j)])';
			XXXX(i,j)=RRR(1);
			YYYY(i,j)=RRR(2);
			ZZZZ(i,j)=RRR(3);
		end
	end
%
%
	figure(5);
	subplot(221);
	surf(XXXX,YYYY,ZZZZ);
	axis([-5 5 -5 5 0 10]);%
	xlabel('x');
	ylabel('y');
	zlabel('z');
	view([45 60 25]);
%
	subplot(222);
	surf(XXXX,YYYY,ZZZZ);
	axis([-5 5 -5 5 0 10]);%
	view([0 90 0]);
	xlabel('x');
	ylabel('y');
	zlabel('z');
%
	subplot(223);
	surf(XXXX,YYYY,ZZZZ);
	axis([-5 5 -5 5 0 10]);%
	view([90 0 0]);
	xlabel('x');
	ylabel('y');
	zlabel('z');
%
	subplot(224);
	surf(XXXX,YYYY,ZZZZ);
	axis([-5 5 -5 5 0 10]);%
	view([180 90]);
	xlabel('x');
	ylabel('y');
	zlabel('z');  
%
	drawnow;
end
%
