function [Fint,KT]=Beam3d(Qa,Qb)
% function [Fint,KT]=Beam3d(Qa,Qb)
% 
% 先行変位Qb,原稿変位Qaを与えてはりの要素力Fintと剛性マトリックスKTを計算する
% 
% Copyright (C) 2007 清水 信行, 曽我部 潔, 藤川 猛 All Rights Reserved.

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

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

fi=zeros(6,1);fj=zeros(6,1); Fint=zeros(Ndof,1) ;KT=zeros(Ndof);
Kg=zeros(6);Ko=zeros(6);KB=zeros(6);Kxx=zeros(6);
Lam6=zeros(6);Lamb=zeros(3);Lama=zeros(3);dLam=zeros(3);

dQ=Qa-Qb;
for ke=1:Numelm
  Eps=sprintf(' %d  beam No.=',ke);
  is=(ke-1)*6;    js=ke*6;
    Lamb=LamdB(:,:,ke);
    Lam6=[Lamb zeros(3);zeros(3) Lamb];
    
   Qij=Qb(is+1:is+3)-Qb(js+1:js+3);  Lb=norm(Qij);%先行長さ
   Qij=Qa(is+1:is+3)-Qa(js+1:js+3);  La=norm(Qij);%現行長さ
   Lo=L0(ke);Lelm(ke)=La;

    dQi=dQ(is+1:is+6);   dQj=dQ(js+1:js+6);
    dqi=Lam6*dQi ;       dqj=Lam6*dQj ;
    Adu  =(dqj(1)-dqi(1))/Lo;
    Adv  =(dqj(2)-dqi(2))/Lo;
    Adw  =(dqj(3)-dqi(3))/Lo;
   % dfai=dqj(4)-dqi(4);   dthe=dqj(5)-dqi(5); 
    dpsi=dqj(6)-dqi(6); mpsi=(dqj(6)+dqi(6))/2;
    
 %---------変形量の計算と保存--------        
   epsL=La-Lo      ;                               
    epsxi= Adv+dqi(4) +epsB(2,ke);
    epsxj= Adv+dqj(4) +epsB(3,ke);
    epsyi=-Adu+dqi(5) +epsB(4,ke);
    epsyj=-Adu+dqj(5) +epsB(5,ke);
    epsZ = dpsi       +epsB(6,ke);                               
   
    Eps=sprintf(' %f  %f  %f  ',epsL,epsyi,epsyj);
    epsB(1,ke)=epsL ; 
    epsB(2,ke)=epsxi; 
    epsB(3,ke)=epsxj; 
    epsB(4,ke)=epsyi;
    epsB(5,ke)=epsyj; 
    epsB(6,ke)=epsZ ; 
    
% -------座標変換マトリックス----------------------------  
    [dLam]=dLamMake(Adu,Adv,Adw,mpsi);
    Lama=dLam*Lamb;
    LamdB(:,:,ke)=Lama;
    Lam6=[Lama zeros(3);zeros(3) Lama];
    
%----------要素力計算------------------------------------
  EAL=EA(ke)/L0(ke)  ;GJL=GJ(ke)/L0(ke);
  EILx=EIx(ke)/L0(ke);EILx2=EILx/La;EILx3=EILx2/La;
  EILy=EIy(ke)/L0(ke);EILy2=EILy/La;EILy3=EILy2/La;
  
    fi(1)= (6*EILy2)*(epsyi+epsyj)  ;fj(1)=-fi(1);
    fi(2)=-(6*EILx2)*(epsxi+epsxj)  ;fj(2)=-fi(2);
    fi(3)=-(EAL)*epsL               ;fj(3)=-fi(3);
    fi(4)= (2*EILx)*(2*epsxi+epsxj) ;fj(4)=(2*EILx)*(epsxi+2*epsxj);
    fi(5)= (2*EILy)*(2*epsyi+epsyj) ;fj(5)=(2*EILy)*(epsyi+2*epsyj);
    fi(6)=-(GJL)*epsZ               ;fj(6)=-fi(6);
% ---------要素力合成------------------------------------
    Fi=Lam6'*fi;
    Fj=Lam6'*fj;
    Fint(is+1:is+6)=Fint(is+1:is+6)+Fi;
    Fint(js+1:js+6)=Fint(js+1:js+6)+Fj;
    
% ----------剛性マトリックス計算--------------------------
    TL= EAL*epsL/La;  
    K11=zeros(6);K12=zeros(6);K22=zeros(6);    
    K11(1,1)= 12*EILy3 +TL      ;K11(1,5)=  6*EILy2;
    K11(2,2)= 12*EILx3 +TL      ;K11(2,4)= -6*EILx2;
    K11(3,3)= EAL;
    K11(4,2)=-6*EILx2           ;K11(4,4)=  4*EILx;
    K11(5,1)= 6*EILy2           ;K11(5,5)=  4*EILy;
    K11(6,6)= GJL;
      K12(1,1)=-12*EILy3 -TL      ;K12(1,5)=  6*EILy2;
      K12(2,2)=-12*EILx3 -TL      ;K12(2,4)= -6*EILx2;
      K12(3,3)=-EAL;
      K12(4,2)= 6*EILx2           ;K12(4,4)=  2*EILx;
      K12(5,1)=-6*EILy2           ;K12(5,5)=  2*EILy;
      K12(6,6)=-GJL;
    K22(1,1)= 12*EILy3 +TL      ;K22(1,5)= -6*EILy2;
    K22(2,2)= 12*EILx3 +TL      ;K22(2,4)=  6*EILx2;
    K22(3,3)= EAL;
    K22(4,2)= 6*EILx2           ;K22(4,4)=  4*EILx;
    K22(5,1)=-6*EILy2           ;K22(5,5)=  4*EILy;
    K22(6,6)= GJL;
    
% ---------剛性マトリックス合成---------------------------
is=INCI(ke,1)*6-6;is1=is+1;is6=is+6;
js=INCI(ke,2)*6-6;js1=js+1;js6=js+6;
    K11=Lam6'*K11*Lam6;
        KT(is1:is6,is1:is6)=KT(is1:is6,is1:is6)+K11;
    K12=Lam6'*K12*Lam6;
        KT(is1:is6,js1:js6)=KT(is1:is6,js1:js6)+K12;
        KT(js1:js6,is1:is6)=KT(js1:js6,is1:is6)+K12';
    K22=Lam6'*K22*Lam6;
        KT(js1:js6,js1:js6)=KT(js1:js6,js1:js6)+K22;
end
