function ex1_normal_anc
% function ex1_normal_anc
% 
% 本プログラムは以下の問題用プログラムである。
% 5章　柔軟多体系の平面動力学解析
% 5.5　ANC法による2次元動力学解析
% 5.5.3　例題および解析例
% (1)単純支持はりの自由振動問題（定式化１）
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 高橋 義考 All Rights Reserved.

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

matlab_ver=ver('MATLAB');	% MATLABのバージョン確認
if str2num(matlab_ver.Version(1))>6
	'feature accel off'
	feature accel off;
end


clear all;

element_num=10;     %要素分割数の設定

LL=1;               %全体の梁の長さ設定

L=LL/element_num;   %１要素の長さ計算


%---一般化座標 q の初期値設定---

for i=1:(element_num+1)
    q((i-1)*4+1,1)=(i-1)*L;
    q((i-1)*4+2,1)=0;
    q((i-1)*4+3,1)=1;
    q((i-1)*4+4,1)=0;
end
    

%---速度 qd の初期値設定---

for i=1:(element_num+1)
    qd((i-1)*4+1,1)=0;
    qd((i-1)*4+2,1)=0;
    qd((i-1)*4+3,1)=0;
    qd((i-1)*4+4,1)=0;
end

%---一般化外力ベクトル Q の設定---

Q=zeros((element_num+1)*4,1);

%---質量行列 M の設定---

A=1.0d-2*1.0d-2;    %断面積
ro=7850d0;          %密度
aro=A*ro;           %断面積×密度
M=zeros((element_num+1)*4,(element_num+1)*4);
Mi=anc_M(L,aro);    %1要素の質量行列
for in=1:element_num
        for i=1:8
        for j=1:8
            M((in-1)*4+i,(in-1)*4+j)=M((in-1)*4+i,(in-1)*4+j)+Mi(i,j);  %行列の重ね合わせ
        end
        end
end

%---ヤコビアン行列の設定---
J_mat=zeros(4,44);
J_mat(1,1)=1;
J_mat(2,2)=1;
J_mat(3,41)=1;
J_mat(4,42)=1;

%---MixDAEの係数行列の逆行列計算---
M=[M J_mat';J_mat zeros(4,4)];
M_inv=inv(M);

I_inv=inv([eye((element_num+1)*4) J_mat';J_mat zeros(4,4)]);

%---数値計算の設定---

figure;
wait_fig=waitbar(0,'Please wait ...');

t=0;            %計算開始時間
t_end=0.5;      %計算終了時間
dt=2.0d-6;      %積分ステップ時間
t_save=0.0005;  %データ保存ステップ時間
dat_x=[];       %データ保存用変数設定
dat_y=[];       %データ保存用変数設定
dat_t=[];       %データ保存用変数設定
dat_q=[];       %データ保存用変数設定
dat_qd=[];      %データ保存用変数設定
i_save=fix(t_save/dt);  %データ保存用変数計算
t_count=fix(t_end/dt);  %数値積分回数計算
i_rk_save=1;            %データ保存用カウンター


%---数値計算（Runge-Kutta法）---

for i_rk=1:t_count
    
    waitbar(i_rk/t_count,wait_fig);    
    [qdd1,qd1]=fafv(t,q,qd,L,element_num,Q,M_inv,I_inv);
    
    q_rk=0.5*dt*qd1;
    qd_rk=0.5*dt*qdd1; 
    [qdd2,qd2]=fafv(t+0.5*dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv,I_inv);
    
    q_rk=0.5*dt*qd2;
    qd_rk=0.5*dt*qdd2;           
    [qdd3,qd3]=fafv(t+0.5*dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv,I_inv);

    q_rk=dt*qd3;
    qd_rk=dt*qdd3;          
    [qdd4,qd4]=fafv(t+dt,q+q_rk,qd+qd_rk,L,element_num,Q,M_inv,I_inv);    

    t=t+dt;
    q=q+dt/6.0*(qd1+2*qd2+2*qd3+qd4);
    qd=qd+dt/6.0*(qdd1+2*qdd2+2*qdd3+qdd4);

    %途中経過出力      
        if i_rk_save==i_save;
            t
            clf;plot(q(1:4:(element_num+1)*4,1),q(2:4:(element_num+1)*4,1),'ro-');axis('equal');
            drawnow;
            dat_x=[dat_x;q(21,1)];
            dat_y=[dat_y;q(22,1)];
            dat_t=[dat_t;t];
            dat_q=[dat_q;q'];
            dat_qd=[dat_qd;qd'];
            i_rk_save=0;
        end
        
    i_rk_save=i_rk_save+1;      
        
end

close(wait_fig);

%---データの保存---
save dat_x dat_x;   %梁中心部のX座標データ保存
save dat_y dat_y;   %梁中心部のY座標データ保存
save dat_t dat_t;   %時間データの保存
save dat_q dat_q;   %一般化座標の保存
save dat_qd dat_qd; %速度データの保存

%---結果のグラフ出力---
 figure;
 plot(dat_t,dat_y,'-');



%--------------------------------------------------------------------------
%　以下は解析に必要な関数定義である。
%--------------------------------------------------------------------------

function [qdd,qd]=fafv(t,q,qd,L,element_num,Q,M_inv,I_inv);
%function [qdd,qd]=fafv(t,q,qd,L,element_num,Q,M_inv,I_inv)
%
%一般化座標、速度、質量行列の逆行列を引数として、
%運動方程式から加速度を求める関数プログラムである。
%例題では梁両端のXY座標を固定するため、DAEとして
%解を求めている。また、拘束誤差の発散を防ぐために、
%Gearの方法を適用している。
%
%  t                時間
%  q                一般化座標
%  qd               一般化座標の速度
%  qdd              一般化座標の加速度
%  L                １要素の長さ
%  element_num      要素分割数
%  Q                一般化外力ベクトル
%  M_inv            質量行列の逆行列
%  I_inv            Gear法に用いる行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%---弾性力の計算---
Qk=zeros((element_num+1)*4,1);
for j=1:element_num
    Qk_i=anc_K(q((j-1)*4+1:(j-1)*4+8,1),L);
    for i=1:8
        Qk((j-1)*4+i,1)=Qk((j-1)*4+i,1)+Qk_i(i,1);
    end
end

%---外力の設定---
if t<=1.0d-3;
    Q(22,1)=-1000;
else;
    Q(22,1)=0;
end

%---加速度と速度の計算
qdd=M_inv*[Q-Qk;zeros(4,1)];
qdd=qdd(1:(element_num+1)*4,1);

qd=I_inv*[qd;zeros(4,1)]; 
qd=qd(1:(element_num+1)*4,1);



%-------------------
%　関数定義
%-------------------

function Qk=anc_K(q,L)
%function Qk=anc_K(q,L)
%
%一般化座標、梁要素の長さを引数とし、梁要素の弾性力
%を求める関数プログラム
%
%  q        一般化座標
%  L        １要素の長さ
%  Qk       梁要素の弾性力
%  E        縦弾性係数
%  A        断面積
%  I        断面２次モーメント
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
E=2.058d11;
A=1.0d-2*1.0d-2;
I=1/12*(1.0d-2)*(1.0d-2)^3;

ea=E*A;
ei=E*I;
el=L;

[a11]=a11mat(el,ea);
[a12]=a12mat(el,ea);
[a21]=a21mat(el,ea);
[a22]=a22mat(el,ea);
[b11]=b11mat(el,ei);
[b12]=b12mat(el,ei);
[b21]=b21mat(el,ei);
[b22]=b22mat(el,ei);
[a1]=a1mat(ea);
[a2]=a2mat(ea);
[dix,diy,djx,djy]=dijmat(q);

x1=q(5,1)-q(1,1);
x2=q(6,1)-q(2,1);
x3=x1*x1+x2*x2;
xi=x1/(sqrt(x3));
yi=x2/(sqrt(x3));
xj=-yi;
yj=xi;

z1=a11*q*xi*xi;
z2=a22*q*yi*yi;
z3=(a12+a21)*q*xi*yi;
z4=b11*q*xj*xj;
z5=b22*q*yj*yj;
z6=(b12+b21)*q*xj*yj;
z7=-a1*xi;
z8=-a2*yi;
z9=q'*a11*q*dix*xi;
z10=q'*(a12+a21)*q*dix*yi*0.5;
z11=-a1'*q*dix;
z12=q'*a22*q*diy*yi;
z13=q'*(a12+a21)*q*diy*xi*0.5;
z14=-a2'*q*diy;
z15=q'*b11*q*djx*xj;
z16=q'*(b12+b21)*q*djx*yj*0.5;
z17=q'*b22*q*djy*yj;
z18=q'*(b12+b21)*q*djy*xj*0.5;

Qk=z1+z2+z3+z4+z5+z6+z7+z8+z9+z10+z11+z12+z13+z14+z15+z16+z17+z18;



%-------------------
%　関数定義
%-------------------

function [a11]=a11mat(el,ea)
%function [a11]=a11mat(el,ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ea       縦弾性係数×断面積
%  a11      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
	a11(1,1)=1.20d0;
	a11(1,2)=0.0d0;
	a11(1,3)=el/10.0d0;
	a11(1,4)=0.0d0;
	a11(1,5)=-1.20d0;
	a11(1,6)=0.0d0;
	a11(1,7)=el/10.0d0;
	a11(1,8)=0.0d0;

	for i=1:8
	    a11(2,i)=0.0d0;
    end

	a11(3,1)=el/10.0d0;
	a11(3,2)=0.0d0;
	a11(3,3)=2.0d0/15.0d0*el*el;
	a11(3,4)=0.0d0;
	a11(3,5)=-el/10.0d0;
	a11(3,6)=0.0d0;
	a11(3,7)=-el*el/30.0d0;
	a11(3,8)=0.0d0;

	for i=1:8
	    a11(4,i)=0.0d0;
    end

	a11(5,1)=-1.20d0;
	a11(5,2)=0.0d0;
	a11(5,3)=-el/10.0d0;
	a11(5,4)=0.0d0;
	a11(5,5)=1.20d0;
	a11(5,6)=0.0d0;
	a11(5,7)=-el/10.0d0;
	a11(5,8)=0.0d0;

	for i=1:8
	    a11(6,i)=0.0d0;
    end

	a11(7,1)=el/10.0d0;
	a11(7,2)=0.0d0;
	a11(7,3)=-el*el/30.0d0;
	a11(7,4)=0.0d0;
	a11(7,5)=-el/10.0d0;
	a11(7,6)=0.0d0;
	a11(7,7)=2.0d0/15.0d0*el*el;
	a11(7,8)=0.0d0;

	for i=1:8
	    a11(8,i)=0.0d0;
    end

    a11=ea/el*a11;



%-------------------
%　関数定義
%-------------------

function [a12]=a12mat(el,ea)
%function [a12]=a12mat(el,ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ea       縦弾性係数×断面積
%  a12      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	a12(1,1)=0.0d0;
	a12(1,2)=1.20d0;
	a12(1,3)=0.0d0;
	a12(1,4)=el/10.0d0;
	a12(1,5)=0.0d0;
	a12(1,6)=-1.20d0;
	a12(1,7)=0.0d0;
	a12(1,8)=el/10.0d0;

	for i=1:8
	a12(2,i)=0.0d0;
    end

	a12(3,1)=0.0d0;
	a12(3,2)=el/10.0d0;
	a12(3,3)=0.0d0;
	a12(3,4)=2.0d0/15.0d0*el*el;
	a12(3,5)=0.0d0;
	a12(3,6)=-el/10.0d0;
	a12(3,7)=0.0d0;
	a12(3,8)=-el*el/30.0d0;

	for i=1:8
	a12(4,i)=0.0d0;
    end

	a12(5,1)=0.0d0;
	a12(5,2)=-1.20d0;
	a12(5,3)=0.0d0;
	a12(5,4)=-el/10.0d0;
	a12(5,5)=0.0d0;
	a12(5,6)=1.20d0;
	a12(5,7)=0.0d0;
	a12(5,8)=-el/10.0d0;

	for i=1:8
	a12(6,i)=0.0d0;
    end

	a12(7,1)=0.0d0;
	a12(7,2)=el/10.0d0;
	a12(7,3)=0.0d0;
	a12(7,4)=-el*el/30.0d0;
	a12(7,5)=0.0d0;
	a12(7,6)=-el/10.0d0;
	a12(7,7)=0.0d0;
	a12(7,8)=2.0d0/15.0d0*el*el;

	for i=1:8
	a12(8,i)=0.0d0;
    end

    a12=ea/el*a12;



%-------------------
%　関数定義
%-------------------

function [a21]=a21mat(el,ea)
%function [a21]=a21mat(el,ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ea       縦弾性係数×断面積
%  a21      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%

	a21(2,1)=1.20d0;
	a21(2,2)=0.0d0;
	a21(2,3)=el/10.0d0;
	a21(2,4)=0.0d0;
	a21(2,5)=-1.20d0;
	a21(2,6)=0.0d0;
	a21(2,7)=el/10.0d0;
	a21(2,8)=0.0d0;

	for i=1:8
	    a21(1,i)=0.0d0;
    end

	a21(4,1)=el/10.0d0;
	a21(4,2)=0.0d0;
	a21(4,3)=2.0d0/15.0d0*el*el;
	a21(4,4)=0.0d0;
	a21(4,5)=-el/10.0d0;
	a21(4,6)=0.0d0;
	a21(4,7)=-el*el/30.0d0;
	a21(4,8)=0.0d0;

	for i=1:8
	    a21(3,i)=0.0d0;
    end

	a21(6,1)=-1.20d0;
	a21(6,2)=0.0d0;
	a21(6,3)=-el/10.0d0;
	a21(6,4)=0.0d0;
	a21(6,5)=1.20d0;
	a21(6,6)=0.0d0;
	a21(6,7)=-el/10.0d0;
	a21(6,8)=0.0d0;

	for i=1:8
	    a21(5,i)=0.0d0;
    end

	a21(8,1)=el/10.0d0;
	a21(8,2)=0.0d0;
	a21(8,3)=-el*el/30.0d0;
	a21(8,4)=0.0d0;
	a21(8,5)=-el/10.0d0;
	a21(8,6)=0.0d0;
	a21(8,7)=2.0d0/15.0d0*el*el;
	a21(8,8)=0.0d0;

	for i=1:8
	    a21(7,i)=0.0d0;
    end

    a21=ea/el*a21;



%-------------------
%　関数定義
%-------------------

function [a22]=a22mat(el,ea)
%function [a22]=a22mat(el,ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ea       縦弾性係数×断面積
%  a22      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	a22(2,1)=0.0d0;
	a22(2,2)=1.20d0;
	a22(2,3)=0.0d0;
	a22(2,4)=el/10.0d0;
	a22(2,5)=0.0d0;
	a22(2,6)=-1.20d0;
	a22(2,7)=0.0d0;
	a22(2,8)=el/10.0d0;

	for i=1:8
	    a22(1,i)=0.0d0;
    end

	a22(4,1)=0.0d0;
	a22(4,2)=el/10.0d0;
	a22(4,3)=0.0d0;
	a22(4,4)=2.0d0/15.0d0*el*el;
	a22(4,5)=0.0d0;
	a22(4,6)=-el/10.0d0;
	a22(4,7)=0.0d0;
	a22(4,8)=-el*el/30.0d0;

	for i=1:8
	    a22(3,i)=0.0d0;
    end

	a22(6,1)=0.0d0;
	a22(6,2)=-1.20d0;
	a22(6,3)=0.0d0;
	a22(6,4)=-el/10.0d0;
	a22(6,5)=0.0d0;
	a22(6,6)=1.20d0;
	a22(6,7)=0.0d0;
	a22(6,8)=-el/10.0d0;

	for i=1:8
	    a22(5,i)=0.0d0;
    end

	a22(8,1)=0.0d0;
	a22(8,2)=el/10.0d0;
	a22(8,3)=0.0d0;
	a22(8,4)=-el*el/30.0d0;
	a22(8,5)=0.0d0;
	a22(8,6)=-el/10.0d0;
	a22(8,7)=0.0d0;
	a22(8,8)=2.0d0/15.0d0*el*el;

	for i=1:8
	    a22(7,i)=0.0d0;
    end

    a22=ea/el*a22;



%-------------------
%　関数定義
%-------------------

function [b11]=b11mat(el,ei)
%function [b11]=b11mat(el,ei)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ei       縦弾性係数×断面２次モーメント
%  b11      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
    b11(1,1)=12.0d0;
	b11(1,2)=0.0d0;
	b11(1,3)=6.0d0*el;
	b11(1,4)=0.0d0;
	b11(1,5)=-12.0d0;
	b11(1,6)=0.0d0;
	b11(1,7)=6.0d0*el;
	b11(1,8)=0.0d0;

	for i=1:8
	    b11(2,i)=0.0d0;
    end

	b11(3,1)=6.0d0*el;
	b11(3,2)=0.0d0;
	b11(3,3)=4.0d0*el*el;
	b11(3,4)=0.0d0;
	b11(3,5)=-6.0d0*el;
	b11(3,6)=0.0d0;
	b11(3,7)=2.0d0*el*el;
	b11(3,8)=0.0d0;

	for i=1:8
	    b11(4,i)=0.0d0;
    end

	b11(5,1)=-12.0d0;
	b11(5,2)=0.0d0;
	b11(5,3)=-6.0d0*el;
	b11(5,4)=0.0d0;
	b11(5,5)=12.0d0;
	b11(5,6)=0.0d0;
	b11(5,7)=-6.0d0*el;
	b11(5,8)=0.0d0;

	for i=1:8
	    b11(6,i)=0.0d0;
    end

	b11(7,1)=6.0d0*el;
	b11(7,2)=0.0d0;
	b11(7,3)=2.0d0*el*el;
	b11(7,4)=0.0d0;
	b11(7,5)=-6.0d0*el;
	b11(7,6)=0.0d0;
	b11(7,7)=4.0d0*el*el;
	b11(7,8)=0.0d0;

	for i=1:8
	    b11(8,i)=0.0d0;
    end

    b11=b11*ei/(el*el*el);



%-------------------
%　関数定義
%-------------------

function [b12]=b12mat(el,ei)
%function [b12]=b12mat(el,ei)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ei       縦弾性係数×断面２次モーメント
%  b12      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
    b12(1,1)=0.0d0;
	b12(1,2)=12.0d0;
	b12(1,3)=0.0d0;
	b12(1,4)=6.0d0*el;
	b12(1,5)=0.0d0;
	b12(1,6)=-12.0d0;
	b12(1,7)=0.0d0;
	b12(1,8)=6.0d0*el;

	for i=1:8
	    b12(2,i)=0.0d0;
    end

	b12(3,1)=0.0d0;
	b12(3,2)=6.0d0*el;
	b12(3,3)=0.0d0;
	b12(3,4)=4.0d0*el*el;
	b12(3,5)=0.0d0;
	b12(3,6)=-6.0d0*el;
	b12(3,7)=0.0d0;
	b12(3,8)=2.0d0*el*el;

	for i=1:8
	    b12(4,i)=0.0d0;
    end

	b12(5,1)=0.0d0;
	b12(5,2)=-12.0d0;
	b12(5,3)=0.0d0;
	b12(5,4)=-6.0d0*el;
	b12(5,5)=0.0d0;
	b12(5,6)=12.0d0;
	b12(5,7)=0.0d0;
	b12(5,8)=-6.0d0*el;

	for i=1:8
	    b12(6,i)=0.0d0;
    end

	b12(7,1)=0.0d0;
	b12(7,2)=6.0d0*el;
	b12(7,3)=0.0d0;
	b12(7,4)=2.0d0*el*el;
	b12(7,5)=0.0d0;
	b12(7,6)=-6.0d0*el;
	b12(7,7)=0.0d0;
	b12(7,8)=4.0d0*el*el;

	for i=1:8
	    b12(8,i)=0.0d0;
    end

    b12=b12*ei/(el*el*el);



%-------------------
%　関数定義
%-------------------

function [b21]=b21mat(el,ei)
%function [b21]=b21mat(el,ei)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ei       縦弾性係数×断面２次モーメント
%  b21      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	b21(2,1)=12.0d0;
	b21(2,2)=0.0d0;
	b21(2,3)=6.0d0*el;
	b21(2,4)=0.0d0;
	b21(2,5)=-12.0d0;
	b21(2,6)=0.0d0;
	b21(2,7)=6.0d0*el;
	b21(2,8)=0.0d0;

	for i=1:8
	    b21(1,i)=0.0d0;
    end

	b21(4,1)=6.0d0*el;
	b21(4,2)=0.0d0;
	b21(4,3)=4.0d0*el*el;
	b21(4,4)=0.0d0;
	b21(4,5)=-6.0d0*el;
	b21(4,6)=0.0d0;
	b21(4,7)=2.0d0*el*el;
	b21(4,8)=0.0d0;

	for i=1:8
	    b21(3,i)=0.0d0;
    end

	b21(6,1)=-12.0d0;
	b21(6,2)=0.0d0;
	b21(6,3)=-6.0d0*el;
	b21(6,4)=0.0d0;
	b21(6,5)=12.0d0;
	b21(6,6)=0.0d0;
	b21(6,7)=-6.0d0*el;
	b21(6,8)=0.0d0;

	for i=1:8
	    b21(5,i)=0.0d0;
    end

	b21(8,1)=6.0d0*el;
	b21(8,2)=0.0d0;
	b21(8,3)=2.0d0*el*el;
	b21(8,4)=0.0d0;
	b21(8,5)=-6.0d0*el;
	b21(8,6)=0.0d0;
	b21(8,7)=4.0d0*el*el;
	b21(8,8)=0.0d0;

	for i=1:8
	    b21(7,i)=0.0d0;
    end

	b21=b21*ei/(el*el*el);



%-------------------
%　関数定義
%-------------------

function [b22]=b22mat(el,ei)
%function [b22]=b22mat(el,ei)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  el       １要素の長さ
%  ei       縦弾性係数×断面２次モーメント
%  b22      定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	b22(2,1)=0.0d0;
	b22(2,2)=12.0d0;
	b22(2,3)=0.0d0;
	b22(2,4)=6.0d0*el;
	b22(2,5)=0.0d0;
	b22(2,6)=-12.0d0;
	b22(2,7)=0.0d0;
	b22(2,8)=6.0d0*el;

	for i=1:8
	    b22(1,i)=0.0d0;
    end

	b22(4,1)=0.0d0;
	b22(4,2)=6.0d0*el;
	b22(4,3)=0.0d0;
	b22(4,4)=4.0d0*el*el;
	b22(4,5)=0.0d0;
	b22(4,6)=-6.0d0*el;
	b22(4,7)=0.0d0;
	b22(4,8)=2.0d0*el*el;

	for i=1:8
	    b22(3,i)=0.0d0;
    end

	b22(6,1)=0.0d0;
	b22(6,2)=-12.0d0;
	b22(6,3)=0.0d0;
	b22(6,4)=-6.0d0*el;
	b22(6,5)=0.0d0;
	b22(6,6)=12.0d0;
	b22(6,7)=0.0d0;
	b22(6,8)=-6.0d0*el;

	for i=1:8
	    b22(5,i)=0.0d0;
    end

	b22(8,1)=0.0d0;
	b22(8,2)=6.0d0*el;
	b22(8,3)=0.0d0;
	b22(8,4)=2.0d0*el*el;
	b22(8,5)=0.0d0;
	b22(8,6)=-6.0d0*el;
	b22(8,7)=0.0d0;
	b22(8,8)=4.0d0*el*el;

	for i=1:8
	    b22(7,i)=0.0d0;
    end

	b22=b22*ei/(el*el*el);



%-------------------
%　関数定義
%-------------------

function [a1]=a1mat(ea)
%function [a1]=a1mat(ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  ea       縦弾性係数×断面積
%  a1       定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	a1(1,1)=-ea;
	a1(2,1)=0.0d0;
	a1(3,1)=0.0d0;
	a1(4,1)=0.0d0;
	a1(5,1)=ea;
	a1(6,1)=0.0d0;
	a1(7,1)=0.0d0;
	a1(8,1)=0.0d0;



%-------------------
%　関数定義
%-------------------

function [a2]=a2mat(ea)
%function [a2]=a2mat(ea)
%
%梁の弾性力を求めるのに必要な定数行列
%
%  ea       縦弾性係数×断面積
%  a1       定数行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
	a2(1,1)=0.0d0;
	a2(2,1)=-ea;
	a2(3,1)=0.0d0;
	a2(4,1)=0.0d0;
	a2(5,1)=0.0d0;
	a2(6,1)=ea;
	a2(7,1)=0.0d0;
	a2(8,1)=0.0d0;



%-------------------
%　関数定義
%-------------------

function [dix,diy,djx,djy]=dijmat(u)
%function [dix,diy,djx,djy]=dijmat(u)
%
%梁の弾性力を求めるのに必要な変数の計算
%
%  u        梁要素の節点座標
%  dix      変数
%  diy      変数
%  djx      変数
%  djy      変数
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%
    a1=-u(1,1)+u(5,1);
	a2=-u(2,1)+u(6,1);
	a3=a1*a1+a2*a2;

	dix(1,1)=(a1*a1)/(sqrt(a3*a3*a3))-1.0d0/(sqrt(a3));
	dix(2,1)=(a1*a2)/(sqrt(a3*a3*a3));
	dix(3,1)=0.0d0;
	dix(4,1)=0.0d0;
	dix(5,1)=-dix(1,1);
	dix(6,1)=-dix(2,1);
	dix(7,1)=0.0d0;
	dix(8,1)=0.0d0;

	diy(1,1)=(a1*a2)/(sqrt(a3*a3*a3));
	diy(2,1)=(a2*a2)/(sqrt(a3*a3*a3))-1.0d0/(sqrt(a3));
	diy(3,1)=0.0d0;
	diy(4,1)=0.0d0;
	diy(5,1)=-diy(1,1);
	diy(6,1)=-diy(2,1);
	diy(7,1)=0.0d0;
	diy(8,1)=0.0d0;

    djx=-diy;
	djy=dix;



%-------------------
%　関数定義
%-------------------

function M=anc_M(el,aro)
%function M=anc_M(el,aro)
%
%梁要素の質量行列を求める関数プログラム
%
%  el       梁要素の長さ
%  aro      断面積×密度
%  M　      梁要素の質量行列
%
%Copyright (c) 2007 日本機械学会 All Rights reserved.
%
%
%

    fm1(1,1)=13.0d0/35.0d0;
	fm1(1,2)=0.0d0;
	fm1(1,3)=11.0d0/210.0d0*el;
	fm1(1,4)=0.0d0;
	fm1(1,5)=9.0d0/70.0d0;
	fm1(1,6)=0.0d0;
	fm1(1,7)=-13.0d0/420.0d0*el;
	fm1(1,8)=0.0d0;
	fm1(2,1)=0.0d0;
	fm1(2,2)=13.0d0/35.0d0;
	fm1(2,3)=0.0d0;
	fm1(2,4)=11.0d0/210.0d0*el;
	fm1(2,5)=0.0d0;
	fm1(2,6)=9.0d0/70.0d0;
	fm1(2,7)=0.0d0;
	fm1(2,8)=-13.0d0/420.0d0*el;
	fm1(3,1)=11.0d0/210.0d0*el;
	fm1(3,2)=0.0d0;
	fm1(3,3)=el*el/105.0d0;
	fm1(3,4)=0.0d0;
	fm1(3,5)=13.0d0/420.0d0*el;
	fm1(3,6)=0.0d0;
	fm1(3,7)=-el*el/140.0d0;
	fm1(3,8)=0.0d0;
	fm1(4,1)=0.0d0;
	fm1(4,2)=11.0d0/210.0d0*el;
	fm1(4,3)=0.0d0;
	fm1(4,4)=el*el/105.0d0;
	fm1(4,5)=0.0d0;
	fm1(4,6)=13.0d0/420.0d0*el;
	fm1(4,7)=0.0d0;
	fm1(4,8)=-el*el/140.0d0;
	fm1(5,1)=9.0d0/70.0d0;
	fm1(5,2)=0.0d0;
	fm1(5,3)=13.0d0/420.0d0*el;
	fm1(5,4)=0.0d0;
	fm1(5,5)=13.0d0/35.0d0;
	fm1(5,6)=0.0d0;
	fm1(5,7)=-11.0d0/210.0d0*el;
	fm1(5,8)=0.0d0;
	fm1(6,1)=0.0d0;
	fm1(6,2)=9.0d0/70.0d0;
	fm1(6,3)=0.0d0;
	fm1(6,4)=13.0d0/420.0d0*el;
	fm1(6,5)=0.0d0;
	fm1(6,6)=13.0d0/35.0d0;
	fm1(6,7)=0.0d0;
	fm1(6,8)=-11.0d0/210.0d0*el;
	fm1(7,1)=-13.0d0/420.0d0*el;
	fm1(7,2)=0.0d0;
	fm1(7,3)=-el*el/140.0d0;
	fm1(7,4)=0.0d0;
	fm1(7,5)=-11.0d0/210.0d0*el;
	fm1(7,6)=0.0d0;
	fm1(7,7)=el*el/105.0d0;
	fm1(7,8)=0.0d0;
	fm1(8,1)=0.0d0;
	fm1(8,2)=-13.0d0/420.0d0*el;
	fm1(8,3)=0.0d0;
	fm1(8,4)=-el*el/140.0d0;
	fm1(8,5)=0.0d0;
	fm1(8,6)=-11.0d0/210.0d0*el;
	fm1(8,7)=0.0d0;
	fm1(8,8)=el*el/105.0d0;

    M=fm1*aro*el;

