clc,clear,close all
tic
nodeCoord=[0 0;0 3;0 6 ;0 9; 5 0;5 3;5 6;5 9];
x=nodeCoord(:,1)'; % 节点x轴方向坐标
y=nodeCoord(:,2)'; % 节点y轴方向坐标
A=0.08;E=3e10;I=0.0128/12; % 定义截面面积和弹性模量
rho=3000;
%单元信息：编号，节点1编号，节点2编号，截面面积，弹性模量
ele=[1 1 2 A E I;2 2 3 A E I; 3 3 4  A E I;4 5 6 A E I; 5 6  7 A E I;6 7 8  A E I;7 2 6  A E I;8 3 7 A E I;9 4 8 A E I];;
%载荷信息：节点编号，x方向力，y方向力，z方向力
Load=[4 2e5 0 0];
%约束：节点编号，x方向约束，y方向约束，z方向约束
Constr=[1 0 0 0;5 0 0 0];
Dofs=3*size(x,2); %总自由度数
EleCount=size(ele,1); %单元总数
K=zeros(Dofs,Dofs); %总体刚度矩阵
M=zeros(Dofs,Dofs); %总体质量矩阵
F=zeros(Dofs,1); %总体载荷列阵
U=zeros(Dofs,1); %总体位移列阵
BarLength=BarsLength(x,y,ele);
figure('Name','Undeformed Truss')
RenderFrame(ele,Load,Constr,x,y,U,1,'-k',1) %绘制框架

hold on
%遍历所有单元，将各单元刚度阵分块组装到总体刚度阵
for iEle =1:EleCount
        %该单元的两个节点的编号
        n1=ele(iEle,2);n2=ele(iEle,3);
        %计算坐标变换矩阵
        R=CoordTransform([x(n1) x(n2)],[y(n1) y(n2)],BarLength(iEle));
        %计算单元刚度矩阵 Ke=R'*ke*R;局部坐标系下的单元刚度阵转换为全局坐标下的单元刚度阵
        ke= FrameElementKe(ele(iEle,4),ele(iEle,5),ele(iEle,6),R,BarLength(iEle));
        me= FrameElementMe(A,rho,R,BarLength(iEle));
        %将各单元刚度分块组装到总刚相应位置
        eleDof=[n1*3-2:n1*3,n2*3-2:n2*3];
        K(eleDof,eleDof)=K(eleDof,eleDof)+ke;
        M(eleDof,eleDof)=M(eleDof,eleDof)+me;
end

for i=1:length(Constr(:,1))
    restrainedDof(1+(i-1)*3:3+(i-1)*3)=(Constr(i,1)-1)*3+[1:3];
end
activeDof=setdiff([1:Dofs]',restrainedDof);

if det(M(activeDof,activeDof))>0
    A=M(activeDof,activeDof)\K(activeDof,activeDof);
    [Vo,Do]=eig(A);
    fo=diag(sqrt(Do)/(2*pi));
    Vf=sortrows([Vo',fo],size(Vo,1)+1);
    V=Vf(:,1:size(Vo,1))';%sort eignvectgors
elseif det(K(activeDof,activeDof))>0
    A=K(activeDof,activeDof)\M(activeDof,activeDof);
    [Vo,Do]=eig(A);
    fo=power(diag(sqrt(Do)),-1)/(2*pi);
    Vf=sortrows([Vo',fo],size(Vo,1)+1);
    V=Vf(:,1:size(Vo,1))';%sort eignvectgors    
end
f=sort(fo);%自振频率
figure
for i=1:5
    subplot(1,5,i)
    ModeNum=i;%第五阶模态
    U=zeros(Dofs,1);
    U(activeDof)=V(:,ModeNum)';
    RenderFrame(ele,Load,Constr,x,y,U,1,'-.b',200) %绘制模态振型
    axis off
end

%自振角频率
w1=2*pi*f(1);
w2=2*pi*f(2);
w3=2*pi*f(3);
w4=2*pi*f(4);
w5=2*pi*f(5);
epsilon=0.05;%阻尼比
a00=(2*epsilon*w1*w3)/(w1+w3);%假定控制频率为w1 w3
a11=(2*epsilon)/(w1+w3);
C=a00*M+a11*K;  %形成阻尼矩阵
%读取加速度时程
[t,acc_x]=textread('x_acceleration.txt','%f,%f,');
dt=t(2)-t(1);
n=length(t);
vel_x = zeros(length(t),1);
dis_x = zeros(length(t),1);
for i = 2:length(t)
    vel_x(i) = trapz(acc_x(1:i),t(1:i));
end

for i = 2:length(t)
    dis_x(i) = trapz(vel_x(1:i),t(1:i));
end

figure
subplot(3,1,1)
plot(t,acc_x)
subplot(3,1,2)
plot(t,vel_x)
subplot(3,1,3)
plot(t,dis_x)
title('地震波')
mm=M(activeDof,activeDof);
kk=K(activeDof,activeDof);
cc=C(activeDof,activeDof);
% plot(tt,vw);
% %%%%%%%%%%%%%%%%
beta=0.25;
gamma=0.5;
%注意newmark-beta法代入的三大类矩阵，可以是划行划列后的，也可以是结构整体矩阵，但后者需要在NewmarkBeta1中施加约束
[uu,vv,aa,ttt]=NewmarkBeta1(t,dt,gamma,beta,mm,cc,kk,acc_x,Constr);%相对位移速度加速度
uuu=zeros(size(M,1),length(t));%位移
vvv=zeros(size(M,1),length(t));%位移
aaa=zeros(size(M,1),length(t));%位移
uuu(activeDof,:)=uu;
vvv(activeDof,:)=vv;
aaa(activeDof,:)=aa;
for i=1:size(uuu,1)
    base_disp(i,:)=dis_x;
end
% uuu_t=uuu+base_disp;

figure%绘制顶部节点的位移速度和加速度
toc %disp(['运行时间: ',num2str(toc)])
subplot(3,1,1);
plot(ttt,uu(end-2,:));
subplot(3,1,2);
plot(ttt,vv(end-2,:));
subplot(3,1,3);
plot(ttt,aa(end-2,:));

figure%绘制最后时刻框架变形
RenderFrame(ele,Load,Constr,x,y,uuu(:,end),1,'-.b',200) %


speed=1;%动画播放速度的
delaytime=0;
RenderFrameMotion(ele,Load,Constr,x,y,uuu,1,'-.b',delaytime,speed) %