2つのベクトルを一致させる座標変換をクォータニオンを使って作る

　こんにちは．changです．非常にマニアックでほぼ自分用のメモですが，ふとした感動があったので書いておきます．クォータニオンです‼️

0. 問題

f:id:changlikesdesktop:20211011140059p:plain:w300 — 下図のベクトルaをベクトルb(z軸)に一致させる座標変換を求めよ

　こんなプログラムを真面目に書いている人なんているのでしょうか？ちなみに，僕の仕事はこんなのばかりです😮‍💨

　話を簡単にする為に，2つのベクトルは共に単位行列で，起点が原点にあることとします．また，ターゲット(回転先)はz軸とします．

　オイラー( Eular)角って何？*1，クォータニオン(Quaternion)って何？*2って話は省略します．

1. 解答1: arccos 2回 + オイラー角で解く

　多くの方(？)が，僕らの時代で言うところの数学2Bの範囲で解こうとすると思います．ベクトルaを２次元に投影するやり方です．例えば，先ずz軸回りに回転させてベクトルをxz平面内に移動し，次にy軸回りに回転します．

　箇条書きにするとこんな感じ:

ベクトルaをxy平面に投影し，ベクトルaをx軸に一致させる為のz軸回転をarccosで求める
1.で求めたz軸回転をベクトルaにかける(=ベクトルa')
ベクトルa'をz軸に一致させるためのy軸回転をarccosを使って求める
↑で求めたy軸回転をベクトルa'にかける

f:id:changlikesdesktop:20211011140634p:plain:w300 — 解答1．直感的な様で直感的ではないですね

　数学2Bの範囲を超えてしまいますが，問題に沿って”座標変換を求める”とこんな感じです．

手順1のz軸回転:

$Rot_{z} = \begin{bmatrix} cos(z) & -sin(z) & 0 \\ sin(z) & cos(z) & 0 \\ 0 & 0 & 1 \end{bmatrix}$

手順3のy軸回転:

$Rot_{y} = \begin{bmatrix} cos(y) & 0 & -sin(z) \\ 0 & 1 & 0 \\ sin(y) & 0 & cos(z) \end{bmatrix}$

求めたい座標変換は:

$M = Rot_{y} \cdot Rot_{z}$

　結構面倒ですね．ちなみに，僕の出身高校では理系に進まない生徒も数学2Bまでは強制的に履修されられました． 4大や短大に進んだ多くの方が，解答1の予備知識をお持ちかと思います．

　実はこの方法，間違いでは無いもののいくつか問題があります．

2回回転するので，答えが一杯ある
arccosを2回使うので処理に時間が掛かる
今回はターゲットがz軸なので簡単だが，一般ベクトル2つで解こうとすると凄く大変
数学的に美しくない

　恥ずかしい話ですが，僕は仕事で解答1を何度も書きました🤫．例えば初音ミクを動かすライブラリ等は間違い無くこんな書き方はしていないと思います．

2. 解答2: arccos 1回 + オイラー角で解く

　解答1の問題点の一つに，答えが一杯あることを挙げました．これ，場合によっては致命的です．今回は座標変換を求めることが目的なので構いませんが，回転角度を求めることが目的の場合には非常に気持ち悪いです． 2つのベクトルを最短でつなぐ，唯一の回転を求めたいところです．

　角度の大きさ自体は，内積で簡単に求まります．

$\theta = arccos(\frac{a \cdot b}{|a||b|})$

　困ったことに，オイラー角はxyz軸の周りにしか回せません．このため，ベクトルaとターゲットを回転軸に合わせて座標変換する必要があります．

手順としては:

内積を使って回転角 $\theta$ を求める
ベクトルaとターゲットの外積(cross 1と呼ぶ)を求める．
2.の外積と，ベクトルaの外積(cross 2と呼ぶ)を求める
ベクトルaをx軸，cross 2をy軸，cross 1をz軸とする座標変換を作る
4.で求めた座標変換をベクトルaとターゲットにかける
1.で求めた回転角だけz軸回りに回す
4.の逆変換を掛ける

f:id:changlikesdesktop:20211011141828p:plain:w300 — 解答2，絵が解りにくくてすみません

　解答2は，僕自身がここ何年か採用していたやり方です．正直これで満足していました．先日「クォータニオンでもっと簡単に解けるじゃん！」と気づいたのが，この記事を書いたきっかけです．

3. 解答3: arccos1回 + クォータニオンで解く

　やっと本題です．クォータニオンは，解答2の手順1で求めた外積cross 1と，手順2で求めた角度 $\theta$ から直接求めらます．

$q = \begin{bmatrix} cross_x \cdot sin(\frac{\theta}{2}) \\ cross_y \cdot sin(\frac{\theta}{2}) \\ cross_z \cdot sin(\frac{\theta}{2}) \\ cos(\frac{\theta}{2}) \end{bmatrix}$

f:id:changlikesdesktop:20211011142132p:plain:w300 — 解答3

クォータニオンを回転行列に変換:

$M = \begin{bmatrix} q_{0}q_{0} - q_{1}q_{1} - q_{2}q_{2} + q_{3}q_{3} & 2q_{0}q_{1} - 2q_{3}q_{2} & 2q_{0}q_{2} + 2q_{3}q_{1} \\ 2q_{0}q_{1} + 2q_{3}q_{2} & - q_{0}q_{0} + q_{1}q_{1} - q_{2}q_{2} + q_{3}q_{3} & 2q_{1}q_{2} - 2q_{3}q_{0} \\ 2q_{0}q_{2} - 2q_{3}q_{1} & 2q_{1}q_{2} + 2q_{3}q_{0} & - q_{0}q_{0} - q_{1}q_{1} + q_{2}q_{2} + q_{3}q_{3} \end{bmatrix}$

　美しいですね．ちなみに，下記のPythonソースで解答2と解答3の速度比較を行ったところ，解答3は解答2の半分の処理時間で済みました．

import numpy as np
import random
import time

from numpy import sqrt, sin, cos, arccos, arctan2

def vec2zaxis(vec):
    matrix = np.zeros([3, 3])
    matrix[0, 0] = 0.0
    matrix[0, 1] = 0.0
    matrix[0, 2] = 1.0
    matrix[2, 0] = vec[1]*matrix[0, 2] - vec[2]*matrix[0, 1]
    matrix[2, 1] = vec[2]*matrix[0, 0] - vec[0]*matrix[0, 2]
    matrix[2, 2] = vec[0]*matrix[0, 1] - vec[1]*matrix[0, 0]
    norm = sqrt(matrix[2, 0]*matrix[2, 0] + matrix[2, 1]*matrix[2, 1] + matrix[2, 2]*matrix[2, 2])
    matrix[2, 0] /= norm
    matrix[2, 1] /= norm
    matrix[2, 2] /= norm
    matrix[1, 0] = matrix[2, 1]*matrix[0, 2] - matrix[2, 2]*matrix[0, 1]
    matrix[1, 1] = matrix[2, 2]*matrix[0, 0] - matrix[2, 0]*matrix[0, 2]
    matrix[1, 2] = matrix[2, 0]*matrix[0, 1] - matrix[2, 1]*matrix[0, 0]

    temp = np.dot(matrix, vec)
    # print(temp)
    # temp = np.dot(matrix, [0, 0, 1])
    # print(temp)

    ang = arctan2(temp[1], temp[0])
    # print("ang = ")
    # print(ang)
    rot = np.zeros([3, 3])
    rot[0, 0] = cos(-ang)
    rot[0, 1] = -sin(-ang)
    rot[0, 2] = 0.0
    rot[1, 0] = sin(-ang)
    rot[1, 1] = cos(-ang)
    rot[1, 2] = 0.0
    rot[2, 0] = 0.0
    rot[2, 1] = 0.0
    rot[2, 2] = 1.0

    temp2 = np.dot(rot, matrix)
    ans = np.dot(np.transpose(matrix), temp2)

    # print("ans = ")
    # print(ans)
    # temp3 = np.dot(ans, vec)
    # print(temp3)

    return ans

def vec2zaxis_Quat(vec):
    target = [0.0, 0.0, 1.0]
    cross = np.zeros(3)
    cross[0] = vec[1]*target[2] - vec[2]*target[1]
    cross[1] = vec[2]*target[0] - vec[0]*target[2]
    cross[2] = vec[0]*target[1] - vec[1]*target[0]
    norm = sqrt(cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2])
    cross[0] /= norm
    cross[1] /= norm
    cross[2] /= norm

    dot = vec[0]*target[0] + vec[1]*target[1] + vec[2]*target[2]
    ang = arccos(dot)

    q = [cross[0]*sin(ang/2.0), cross[1]*sin(ang/2.0), cross[2]*sin(ang/2.0), cos(ang/2.0)]
    rot = np.zeros([3, 3])
    rot[0, 0] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]
    rot[0, 1] = 2.0*q[0]*q[1] - 2.0*q[3]*q[2]
    rot[0, 2] = 2.0*q[0]*q[2] + 2.0*q[3]*q[1]
    rot[1, 0] = 2.0*q[0]*q[1] + 2.0*q[3]*q[2]
    rot[1, 1] = - q[0]*q[0] + q[1]*q[1] - q[2]*q[2] + q[3]*q[3]
    rot[1, 2] = 2.0*q[1]*q[2] - 2.0*q[3]*q[0]
    rot[2, 0] = 2.0*q[0]*q[2] - 2.0*q[3]*q[1]
    rot[2, 1] = 2.0*q[1]*q[2] + 2.0*q[3]*q[0]
    rot[2, 2] = - q[0]*q[0] - q[1]*q[1] + q[2]*q[2] + q[3]*q[3]

    return rot

if __name__ == "__main__":
    x = random.uniform(-1.0, 1.0)
    y = random.uniform(-1.0, 1.0)
    z = random.uniform(-1.0, 1.0)
    norm = np.sqrt(x*x + y*y + z*z)
    x = x/norm
    y = y/norm
    z = z/norm
    vec = [x, y, z]

    start = time.time()
    mat = vec2zaxis(vec)
    elapsed_time = time.time() - start
    print(vec)
    print(np.dot(mat, vec))
    print ("elapsed_time:{0}".format(elapsed_time) + "[sec]")

    start = time.time()
    mat = vec2zaxis_Quat(vec)
    elapsed_time = time.time() - start
    print(vec)
    print(np.dot(mat, vec))
    print ("elapsed_time:{0}".format(elapsed_time) + "[sec]")

4. むすび

　学生時代を含めるとかれこれ10年以上座標変換を書いていますが，，，自分の勉強不足を実感させられました．言い訳になりますが，仕事という観点で言えば，答えが正しく出て，プログラムが滞り無く動けばそれで良いのですよ🙄．「処理時間を短くしたいな」とか，「ソースをダイエットしたいな」とかって思ったときに，調べて見識を広げたり，感動を味わったりすることが，楽しく仕事を続ける支えになると信じています．もっと速く，エレガントに解く方法があるかも知れないと思うと，ワクワクしますね😀

　本編と関係ないのですが，10年ぶりにTexで数式を書いたら吐き気がしました🤢

*1:https://ja.wikipedia.org/wiki/オイラー角

*2:https://ja.wikipedia.org/wiki/四元数