% FM2d2main.m
% 
% 更新枠法による2次元解析のメイン部分
% 呼び出される関数：Beam2d2.m
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 藤川 猛 All Rights Reserved.

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

clear all
global Ndof Numelm  Fint KT  epsB LamdB L0 EA EI ;

%----- 解析データ設定 ------------------------------
Ndof=9; Numelm=2;NumP=3;                %自由度数，はり要素数，節点数の設定
mass=10;Jmass=0.001;Spri=1.e9;Grav=9.8; %質量，慣性モーメント，ばね定数，重力加速度
EA0=200e9*3.14e-4;EI0=200e9*0.785e-8;   %伸び剛性，曲げ剛性
iTend=25;itsL=3;                        %ステップ数
h=0.02;                                 %時間刻み

%-----マトリックス，定数など設定 --------    
h2=h/2;hh2=h*h/2;hhb=h*h/4;
LamdB=zeros(3,3,2);Lelm=zeros(3,1);
Vzero=zeros(Ndof,1);Mzero=zeros(Ndof,Ndof);Tim=[];Pos=[];
 DM=Mzero; DC=Mzero;DK=Mzero; KT=Mzero;
 A=Vzero; A1=Vzero; V1=Vzero; D1=Vzero;
 F1=Vzero; Ger=Vzero; Fint=Vzero;
 L0=[1 1]; EA=[EA0 EA0]; EI=[EI0 EI0];
KL=Mzero;KL(1,1)=Spri;KL(2,2)=Spri;
for Np=1:NumP;is=(Np-1)*3;
   DM(is+1,is+1)=mass ;DM(is+2,is+2)=mass ;DM(is+3,is+3)=Jmass;
   F1(is+1)=-mass*Grav;
end

%---初期値---   
    Q0=[0 0 0  -1 0 0  -1 1 0]';
    LamdB(:,:,1)=[0 1 0;-1 0 0;0 0 1];
    LamdB(:,:,2)=[1 0 0; 0 1 0; 0 0 1]; 
    epsB=zeros(3,3);
    D=Vzero; V=Vzero;  Qb=Q0+D;
    A=inv(DM)*(F1-DC*V-KL*D); 
   
% ========== 時間積分 ============================================-
for it=1:iTend;
    Tim=[Tim;it*h];Pos=[Pos;Qb'];
for its=1:itsL
    A1=A;
    V1=V+h.*A;
    D1=D+h.*V+hh2.*A;               %近似値設定
    Qa=Q0+D1; 
 for inew=1:15;                     %ニュートン反復
    [Fint,KT]=Beam2d2(Qa,Qb);       %要素力，剛性マトリックス計算
     Ger=DM*A1 +DC*V1 +KL*D1+Fint-F1;%誤差
        gerr=sqrt(dot(Ger,Ger));
         if gerr<=0.0001; break;end
     J=DM+h2.*DC+hhb.*(KL+KT);      %ヤコビアン
     da=-inv(J)*Ger;
         Qb=Qa;
         A1=A1+da;     V1=V1+h2.*da;     D1=D1+hhb.*da;  %修正
         Qa=Q0+D1;
   end   
 % gosa=sprintf('gosa=  %d  %d  %e  ',it,inew,gerr);
    A=A1;    V=V1;    D=D1; Qb=Qa;  %更新
end;end
% ==========時間積分終了========================================
% ----------結果の表示------
hold off
for it=1:iTend
    PPX=[Pos(it,1) Pos(it,4) Pos(it,7)];
    PPZ=[Pos(it,2) Pos(it,5) Pos(it,8)];
    plot(PPZ,PPX); hold on
end
   axis([-1.1 1.1 -2.0 0.2]);
