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

## 概要

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

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

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


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

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

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


### 任意軸の回転行列

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

```math
\begin{vmatrix}
n_x^2(1 - cos \theta) + cos \theta &n_xn_y(1 - cos \theta) + n_z sin \theta &n_xn_z(1 - cos \theta) - n_y sin \theta &0 \\

n_xn_y(1 - cos \theta) + n_z sin \theta &n_y^2(1 - cos \theta) + cos \theta &n_yn_z(1 - cos \theta) - n_x sin \theta &0 \\

n_xn_z(1 - cos \theta) + n_y sin \theta &n_yn_z(1 - cos \theta) - n_x sin \theta &n_z^2(1 - cos \theta) + cos \theta &0 \\

0 &0 &0 &1
\end{vmatrix}
```


### クォータニオンの成分

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

```math
x = n_x sin(\frac{\theta}{2}) \\
y = n_y sin(\frac{\theta}{2}) \\
z = n_z sin(\frac{\theta}{2}) \\
w = cos(\frac{\theta}{2})
```

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

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

```math
\begin{vmatrix}
m00 &m01 &m02 &m03 \\
m10 &m11 &m12 &m13 \\
m20 &m21 &m22 &m23 \\
m30 &m31 &m32 &m33
\end{vmatrix}
```

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


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

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

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


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

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

```math
sin(α±β) = sin(α)cos(β) ± cos(α)sin(β) \\
cos(α±β) = cos(α)cos(β) ± sin(α)sin(β)
```

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


```math
1 - 2y^2 - 2z^2
```

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

```math
y = ny * sin(\frac{\theta}{2})
```

なので、2乗すると、

```math
-2y^2 = -2 * (ny * sin(\frac{\theta}{2})) * (ny * sin(\frac{\theta}{2})) \\
= -2 * ny^2 * sin^2(\frac{\theta}{2})
```

となります。
さて、ここで「$cos(\theta) = 1 - 2sin^2(\frac{\theta}{2})$」を使います。
上記にこれを代入すると、

```math
cosθ = 1 -2sin^2(\frac{\theta}{2}) \\
-2sin^2(\frac{\theta}{2}) = cos(\theta) - 1 \\
よって、
ny^2 * -2sin^2(\frac{\theta}{2}) = ny^2 * (cos(\theta) - 1) \\
```

となります。

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

```math
ny^2 * (cosθ - 1) \\
nz^2 * (cosθ - 1)
```

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

```math
\sqrt[]{nx^2 + ny^2 + nz^2} = 1 ... 正規化ベクトルの長さは1 \\
\sqrt[]{nx^2 + ny^2 + nz^2}^2 = 1^2 \\
nx^2 + ny^2 + nz^2 = 1
```

つまり、

```math
nz^2 = 1 - nx^2 - ny^2
```

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

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

```math
1 + (ny^2 * (cosθ - 1)) + (nz^2 * (cosθ - 1))
```

これに「$nz^2 = 1 - nx^2 - ny^2$」を代入すると、

```math
1 + (ny^2(cosθ - 1)) + ((1 - nx^2 - ny^2)(cosθ - 1)) \\
= 1 + (1 - nx^2 - ny^2 + ny^2)(cosθ - 1) \\
= 1 + (1 - nx^2)(cosθ - 1)
```

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

```math
1 + (cosθ - 1 - nx^2cosθ + nx^2) \\
1 + (nx^2(1 - cosθ) + cosθ - 1) \\
nx^2(1 - cosθ) + cosθ
```

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

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

```math
\begin{vmatrix}
1 - 2y^2 - 2z^2 &2xy + 2wz &2xz - 2wy &0 \\
2xy - 2wz &1 - 2x^2 - 2z^2 &2yz + 2wx &0 \\
2xz + 2wy &2yz - 2wx &1 - 2x^2 - 2y^2 &0 \\
0 &0 &0 &1
\end{vmatrix}
```


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

回転行列からオイラー角を求める部分についてはこちらの記事（[回転行列からオイラー角のパラメータ抽出を行う | It_lives_vainlyの日記](http://d.hatena.ne.jp/It_lives_vainly/20070829/1188384519)）を参考にさせてもらいました。


そして、Unityでの回転行列の作り方（各軸の回転行列のかける順番）は、上記記事と同じくZXYです。
ドキュメント→ [Unity DOCUMENT | Transform.eulerAngles](https://docs.unity3d.com/ja/current/ScriptReference/Transform-eulerAngles.html)

実際に行列をそれぞれ合成していくと以下のようになります。
*※ 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
```

さて、ここで$m21$を見ると$Sin(x)$となっています。
ここから、

```math
m21 = sin(x) \\
θx = Asin(m21)
```

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

```math
tan = \frac{sin}{cos}
```

を利用して、

```math
\frac{-Sz}{Cz} = \frac{m01}{m11} = -tan(x) \\
θz = Atan2(-m01, m11)
```

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

同様にして、

```math
\frac{-Sy}{Cy} = \frac{m20}{m22} = -tan(x) \\
θy = Atan2(-m20, m22)
```

と、$\theta y$が求まります。


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

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

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

```math
\begin{vmatrix}
CzCy±SzSy   &0  &CzSy±SzCy \\
SzCy±CzSy   &0  &SzSy±CzCy \\
        0  &±1          &0
\end{vmatrix}
```

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

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

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

```math
sin(α±β) = sin(α)cos(β) ± cos(α)sin(β) \\
cos(α±β) = cos(α)cos(β) ± sin(α)sin(β)
```

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

```math
\begin{vmatrix}
cos(θz±θy)  &0 &sin(θz±θy) \\
sin(θz±θy)  &0 &cos(θz±θy) \\
         0 &±1         &0
\end{vmatrix}
```

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

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

```math
\frac{m10}{m00} = \frac{-sin(\theta z)}{cos(\theta z)} = -tan(\theta z)
```
となります。つまり、

```math
\theta z = Atan2(m10, m00)
```

ですね。

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

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

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


ここまでをまとめると、

$sin(\theta x) = ±1$以外のとき、

```math
\theta x = Asin(m21) \\
\theta y = Atan2(-m20, m22) \\
\theta z = Atan2(-m01, m11)
```

$sin(\theta x) = 1$のとき（$m21 = 1$のとき）、

```math
\theta x = \frac{\pi}{2} \\
\theta y = 0 \\
\theta z = Atan2(m10, m00)
```

$sin(θx) = -1$のとき（$m21 = -1$のとき）、

```math
\theta x = \frac{-\pi}{2} \\
\theta y = 0 \\
\theta z = Atan2(m10, m00)
```

となります。



### Unityで実行してみる

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

```csharp
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);
}
```

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

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

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

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

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

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

**[追記]**
コメントで指摘してもらいましたが、そもそも参考にした記事が、座標変換のための回転行列と、位置ベクトルの回転行列による話で、そもそも対象としているのが違うためでは、ということでした。
「教えて！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
```