close all
clear all
clc

m1=3;
m2=2;
L=1;
L1=0.25;
J1=0.25;


k1=5000;
k2=150;
c1=0;
c2=1;

g=9.81;

MP=diag([m1,J1,m2]);
KP=diag([k1,k2]);
CP=diag([c1,c2]);

JM=[1 -L/2 0
0 1 0
1 0 0];
JK=[0 L1 0
-1 0 1];
JF=[1 -L 0
0 0 1];

M=JM'*MP*JM;
Kel=JK'*KP*JK;
C=JK'*CP*JK;


Kg=[0 0 0
0 -m1*g*L/2 0
0 0 0];
K=Kg+Kel;

Mff=M(1:2,1:2);
Cff=C(1:2,1:2);
Kff=K(1:2,1:2);

Mfc=M(1:2,3);
Cfc=C(1:2,3);
Kfc=K(1:2,3);

Mcf=M(3,1:2);
Kcf=K(3,1:2);
Ccf=C(3,1:2);

Mcc=M(3,3);
Kcc=K(3,3);
Ccc=C(3,3);

[v,d]=eig(inv(Mff)*Kff);
fre=sqrt(diag(d))/2/pi
[a,ind]=max(abs(v));
for k=1:length(Mff)
modi(:,k)=v(:,k)/v(ind(k),k);
end
modi
modi=v

freq=[0:0.001:10];
F=[1;0];
Q=JF'*F;
f0=Q(1:2);
for j=1:length(freq)
ome=2*pi*freq(j);
A=-ome^2*Mff+i*ome*Cff+Kff;
B=-ome^2*Mcf+i*ome*Ccf+Kcf;
x0=inv(A)*f0;
xa(j)=x0(1)+L*x0(2);
xb(j)=x0(1);
Re(j)=B*x0;
end
figure(1)
subplot(211)
plot(freq,abs(xa));grid
title('Frf x_a/F')
subplot(212)
plot(freq,angle(xa));grid
figure(2)
subplot(211)
plot(freq,abs(xb));grid
title('Frf x_b/F')
subplot(212)
plot(freq,angle(xb));grid
figure(3)
subplot(211)
plot(freq,abs(Re));grid
title('Frf Re/F')
subplot(212)
plot(freq,angle(Re));grid

% for j=1:length(freq)
% ome=2*pi*freq(j);
% A=-ome^2*Mff+i*ome*Cff+Kff;
% f0=-Kfc-i*ome*Cfc+ome^2*Mfc;
% x0=inv(A)*f0;
% xa_v(j)=x0(1)-L*x0(2);
% xb_v(j)=x0(1);
% end
%
% figure(4)
% subplot(211)
% plot(freq,abs(xa_v));grid
% title('FRF x_a/z')
% subplot(212)
% plot(freq,angle(xa_v));grid
% figure(5)
% subplot(211)
% plot(freq,abs(xb_v));grid
% title('FRF x_b/z')
% subplot(212)
% plot(freq,angle(xb_v));grid