octaveで6自由度飛行シミュレーションを行う(3)

6DOF flight simulation編の最終回．

飛行機の絵を書こう

飛行運動の可視化には様々なスタイルがあるけれど，やはり3次元的な飛行軌道の可視化が最も良い。octaveにもそういう機能はある(comet3関数)．ただこれは質点運動をアニメーションしてくれるのみで，姿勢情報が分からない。そこで関数を自作することにした。

書いた

ポリゴンの全ての頂点を手打ちした後，回転写像や平行移動写像で頂点を移動させ，頂点同士を線で結ぶという方法を採用した。前回の記事で作った回転関数を再利用してオブジェクトを回転させる。
これは関数スクリプトなので，ファイル名は"aircraft_figure.m"としておく．

% aircraft_figure.m 
function [] = aircraft_figure(x,y,z,Psi,Theta,Phi,scale)
	%飛行機の3Dグラフィックを描画する関数
	%引数は重心座標，ロールピッチヨー角，飛行機のサイズ
	%ポリゴンの頂点を定義して，lineオブジェクトで繋ぐ

	%回転行列を定義したスクリプトを読み込む
	source('Rotation_Matrix.m');

	%xz平面に対する鏡像ベクトルを作るための写像行列
	mirror = [1,0,0;0,-1,0;0,0,1];

	%主翼オブジェクト
	p1 = [0+3,0 ,-0];
	p2 = [-4+3,1 ,-0];
	p3 = [-4+3,-1,-0];
	p4 = [-3+3,0,-0.2];
	w1 = vertcat(p1,p2,p3,p1);%主翼
	w2 = vertcat(p1,p2,p4,p1);%右舷
	w3 = (mirror * w2')';%左舷

	%尾翼オブジェクト
	p1 = [-3.0+3,0.3,-0];
	p2 = [-3.8+3,0.5,-0.6];
	p3 = [-4.2+3,0.5,-0.6];
	p4 = [-4.0+3,0.3,-0];
	tail_r = vertcat(p1,p2,p3,p4,p1);%右尾翼
	tail_l = (mirror * tail_r')';%左尾翼

	%オブジェクトのスケールを変更する
	w1 = w1 * scale;
	w2 = w2 * scale;
	w3 = w3 * scale;
	tail_r = tail_r * scale;
	tail_l = tail_l * scale;

	%オブジェクトを回転させる
	DCM = Rotation_X(-Psi) * Rotation_Y(-Theta) * Rotation_Z(-Phi);
	w1 = (DCM * w1')';
	w2 = (DCM * w2')';
	w3 = (DCM * w3')';
	tail_r = (DCM * tail_r')';
	tail_l = (DCM * tail_l')';

	%オブジェクトを平行移動させる
	w1     = translational_shift(w1,x,y,z);
	w2     = translational_shift(w2,x,y,z);
	w3     = translational_shift(w3,x,y,z);
	tail_r = translational_shift(tail_r,x,y,z);
	tail_l = translational_shift(tail_l,x,y,z);

	%描画する
	line(w1(:,1),w1(:,2),w1(:,3));
	line(w2(:,1),w2(:,2),w2(:,3));
	line(w3(:,1),w3(:,2),w3(:,3));
	line(tail_r(:,1),tail_r(:,2),tail_r(:,3));
	line(tail_l(:,1),tail_l(:,2),tail_l(:,3));

end

function result = translational_shift(object,x,y,z)
	%頂点群objectを[x,y,z]だけ平行移動させる
	shift_x = x*ones(rows(object),1);
	shift_y = y*ones(rows(object),1);
	shift_z = z*ones(rows(object),1);
	shift   = horzcat(shift_x,shift_y,shift_z);

	result = object + shift;
end

飛行機はデルタ翼で表現し。コダワリポイントとして、どの角度から見ても立体的に見えるように主翼に厚みを持たせ，尾翼を斜めに付けた。

使ってみる

まず簡単なテストから．サイズや姿勢を変えながら2機の飛行機を描画する.

octave:1 > axis equal
octave:2 > aircraft_figure(0,0,0, 0,0,0, 1.0);
octave:3 > aircraft_figure(3,2,1, 0.2,0.2,0.2, 2.0);
octave:4 > view(3)

結果はこうなる．イメージ通り.

次はいよいよ本番である。
アニメーションにしたいから，逐次処理を組む。
適当な時間間隔で飛行機オブジェクトを次々に出現させていく。飛行機オブジェクトの位置や姿勢は、フライトログの値を使う。以下のスクリプトを，前回作ったスクリプトの末尾にコピペして挿入する．

figure(); hold on; box('off')
axis equal; set(gca,'YDir','rev','ZDir','rev'); %飛行機でよく使われる右手系にする
xlabel('x');ylabel('y');zlabel('z');
grid on;view (3)

%見やすくなるように適宜調整する
interval = step*5; %飛行機の表示間隔 
scale = 50; %飛行機のサイズ

for i=1:interval:endurance*step
	aircraft_figure(x(i,10),x(i,11),x(i,12),x(i,9),x(i,4),x(i,8),scale);
	stem3(x(1:interval:i,10),x(1:interval:i,11),x(1:interval:i,12),'MarkerSize',0);
	plot3(x(1:1:i,10),x(1:1:i,11),x(1:1:i,12));
	refresh;
end