function FlyingRod
% function FlyingRod
% 
% 本プログラムは以下の問題用プログラムである。
% 5章　柔軟多体系の平面動力学解析
% 5.5　ANC法による2次元動力学解析
% 5.5.3　例題および解析例
% (2)フライング・フレキシブル・ロッド問題
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 高橋 義考 All Rights Reserved.

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

matlab_ver=ver('MATLAB');	% MATLABのバージョン確認
if str2num(matlab_ver.Version(1))>6
	'feature accel off'
	feature accel off;
end


clear all;

element_num=10;     %要素分割数の設定

LL=10;               %全体の梁の長さ設定

L=LL/element_num;   %１要素の長さ計算


%---一般化座標 q の初期値設定---

for i=1:(element_num+1)
    q((i-1)*4+1,1)=(i-1)*L;
    q((i-1)*4+2,1)=0;
    q((i-1)*4+3,1)=1;
    q((i-1)*4+4,1)=0;
end

%---速度 qd の初期値設定---

for i=1:(element_num+1)
    qd((i-1)*4+1,1)=0;
    qd((i-1)*4+2,1)=0;
    qd((i-1)*4+3,1)=0;
    qd((i-1)*4+4,1)=0;
end

%---一般化外力ベクトル Q の設定---

Q=zeros((element_num+1)*4,1);

%---質量行列 M の設定---

A=1.0d-2*1.0d-2;    %断面積
ro=7850d0;          %密度
aro=A*ro;           %断面積×密度
M=zeros((element_num+1)*4,(element_num+1)*4);
Mi=anc_M(L,aro);    %1要素の質量行列
for in=1:element_num
    for i=1:8
        for j=1:8
            M((in-1)*4+i,(in-1)*4+j)=M((in-1)*4+i,(in-1)*4+j)+Mi(i,j);  %行列の重ね合わせ
        end
    end
end

%---質量行列の逆行列計算---

M_inv=inv(M);


%---数値計算の設定---

figure;
wait_fig=waitbar(0,'Please wait ...');

t=0;                %計算開始時間
t_end=10.1;          %計算終了時間
dt=1.0d-3;          %積分ステップ時間
t_save=1;  %データ保存ステップ時間
dat_x=[];           %データ保存用変数設定
dat_y=[];           %データ保存用変数設定
dat_t=[];           %データ保存用変数設定
dat_q=[];           %データ保存用変数設定
dat_qd=[];          %データ保存用変数設定
i_save=fix(t_save/dt);  %データ保存用変数計算
t_count=fix(t_end/dt);  %数値積分回数計算
i_rk_save=1;            %データ保存用カウンター

%---数値計算（Runge-Kutta法）---

for i_rk=1:t_count
    
    waitbar(i_rk/t_count,wait_fig);    
    [qdd1,qd1]=fafv(t,q,qd,L,element_num,Q,M_inv);
    
    q_rk=0.5*dt*qd1;
    qd_rk=0.5*dt*qdd1; 
    [qdd2,qd2]=fafv(t+0.5*dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv);
    
    q_rk=0.5*dt*qd2;
    qd_rk=0.5*dt*qdd2;           
    [qdd3,qd3]=fafv(t+0.5*dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv);

    q_rk=dt*qd3;
    qd_rk=dt*qdd3;          
    [qdd4,qd4]=fafv(t+dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv);    

    t=t+dt;
    q=q+dt/6.0*(qd1+2*qd2+2*qd3+qd4);
    qd=qd+dt/6.0*(qdd1+2*qdd2+2*qdd3+qdd4);

    %途中経過出力      
        if i_rk_save==i_save;
            t
            clf;plot(q(1:4:(element_num+1)*4,1),q(2:4:(element_num+1)*4,1),'ro-');axis('equal');
            drawnow;
            dat_x=[dat_x;q(1:4:(element_num+1)*4,1)'];
            dat_y=[dat_y;q(2:4:(element_num+1)*4,1)'];
            dat_t=[dat_t;t];
            dat_q=[dat_q;q'];
            dat_qd=[dat_qd;qd'];
            i_rk_save=0;
        end
        
    i_rk_save=i_rk_save+1;      
        
end

close(wait_fig);

%---データの保存---
save dat_x dat_x;   %全節点のX座標データ保存
save dat_y dat_y;   %全節点のY座標データ保存
save dat_t dat_t;   %時間データの保存
save dat_q dat_q;   %一般化座標の保存
save dat_qd dat_qd; %速度データの保存

%---結果のグラフ出力---
figure;
for i=1:10;hold on;plot(dat_x(i,:),dat_y(i,:),'.-');axis('equal');end


%--------------------------------------------------------------------------
%　以下は解析に必要な関数定義である。
%--------------------------------------------------------------------------

function [qdd,qd]=fafv(t,q,qd,L,element_num,Q,M_inv);
%function [qdd,qd]=fafv(t,q,qd,L,element_num,Q,M_inv,I_inv)
%
%一般化座標、速度、質量行列の逆行列を引数として、
%運動方程式から加速度を求める関数プログラムである。
%例題では梁両端のXY座標を固定するため、DAEとして
%解を求めている。また、拘束誤差の発散を防ぐために、
%Gearの方法を適用している。
%
%  t                時間
%  q                一般化座標
%  qd               一般化座標の速度
%  qdd              一般化座標の加速度
%  L                １要素の長さ
%  element_num      要素分割数
%  Q                一般化外力ベクトル
%  M_inv            質量行列の逆行列
%  I_inv            Gear法に用いる行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
%
%
%
%---弾性力の計算---
Qk=zeros((element_num+1)*4,1);
for j=1:element_num
    Qk_i=anc_K(q((j-1)*4+1:(j-1)*4+8,1),L);
    for i=1:8
        Qk((j-1)*4+i,1)=Qk((j-1)*4+i,1)+Qk_i(i,1);
    end
end
       
%---外力の設定--- 
if t<=2.5;
    Q(element_num*4+1,1)=4.8;
    Q(element_num*4+2,1)=6.4;
    Q(element_num*4+3,1)=-80*(-q(element_num*4+4,1))/sqrt(q(element_num*4+3,1)^2+q(element_num*4+4,1)^2);
    Q(element_num*4+4,1)=-80*q(element_num*4+3,1)/sqrt(q(element_num*4+3,1)^2+q(element_num*4+4,1)^2);
else
    Q(element_num*4+1,1)=0;
    Q(element_num*4+2,1)=0;
    Q(element_num*4+3,1)=0;
    Q(element_num*4+4,1)=0;
end

%---加速度と速度の計算
qdd=M_inv*(Q-Qk);
qd=qd;



%------------------
%　関数定義
%------------------

function Qk=anc_K(q,L)
%function Qk=anc_K(q,L)
%
%一般化座標、梁要素の長さを引数とし、梁要素の弾性力
%を求める関数プログラム
%
%  q        一般化座標
%  L        １要素の長さ
%  Qk       梁要素の弾性力
%  EA       縦弾性係数×断面積
%  EI       縦弾性係数×断面２次モーメント
%    
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
EA=10000;
EI=500;

K_T=[12 0 6*L 0 -12 0 6*L 0; ...
   0 12 0 6*L 0 -12 0 6*L; ...
   6*L 0 4*L*L 0 -6*L 0 2*L*L 0; ...
   0 6*L 0 4*L*L 0 -6*L 0 2*L*L; ...
   -12 0 -6*L 0 12 0 -6*L 0; ...
   0 -12 0 -6*L 0 12 0 -6*L; ...
   6*L 0 2*L*L 0 -6*L 0 4*L*L 0; ...
   0 6*L 0 2*L*L 0 -6*L 0 4*L*L];

K_T=K_T*EI/L/L/L;

Ld=sqrt((q(1,1)-q(5,1))^2+(q(2,1)-q(6,1))^2);

K_L=[1 0 0 0 -1 0 0 0; ...
   0 1 0 0 0 -1 0 0; ...
   0 0 0 0 0 0 0 0; ...
   0 0 0 0 0 0 0 0; ...
   -1 0 0 0 1 0 0 0; ...
   0 -1 0 0 0 1 0 0; ...
   0 0 0 0 0 0 0 0; ...
   0 0 0 0 0 0 0 0];

K_L=(EA*(Ld-L)/L)/Ld*K_L;

Qk=(K_T+K_L)*q;



%------------------
%　関数定義
%------------------

function M=anc_M(el,aro)
%function M=anc_M(el,aro)
%
%梁要素の質量行列を求める関数プログラム
%
%  el       梁要素の長さ
%  aro      断面積×密度
%  iro      断面２次モーメント×密度
%  M　      梁要素の質量行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
aro=1;
iro=10;

    fm1(1,1)=13.0d0/35.0d0;
	fm1(1,2)=0.0d0;
	fm1(1,3)=11.0d0/210.0d0*el;
	fm1(1,4)=0.0d0;
	fm1(1,5)=9.0d0/70.0d0;
	fm1(1,6)=0.0d0;
	fm1(1,7)=-13.0d0/420.0d0*el;
	fm1(1,8)=0.0d0;
	fm1(2,1)=0.0d0;
	fm1(2,2)=13.0d0/35.0d0;
	fm1(2,3)=0.0d0;
	fm1(2,4)=11.0d0/210.0d0*el;
	fm1(2,5)=0.0d0;
	fm1(2,6)=9.0d0/70.0d0;
	fm1(2,7)=0.0d0;
	fm1(2,8)=-13.0d0/420.0d0*el;
	fm1(3,1)=11.0d0/210.0d0*el;
	fm1(3,2)=0.0d0;
	fm1(3,3)=el*el/105.0d0;
	fm1(3,4)=0.0d0;
	fm1(3,5)=13.0d0/420.0d0*el;
	fm1(3,6)=0.0d0;
	fm1(3,7)=-el*el/140.0d0;
	fm1(3,8)=0.0d0;
	fm1(4,1)=0.0d0;
	fm1(4,2)=11.0d0/210.0d0*el;
	fm1(4,3)=0.0d0;
	fm1(4,4)=el*el/105.0d0;
	fm1(4,5)=0.0d0;
	fm1(4,6)=13.0d0/420.0d0*el;
	fm1(4,7)=0.0d0;
	fm1(4,8)=-el*el/140.0d0;
	fm1(5,1)=9.0d0/70.0d0;
	fm1(5,2)=0.0d0;
	fm1(5,3)=13.0d0/420.0d0*el;
	fm1(5,4)=0.0d0;
	fm1(5,5)=13.0d0/35.0d0;
	fm1(5,6)=0.0d0;
	fm1(5,7)=-11.0d0/210.0d0*el;
	fm1(5,8)=0.0d0;
	fm1(6,1)=0.0d0;
	fm1(6,2)=9.0d0/70.0d0;
	fm1(6,3)=0.0d0;
	fm1(6,4)=13.0d0/420.0d0*el;
	fm1(6,5)=0.0d0;
	fm1(6,6)=13.0d0/35.0d0;
	fm1(6,7)=0.0d0;
	fm1(6,8)=-11.0d0/210.0d0*el;
	fm1(7,1)=-13.0d0/420.0d0*el;
	fm1(7,2)=0.0d0;
	fm1(7,3)=-el*el/140.0d0;
	fm1(7,4)=0.0d0;
	fm1(7,5)=-11.0d0/210.0d0*el;
	fm1(7,6)=0.0d0;
	fm1(7,7)=el*el/105.0d0;
	fm1(7,8)=0.0d0;
	fm1(8,1)=0.0d0;
	fm1(8,2)=-13.0d0/420.0d0*el;
	fm1(8,3)=0.0d0;
	fm1(8,4)=-el*el/140.0d0;
	fm1(8,5)=0.0d0;
	fm1(8,6)=-11.0d0/210.0d0*el;
	fm1(8,7)=0.0d0;
	fm1(8,8)=el*el/105.0d0;

    fm1=fm1*aro*el;
 
    fme1=(6.0d0/5.0d0)/el;
	fme2=0.10d0;
	fme3=(2.0d0/15.0d0)*el;
	fme4=(-1.0d0/30.0d0)*el;
	
	fm2(1,1)=fme1;
	fm2(1,2)=0.0d0;
	fm2(1,3)=fme2;
	fm2(1,4)=0.0d0;
	fm2(1,5)=-fme1;
	fm2(1,6)=0.0d0;
	fm2(1,7)=fme2;
	fm2(1,8)=0.0d0;
	fm2(2,1)=0.0d0;
	fm2(2,2)=fme1;
	fm2(2,3)=0.0d0;
	fm2(2,4)=fme2;
	fm2(2,5)=0.0d0;
	fm2(2,6)=-fme1;
	fm2(2,7)=0.0d0;
	fm2(2,8)=fme2;
	fm2(3,1)=fme2;
	fm2(3,2)=0.0d0;
	fm2(3,3)=fme3;
	fm2(3,4)=0.0d0;
	fm2(3,5)=-fme2;
	fm2(3,6)=0.0d0;
	fm2(3,7)=fme4;
	fm2(3,8)=0.0d0;
	fm2(4,1)=0.0d0;
	fm2(4,2)=fme2;
	fm2(4,3)=0.0d0;
	fm2(4,4)=fme3;
	fm2(4,5)=0.0d0;
	fm2(4,6)=-fme2;
	fm2(4,7)=0.0d0;
	fm2(4,8)=fme4;	
	fm2(5,1)=-fme1;
	fm2(5,2)=0.0d0;
	fm2(5,3)=-fme2;
	fm2(5,4)=0.0d0;
	fm2(5,5)=fme1;
	fm2(5,6)=0.0d0;
	fm2(5,7)=-fme2;
	fm2(5,8)=0.0d0;
	fm2(6,1)=0.0d0;
	fm2(6,2)=-fme1;
	fm2(6,3)=0.0d0;
	fm2(6,4)=-fme2;
	fm2(6,5)=0.0d0;
	fm2(6,6)=fme1;
	fm2(6,7)=0.0d0;
	fm2(6,8)=-fme2;
	fm2(7,1)=fme2;
	fm2(7,2)=0.0d0;
	fm2(7,3)=fme4;
	fm2(7,4)=0.0d0;
	fm2(7,5)=-fme2;
	fm2(7,6)=0.0d0;
	fm2(7,7)=fme3;
	fm2(7,8)=0.0d0;
	fm2(8,1)=0.0d0;
	fm2(8,2)=fme2;
	fm2(8,3)=0.0d0;
	fm2(8,4)=fme4;
	fm2(8,5)=0.0d0;
	fm2(8,6)=-fme2;
	fm2(8,7)=0.0d0;
	fm2(8,8)=fme3;

    fm2=fm2*iro;  
    M=fm1+fm2;
    
