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

6DOF flight simulation編の最終回．

飛行機の絵を書こう

飛行機の運動の可視化には様々な方法があるけれど，やっぱり飛行機がグリグリ動くのが一番楽しいよね．octaveにそういうアニメーションを書く機能があるかいうと，一応ある(comet3関数)．ただこれは質点の運動をアニメーションしてくれるだけで，飛行機の姿勢が分からない．到底満足できないので，ここは自分で書かなきゃいけない．一度作っておけば便利そうだし，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)

結果はこうなる．イメージ通り.
f:id:aeroelasticity:20141118165212p:plain

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

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

高度が分かりやすいようにstem3関数で地面までの垂直線を下ろした(UAVな人はよくやるよね）．ついでにplot3関数で軌跡も表示させた．あとの細々した設定は，octaveで見やすくアニメーションするための一般的な仕掛け．ググればすぐ分かるから，解説は省略する．

で，実行．

octave:5 > aircraft_dynamics

結果はこうなる．
f:id:aeroelasticity:20141117011527p:plain
フライトシミュレーションが実行されて，その結果から飛行経路がアニメーションされた．子供の頃によくやった紙飛行機遊びの挙動のイメージとよく一致している. 一見して複雑に思える飛行機の運動も、実は線形ダイナミクスの擾乱を積分しただけだということを直感的に理解してもらえたと思う.
テキストの表紙の絵に近いものを描けたので、このテキストの山場はクリアできたと言えるだろう.