% Parameters
nr_elements = 10;
rho = 8000;     % [kg/m^3]
E = 210e9;      % [Pa]

k = 1e10;

% Euler beam element
% q = [v1 p1 v2 p2 v3 p3 v4 p4 v5 p5]';
le = 4/nr_elements;
Ae = 420/1/rho;
Ie = 1^3/E;

Ke = E*Ie/le^3*[12 6*le -12 6*le; 6*le 4*le^2 -6*le 2*le^2; -12 -6*le 12 -6*le; 6*le 2*le^2 -6*le 4*le^2];
Me = rho*Ae*le/420*[156 22*le 54 -13*le; 22*le 4*le^2 13*le -3*le^2; 54 13*le 156 -22*le; -13*le -3*le^2 -22*le 4*le^2];

dofs = (nr_elements+1)*2; % degree of freedom: translation and rotation of end points of each beam element
MT = zeros(dofs,dofs); 
KT = zeros(dofs,dofs);

for iii = 1:2:2*nr_elements-1 
    MT(iii:iii+3,iii:iii+3) = MT(iii:iii+3,iii:iii+3)+Me;
    KT(iii:iii+3,iii:iii+3) = KT(iii:iii+3,iii:iii+3)+Ke;
end

KT(1,1)=KT(1,1)+k;
KT(end-1,end-1)=KT(end-1,end-1)+k;

% state space model
% y = [q dqdt]'; ==> dydt = [dqdt d2qdt2]
OT = zeros(size(MT));
C = [OT MT; MT OT];
D = [KT OT; OT -MT];

[V,L] = eig(D,C);
[lambda,order] = sort(diag(L));

omega0 = abs(lambda(1:2:dofs));
f0 = omega0/2/pi;

lambda(1:6)
f0(1:3)

V = V(:,order);

for ccc = 1:size(V,2)
    V(:,ccc) = V(:,ccc)./max(abs(V(:,ccc)));
end