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

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

clear all
global Ndof Numelm  Fint KT epsB LamdB L0 EA EIx EIy GJ Lelm INCI;

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

%-----マトリックス，定数など設定 --------    
    h2=h/2;hh2=h*h/2;hhb=h*h/4;
    Vzero=zeros(Ndof,1);Mzero=zeros(Ndof,Ndof);Tim=[];Pos=[];
    DK=Mzero;  KT=Mzero;
    A1=Vzero; V1=Vzero; D1=Vzero;Qb=Vzero;Qa=Vzero;
    F1=Vzero; Ger=Vzero; Fint=Vzero; 

    L0=[1 1]; EA=[EA0 EA0]; EIx=[EI0 EI0];EIy=[EI0 EI0];GJ=[GJ0 GJ0];
    INCI=[1 2;2 3]; LamdB=zeros(3,3,2);Lelm=zeros(3,1);
 KL=Mzero;;KL(1,1)=Spri;KL(2,2)=Spri;KL(3,3)=Spri;
 DC=Mzero;DM=Mzero; 
 for Np=1:NumP;is=(Np-1)*6;
   for j=1:3;DM(is+j,is+j)=mass ;DM(is+3+j,is+3+j)=Jmass;end
   F1(is+1)=-Grav*mass;
 end
   F1(14)=98;
   
%---初期値---   
   Q0=[0 0 0 0 0 0  -1 0 0 0 0 0  -1 0 1 0 0 0]';
   epsB=zeros(6,3);
    LamdB(:,:,1)=[0 0 1;0 1 0;-1 0 0];
    LamdB(:,:,2)=[1 0 0;0 1 0; 0 0 1];   
   V=Vzero; D=Vzero;Qb=Q0+D;
   A=inv(DM)*(F1-DC*V-KL*D); %inital A
   
% ========== 時間積分 ============================================-
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;                   %近似値設定
    Qb=Q0+D ; Qa=Q0+D1; dQ=(D1-D);
  for inew=1:20;                        %ニュートン反復
    [Fint,KT]=Beam3d(Qa,Qb);            %要素力，剛性マトリックス計算
     Ger=DM*A1 +DC*V1 +KL*D1+Fint-F1;   %誤差
        gerr=sqrt(dot(Ger,Ger));
         if gerr<=0.000001; break;end
     J=DM+h2.*DC+hhb.*(KL+KT);          %ヤコビアン
     da=-inv(J)*Ger;
         A1=A1+da;     V1=V1+h2.*da;     D1=D1+hhb.*da; %修正
         Qb=Qa;dQ=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
% ==========時間積分終了========================================
% ---結果の表示---%記号を取り替えることによりY-X,Z-Y面内での軌跡をプロット---
hold off
PPX=[];PPZ=[];
for it=1:iTend
    PPX=[Pos(it,1) Pos(it,7) Pos(it,13)];
    PPY=[Pos(it,2) Pos(it,8) Pos(it,14)];
    PPZ=[Pos(it,3) Pos(it,9) Pos(it,15)];
    plot(PPZ,PPX);hold on
%   plot(PPY,PPX);hold on
%   plot(PPZ,-PPY);hold on
end
   hold on
   axis([-1.10 1.10 -1.7 0.50] );
%  xlabel('x'); ylabel('y'); zlabel('z');
