Hatena::Groupfatalemployeetraining

資材部の懲りない面々・あれこれブログ

「間違った社員教育」製品版委託頒布中!

2011-08-23

いまさら相対座標指定ライブラリの解説

20:10

相対座標指定ライブラリの解説を遅ればせながら、してみます。

referの説明はついったーで昨日やったから…だれかtogetterか何かでまとめてくださると?

道具立ての準備

20:10

まず、いろいろ道具が必要なので準備します。

円周率必要なので最初に定義します。

sqrt()はルート(平方根)を計算する関数です。

これもあると便利なのでとりあえず準備しておきます。


続いて、ベクトルのx軸周りの回転、y軸周りの回転、z軸周りの回転を準備します。

回転行列ですが、実は覚えてなくても簡単に作ることができます。

たとえば2次元ベクトル(x1,y1)をθだけ回転したら(x2,y2)になるような次のような回転行列の各成分を求めたいとします。

\begin{pmatrix}x_2 \\y_2\end{pmatrix}=\begin{pmatrix}a & b \\c & d\end{pmatrix}\begin{pmatrix}x_1 \\y_1\end{pmatrix}

この場合、各座標軸方向の単位ベクトル(長さ1のベクトル)を上の式に従ってθだけ回転してみます。

すると、(x1,y1)=(1,0)の回転結果は(x2,y2)=(a,c)。

(x1,y1)=(0,1)の回転結果は(x2,y2)=(b,d)となりますね。

では実際に絵を書いて、各座標軸方向の単位ベクトルを回転するとどうなるか確認します。

f:id:yumeno:20110823200707p:image

図より、a=cosθ, c=sinθ, b=-sinθ, d=cosθであることがわかりますね!

3次元ベクトルの回転行列についても全く同様に求めることができます。

\begin{pmatrix}x_2 \\y_2 \\z_2 \end{pmatrix}=\begin{pmatrix}a & b & c \\d & e & f \\g & h & i\end{pmatrix}\begin{pmatrix}x_1 \\y_1 \\z_1 \end{pmatrix}

このように回転行列を置いておいて、各軸方向の単位ベクトル(1,0,0)、(0,1,0)、(0,0,1)を回してみて各成分を求めればOKです。

(1,0,0)を回した結果が(a,d,g)、(0,1,0)を回した結果が(b,e,h)、(0,0,1)を回した結果が(c,f,i)になりますので、あとは作図して実際に成分を求める作業です。

それぞれRaiders Sphereの座標系&回転方向だと、次のようになります。

ベクトル(x1,y1,z1)をx軸まわりにθだけ回すとすると(_rot_x_axis)、

f:id:yumeno:20110823200808p:image

このようになります。回転軸であるx軸の単位ベクトル(1,0,0)は回転しても変化せず(1,0,0)のままですので、回転行列は以下の通りです。

\begin{pmatrix}x_2 \\y_2 \\z_2 \end{pmatrix}=\begin{pmatrix}1 & 0 & 0 \\0 & \cos{\theta} & sin{\theta} \\0 & -\sin{\theta} & \cos{\theta}\end{pmatrix}\begin{pmatrix}x_1 \\y_1 \\z_1 \end{pmatrix}

同様にy軸まわりにψだけ回すとすると(_rot_y_axis)、

f:id:yumeno:20110823200809p:image

このようになります。回転軸であるy軸の単位ベクトル(0,1,0)は回転しても変化せず(0,1,0)のままですので、回転行列は以下の通りです。

\begin{pmatrix}x_2 \\y_2 \\z_2 \end{pmatrix}=\begin{pmatrix}\cos{\psi} & 0 & \sin{\psi} \\0 & 1 & 0 \\-\sin{\psi} & 0 & \cos{\psi}\end{pmatrix}\begin{pmatrix}x_1 \\y_1 \\z_1 \end{pmatrix}

同様にz軸まわりにφだけ回すとすると(_rot_z_axis)、

f:id:yumeno:20110823200810p:image

このようになります。回転軸であるz軸の単位ベクトル(0,0,1)は回転しても変化せず(0,0,1)のままですので、回転行列は以下の通りです。

\begin{pmatrix}x_2 \\y_2 \\z_2 \end{pmatrix}=\begin{pmatrix}\cos{\phi} & -\sin{\phi} & 0 \\\sin{\phi} & \cos{\phi} & 0 \\0 & 0 & 1\end{pmatrix}\begin{pmatrix}x_1 \\y_1 \\z_1 \end{pmatrix}

です。これらを計算する関数をそれぞれ用意しておきます。

また、同じ計算をするが、使用する引数を簡単化した関数(rot_x_axis, rot_y_axis, rot_z_axis)を用意します。

ヨー軸回転の話は…

20:18

飛ばします!

解説書きかけているうちに、もっといい書き方がある気がしてきたんだ…

(calc_rxryrzとrel_rot_yawですね。どちらもこの後の関数では使わないのです)

あるユニットを別のユニットの持つ相対座標系で指定した座標に移動するには…

20:18

次は相対座標系を利用した移動の処理です。おおまかに次のような流れで進みます。

  1. 基準とする相対座標系を持つ機体の、絶対座標系における位置(px,py,pz)を求める
  2. 基準とする相対座標系を持つ機体の姿勢(ピッチ角rx、方位角ry、バンク角rz)を求める
  3. 相対座標系におけるベクトル(x, y, z)が、絶対座標系基準ではどのような値になるか求め(x, y, z)に上書きする(calc_rel_posを使う)
  4. 移動したい機体を、まず基準とする相対座標系を持つ機体と同じ位置に移動する
  5. 続いて移動したい機体を、ベクトル(x, y, z)ぶんだけ平行移動する
  6. 機体の向きを、基準とする相対座標系を持つ機体とそろえる

以上です。これがset_rel_pos_xyz_absの中身です。

実は、「相対座標系におけるベクトル(x, y, z)が、絶対座標系基準ではどのような値になるか求め(x, y, z)に上書きする」の部分以外はそんなに難しくありません。

相対座標系におけるベクトルが、絶対座標系基準ではどのような値になるか求める

20:18

2次元世界の場合の相対座標系と絶対座標系の座標変換について、ちょっと考えてみます。

下図左のように、平面上の機体に相対座標系が割りつけられていて、赤いベクトルの相対座標系での値(x,y)が分かっていたとして、赤いベクトルの絶対座標系での値を求めたい、とします。

f:id:yumeno:20110823201341p:image

実はその計算は、上図右のように、(相対座標系の座標軸が絶対座標系の座標軸を回転して得られるなら)赤いベクトルを回転することで得られます。

(もし座標系の座標軸が平行移動も含むなら、平行移動して回転すればOKです)

3次元世界の場合でも同じですから、

機体姿勢がピッチ角rx、方位角ry、バンク角rzの場合、相対座標系におけるベクトル(x,y,z)を、z軸周りにrz回転させ、次にx軸周りにrx回転させ、次にy軸周りにryだけ回転させれば!

そのベクトルの絶対座標系における値が計算できるのです。

この処理を行っているのが、_calc_rel_pos(同じくcalc_rel_pos)ですね。