clc;
clear;
close all;

L = 0.2;
m = 2;
g = 9.81;
zeta = 0.05;

M = m*L^2 * [3 2 1;
             2 2 1;
             1 1 1];

K = m*g*L * [3 0 0;
             0 2 0;
             0 0 1];

% Eigenvalue problem
[V,D] = eig(K,M);

[lambda,idx] = sort(diag(D));
V = V(:,idx);

wn = sqrt(lambda);

% Mass-normalize eigenvectors
for i = 1:3
    V(:,i) = V(:,i)/sqrt(V(:,i)'*M*V(:,i));
end

% Modal damping matrix
Cmodal = diag(2*zeta*wn);

% Convert modal damping to physical coordinates
C = M*V*Cmodal*V'*M;

% Harmonic generalized force
Q = [-0.4; 0.5; 0];

% Excitation frequency range
w = 0.01:0.01:25;

Theta = zeros(3,length(w));

% Calculate frequency response
for j = 1:length(w)

    D = K - w(j)^2*M + 1i*w(j)*C;

    Theta(:,j) = D\Q;

end

% Amplitudes
A1 = abs(Theta(1,:));
A2 = abs(Theta(2,:));
A3 = abs(Theta(3,:));

% Plot
figure;

subplot(3,1,1);
plot(w,A1,'LineWidth',1.5);
grid on;
ylabel('|\Theta_1| (rad)');
title('Frequency Response Functions');

subplot(3,1,2);
plot(w,A2,'LineWidth',1.5);
grid on;
ylabel('|\Theta_2| (rad)');

subplot(3,1,3);
plot(w,A3,'LineWidth',1.5);
grid on;
ylabel('|\Theta_3| (rad)');
xlabel('Excitation frequency (rad/s)');

disp('Natural frequencies (rad/s):');
disp(wn);