Edited at 2017-10-19

クォータニオンからオイラー角を求めてみる

C#Unity 数学

概要

Unityは基本的に回転をクォータニオンで管理しています。
その他のエンジンも、おそらくクォータニオンで回転を制御していると思います。
というのも、オイラー角では「ジンバルロック」などの問題があるため、制御するのに不安定さがあるためです。

しかし、場合によってはオイラー角で値を求めたい場合があります。

Unityであれば、クォータニオンから簡単にオイラー角を取得することができますが、では内部ではどういう処理が行われているのか。
それを色々な記事を参考にまとめてみたいと思います。

クォータニオンから回転行列を取り出す

まずはクォータニオンから回転行列を取り出します。
回転行列の要素から、オイラー角を推測するためです。

まるぺけさんのこちらの記事（その58 やっぱり欲しい回転行列⇔クォータニオン相互変換）を参考にさせてもらうと、クォータニオンの各成分から回転行列を求めることができるようです。

任意軸の回転行列

まず、任意軸の回転行列は以下のように構成されます。
$n$ は回転の軸ベクトルの各成分です。

| \begin{matrix} n_{x}^{2} (1 - c o s θ) + c o s θ & n_{x} n_{y} (1 - c o s θ) + n_{z} s i n θ & n_{x} n_{z} (1 - c o s θ) - n_{y} s i n θ & 0 \\ n_{x} n_{y} (1 - c o s θ) + n_{z} s i n θ & n_{y}^{2} (1 - c o s θ) + c o s θ & n_{y} n_{z} (1 - c o s θ) - n_{x} s i n θ & 0 \\ n_{x} n_{z} (1 - c o s θ) + n_{y} s i n θ & n_{y} n_{z} (1 - c o s θ) - n_{x} s i n θ & n_{z}^{2} (1 - c o s θ) + c o s θ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} |

クォータニオンの成分

クォータニオンの各成分は、回転軸ベクトル $\vec{N}$ と角度 $θ$ を用いて以下のように構成されます。

x = n_{x} s i n (\frac{θ}{2}) y = n_{y} s i n (\frac{θ}{2}) z = n_{z} s i n (\frac{θ}{2}) w = c o s (\frac{θ}{2})

回転行列の各要素を計算する

行列の位置を明確化するために、行列を以下のようにナンバリングしておきます。

| \begin{matrix} m 00 & m 01 & m 02 & m 03 \\ m 10 & m 11 & m 12 & m 13 \\ m 20 & m 21 & m 22 & m 23 \\ m 30 & m 31 & m 32 & m 33 \end{matrix} |

※ m03, m13, m23, m30, m31, m32はすべて0、m33は1です。

さて、ではまずはじめに、m00の値（ $n_{x}^{2} (1 - c o s θ) + c o s θ$ ）を見てみましょう。
これを、クォータニオンの成分から表すと $1 - 2 y^{2} - 2 z^{2}$ となるようです。

一瞬、「なんでやねん」と思うかもしれませんが（自分は思いました）、倍角の公式などを使って展開してやるとしっかりとその値が出現します。

ちなみに三角関数の公式などは前にまとめた記事があるのでそちらも参照してみてください。（[数学] 三角関数について整理する）

三角関数の公式を使って導き出す

倍角の公式は以下になります。

s i n (α \pm β) = s i n (α) c o s (β) \pm c o s (α) s i n (β) c o s (α \pm β) = c o s (α) c o s (β) \pm s i n (α) s i n (β)

試しにやってみましょう。

1 - 2 y^{2} - 2 z^{2}

まずは $- 2 y^{2}$ から展開してみましょう。

y = n y * s i n (\frac{θ}{2})

なので、2乗すると、

- 2 y^{2} = - 2 * (n y * s i n (\frac{θ}{2})) * (n y * s i n (\frac{θ}{2})) = - 2 * n y^{2} * s i n^{2} (\frac{θ}{2})

となります。
さて、ここで「 $c o s (θ) = 1 - 2 s i n^{2} (\frac{θ}{2})$ 」を使います。
上記にこれを代入すると、

c o s θ = 1 - 2 s i n^{2} (\frac{θ}{2}) - 2 s i n^{2} (\frac{θ}{2}) = c o s (θ) - 1 よ っ て 、 n y^{2} * - 2 s i n^{2} (\frac{θ}{2}) = n y^{2} * (c o s (θ) - 1)

となります。

同様に、 $- 2 z^{2}$ を展開して以下を得ます。

n y^{2} * (c o s θ - 1) n z^{2} * (c o s θ - 1)

さてここで、各 $n$ 成分は正規化したベクトルなので以下が成り立ちます。

\sqrt{n x^{2} + n y^{2} + n z^{2}} = 1 . . . 正 規 化 ベ ク ト ル の 長 さ は 1 {\sqrt{n x^{2} + n y^{2} + n z^{2}}}^{2} = 1^{2} n x^{2} + n y^{2} + n z^{2} = 1

つまり、

n z^{2} = 1 - n x^{2} - n y^{2}

求めたい値は $1 - 2 y^{2} - 2 z^{2}$ です。

現状の状態で整理すると以下のようになります。

1 + (n y^{2} * (c o s θ - 1)) + (n z^{2} * (c o s θ - 1))

これに「 $n z^{2} = 1 - n x^{2} - n y^{2}$ 」を代入すると、

1 + (n y^{2} (c o s θ - 1)) + ((1 - n x^{2} - n y^{2}) (c o s θ - 1)) = 1 + (1 - n x^{2} - n y^{2} + n y^{2}) (c o s θ - 1) = 1 + (1 - n x^{2}) (c o s θ - 1)

ゴールが見えてきました。
これを展開すると、

1 + (c o s θ - 1 - n x^{2} c o s θ + n x^{2}) 1 + (n x^{2} (1 - c o s θ) + c o s θ - 1) n x^{2} (1 - c o s θ) + c o s θ

最初に示したm00の値が出てきました！なんともすばらしいですね。

あとは、他の要素もこうして算出していくと、最終的に以下のようになります。

| \begin{matrix} 1 - 2 y^{2} - 2 z^{2} & 2 x y + 2 w z & 2 x z - 2 w y & 0 \\ 2 x y - 2 w z & 1 - 2 x^{2} - 2 z^{2} & 2 y z + 2 w x & 0 \\ 2 x z + 2 w y & 2 y z - 2 w x & 1 - 2 x^{2} - 2 y^{2} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} |

回転行列からオイラー角を求める

回転行列からオイラー角を求める部分についてはこちらの記事（回転行列からオイラー角のパラメータ抽出を行う | It_lives_vainlyの日記）を参考にさせてもらいました。

そして、Unityでの回転行列の作り方（各軸の回転行列のかける順番）は、上記記事と同じくZXYです。
ドキュメント→ Unity DOCUMENT | Transform.eulerAngles

実際に行列をそれぞれ合成していくと以下のようになります。
※ CnはCos、SnはSinを表し、それぞれの添字はどの回転行列のものか、を示しています。

-----------------------------------------------------------------
【z x y合成（Unityの場合）】

Z:         |  X:          |  Y:        
---------- |  ----------  |  ----------
Cz -Sz  0  |  1   0   0   |  Cy  0  Sy 
Sz  Cz  0  |  0  Cx -Sx   |   0  1   0 
 0   0  1  |  0  Sx  Cx   | -Sy  0  Cy 

ZX:
----------------
Cz  -SzCx  SzSx
Sz   CzCx -CzSx
 0     Sx    Cx

ZXY:
----------------
 CzCy-SzSxSy  -SzCx  CzSy+SzSxCy
 SzCy+CzSxSy   CzCx  SzSy-CzSxCy
-CxSy            Sx         CxCy

さて、ここで $m 21$ を見ると $S i n (x)$ となっています。
ここから、

m 21 = s i n (x) θ x = A s i n (m 21)

が導き出されます。
次に、

t a n = \frac{s i n}{c o s}

を利用して、

\frac{- S z}{C z} = \frac{m 01}{m 11} = - t a n (x) θ z = A t a n 2 (- m 01, m 11)

として、 $θ z$ が求まります。

同様にして、

\frac{- S y}{C y} = \frac{m 20}{m 22} = - t a n (x) θ y = A t a n 2 (- m 20, m 22)

と、 $θ y$ が求まります。

これで無事、各値が求まった・・と思いきや、特殊なケースが存在します。
それが、 $c o s (θ x) = 0$ のときです。

$c o s$ が $0$ の場合、そう、 $θ x$ が $\frac{π}{2}$ と $\frac{- π}{2}$ のときですね。
Cxが0の場合、すべての値0になってしまって答えを求めることができなくなってしまいます。

そこで、その特殊なケースを場合分けして以下のように考えます。
まず、 $c o s (θ x) = 0$ のとき、回転行列がどうなっているかを見てましょう。

| \begin{matrix} C z C y \pm S z S y & 0 & C z S y \pm S z C y \\ S z C y \pm C z S y & 0 & S z S y \pm C z C y \\ 0 & \pm 1 & 0 \end{matrix} |

Cxを使っている部分がすべて0となり、Sxの部分が $\pm 1$ になります。

※ cos(θx) = 0のとき、sin(θx) = ±1 ... $θ x$ = 90° or 270°

さて、ここで三角関数の加法定理を持ち出すと、

s i n (α \pm β) = s i n (α) c o s (β) \pm c o s (α) s i n (β) c o s (α \pm β) = c o s (α) c o s (β) \pm s i n (α) s i n (β)

これを使って上の行列は以下のように整理することができます。

| \begin{matrix} c o s (θ z \pm θ y) & 0 & s i n (θ z \pm θ y) \\ s i n (θ z \pm θ y) & 0 & c o s (θ z \pm θ y) \\ 0 & \pm 1 & 0 \end{matrix} |

なんともきれいに加法定理の形になってますねｗ
さて、これだけでは加減算された形での値しか求まっていないので、一意に角度を求めることができません。

そこで、適当に $θ y = 0$ と置くと、

\frac{m 10}{m 00} = \frac{- s i n (θ z)}{c o s (θ z)} = - t a n (θ z)

となります。つまり、

θ z = A t a n 2 (m 10, m 00)

ですね。

問題点として、適当に $θ y = 0$ とおいているため、回転を表すオイラー角としては正しい姿勢になるものの、もともと設定した $y$ と $z$ の値が復元できないことが分かります。

とはいえ、 $θ x = 90 ° o r 270 °$ という限定された状態のみなので、だいたいの場合は問題にならないでしょう。

つまり、 $m 21 = 1$ の場合は $θ x = \frac{π}{2} = 90 °$ 、 $m 21 = - 1$ の場合は $θ x = \frac{- π}{2} = 270 °$ となります。
この状態のときだけ条件分岐してやればいいわけですね。

ここまでをまとめると、

$s i n (θ x) = \pm 1$ 以外のとき、

θ x = A s i n (m 21) θ y = A t a n 2 (- m 20, m 22) θ z = A t a n 2 (- m 01, m 11)

$s i n (θ x) = 1$ のとき（ $m 21 = 1$ のとき）、

θ x = \frac{π}{2} θ y = 0 θ z = A t a n 2 (m 10, m 00)

$s i n (θ x) = - 1$ のとき（ $m 21 = - 1$ のとき）、

θ x = \frac{- π}{2} θ y = 0 θ z = A t a n 2 (m 10, m 00)

となります。

Unityで実行してみる

これを元に、実際にUnity上で実行してみました。
実行したコードは以下になります。

private Vector3 Calc()
{
    Quaternion r = transform.rotation;
    float x = r.x;
    float y = r.y;
    float z = r.z;
    float w = r.w;

    float x2 = x * x;
    float y2 = y * y;
    float z2 = z * z;

    float xy = x * y;
    float xz = x * z;
    float yz = y * z;
    float wx = w * x;
    float wy = w * y;
    float wz = w * z;

    // 1 - 2y^2 - 2z^2
    float m00 = 1f - (2f * y2) - (2f * z2);

    // 2xy + 2wz
    float m01 = (2f * xy) + (2f * wz);

    // 2xy - 2wz
    float m10 = (2f * xy) - (2f * wz);

    // 1 - 2x^2 - 2z^2
    float m11 = 1f - (2f * x2) - (2f * z2);

    // 2xz + 2wy
    float m20 = (2f * xz) + (2f * wy);

    // 2yz+2wx
    float m21 = (2f * yz) - (2f * wx);

    // 1 - 2x^2 - 2y^2
    float m22 = 1f - (2f * x2) - (2f * y2);


    float tx, ty, tz;

    if (Mathf.Approximately(m21, 1f))
    {
        tx = Mathf.PI / 2f;
        ty = 0;
        tz = Mathf.Atan2(m10, m00);
    }
    else if (Mathf.Approximately(m21, -1f))
    {
        tx = -Mathf.PI / 2f;
        ty = 0;
        tz = Mathf.Atan2(m10, m00);
    }
    else
    {
        tx = Mathf.Asin(-m21);
        ty = Mathf.Atan2(m20, m22);
        tz = Mathf.Atan2(m01, m11);
    }

    tx *= Mathf.Rad2Deg;
    ty *= Mathf.Rad2Deg;
    tz *= Mathf.Rad2Deg;

    return new Vector3(tx, ty, tz);
}

これを実行すると無事、同じ回転を設定することができました。
が、実はよーく見てもらうと上で求めた式とは若干違う部分があります。

tx = Mathf.Asin(-m21);
ty = Mathf.Atan2(m20, m22);
tz = Mathf.Atan2(m01, m11);

引数に与えている成分のマイナス記号が逆になっているんですね。
そして、なぜここが逆転するのかは分かりませんでした・・。
ただ、マイナスを逆転したら正常に回転がコピーできました。

~~そもそも、UnityのAPIである`Quatarnion.eulerAngles`の値はさらに違った値になっていました。（360°から該当の角度を減算したものになっていた）~~

このあたりがもしかしたら逆転している理由かもしれません。
が、値としては想定した値になっていたので、ひとまずはOKとしました・・。
もしなにかご存知の人いたら教えてください(´・ω・`)

これについてはコメントで指摘をもらって、以下に追記しました。

[追記]
コメントで指摘してもらいましたが、そもそも参考にした記事が、座標変換のための回転行列と、位置ベクトルの回転行列による話で、そもそも対象としているのが違うためでは、ということでした。
「教えて！goo」に似た質問とそれに対する回答があったので追記しておきます。

https://oshiete.goo.ne.jp/qa/8764676.html

見てもらうと分かりますが、まさに符号だけが逆転しているのが分かります。

その他メモ

最初間違えてUnityのつもりで違う順番で行列を計算しちゃったけどメモとして残しておきますｗ

-----------------------------------------------------------------
【z y x合成】

Z:          |  Y:          |  X:
----------  |  ----------  |  ----------
Cz -Sz  0   |  Cy  0  Sy   |  1   0   0
Sz  Cz  0   |   0  1   0   |  0  Cx -Sx
 0   0  1   | -Sy  0  Cy   |  0  Sx  Cx

ZY:
-----------------
CzCy  -Sz  CzSy
SzCy   Cz  SzSy
-Sy     0    Cy

ZYX:
-------------------------------
CzCy  -SzCx+CzSySx   SzSx+CzSyCx
SzCy   CzCx+SzSySx  -CzSx+SzSyCx
 -Sy          CySx          CyCx

Kazuya Hiruma @edo_m18

現在はUnity VRエンジニア。主にVRでコンテンツ制作をしています。趣味でWebGLもいじってます。

概要
クォータニオンから回転行列を取り出す
回転行列からオイラー角を求める
- Unityで実行してみる
- その他メモ

ozwk

215contribution

2017-10-19 10:44

以前Unityと全然関係ないところで同様のことやりましたが、
その時のコード見ると確かに符号がこの記事と逆、例えばroll角がAsin(-m21)になってました。

詳細を覚えていないのでここからまるっきりの勘ですが、
回転移動と座標変換では回転行列の一部符号が変わるのでそのあたりでは?

edo_m18

12983contribution

2017-10-19 10:52

@ozwk コメントありがとうございます！
他でも同様になったんですね・・。となると確かにそうした理由はあるかしれませんね。
ちなみに座標変換は、ビュー座標変換あたりのことを指してますか？（ちょっとどのことを言っているのかピンとこず( ;´Д`)）

ozwk

215contribution

2017-10-19 11:01

座標系自体が回転する変換ですね。
「その58 やっぱり欲しい回転行列⇔クォータニオン相互変換」に出てくる回転は座標系の回転で、
「回転行列からオイラー角のパラメータ抽出を行う | It_lives_vainlyの日記」に出てくる回転は位置ベクトルの回転です。

edo_m18

12983contribution

2017-10-19 11:35

なるほど！
確かにそれは関係ありそうですね。
時間があったら調べて追記してみようと思います！
ありがとうございます！

edo_m18

12983contribution

2017-10-19 11:39

このへんがちょうどまさに同じことを回答してますね。
https://oshiete.goo.ne.jp/qa/8764676.html

例えば x, y 座標系で (rcosα, rsinα)の点を反時計回りに角度β
回転させると、角度の加法定理から

(rcos(α+β), rsin(α+β)) = (r(cosαcosβ-sinαsinβ), r(sinαcosβ+sinαcosβ))

元の座標位置を (a, b) = (rcosα, rsinα) とすれば

(r(cosαcosβ-sinαsinβ), r(sinαcosβ+sinαcosβ)) = cosβa - sinβb, sinβa + cosβb

これがいわゆる回転変換。

座標軸の回転では XY座標の左回転に対して、点は右に回転するから、角度の符号を反転すればよい

(cos(-β)a - sin(-β)b, sin(-β)a + cos(-β)b) = (cosβa + sinβb, -sinβa + cosβb)

これが回転の座標変換の式です。角度の向きが逆なだけです。

If you already have a Qiita account log in.

クォータニオンからオイラー角を求めてみる

概要

クォータニオンから回転行列を取り出す

任意軸の回転行列

クォータニオンの成分

回転行列の各要素を計算する

三角関数の公式を使って導き出す

回転行列からオイラー角を求める

Unityで実行してみる

その他メモ

Report article

Feedback