%解析法求非周期激励的响应。采样、卷积积分法。
%SH760小轿车空载主要参数
clear;
m=1340;
a=1.54;
b=1.29;
l=a+b;
Ic=2395; %绕质心的转动惯量
rou=sqrt(Ic/m);
k1=40*1000;
k2=44*1000;
M=[m*(b^2+rou^2)/l^2,m*(a*b-rou^2)/l^2;m*(a*b-rou^2)/l^2,m*(a^2+rou^2)/l^2];
K=[k1,0;0,k2];
 
%用matlab特征值分解法求主振型------------------------------------------------
 
[eig_vec,eig_val] = eig(inv(M)*K);
[omeg,w_order]    = sort(sqrt(diag(eig_val)));   %频率用sort排序
mode_vec = eig_vec(:,w_order); %振型
mode_vec_n=mode_n(mode_vec,M);
T=2.*pi./omeg;    %周期
%mode_vec(:,1)=mode_vec(:,1)./mode_vec(1,1);
%mode_vec(:,2)=mode_vec(:,2)./mode_vec(1,2);
 
w1=sqrt((k1*l^2)/(m*(b^2+rou^2)));
w2=sqrt((k2*l^2)/(m*(a^2+rou^2)));
w1_pian=sqrt((k1*l)/(m*b));
w2_pian=sqrt((k2*l)/(m*a));
 
subplot(3,3,1)
plot([1;2],mode_vec(:,1))
title(strcat('w_1=',num2str(omeg(1)),';w_1pian=',num2str(w1_pian)));
subplot(3,3,2)
plot([1;2],mode_vec(:,2))
title(strcat('w_2=',num2str(omeg(2)),';w_2pian=',num2str(w2_pian)));
x0=[0;0];dx0=[0;0]; %初始条件
q0=inv(mode_vec_n)*x0;
dq0=inv(mode_vec_n)*dx0;
syms t;

tf=2;dt=0.02; %时间向量
ti=0:dt:tf;
F={'20883*1*(t<0.2)';'-4353*(t<0.2)'}; %以字符串元胞数组表示输入力函数
Fi=zeros(2,length(ti));
for k=1:2 %对输入力进行采样！！！
    for kk=1:length(ti)
        if k==1
            Fi(k,kk)=subs(F{k},'t',ti(kk)); %对输入力按dt间隔进行采样！
            Fi(k,kk)=20883*1*Fi(k,kk);
        else
            Fi(k,kk)=subs(F{k},'t',ti(kk)); %对输入力按dt间隔进行采样！
            Fi(k,kk)=-4353*1*Fi(k,kk);
        end
    end
end

fi=mode_vec_n'*Fi;
%q=sym(zeros(2,1));
for k=1:2
    %q(k)=q0(k)*cos(omeg(k)*t)+dq0(k)/omeg(k)*sin(omeg(k)*t);%初始状态响应形式一
    %A=sqrt(q0(k)^2+(dq0(k)/omeg(k))^2);
    %theta=atan(omeg(k)*q0(k)/dq0(k));
    %q(k)=A*sin(omeg(k)*t+theta);%初始状态响应形式二
    h(k)=1/omeg(k)*sin(omeg(k)*t); %单位脉冲响应
end

hi=zeros(2,length(ti));
hifi=zeros(2,length(ti)*2-1);
for k=1:2 %用单位脉冲响应求卷积,注意按dt间隔进行采样！！！
    hi(k,:)=subs(h(k),'t',ti); %对单位脉冲响应按dt间隔进行采样！
    hifi(k,:)=conv(hi(k,:),fi(k,:))*dt; %把脉冲响应与外加力作卷积 %卷积
end
qi=hifi(:,1:length(ti));
xi=mode_vec_n*qi;
subplot(3,3,3)
plot([0;1],[0,0;mode_vec(2)/mode_vec(1),mode_vec(4)/mode_vec(3)]),hold
plot(xi(1,:)',xi(2,:)')
xlabel('x_1');ylabel('x_2')
subplot(3,3,4),plot(ti',xi'),grid
xlabel('t');ylabel('x_1,x_2')
subplot(3,3,5)
plot3([0;1],[0,0;mode_vec(2)/mode_vec(1),mode_vec(4)/mode_vec(3)],[0,0]),hold
plot3(xi(1,:)',xi(2,:)',ti'),grid
xlabel('x_1');ylabel('x_2');zlabel('t');
subplot(3,3,7)
plot(xi(1,2:length(xi(1,:)))',diff(xi(1,:)')/dt),grid
xlabel('x_1');ylabel('x_1''');
subplot(3,3,8)
plot(xi(2,2:length(xi(2,:)))',diff(xi(2,:)')/dt),grid
xlabel('x_2');ylabel('x_2''');
subplot(3,3,6)
plot(ti',Fi'),grid
xlabel('t');ylabel('F_1,F_2');

%由振型矩阵mode_vec（特征列向量组成）、质量矩阵M求正则振型矩阵mode_vec_n
function mode_vec_n= mode_n(mode_vec,M)
M_p=mode_vec'*M*mode_vec;%求主质量阵
n=length(mode_vec);
mode_vec_n=zeros(n);
for i=1:n
     mode_vec_n(:,i)=mode_vec(:,i)/sqrt(M_p(i,i));
end
%mode_vec_n'*M*mode_vec_n=I
%mode_vec_n^(-1)=mode_vec_n'*M
%I*q_n"+[w_n^2]*q_n=[0]
%x=mode_vec_n*q_n
%正则坐标 q_n=mode_vec_n^(-1)*x