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

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

clear all
global Ndof Numelm  Fint KT  L0 EA0 EI0 alpha0;
%----- 解析データ設定 ------------------------------
    Ndof=9; Numelm=2; NumP=3;               %自由度数，はり要素数，節点数の設定
    mass=10;Jmass=0.001;Spri=1.e9;Grav=9.80;%質量，慣性モーメント，ばね定数，重力加速度
    EA=200e9*3.14e-4;EI=200e9*0.785e-8;     %伸び剛性，曲げ剛性
    iTend=25;itsL=5;                        %ステップ数
    h=0.02;                                 %時間刻み
    
%-----マトリックス，定数など設定 --------    
 h2=h/2;hh2=h*h/2;hhb=h*h/4;
 Vzero=zeros(Ndof,1);Mzero=zeros(Ndof,Ndof);Tim=[];Qsol=[];Vsol=[];
 KT=Mzero; Time=0; gerr=0; inew=0;
 A1=Vzero; V1=Vzero; D1=Vzero;F1=Vzero;
 L0=[1 1]; EA0=[EA EA]; EI0=[EI EI];
   DM=diag([mass mass Jmass mass mass Jmass mass mass Jmass]);
   DC=Mzero;
   KL=Mzero;DK=Mzero;KL(1,1)=Spri;KL(2,2)=Spri;
   mg=mass*Grav;F1=[0 -mg 0 0 -mg 0 0 -mg 0]';
   
%---初期値---   
    Q0=[0 0 0 0 -1 0 1 -1.001 0]';
    alpha0=[1.5*pi 1.5*pi; 2*pi 2*pi];
    V=Vzero; D=Vzero;Q=Q0+D;
    A=inv(DM)*(F1-DC*V-KL*D);       %初期加速度

% ========== 時間積分 ============================================-
for it=1:iTend;
Tim=[Tim;it Time inew];Qsol=[Qsol;Q'];Vsol=[Vsol;V'];   %出力用データ保存　
for its=1:itsL  
    Time=Time+h;
    A1=A;
    V1=V+h.*A;
    D1=D+h.*V+hh2.*A;
    Q1=Q0+D1; dQ=(D1-D);
  for inew=1:10;                            %ニュートン反復
    [Fint,KT]=Beam2d1(Q1);                  %要素力，剛性マトリックス計算
     Ger=DM*A1 +DC*V1 +KL*D1+Fint-F1;       %誤差
        gerr=sqrt(dot(Ger,Ger));
        if gerr<=1e-4; break;end
     J=DM+h2.*DC+hhb.*(KL+KT);              %ヤコビアン 
     da=-inv(J)*Ger;
         A1=A1+da;     V1=V1+h2.*da;   D1=D1+hhb.*da;   %修正
         dQ=hhb.*da;   Q1=Q1+dQ;
  end   
   % gosa=sprintf('gosa=  %d  %d  %e  ',it,inew,gerr)   %誤差の表示
    A=A1;    V=V1;    D=D1;  Q=Q1;                      %更新
end;end
% ==========時間積分終了========================================
%----- -----結果の表示------
hold off
plotcase=1;  %=1のときは運動軌跡　　　=2のときは速度応答の波形
switch plotcase
case 1
  for it=1:iTend
   for j=1:NumP;js=(j-1)*3; PX(j)=Qsol(it,js+1);   PY(j)=Qsol(it,js+2); end
   plot(PX,PY);   hold on
   axis([-1.1 1.1 -2.1 0.1]);
  end
case 2
  plot(Tim(:,2),Vsol(:,7),'r');hold on
  plot(Tim(:,2),Vsol(:,8),'b');hold on
end