!DEMプログラム:2D円筒要素のみ
!試しにつくってみる

!初期の座標、半径やその他の材料定数は"syoki.dat"に記入しておく。
!配列の要素数、計算ステップ数は、適宜増やす。

!計算結果(実行↓)は、paraviewで見れるvtkファイルに出力。
!!$ paraview /kekka/kekka..vtk&

!参考にしたのは↓
!桝谷　浩「落石覆工の落籍による衝撃力に関する研究」の第3章,平成元年4月

!!主に宣言とか
      implicit none
!!! kekka:結果の出力ファイル名,k:実数→文字列 時に使用
      character :: kekka*30,k*10
!!! nyou:要素数,nkei:計算回数,id:要素番号
      integer :: nyou,nkei,id(100)
!!! cj,i,j,t:do文で使っただけで、特に意味はない
      integer :: cj,i,j,t
!!! dt:計算間隔,tf:開始時刻,te:終了時刻
      real :: dt,tf,te
!!! g:重力加速度,yuka:床の高さ,hn/hs:減衰係数,s:逓減率
      real :: g,yuka,hn,hs,s
!!! r:半径,rho:密度,moi:慣性モーメント,e:ヤング率,nu:ポアソン比
      real ::  r(100),rho(100),moi(100),e(100),nu(100)
!!! kn/s:バネ定数,cn/s:粘性係数,m:質量,mu:摩擦係数
      real ::  kn,ks,cn,cs,m(100),mu(100)
!!! u:座標,phy:回転(x軸からy軸に畳む方向を正)
      real :: u(100,10001,2),phy(100,10001)
!!! 並進の  v:各要素の速度,a:各要素の加速度
      real :: v(100,10001,2),a(100,10001,2)
!!! 回転の  omega:各要素の角速度,alpha:各要素の角加速度
      real :: omega(100,10001),alpha(100,10001)
!!! du:変位増分,dphy:回転増分(x軸からy軸に畳む方向を正)
      real :: du(100,10001,2),dphy(100,10001)
!!! duns:ns方向の変位増分...nは要素間の法線方向,sは要素間の接線方向
      real :: duns(100,10001,2)
!!! fxy:xy方向の接触力,fns:ns方向(法線と接線)の接触力
      real :: fxy(100,10001,2),fns(100,10001,2)
!!! ens:ns方向の変位に比例した抗力,dns:ns方向の速度に比例した抗力
      real :: ens(100,10001,2),dns(100,10001,2)
!!! mom:モーメント力,fsum:接触力の合計,msum:モーメント力の合計
      real :: mom(100,10001),fsum(100,10001,2),msum(100,10001)
!!! l:各要素の中心距離,pi:円周率(rad)
      real :: l,pi
!!! cosa:cos(alpha),sina:sin(alpha)...alphaはxy座標とns座標のなす角
      real :: cosa,sina

!!手で入力すべし!!!!!!!!!!!!!!!!!!!!!!!!
      tf=0.000 !開始時刻(sec)
      te=2.000 !終了時刻(sec)
      dt=0.001 !計算間隔(sec)
      nyou=3 !要素の個数
      yuka=0.750 !床のx座標(m)...x軸は下向きに正、y軸は右向きに正
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      nkei=int((te-tf)/dt) !計算回数
      pi=2.*asin(1.0) !円周率(rad)
      g=9.81 !重力加速度(m/sec^2)
      hn=0.2 !法線方向の減衰係数(参考文献のまんま)
      hs=1.0 !接線方向の減衰係数(参考文献のまんま)
      s=0.25 !ks=s*knの逓減率s(参考文献のまんま)

!!要素の初期条件を取得
      open(1,file=('syoki.dat'),status='old') !syoki.datはファイル名
      do i=1,2
       read(1,'()') !初めのコメント文をカット(2行だけ)
      end do
      do i=1,nyou
       read(1,*) id(i),u(id(i),1,1),u(id(i),1,2)&
           &,r(id(i)),rho(id(i)),mu(id(i))&
           &,e(id(i)),nu(id(i))
       m(id(i))=4./3.*rho(id(i))*pi*r(id(i))**3. !質量の計算
       moi(id(i))=2./5.*m(id(i))*r(id(i))**2. !(円筒でなく)球の慣性モーメント
      end do

!!床・壁条件を取得

!!要素の初期条件
      do i=1,nyou
!      do i=1,2
       v(i,1,1)=g*dt !1ステップ目のx速度
       v(i,1,2)=0.*dt !1ステップ目のy速度
       omega(i,1+1)=0.*dt !1ステップ目の角速度
       du(i,1,1)=v(i,1,1)*dt !1ステップ目のx変位増分
       du(i,1,2)=v(i,1,2)*dt !1ステップ目のy変位増分
       dphy(i,1)=omega(i,1)*dt !1ステップ目の回転角増分
      end do
!球No.3に初速度を与えてみる
!       v(3,1,1)=-g/2.*dt !1ステップ目のx速度
!       v(3,1,2)=g*2.*dt !1ステップ目のy速度
!       omega(3,1+1)=0.*dt !1ステップ目の角速度
!       du(3,1,1)=v(i,1,1)*dt !1ステップ目のx変位増分
!       du(3,1,2)=v(i,1,2)*dt !1ステップ目のy変位増分
!       dphy(3,1)=omega(i,1)*dt !1ステップ目の回転角増分

!!初期の座標をparaview用のvtkフォーマットに出力
       kekka="kekka/kekka0.vtk" !余白を消す
       open(3,file=(kekka)) 
       write(3,'(26a)') "# vtk DataFile Version 2.0" !決まり文句
       write(3,'(14a)') "kekka0.vtk" !何でもいい
       write(3,'(5a)') "ASCII" !決まり文句
       write(3,'(16a)') "DATASET UNSTRUCTURED_GRID"
!!!座標の出力
      write(k,'(i10)') nyou !整数or実数の文字列化
      write(3,'(20a)') "POINTS "//trim(adjustl(k))//" float" !タイプが点
      do i=1,nyou
       write(3,'(1e11.4,a,1e11.4,$)') u(i,1,1)," ",u(i,1,2)
       write(3,'(a2,f3.1)') "  ",0.0 
      end do !i=1,nyou
!!!タイプの設定
      write(3,'(26a)') "CELL_TYPES "//trim(adjustl(k))
      do i=1,nyou
       write(3,'(i1)') 1
      end do !i=1,nyou
!!!半径の設定
      write(3,'(16a)') "POINT_DATA "//trim(adjustl(k))
      write(3,'(26a)') "SCALARS radius float"
      write(3,'(17a)') "LOOKUP_TABLE default"
      do i=1,nyou
       write(3,'(1e11.4)') r(i)
      end do !i=1,nyou

!!時刻変化
      do t=1,nkei
      print '(a2,f6.3,a3)', "T=",t*dt," 秒"

!!円筒要素同士の衝突判定
      cj=1 !衝突の組み合わせの初期化
      do i=1,nyou
       if (i<nyou) then
        cj=cj+1
        if (cj<=nyou) then
         do j=cj,nyou
          l=sqrt((u(i,t,1)-u(j,t,1))**2&
           &+(u(i,t,2)-u(j,t,2))**2) !要素iとjの中心距離
          if (l<=(r(i)+r(j))) then !衝突
!!円筒要素同士の接触力の計算
           sina=-(u(i,t,2)-u(j,t,2))/l !x軸とu軸がなす角度a(lpha)
           cosa=-(u(i,t,1)-u(j,t,1))/l
           print *, "接触!!",i,"と",j," L=",l
!           print *, "sin(alpha)=",sina,",cos(alpha)=",cosa

           duns(i,t,1)=cosa*(du(i,t,1)-du(j,t,1))&
               &+sina*(du(i,t,2)-du(j,t,2)) !n方向の変位増分
           duns(i,t,2)=-sina*(du(i,t,1)-du(j,t,1))&
               &+cosa*(du(i,t,2)-du(j,t,2))&
               &+dphy(i,t)*r(i)+dphy(j,t)*r(j) !s方向の変位増分
           
!           kn=4./3.*sqrt(duns(i,t,1)/&
!               &((1./r(i)+1./r(j))&
!               &*((1.-nu(i)**2)/e(i)+(1.-nu(j)**2)/e(j))**2)) !ヘルツの接触
           kn=800000 !参考文献のまんま
           ks=s*kn
           cn=2*hn*sqrt(m(id(i))*kn)
           cs=2*hs*sqrt(m(id(i))*ks)

           ens(i,t+1,1)=ens(i,t,1)+kn*duns(i,t,1)
           !n方向の相対変位増分に関する抗力
           dns(i,t+1,1)=cn*duns(i,t,1)/dt
           !n方向の相対速度に比例した抗力
           ens(i,t+1,2)=ens(i,t,2)+ks*duns(i,t,2)
           !s方向の相対変位増分に関する抗力
           dns(i,t+1,2)=cs*duns(i,t,2)/dt
           !s方向の相対速度に比例した抗力
!           print *, ens(i,t+1,1),dns(i,t+1,1),ens(i,t+1,2),dns(i,t+1,2)

           if (ens(i,t,1)<0) then
            fns(i,t,1)=0.
            fns(i,t,2)=0.
           else
            fns(i,t,1)=ens(i,t,1)+dns(i,t,1) !n方向の作用力
            fns(i,t,2)=ens(i,t,2)+dns(i,t,2) !s方向の作用力
            if (abs(ens(i,t,2))<mu(i)*(ens(i,t,1))) then
             fns(i,t,2)=mu(i)*sign(ens(i,t,1),ens(i,t,2))
            end if !摩擦
           end if !離れる時、抵抗しない
           print *, "Fn=",fns(i,t,1),",Fs=",fns(i,t,2)
           fxy(i,t,1)=-cosa*fns(i,t,1)+sina*fns(i,t,2)
           !要素iの法線方向に働く力
           fxy(i,t,2)=-sina*fns(i,t,1)-cosa*fns(i,t,2)
           !要素iの接線方向に働く力
           mom(i,t)=-fns(i,t,2)*r(i)
           !要素iに働くモーメント力
          end if !衝突
           fsum(i,t,1)=fsum(i,t,1)+fxy(i,t,1)
           !要素iの法線方向に働く力の合計
           fsum(i,t,2)=fsum(i,t,2)+fxy(i,t,2)
           !要素iの接線方向に働く力の合計
           msum(i,t)=msum(i,t)+mom(i,t)
           !要素iに働くモーメント力の合計
         end do !j=cj,nyou
        end if !j<=nyou
       end if !i<nyou
!      print *, i,"Σx=",fsum(i,t,1),"Σfy=",fsum(i,t,2),"ΣM=",msum(i,t)
      end do !i=1,nyou
      
!!円筒要素と床・壁面との衝突判定
      do i=1,nyou
!!円筒要素と床の接触力の計算
       if (u(i,t,1)+r(i)>=yuka) then !衝突
        print *, "要素",i,"が床に衝突!"
        duns(i,t,1)=du(i,t,1) !x方向の変位増分
!        duns(i,t,1)=u(i,t,1)+r(i)-yuka !x方向の変位増分
        duns(i,t,2)=du(i,t,2)+dphy(i,t)*r(i) !y方向の変位増分

!        kn=4./3.*sqrt(duns(i,t,1)/&
!            &((1./r(i))*((1.-nu(i)**2)/e(i))**2)) !ヘルツの接触
        kn=800000 !参考文献のまんま
        print *, "kn=",kn
        ks=s*kn
        cn=2*hn*sqrt(m(id(i))*kn)
        cs=2*hs*sqrt(m(id(i))*ks)
           
        ens(i,t+1,1)=ens(i,t,1)+kn*duns(i,t,1)
        !n(x)方向の相対変位増分に関する抗力
        dns(i,t+1,1)=cn*duns(i,t,1)/dt
        !n(x)方向の相対速度に比例した抗力
        ens(i,t+1,2)=ens(i,t,2)+ks*duns(i,t,2)
        !s(y)方向の相対変位増分に関する抗力
        dns(i,t+1,2)=cs*duns(i,t,2)/dt
        !s(y)方向の相対速度に比例した抗力

        if (ens(i,t,1)<0) then
         fns(i,t,1)=0.
         fns(i,t,2)=0.
        else
         fns(i,t,1)=ens(i,t,1)+dns(i,t,1) !n(x)方向の作用力
         fns(i,t,2)=ens(i,t,2)+dns(i,t,2) !s(y)方向の作用力
         if (abs(ens(i,t,2))<mu(i)*(ens(i,t,1))) then
          fns(i,t,2)=mu(i)*sign(ens(i,t,1),ens(i,t,2))
         end if !摩擦
        end if !離れる時、抵抗しない
        print *, "Fn=",fns(i,t,1),",Fs=",fns(i,t,2)
!        fxy(i,t,1)=-cosa*fns(i,t,1)+sina*fns(i,t,2)
        fxy(i,t,1)=-fns(i,t,1)
        !要素iの法線方向に働く力
!        fxy(i,t,2)=-sina*fns(i,t,1)-cosa*fns(i,t,2)
        fxy(i,t,2)=-fns(i,t,2)
        !要素iの接線方向に働く力
        mom(i,t)=-fns(i,t,2)*r(i)
        !要素iに働くモーメント力
        fsum(i,t,1)=fsum(i,t,1)+fxy(i,t,1)
        !要素iの法線方向に働く力の合計
        fsum(i,t,2)=fsum(i,t,2)+fxy(i,t,2)
        !要素iの接線方向に働く力の合計
        msum(i,t)=msum(i,t)+mom(i,t)
        !要素iに働くモーメント力の合計
       end if !床との衝突判定

!!!円筒要素と壁の衝突
       
      end do !i=1,nyou

!!dt後の要素
      do i=1,nyou
       a(i,t+1,1)=fsum(i,t,1)/m(i)+g !x加速度
       a(i,t+1,2)=fsum(i,t,2)/m(i) !y加速度
       alpha(i,t+1)=msum(i,t)/moi(i) !角加速度

       v(i,t+1,1)=v(i,t,1)+a(i,t+1,1)*dt !x速度
       v(i,t+1,2)=v(i,t,2)+a(i,t+1,2)*dt !y速度
       omega(i,t+1)=omega(i,t)+alpha(i,t+1)*dt !角速度

       du(i,t+1,1)=v(i,t+1,1)*dt !x変位増分
       du(i,t+1,2)=v(i,t+1,2)*dt !y変位増分
       dphy(i,t+1)=omega(i,t+1)*dt !回転角増分

       u(i,t+1,1)=u(i,t,1)+du(i,t+1,1) !次のstepのx座標
       u(i,t+1,2)=u(i,t,2)+du(i,t+1,2) !次のstepのy座標
       phy(i,t+1)=phy(i,t)+dphy(i,t+1) !次のstepの回転角

       print *, i," m=",m(i)
       print *, i," ax=",a(i,t+1,1)," ay=",a(i,t+1,2)
       print *, i," vx=",v(i,t+1,1)," vy=",v(i,t+1,2)
       print *, i," x=",u(i,t+1,1)," y=",u(i,t+1,2)
      end do !i=1,nyou
       
!!結果の出力
!!!paraview用のvtkフォーマット↓
      j=t 
      write(k,'(i10)') j !整数or実数の文字列化
      kekka="kekka/kekka"//trim(adjustl(k))//".vtk" !余白を消す
      open(4,file=(kekka)) 
      write(4,'(26a)') "# vtk DataFile Version 2.0" !決まり文句
      write(4,'(14a)') "kekka"//trim(adjustl(k))//".vtk" !何でもいい
      write(4,'(5a)') "ASCII" !決まり文句
      write(4,'(16a)') "DATASET UNSTRUCTURED_GRID" !よく分からぬ
!!!座標の出力
      write(k,'(i10)') nyou !整数or実数の文字列化
      write(4,'(20a)') "POINTS "//trim(adjustl(k))//" float" !タイプが点
      do i=1,nyou
       write(4,'(1e11.4,a,1e11.4,$)') u(i,t+1,1)," ",u(i,t+1,2)
       write(4,'(a2,f3.1)') "  ",0.0 
      end do !i=1,nyou
!!!タイプの設定
      write(4,'(26a)') "CELL_TYPES "//trim(adjustl(k))
      do i=1,nyou
       write(4,'(i1)') 1
      end do !i=1,nyou
!!!半径の設定
      write(4,'(16a)') "POINT_DATA "//trim(adjustl(k))
      write(4,'(21a)') "SCALARS radius float "
      write(4,'(17a)') "LOOKUP_TABLE default"
      do i=1,nyou
       write(4,'(1e11.4)') r(i)
      end do !i=1,nyou

      close(4)
      end do !t=1,nkei
      close(3)
!      close(2) !壁data
      close(1)
      end
