clear;
%SH760小轎車空載主要引數
m=1340;
a=1.54;
b=1.29;
Ic=2395; %繞質心的轉動慣量
k1=40000;
k2=44000;
M=[m,0;0,Ic];
K=[k1+k2,-(k1*a-k2*b);-(k1*a-k2*b),k1*a^2+k2*b^2];
 
[eig_vec,eig_val] = eig(inv(M)*K);
[omeg,w_order]    = sort(sqrt(diag(eig_val)));   %頻率
mode_vec = eig_vec(:,w_order); %振型
T=2.*pi./omeg;    %週期
mode_vec(:,1)=mode_vec(:,1)./mode_vec(1,1);
mode_vec(:,2)=mode_vec(:,2)./mode_vec(1,2);
subplot(2,2,1)
plot([1;2],mode_vec(:,1))
title(strcat('w1=',num2str(omeg(1))));
subplot(2,2,2)
plot([1;2],mode_vec(:,2))
title(strcat('w2=',num2str(omeg(2))));

x0=[0.04;-atan(0.04/b)];xd0=[0;0]; %初始條件
tf=11.8;dt=0.02; %時間向量
A=[zeros(2,2),eye(2);-M\K,zeros(2,2)]; %四階引數矩陣Y'=AY-->Y=expm(A*t)*Y0 Y=[x1;x2;x1';x2']
%expm(A)的意義是將座標先變換到主座標系，對對角值進行exp運算後再變換到原座標系，如同張量座標變換help expm
y0=[x0;xd0]; %四元變數的初始條件
for i=1:round(tf/dt)+1 %設定計算點，作迴圈計算
    tj(i)=dt*(i-1);
    y(:,i)=expm(A*tj(i))*y0; %迴圈計算矩陣指數
end
subplot(2,2,4),plot(tj,[y(1,:)',y(2,:)']),grid

World=vrworld('vrml_2mk');
open(World);
view(World);
Car=vrnode(World,'Octavia_body');
for i=1:round(tf/dt)+1 
    Car.translation=[0,y(1,i),0];
    Car.rotation=[0,0,1,-y(2,i)];vrdrawnow
    pause(dt);
end
%close(World);
%delete(World);