特定の方向からオイラー角を求める

#はじめに
みなさん、特定の方向を向いたジオメトリがX軸、Y軸、Z軸をそれぞれ何度回転したものなのか(これをオイラー角と呼びます)知りたいときありませんか？

このような3本の線(赤がX軸、緑がY軸、青がZ軸)が傾いていたとします。
取得できる情報はそれぞれの線の方向ベクトルだけと仮定します。

Houdiniで用意されているVEX関数を調べると

• クォータニオンからオイラー角を返すquaterniontoeuler関数( https://www.sidefx.com/ja/docs/houdini/vex/functions/quaterniontoeuler.html )。
• ベクトルAをベクトルBに向けるクォータニオンを返すdihedral関数( https://www.sidefx.com/ja/docs/houdini/vex/functions/dihedral.html )。

があります。
これら2つの関数を使用すれば特定の方向を向くオイラー角が求まるのですが、そのクォータニオンによる回転だと、ある軸を球状に回転しつつ、その軸も同時に回転させることができるため、dihedral関数で例えば赤のX軸を指定するとその軸は正しい向きになっても他のベクトル(Y軸とZ軸)は期待通りの向きにはなりません。
1軸だけを考慮したオイラー角の計算ならばこの2つの関数で十分なのですが、今回の目的は3軸から正しいオイラー角を求めたいです。
ただ、特定の方向になるオイラー角は複数存在するわけなので何かしらの条件を決め打ちしてオイラー角を求めようと思います。

今回の目的を満たすVEX関数がなかったので数学から解を求めてそれをWrangleで実装したいと思います。

#回転行列を求めてそこからオイラー角を逆算する
まずは特定の方向をオイラー回転させるための回転行列を求めます。
オイラー回転の順序は、XYZ、XZY、YXZ、YZX、ZXY、ZYXの6パターンがありますが、ここではXYZの場合で考えてみます。

ある方向をXYZ順でオイラー回転する場合の回転行列は、XYZの順でそれぞれの軸回転の回転行列を乗算したものなので、XYZ軸それぞれの回転角度をα,β,γとすると

\begin{eqnarray}
\left(
  \begin{array}{ccc}
    \cos\beta * \cos\gamma & \cos\beta * \sin\gamma & -\sin\beta \\
    \sin\alpha * \sin\beta * \cos\gamma - \cos\alpha * \sin\gamma & \sin\alpha * \sin\beta * \sin\gamma + \cos\alpha * \cos\gamma & \sin\alpha * \cos\beta \\
    \cos\alpha * \sin\beta * \cos\gamma + \sin\alpha * \sin\gamma & \cos\alpha * \sin\beta * \sin\gamma - \sin\alpha * \cos\gamma & \cos\alpha * \cos\beta
 
  \end{array}
\right) \\
\end{eqnarray}

となります。

回転後のX軸ベクトルを(XI,XJ,XK)、回転後のY軸ベクトルを(YI,YJ,YK)、回転後のZ軸ベクトルを(ZI,ZJ,ZK)としてそこからα,β,γを逆算すると

\alpha=\arctan(YK/ZK)\\
\beta=\arcsin(-XK)\\
\gamma=\arctan(XK/XI)

となります。この計算の過程は長くなるので説明を省略します。

#算出された数式の解が一意でない
数式としてはオイラー角が求まったように見えるのですが、arcsin、arccos、arctanって解が複数存在するんですよね。
例えばtan(θ)=0.0の場合、θ=arctan(0.0)だとθの解が0°でも180°でも-180°でもtan(θ)は0.0になります。
なので、算出された数式の複数候補ある解のどれかの組み合わせが正解で、それ以外はまったく違う向きとか反転するといった現象が起こります。
どうやって一意の解を求めるか？
Pythonのmathモジュールだとarctanに対してatanとatan2が存在します。
何も考えなければatan関数を使いがちですがatan2がすごく便利です。
VEXではもちろんatan関数もatan2関数もどちらも存在します。
atan関数:
https://www.sidefx.com/ja/docs/houdini/vex/functions/atan.html
atan2関数:
https://www.sidefx.com/ja/docs/houdini/vex/functions/atan2.html
通常のatan関数は引数が1つなのに対してatan2関数は引数が2つで、X成分とY成分の値を別々に引数として指定することができます。
これによって象限を判定することができます。
残るβの角度(2番目の回転角)はcos(β)=0.0の時にジンバルロックが発生するので1番目と3番目の回転角のどちらかの角度を決め打ちしなければなりません。
ここではcos(β)が0の時に1番目の角度を0にすることにします。
これで一意の解を求めます。

#特定の方向からオイラー角を求めるWrangleコード

Wrangleサブルーチン

//X軸となる方向とUpベクトルと回転順からオイラー角を返すサブルーチン
vector directionToEuler(vector dir;vector up;string order)
{
    vector X = normalize(dir);
    vector Y = normalize(up - dot(X,up)*X);
    vector Z = cross(X,Y);
    vector angle = {0.0,0.0,0.0};
    float judge = 0.0;
    
    if(order=="xyz"){
        angle.x = degrees( atan2(Y.z,Z.z) );
        angle.y = degrees( asin(-X.z) );
        angle.z = degrees( atan2(X.y,X.x) );
        judge = abs(cos(radians(angle.y))) ;
        if(judge<1e-6){
            angle.x = 0.0;
            angle.z = degrees( atan2(-Y.x,Y.y) );
        }
    }
    else if(order=="xzy"){
        angle.x = degrees( atan2(-Z.y,Y.y) );
        angle.z = degrees( asin(X.y) );
        angle.y = degrees( atan2(-X.z,X.x) );
        judge = abs(cos(radians(angle.z))) ;
        if(judge<1e-6){
          angle.x = 0.0;
          angle.y = degrees( atan2(Z.x,Z.z) );
        }
    }
    else if(order=="yxz"){
        angle.y = degrees( atan2(-X.z,Z.z) );
        angle.x = degrees( asin(Y.z) );
        angle.z = degrees( atan2(-Y.x,Y.y) );
        judge = abs(cos(radians(angle.x))) ;
        if(judge<1e-6){
          angle.y = 0.0;
          angle.z = degrees( atan2(X.y,X.x) );
        }
    }
    else if(order=="yzx"){
        angle.y = degrees( atan2(Z.x,X.x) );
        angle.z = degrees( asin(-Y.x) );
        angle.x = degrees( atan2(Y.z,Y.y) );
        judge = abs(cos(radians(angle.z))) ;
        if(judge<1e-6){
          angle.x = 0.0;
          angle.y = degrees( atan2(-Z.y,Z.z) );
        }
    }
    else if(order=="zxy"){
        angle.z = degrees( atan2(X.y,Y.y) );
        angle.x = degrees( asin(-Z.y) );
        angle.y = degrees( atan2(Z.x,Z.z) );
        judge = abs(cos(radians(angle.x))) ;
        if(judge<1e-6){
          angle.z = 0.0;
          angle.y = degrees( atan2(-X.z,X.x) );
        }
    }
    else if(order=="zyx"){
        angle.z = degrees( atan2(-Y.x,X.x) );
        angle.y = degrees( asin(Z.x) );
        angle.x = degrees( atan2(-Z.y,Z.z) );
        judge = abs(cos(radians(angle.y))) ;
        if(judge<1e-6){
          angle.z = 0.0;
          angle.x = degrees( atan2(Y.z,Y.y) );
        }
    }
   return angle;
}

#指定したオイラー角と回転順から回転マトリックスを返すWrangleコード

Wrangleサブルーチン

//指定したオイラー角と回転順からトランスフォームを返すサブルーチン
matrix3 rotateFromEuler(vector angle;string order)
{
    matrix3 rotMatrix = {0,0,0,0,0,0,0,0,0};
    if(order=="xyz"){
            rotMatrix = set(
             cos(radians( angle.y )) * cos(radians( angle.z ))
            , cos(radians( angle.y )) * sin(radians( angle.z ))
            , -sin(radians( angle.y ))
            , sin(radians( angle.x )) * sin(radians( angle.y )) * cos(radians( angle.z )) - cos(radians( angle.x )) * sin(radians( angle.z ))
            , sin(radians( angle.x )) * sin(radians( angle.y )) * sin(radians( angle.z )) + cos(radians( angle.x )) * cos(radians( angle.z ))
            , sin(radians( angle.x )) * cos(radians( angle.y ))
            , cos(radians( angle.x )) * sin(radians( angle.y )) * cos(radians( angle.z )) + sin(radians( angle.x )) * sin(radians( angle.z ))
            , cos(radians( angle.x )) * sin(radians( angle.y )) * sin(radians( angle.z )) - sin(radians( angle.x )) * cos(radians( angle.z ))
            , cos(radians( angle.x )) * cos(radians( angle.y ))
            );
    }
    else if(order=="xzy"){
            rotMatrix = set(
             cos( radians( angle.y ) )*cos( radians( angle.z ) )
            , sin( radians( angle.z ) )
            , -sin( radians( angle.y ) )*cos( radians( angle.z ) )
            , sin( radians( angle.x ) )*sin( radians( angle.y ))-cos( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.x ))*cos( radians( angle.y ))
            , sin( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))+cos( radians( angle.x ))*sin( radians( angle.y ))
            , -sin( radians( angle.x ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.y ))-sin( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))
            );
    }
    else if(order=="yxz"){
            rotMatrix = set(
             cos( radians( angle.y ))*cos( radians( angle.z ))-sin( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))
            , cos( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.x ))*sin( radians( angle.y ))*cos( radians( angle.z ))
            , -cos( radians( angle.x ))*sin( radians( angle.y ))
            , -cos( radians( angle.x ))*sin( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.z ))
            , sin( radians( angle.x ))
            , sin( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.y ))*cos( radians( angle.z ))
            , sin( radians( angle.y ))*sin( radians( angle.z ))-sin( radians( angle.x ))*cos( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.y ))
            );
    }
    else if(order=="yzx"){
            rotMatrix = set(
            cos( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.x ))*sin( radians( angle.y ))
            , sin( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))-cos( radians( angle.x ))*sin( radians( angle.y ))
            , -sin( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.z ))
            , sin( radians( angle.x ))*cos( radians( angle.z ))
            , sin( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))-sin( radians( angle.x ))*cos( radians( angle.y ))
            , sin( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))+cos( radians( angle.x ))*cos( radians( angle.y ))
            );
    }
    else if(order=="zxy"){
            rotMatrix = set(
            sin( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))+cos( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.z ))
            , sin( radians( angle.x ))*cos( radians( angle.y ))*sin( radians( angle.z ))-sin( radians( angle.y ))*cos( radians( angle.z ))
            , sin( radians( angle.x ))*sin( radians( angle.y ))*cos( radians( angle.z ))-cos( radians( angle.y ))*sin( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.z ))
            , sin( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.x ))*cos( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.y ))
            , -sin( radians( angle.x ))
            , cos( radians( angle.x ))*cos( radians( angle.y ))
            );
    }
    else if(order=="zyx"){
            rotMatrix = set(
            cos( radians( angle.y ))*cos( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.z ))+sin( radians( angle.x ))*sin( radians( angle.y ))*cos( radians( angle.z ))
            , sin( radians( angle.x ))*sin( radians( angle.z ))-cos( radians( angle.x ))*sin( radians( angle.y ))*cos( radians( angle.z ))
            , -cos( radians( angle.y ))*sin( radians( angle.z ))
            , cos( radians( angle.x ))*cos( radians( angle.z ))-sin( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))
            , cos( radians( angle.x ))*sin( radians( angle.y ))*sin( radians( angle.z ))+sin( radians( angle.x ))*cos( radians( angle.z ))
            , sin( radians( angle.y ))
            , -sin( radians( angle.x ))*cos( radians( angle.y ))
            , cos( radians( angle.x ))*cos( radians( angle.y ))
            );
    }

    return rotMatrix;
}

#実装した結果
これらのWrangleコードを使って実際に特定の向きからオイラー角を抽出しました。
下図の左側は今回のコードによってオイラー角度を抽出した結果
下図の右側はdihedralとquaterniontoeulerの関数を使って抽出した結果です。

このhipファイルはこちらです。
https://drive.google.com/file/d/1igbGlmqsi-K8yKPXzHmSyQiIeRcSmCPf/view?usp=sharing