e.blog

主にUnity/UE周りのことについてまとめていきます

クォータニオン(回転)についてメモ

概要

Unityなどのゲームエンジンや、3D関連を扱っていると頻繁に出てくるクォータニオン
しかしオイラー角に比べると各要素の意味が分かりづらく、直にクォータニオンをいじるケースはあまり多くないと思います。

しかし、少し凝ったことをしようとしたり、あるいはUnityにない機能を実装しようとした場合、クォータニオンを理解していないと困ることがあります。

ということで、今回はクォータニオンについてのちょっとした整理とTipsなどを書いていきたいと思います。

クォータニオンとは

Wikipediaから引用させてもらうと、

数学における四元数(しげんすう、英: quaternion(クォターニオン))は複素数を拡張した数体系である。四元数についての最初の記述は、1843年にアイルランドの数学者ウィリアム・ローワン・ハミルトンによってなされ[1][2]、三次元空間の力学に応用された。四元数の特徴は、二つの四元数の積が非可換となることである。ハミルトンは、四元数を三次元空間内の二つの有向直線の商として定義した[3]。これは二つのベクトルの商と言っても同じである[4]。四元数スカラーと三次元のベクトルとの和として表すこともできる。

と記載があります。
(ちなみにクォータニオンは「四元数」とも言われます。内容的には4次元ベクトルの形だからでしょうか)

正直なんのこっちゃですが、(逆に「そうそう」って思える人はこの記事を読んでないでしょうw) 要はクォータニオンを使うことで3次元空間のベクトルの回転が行える、ということです。

またクォータニオンには行列のように積が非可換である、という点も挙げられます。
が、Unityで使っている上ではそこは気にしなくて大丈夫です。
Unity側でよしなにしてくれるので。

クォータニオンについての詳細は以下の記事がとても有用です。
記事には数式がたくさん出てきて、さらにそれらを巧みに変換しながら美しく解説していくので正直まだ全然理解できていませんが、雰囲気を掴むにはいいと思います。

nyahoon.com

クォータニオンの各要素

クォータニオンは4次元ベクトルの体をなしているので、\((x, y, z, w)\)の4成分からなります。
ただ、オイラー角のように各成分にそれぞれの回転が格納されているわけではありません。

じゃあなにが入っているか、というと以下のような計算結果が格納されています。

  • axis = 回転軸
  • theta = 回転角

とした場合に、以下の計算結果が格納されている。

$$ x = axis.x * sin\biggl(\frac{\theta}{2}\biggr) \\ y = axis.y * sin\biggl(\frac{\theta}{2}\biggr) \\ z = axis.z * sin\biggl(\frac{\theta}{2}\biggr) \\ w = cos\biggl(\frac{\theta}{2}\biggr) $$

なのでx要素だけを取り出しても、軸の値と\(sin\)の値の合成なのでそれがどんな意味があるのかは分からない、というわけです。

クォータニオンTips

さてここからは、Unityなど実際の開発に使えるちょっとしたTips、覚えておくと幸せになれるかもしれないものをつらつらと書いていきます。

内積

3次元ベクトルと同様、クォータニオンにも内積が定義されています。

Quaternion q1 = /* any quartenion */
Quaternion q2 = /* any quartenion */
float cos = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
float angle = Mathf.Acos(cos) * Mathf.Rad2Deg;

上記のように、内積は3次元ベクトルと同様、ふたつのクォータニオンが成す角度\(\cos(\theta)\)となります。

UnityのAPIにもQuaternion.Dot(q1, q2)というメソッドが用意されています。

回転軸

先述した通り、クォータニオンは正規化した回転軸の各要素と\(sin\bigl(\frac{\theta}{2}\bigr)\)との掛け算により計算されます。

ただ、掛ける値は\(x, y, z\)各要素とも同じ値なので、これを正規化すれば回転軸のベクトルを得ることができます。

また、クォータニオンの各要素を考えると\(sin\bigl(\frac{\theta}{2}\bigr)\)と同じ値が得られれば、それで割ることで回転軸の値を得ることもできます。

// 2通りの方法で回転軸を計算する
private void ShowRotationAxis()
{
    Quaternion q = transform.rotation;
    
    Vector3 axis1 = new Vector3(q.x, q.y, q.z);
    axis1.Normalize();

    // 以下のようにsinの値で割っても同じ結果がられる
    Vector3 axis2 = new Vector3(q.x, q.y, q.z);
    float theta = Mathf.Acos(q.w);
    float sin = Mathf.Sin(theta);

    axis2.x /= sin;
    axis2.y /= sin;
    axis2.z /= sin;

    Debug.LogFormat("x1: {0}", axis1.x);
    Debug.LogFormat("y1: {0}", axis1.y);
    Debug.LogFormat("z1: {0}", axis1.z);

    Debug.LogFormat("x2: {0}", axis2.x);
    Debug.LogFormat("y2: {0}", axis2.y);
    Debug.LogFormat("z2: {0}", axis2.z);
}

ワールド空間での回転クォータニオンをローカル空間に変換する

あまり実用性はないかもしれませんが、ワールド空間で表されているクォータニオンを、ローカル空間でのクォータニオンに変換する処理です。

transform.rotation = q;とすると決まる回転を、transform.localRotation = q;として設定できるようにするイメージです。

private Quaternion ConvertToLocal(Quaternion q)
{
    Quaternion result = Quaternion.identity;

    Transform current = transform.parent;

    while (current != null)
    {
        result *= Quaternion.Inverse(current.localRotation);
        current = current.parent;
    }

    return result * q;
}

考え方はシンプルに、親のlocalRotationの逆クォータニオンを合算して戻します。
クォータニオンは掛ける順番が変わると結果が変わるので注意が必要です。

クォータニオンのSlerp

クォータニオンにもベクトル同様、LerpSlerpという補間処理があります。
そしてSlerpの意味は「球面線形補間」です。

基本的にはUnity APIにすでにあるのでそれを利用するのがいいと思いますが、どういう処理がされているのかを把握しておくことは大事でしょう。

※ ちなみに余談ですが、今回の実装では補間するweightを0〜1に限定していないので、マイナス値や1以上の値を指定することができます。UnityのAPIでは0〜1の間に限定しているので、それを超えた補間(といっていいか分かりませんがw)をする場合は独自で実装する必要があります。

ということでSlerp処理のアルゴリズムです。
計算のアルゴリズム自体はこちらのC++実装(slerp.cpp - Sony CSL)を参考にしました。

また、いつもお世話になっているマルペケさんの記事にとても分かりやすい解説があるので合わせて読んでみるといいと思います。

参考:その57 クォータニオンを"使わない"球面線形補間

Slerp処理の手順を先に書いておくと、

  1. ふたつのクォータニオンの回転差分を計算する
  2. 補間の係数(tと1-tのふたつ)を元に、(1)で計算した角度の比率として係数を掛けた値を計算する
  3. (2)で求めた値を\(\sin(\theta)\)で割る
  4. (3)で求めた値を係数として、ふたつのクォータニオンを合成する

という流れです。

まずは以下の図を見てください。

f:id:edo_m18:20180624091805p:plain

上図はふたつのベクトルを球面上で線形に補間する様子を示しています。
まず、StartからEndに向かってベクトルが補間されていきます。

その補間の度合いを決めるのが媒介変数\(t\)です。
そしてふたつのベクトルが成す角が\(\theta\)です。

球面線形補間を行うということは、ふたつのベクトル間を等速の角速度で移動する必要があります。
つまり、媒介変数\(t\)を利用すると\(t\theta\)で値が変化していけばいいわけです。

このとき、\(t\)の位置に向かうベクトルはふたつのベクトルの合成で表すことができます。
具体的にはStartベクトルとEndベクトルに平行なベクトルです。
図では黒い矢印になっている部分ですね。

さて、ではそのベクトルの長さはどう求めたらいいでしょうか。
ここで、三角形の比率の話を思い出すと、縦軸に書かれている\(\sin(\theta)\)と\(\sin(\theta(1-t))\)の比率がそのままStartに向かうベクトルと\(t\)の位置のベクトルとの比率になるのが分かるかと思います。

そしてStartベクトルは単位ベクトルなので長さは1です。
また求めたいベクトルを仮に\(V(t)\)と置くと、 「薄い緑のベクトル(1) : 濃い緑のベクトル(\(V(t)\))」の比率は「\(\sin(\theta)\) : \(\sin(\theta(1-t))\)」という比率になります。

これを整理すると以下のようになります。

$$ 1 : V(t) = \sin(\theta) : \sin(\theta(1-t)) \\ V(t) = \frac{\sin(\theta(1-t))}{\sin(\theta)} $$

以上を踏まえて以下の実装を見てみると、\(t\theta\)と\((1-t)\theta\)で係数を計算して、それぞれのクォータニオンを合成しているのが分かるかと思います。

/// <summary>
/// Quaternion's slerp
///
/// Refer: https://www.sonycsl.co.jp/person/nielsen/visualcomputing/programs/slerp.cpp
/// </summary>
/// <param name="a">Start Quaternion</param>
/// <param name="b">End Quaternion</param>
/// <param name="weight">Weight</param>
/// <returns>Mixed quaternion by weight.</returns>
static public Quaternion Slerp(Quaternion a, Quaternion b, float t)
{
    float dot = Quaternion.Dot(a, b);
    float theta = Mathf.Acos(dot);

    if (theta < 0.0)
    {
        theta = -theta;
    }

    float st = Mathf.Sin(theta);

    if (st == 0)
    {
        return a;
    }

    float sut = Mathf.Sin(theta * t);
    float sout = Mathf.Sin(theta * (1f - t));

    float coeff1 = sout / st;
    float coeff2 = sut / st;

    float x = coeff1 * a.x + coeff2 * b.x;
    float y = coeff1 * a.y + coeff2 * b.y;
    float z = coeff1 * a.z + coeff2 * b.z;
    float w = coeff1 * a.w + coeff2 * b.w;

    return new Quaternion(x, y, z, w);
}

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

ちょっとしたメモということで、今度はオイラー角からクォータニオンを求めることをやってみようと思います。

計算式についてはWikipediaに記載があるので引用させてもらうと、

$$ q_{lB} = \begin{vmatrix} \cos\bigl(\frac{\psi}{2}\bigr) \\ 0 \\ 0 \\ \sin\bigl(\frac{\psi}{2}\bigr) \end{vmatrix} \begin{vmatrix} \cos\bigl(\frac{\theta}{2}\bigr) \\ 0 \\ \sin\bigl(\frac{\theta}{2}\bigr) \\ 0 \\ \end{vmatrix} \begin{vmatrix} \cos\bigl(\frac{\phi}{2}\bigr) \\ \sin\bigl(\frac{\phi}{2}\bigr) \\ 0 \\ 0 \\ \end{vmatrix} \\ = \begin{vmatrix} \cos\bigl(\frac{\phi}{2}\bigr) \cos\bigl(\frac{\theta}{2}\bigr) \cos\bigl(\frac{\psi}{2}\bigr) + \sin\bigl(\frac{\phi}{2}\bigr) \sin\bigl(\frac{\theta}{2}\bigr) \sin\bigl(\frac{\psi}{2}\bigr) \\ \sin\bigl(\frac{\phi}{2}\bigr) \cos\bigl(\frac{\theta}{2}\bigr) \cos\bigl(\frac{\psi}{2}\bigr) - \cos\bigl(\frac{\phi}{2}\bigr) \sin\bigl(\frac{\theta}{2}\bigr) \sin\bigl(\frac{\psi}{2}\bigr) \\ \cos\bigl(\frac{\phi}{2}\bigr) \sin\bigl(\frac{\theta}{2}\bigr) \cos\bigl(\frac{\psi}{2}\bigr) + \sin\bigl(\frac{\phi}{2}\bigr) \cos\bigl(\frac{\theta}{2}\bigr) \sin\bigl(\frac{\psi}{2}\bigr) \\ \cos\bigl(\frac{\phi}{2}\bigr) \cos\bigl(\frac{\theta}{2}\bigr) \sin\bigl(\frac{\psi}{2}\bigr) - \sin\bigl(\frac{\phi}{2}\bigr) \sin\bigl(\frac{\theta}{2}\bigr) \cos\bigl(\frac{\psi}{2}\bigr) \\ \end{vmatrix} $$

という計算を行うことで求めることができます。
ここで最初のベクトルの計算は、オイラー角なのでXYZ軸が回転していない通常の座標空間において、XYZ軸それぞれの回転を合成したクォータニオン、と見ることができます。

なので、上記の合成では「ZYX」の順で合成していることになりますね。
ただこの合成の順番は特に決まりがあるわけではなく、エンジンごとにどういう順番で掛けるのか、を決めておく必要があります。
調べたところ、Unityでは「YXZ」の順で掛けているようです。

※ 余談ですが、Three.jsの実装を見てみると、オーダーを指定できるようになっており、そこから色々試したところ「YXZ」が同じ結果になりました。

それらを踏まえて、実際のC#コードは以下のようになります。

mary>
/// Convert euler to quaternion
/// </summary>
/// <param name="x">Euler's x</param>
/// <param name="y">Euler's y</param>
/// <param name="z">Euler's z</param>
/// <returns>Quternion from euler angles.</returns>
static public Quaternion Euler(float x, float y, float z)
{
    x *= Mathf.Deg2Rad;
    y *= Mathf.Deg2Rad;
    z *= Mathf.Deg2Rad;

    float c1 = Mathf.Cos(x * 0.5f);
    float c2 = Mathf.Cos(y * 0.5f);
    float c3 = Mathf.Cos(z * 0.5f);

    float s1 = Mathf.Sin(x * 0.5f);
    float s2 = Mathf.Sin(y * 0.5f);
    float s3 = Mathf.Sin(z * 0.5f);

    // Unity's order is 'YXZ'
    float qx = s1 * c2 * c3 + c1 * s2 * s3;
    float qy = c1 * s2 * c3 - s1 * c2 * s3;
    float qz = c1 * c2 * s3 - s1 * s2 * c3;
    float qw = c1 * c2 * c3 + s1 * s2 * s3;

    // 'ZXY'
    // float qx = s1 * c2 * c3 - c1 * s2 * s3;
    // float qy = c1 * s2 * c3 + s1 * c2 * s3;
    // float qz = c1 * c2 * s3 + s1 * s2 * c3;
    // float qw = c1 * c2 * c3 - s1 * s2 * s3;
    
    // 'XYZ'
    // float qx = s1 * c2 * c3 + c1 * s2 * s3;
    // float qy = c1 * s2 * c3 - s1 * c2 * s3;
    // float qz = c1 * c2 * s3 + s1 * s2 * c3;
    // float qw = c1 * c2 * c3 - s1 * s2 * s3;

    // 'ZYX'
    // float qx = s1 * c2 * c3 - c1 * s2 * s3;
    // float qy = c1 * s2 * c3 + s1 * c2 * s3;
    // float qz = c1 * c2 * s3 - s1 * s2 * c3;
    // float qw = c1 * c2 * c3 + s1 * s2 * s3;

    // 'YZX'
    // float qx = s1 * c2 * c3 + c1 * s2 * s3;
    // float qy = c1 * s2 * c3 + s1 * c2 * s3;
    // float qz = c1 * c2 * s3 - s1 * s2 * c3;
    // float qw = c1 * c2 * c3 - s1 * s2 * s3;

    // 'XZY'
    // float qx = s1 * c2 * c3 - c1 * s2 * s3;
    // float qy = c1 * s2 * c3 - s1 * c2 * s3;
    // float qz = c1 * c2 * s3 + s1 * s2 * c3;
    // float qw = c1 * c2 * c3 + s1 * s2 * s3;

    return new Quaternion(qx, qy, qz, qw);
}

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

さて最後に、逆のクォータニオンからオイラー角を求める計算・・なのですが、それについては以前Qiitaで書いたのでそちらを参照ください。

qiita.com

最後に

クォータニオン、奥が深いですね。
Unityなどで利用する限りにおいては基本的にエンジン側で色々よしなにしてくれるので気にすることはありませんが、だいぶ複雑なことをやってくれているんだなーと実感させられます。