clear all;
close all;
clc
global M1 M2 B K1 K2 

%Zobrazení grafù ---> 0/1 vypíná/zapíná 
odezvy=0;
vlastni_tvary=0;
vlastni_tvary_vybrane=0;
normovane_vlastni_tvary=0;
normovane_vlastni_tvary_vybrane=0;
tspan=[0 2.5];          %èas výpotu odezvy
cara=1.5;                 %tlouš�ka èáry v grafech

%%%---------------------------------------------------------------------%%%
%%%------------------------- zadání parametrù --------------------------%%%
%%%---------------------------------------------------------------------%%%
%rozmery
p=1.8; %[m]
wk=0.41; %[m]
wb=0.71; %[m]
L1=4.3375; %[m]
L2=6.8925; %[m]
L3=2.1650; %[m]
L4=6.8925; %[m]
L5=2.1725; %[m]
a1=18.115; %[m] délka skøínì 1
a2=13.785; %[m] délka skøínì 2
hmot=975; %[kg/m] hmotnost skøínì na 1 metr délky
L=[L1 L2 L3 L4 L5];
%hmotnosti cestujících
sedici1=3500; %[kg]
sedici2=2380; %[kg]
stojici1_1=1346; %[kg] stojici"cislo skrine"_"hustota obsazeni"
stojici1_2=2691;
stojici1_3=4037;
stojici1_4=5382;
stojici1_5=6728;
stojici1_6=8073;
stojici1_7=9419;
stojici1_73=9823;
stojici1_8=10765;
stojici2_1=980;
stojici2_2=1960;
stojici2_3=2941;
stojici2_4=3921;
stojici2_5=4901;
stojici2_6=5881;
stojici2_7=6861;
stojici2_73=7155;
stojici2_8=7841;
%hmotnosti a momenty setrvacnosti podvozkù
mp1=4300; %[kg]
mp2=3200; %[kg]
mp3=4300; %[kg]
Ip1=3500; %[kg*m2]
Ip2=2500; %[kg*m2]
Ip3=3500; %[kg*m2]
%hmotnosti a momenty setrvacnosti prázdné skøínì
ms1=a1*hmot; %[kg]
ms2=a2*hmot; %[kg]
Is1=ms1*a1^2/12; %[kg*m2]
Is2=ms2*a2^2/12; %[kg*m2]
%hmotnosti a momenty setrvacnosti plné skøínì
ms1p=ms1+sedici1+stojici1_4; %[kg]
ms2p=ms2+sedici2+stojici2_4; %[kg]
Is1p=ms1p*a1^2/12; %[kg*m2]
Is2p=ms2p*a2^2/12; %[kg*m2]

%Vypružení podvozkù
kp1=4170000; %[N/m]
kp2=6000000; %[N/m]
kp3=4170000; %[N/m]
bp1=2500; %[Ns/m]
bp2=2500; %[Ns/m]
bp3=2500; %[Ns/m]
%Vypružení prázdné skøínì
ks1=770000; %[N/m]
ks2=1135000; %[N/m]
ks3=770000; %[N/m]
bs1=50000; %[Ns/m]
bs2=60000; %[Ns/m]
bs3=50000; %[Ns/m]
%Vypružení plné skøínì
%ks1p=1150000; %[N/m]
%ks2p=1510000; %[N/m]
%ks3p=1150000; %[N/m]
ks1p=770000; %[N/m]
ks2p=1135000; %[N/m]
ks3p=770000; %[N/m]

%%%---------------------------------------------------------------------%%%
%%%----------------------------- plnìní matic --------------------------%%%
%%%---------------------------------------------------------------------%%%
%pomocné promenné pro plnení matic
pom1=Is2/(L4^2);
pom2=L5/L4;
pom3=L2+L3;
pom4=1/(L4^2);
pom5=Is2p/(L4^2);
%matice hmotnosti prázdného vozidla
M1=[mp1 0 0 0 0 0 0 0 0
    0 Ip1 0 0 0 0 0 0 0
    0 0 mp2 0 0 0 0 0 0 
    0 0 0 Ip2 0 0 0 0 0
    0 0 0 0 mp3 0 0 0 0
    0 0 0 0 0 Ip3 0 0 0
    0 0 0 0 0 0 ms1+pom1 pom1*(L2+L3) -pom1
    0 0 0 0 0 0 pom1*(L2+L3) Is1+pom1*(L2+L3) -pom1*(L2+L3)
    0 0 0 0 0 0 -pom1 -pom1*(L2+L3) ms2+pom1];
%matice hmotnosti plného vozidla
M2=[mp1 0 0 0 0 0 0 0 0
    0 Ip1 0 0 0 0 0 0 0
    0 0 mp2 0 0 0 0 0 0 
    0 0 0 Ip2 0 0 0 0 0
    0 0 0 0 mp3 0 0 0 0
    0 0 0 0 0 Ip3 0 0 0
    0 0 0 0 0 0 ms1p+pom5 pom5*(L2+L3) -pom5
    0 0 0 0 0 0 pom5*(L2+L3) Is1p+pom5*(L2+L3) -pom5*(L2+L3)
    0 0 0 0 0 0 -pom5 -pom5*(L2+L3) ms2p+pom5];
%matice tlumení
B=[2*bs1+2*bp1 0 0 0 0 0 -2*bs1 2*bs1*L1 0
   0 bs1*wb+p*bp1 0 0 0 0 0 -bs1*wb 0
   0 0 2*bs2+2*bp2 0 0 0 -2*bs2 -2*bs2*L2 0
   0 0 0 bs2*wb+p*bp2 0 0 0 -bs2*wb 0
   0 0 0 0 2*bs3+2*bp3 0 2*bs3*pom2 2*bs3*pom2*pom3 -2*bs3*(1+pom2)
   0 0 0 0 0 bs3*wb+p*bp3 bs3*wb/L4 bs3*wb*pom3/L4 -bs3*wb/L4
   -2*bs1 0 -2*bs2 0 2*bs3*pom2 bs3*wb/L4 2*bs1+2*bs2+2*bs3*(pom2^2+wb/L4^2) ...
   -2*bs1*L1+2*bs2*L2+2*bs3*pom3*pom4*(L5^2+wb/2) -2*bs3*pom2*(1+pom2)-bs3*wb*pom4 
   2*bs1*L1 -bs1*wb -2*bs2*L2 -bs2*wb 2*bs3*pom2*pom3 bs3*wb*pom3/L4 -2*bs1*L1+2*bs2*L2+2*bs3*pom3*pom4*(wb/2+L5^2) ...
   2*bs1*L1^2+2*bs2*L2^2+bs1*wb+bs2*wb+2*bs3*pom3^2*pom4*(wb+2*L5^2) -2*bs3*pom2*pom3*(1+pom2)-bs3*wb*pom3*pom4
   0 0 0 0 -2*bs3*(1+pom2) -bs3*wb/L4 -2*bs3*pom2*(1+pom2)-bs3*wb*pom4 -2*bs3*pom2*pom3*(1+pom2)-bs3*wb*pom3*pom4 ...
   2*bs3+2*bs3*pom2*(1+pom2)+bs3*wb*pom4];
%matice tuhosti prázdného vozidla
K1=[2*ks1+2*kp1 0 0 0 0 0 -2*ks1 2*ks1*L1 0
   0 ks1*wk+p*kp1 0 0 0 0 0 -ks1*wk 0
   0 0 2*ks2+2*kp2 0 0 0 -2*ks2 -2*ks2*L2 0
   0 0 0 ks2*wk+p*kp2 0 0 0 -ks2*wk 0
   0 0 0 0 2*ks3+2*kp3 0 2*ks3*pom2 2*ks3*pom2*pom3 -2*ks3*(1+pom2)
   0 0 0 0 0 ks3*wk+p*kp3 ks3*wk/L4 ks3*wk*pom3/L4 -ks3*wk/L4
   -2*ks1 0 -2*ks2 0 2*ks3*pom2 ks3*wk/L4 2*ks1+2*ks2+2*ks3*(pom2^2+wk/L4^2) ...
   -2*ks1*L1+2*ks2*L2+2*ks3*pom3*pom4*(L5^2+wk/2) -2*ks3*pom2*(1+pom2)-ks3*wk*pom4 
   2*ks1*L1 -ks1*wk -2*ks2*L2 -ks2*wk 2*ks3*pom2*pom3 ks3*wk*pom3/L4 -2*ks1*L1+2*ks2*L2+2*ks3*pom3*pom4*(wk/2+L5^2) ...
   2*ks1*L1^2+2*ks2*L2^2+ks1*wk+ks2*wk+2*ks3*pom3^2*pom4*(wk+2*L5^2) -2*ks3*pom2*pom3*(1+pom2)-ks3*wk*pom3*pom4
   0 0 0 0 -2*ks3*(1+pom2) -ks3*wk/L4 -2*ks3*pom2*(1+pom2)-ks3*wk*pom4 -2*ks3*pom2*pom3*(1+pom2)-ks3*wk*pom3*pom4 ...
   2*ks3+2*ks3*pom2*(1+pom2)+ks3*wk*pom4];
%matice tuhosti plného vozidla
K2=[2*ks1p+2*kp1 0 0 0 0 0 -2*ks1p 2*ks1p*L1 0
   0 ks1p*wk+p*kp1 0 0 0 0 0 -ks1p*wk 0
   0 0 2*ks2p+2*kp2 0 0 0 -2*ks2p -2*ks2p*L2 0
   0 0 0 ks2p*wk+p*kp2 0 0 0 -ks2p*wk 0
   0 0 0 0 2*ks3p+2*kp3 0 2*ks3p*pom2 2*ks3p*pom2*pom3 -2*ks3p*(1+pom2)
   0 0 0 0 0 ks3p*wk+p*kp3 ks3p*wk/L4 ks3p*wk*pom3/L4 -ks3p*wk/L4
   -2*ks1p 0 -2*ks2p 0 2*ks3p*pom2 ks3p*wk/L4 2*ks1p+2*ks2p+2*ks3p*(pom2^2+wk/L4^2) ...
   -2*ks1p*L1+2*ks2p*L2+2*ks3p*pom3*pom4*(L5^2+wk/2) -2*ks3p*pom2*(1+pom2)-ks3p*wk*pom4 
   2*ks1p*L1 -ks1p*wk -2*ks2p*L2 -ks2p*wk 2*ks3p*pom2*pom3 ks3p*wk*pom3/L4 -2*ks1p*L1+2*ks2p*L2+2*ks3p*pom3*pom4*(wk/2+L5^2) ...
   2*ks1p*L1^2+2*ks2p*L2^2+ks1p*wk+ks2p*wk+2*ks3p*pom3^2*pom4*(wk+2*L5^2) -2*ks3p*pom2*pom3*(1+pom2)-ks3p*wk*pom3*pom4
   0 0 0 0 -2*ks3p*(1+pom2) -ks3p*wk/L4 -2*ks3p*pom2*(1+pom2)-ks3p*wk*pom4 -2*ks3p*pom2*pom3*(1+pom2)-ks3p*wk*pom3*pom4 ...
   2*ks3p+2*ks3p*pom2*(1+pom2)+ks3p*wk*pom4];
%%%---------------------------------------------------------------------%%%
%%%-------------------------- Analýza systému --------------------------%%%
%%%---------------------------------------------------------------------%%%
%vlastni frekvence netlumene soustavy
[V1,D1]=eig(inv(M1)*K1);
[V2,D2]=eig(inv(M2)*K2);
[W1,C1]=eig(inv(M1')*K1');
[W2,C2]=eig(inv(M2')*K2');
frekvence1=(diag((sqrt(D1))/(2*pi)));
frekvence2=(diag((sqrt(D2))/(2*pi)));
%disp('Vlastní frekvence prázdného vozidla: ')
%disp(sort(frekvence1))
%disp('Vlastní frekvence plného vozidla: ')
%disp(sort(frekvence2))

%analýza tlumené soustavy
A1=[zeros(9,9) eye(9,9);
    -inv(M1)*K1 -inv(M1)*B];
A2=[zeros(9,9) eye(9,9);
    -inv(M2)*K2 -inv(M2)*B];
[Wn1,zeta1,P1] = damp(A1);
[Wn2,zeta2,P2] = damp(A2);
[fr1,poradi1]=sort(Wn1/(2*pi));
[fr2,poradi2]=sort(Wn2/(2*pi));
r1=zeros(18,1);
r2=zeros(18,1);
for i = 1:18,
    [j] = poradi1(i);
    r1(i) = zeta1(j);
end
for i = 1:18,
    [j] = poradi2(i);
    r2(i) = zeta2(j);
end
fr1=fr1(1:2:18);
fr2=fr2(1:2:18);
r1=r1(1:2:18);
r2=r2(1:2:18);
disp('Vlastní tlumené frekvence prázdného vozidla: fr1')
disp(fr1)
disp('Pomìrný útlum: ')
disp(r1)
disp('Vlastní tlumené frekvence obsazeného vozidla: fr2')
disp(fr2)
disp('Pomìrný útlum: ')
disp(r2)
%disp('Vlastní frekvence a útlumy prázdného/obsazeného vozidla:')
%xx=[fr1,r1,fr2,r2];
%disp(xx)

%%%---------------------------------------------------------------------%%%
%%%----------------------------- Grafy odezvy --------------------------%%%
%%%---------------------------------------------------------------------%%%
%posuvy=[z1 phi1 z2 phi2 z3 phi3 z4 phi4 z5]; poèátení podmínky
posuvy=[0.003; 0.00; 0.003; -0.00; -0.003; 0.00; 0.015; 0.00; -0.015;];
%posuvy=[-0.00429; 0.001019; 0.004612; 0.000285; -0.000231; -0.000965; -0.002595; 0.003507; 0.00626;];
%rychlosti=[dz1 dphi1 dz2 dphi2 dz3 dphi3 dz4 dphi4 dz5]; poèátení podmínky
rychlosti=[0.0; 0; 0.0; 0; 0.0; 0; -0.0; 0; -0.0;];
y0=[posuvy; rychlosti];

if odezvy==1;
    %odezva prazdneho 
    [t,y3]=ode45('odezva3',tspan,y0);
    figure(3)
    subplot(2,1,1);
    plot(t,1000*y3(:,1),'m-.','LineWidth',cara); hold on;
    plot(t,1000*y3(:,3),'g-.','LineWidth',cara);
    plot(t,1000*y3(:,5),'r-.','LineWidth',cara);
    plot(t,1000*y3(:,7),'b','LineWidth',cara);
    plot(t,1000*y3(:,9),'k','LineWidth',cara);
    grid on
    xlabel('èas [s]')
    ylabel('výchylka z_i [mm]')
    title('Odezva prázdného vozidla')
    legend('z_1 podvozek1','z_2 podvozek2','z_3 podvozek3','z_4 skrin 1','z_5 skrin 2')
    subplot(2,1,2);
    plot(t,180*y3(:,2)/pi,'m-.','LineWidth',cara); hold on;
    plot(t,180*y3(:,4)/pi,'g-.','LineWidth',cara);
    plot(t,180*y3(:,6)/pi,'r-.','LineWidth',cara);
    plot(t,180*y3(:,8)/pi,'b','LineWidth',cara);
    phi5=(y3(:,9)-y3(:,7)-(L2+L3)*y3(:,8))/L4;
    plot(t,180*phi5/pi,'k','LineWidth',cara);
    grid on
    xlabel('èas [s]')
    ylabel('natoèení \phi_i [°]')
    legend('\phi_1 podvozek 1','\phi_2 podvozek 2','\phi_3 podvozek 3','\phi_4 skrin 1','\phi_5 skrin 2')
    
    %odezva plneho 
    [t,y4]=ode45('odezva4',tspan,y0);
    figure(4)
    subplot(2,1,1);
    plot(t,1000*y4(:,1),'m-.','LineWidth',cara); hold on;
    plot(t,1000*y4(:,3),'g-.','LineWidth',cara);
    plot(t,1000*y4(:,5),'r-.','LineWidth',cara);
    plot(t,1000*y4(:,7),'b','LineWidth',cara);
    plot(t,1000*y4(:,9),'k','LineWidth',cara);
    grid on
    xlabel('èas [s]')
    ylabel('svislá výchylka z_i [mm]')
    title('Odezva provoznì obsazeného vozidla')
    legend('z_1 podvozek1','z_2 podvozek2','z_3 podvozek3','z_4 skrin 1','z_5 skrin 2')
    subplot(2,1,2);
    plot(t,180*y4(:,2)/pi,'m-.','LineWidth',cara); hold on;
    plot(t,180*y4(:,4)/pi,'g-.','LineWidth',cara);
    plot(t,180*y4(:,6)/pi,'r-.','LineWidth',cara);
    plot(t,180*y4(:,8)/pi,'b','LineWidth',cara);
    phi5p=(y4(:,9)-y4(:,7)-(L2+L3)*y4(:,8))/L4;
    plot(t,180*phi5p/pi,'k','LineWidth',cara);
    grid on
    xlabel('èas [s]')
    ylabel('natoèení \phi_i [°]')
    legend('\phi_1 podvozek 1','\phi_2 podvozek 2','\phi_3 podvozek 3','\phi_4 skrin 1','\phi_5 skrin 2')
end
%%%---------------------------------------------------------------------%%%
%%%------------------------- Vlastní tvary kmitu -----------------------%%%
%%%---------------------------------------------------------------------%%%
if vlastni_tvary==1
    %Vlastni tvary kmitu 
    x=1:1:9;
    figure(6);
    plot(x,V2(:,1),'k','LineWidth',cara); hold on;
    plot(x,V2(:,2),'r-.','LineWidth',cara);
    plot(x,V2(:,3),'g-.','LineWidth',cara);
    plot(x,V2(:,4),'m:','LineWidth',cara);
    plot(x,V2(:,5),'b:','LineWidth',cara);
    plot(x,V2(:,6),'k-.','LineWidth',cara);
    plot(x,V2(:,7),'r-','LineWidth',cara);
    plot(x,V2(:,8),'g--','LineWidth',cara);
    plot(x,V2(:,9),'m--','LineWidth',cara);
    title('Vlastní tvary provozne lozeny');
    xlabel('souradnice [z_1, \phi_1, z_2, \phi_2, z_3, \phi_3, z_4, \phi_4, z_5]');
    ylabel('vlastní vektor');
    pop1=sprintf('1. tvar pro %0.2f [Hz]',frekvence2(1));
    pop2=sprintf('2. tvar pro %0.2f [Hz]',frekvence2(2));
    pop3=sprintf('3. tvar pro %0.2f [Hz]',frekvence2(3));
    pop4=sprintf('4. tvar pro %0.2f [Hz]',frekvence2(4));
    pop5=sprintf('5. tvar pro %0.2f [Hz]',frekvence2(5));
    pop6=sprintf('6. tvar pro %0.2f [Hz]',frekvence2(6));
    pop7=sprintf('7. tvar pro %0.2f [Hz]',frekvence2(7));
    pop8=sprintf('8. tvar pro %0.2f [Hz]',frekvence2(8));
    pop9=sprintf('9. tvar pro %0.2f [Hz]',frekvence2(9));
    legend(pop1,pop2,pop3,pop4,pop5,pop6,pop7,pop8);
end 
 if vlastni_tvary_vybrane==1
    %Vlastni tvary kmitu 5,6,7
    x=1:1:9;
    figure(8);
    plot(x,V2(:,3),'b:','LineWidth',cara);hold on;
    plot(x,V2(:,4),'k-.','LineWidth',cara);
    plot(x,V2(:,5),'r-','LineWidth',cara);
    plot(x,zeros(9,1),'k:');
    title('Vlastní tvary xxx');
    xlabel('souradnice [z_1, \phi_1, z_2, \phi_2, z_3, \phi_3, z_4, \phi_4, z_5]');
    ylabel('vlastní vektor');
    pop2=sprintf('3. tvar pro %0.2f [Hz]',frekvence2(3));
    pop3=sprintf('4. tvar pro %0.2f [Hz]',frekvence2(4));
    pop4=sprintf('5. tvar pro %0.2f [Hz]',frekvence2(5));
    legend(pop2,pop3,pop4);
end

% Razeni levostrannych vlastnich vektoru
Y1 = W1;
for i = 1:9,
 pom1 = diag(D1);
 [j] = find(C1(i,i) == pom1);
 Y1(:,i) = W1(:,j);
end
W1 = Y1;
% Normovani vlastnich vektoru
Mmod1 = W1'*M1*V1;
V1 = V1*inv(sqrt(Mmod1));
W1 = W1*inv(sqrt(Mmod1));

if normovane_vlastni_tvary==1
    %normované Vlastni tvary kmitu 
    x=1:1:9;
    figure(9);
    plot(x,W1(:,1),'k','LineWidth',cara); hold on;
    plot(x,W1(:,2),'r-.','LineWidth',cara);
    plot(x,W1(:,3),'g-.','LineWidth',cara);
    plot(x,W1(:,4),'m:','LineWidth',cara);
    plot(x,W1(:,5),'b:','LineWidth',cara);
    plot(x,W1(:,6),'k-','LineWidth',cara);
    plot(x,W1(:,7),'r-','LineWidth',cara);
    plot(x,W1(:,8),'g--','LineWidth',cara);
    plot(x,W1(:,9),'m--','LineWidth',cara);
    title('Normované vlastní tvary');
    xlabel('souradnice [z_1, \phi_1, z_2, \phi_2, z_3, \phi_3, z_4, \phi_4, z_5]');
    ylabel('vlastní vektor');
    
end 
if normovane_vlastni_tvary_vybrane==1
    %Normovane Vlastni tvary kmitu 5,6,7
    x=1:1:9;
    figure(10);
    plot(x,W1(:,3),'b:','LineWidth',cara);hold on;
    plot(x,W1(:,4),'k-','LineWidth',cara);
    plot(x,W1(:,5),'r-','LineWidth',cara);
    plot(x,zeros(9,1),'k:');
    title('Normované vlastní tvary 2,3,4');
    xlabel('souradnice [z_1,  \phi_1,  z_2,  \phi_2,  z_3,  \phi_3,  z_4,  \phi_4,  z_5]');
    ylabel('vlastní vektor');
    pop2=sprintf('2. tvar pro %0.2f [Hz]',frekvence2(3));
    pop3=sprintf('3. tvar pro %0.2f [Hz]',frekvence2(4));
    pop4=sprintf('4. tvar pro %0.2f [Hz]',frekvence2(5));
    legend(pop2,pop3,pop4);
     
end    