clear all
clc
close all
m=[5.98000 1.08000 1.04000 2.91300 2.91300 2.91300 2.91300 2.91300 51.46300];
c=[0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000];
k=[0.00000 8.20000 39.20000 15.00000 11.78000 11.78000 11.78000 11.78000 11.78000 16.66000 0.00000];
F=[0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000 0.00000];
n=length(F);
M=zeros(n);
C=zeros(n);
K=zeros(n);
for i=1:n
    M(i,i)=m(i);
end
kk=kk*1.0e5
for i=1:n
   K(i,i)=k(i)+k(i+1);
    c(i,i)=c(i)+c(i+1);
end
for i=1:n-1
            K(i,i+1)=-k(i+1);
            C(i,i+1)=-c(i+1);
            K(i+1,i)=-k(i+1);
            C(i+1,i)=-c(i+1);
end
%M=[1 0;0 1];%2
%C=[2 -1;-1 1];%2
%K=[6 -5;-5 5];%2
%F=[0;0];%2
%M=[1 0 0;0 1 0;0 0 1];%3
%C=[0 0 0;0 0 0;0 0 0];%3
%K=[2 -1 0;-1 2 -1;0 -1 1];%3
%F=[1;1;1];
n=length(F);
t=0;
S=inv(M)*K;
[px pn]=eig(S);
pn=pn^0.5;
p=diag(pn);
fmax=min(p);
T=1/fmax;
h=T/400;
%h=pi/40;
dn=rem(T,h);
dnn=T-dn;
n1=T/h;
%f0=F*sin(t);
f0=F*sin(t);
n=length(F);
i=1;
x1=[-0.000001;-0.000003;-0.000002;-0.000002];%3
x2=[0.0001;0.0002;0.0003;0.0002;0.0001;0.0004];%3
%x1=[-7/15;-3/5];%2
%x2=[-3/5;-8/15];%2
%x1=[0;0;0];
%x2=[0;0;0];
x3=inv(M)*(f0-C*x2-K*x1);
xy1=zeros(n+1,3000);
xy1(1,i)=t;
xy1(2:n+1,i)=x1;

b=1.4;
a0=6/b^2/h^2;
a1=3/b/h;
a2=2*a1;
a3=b*h/2;
a4=a0/b;
a5=-a2/b;
a6=1-3/b;
a7=h/2;
a8=h^2/6;
 K1=K+a0*M+a1*C;
for i=2:3000
t=t+h;
fa=F*sin(t-h)+b*(F*sin(t)-F*sin(t-h))+M*(a0*x1+a2*x2+2*x3)+C*(a1*x1+2*x2+a3*x3);
xb=inv(K1)*fa;
xx3=a4*(xb-x1)+a5*x2+a6*x3;
xx2=x2+a7*(xx3+x3);
xx1=x1+h*x2+a8*(xx3+2*x3);
x1=xx1;
x2=xx2;
x3=xx3;
xy1(1,i)=t;
xy1(2:n+1,i)=x1;
end
figure(1);
plot(xy1(1,:),xy1(2,:));
figure(2);
plot(xy1(1,:),xy1(3,:));
figure(3);
plot(xy1(1,:),xy1(4,:));