ましろの興味が書く

UnityとかVRChatとかプログラミングとか電子工作とかロマンあふれるガジェットに興味があるぞ

クォータニオンでの回転とPython(ある座標に向かわせたい)

本記事の目的はある座標(ある方向)に回転させたい物体を回転させるプログラムを書くことです。副目的として数学を学ぶことが挙げられます。

本記事ではBlenderPythonを使用します。 Blender2.93 Python3.8.8

回転させたい物体にクォータニオンを使用すると回転軸と角度の4つのパラメーターで回転をさせることができます。

回転軸→外積を使って求める
角度→内積を使って求める
クォータニオン→回転軸と角度を使って求める

計算に手を出す前に,クォータニオンのイメージを想像するとわかりやすくなります。以下の動画には丁寧にわかりやすくイメージを見せてくれています。(33分47秒辺りから)

【Unity道場 博多スペシャル 2017】クォータニオン完全マスター youtu.be

クォータニオンを計算するのは

1.向かせたいベクトルと「上」方向のベクトルを取得
2.上記2つのベクトルとの外積を取る(これが回転軸となる)
3.上記2つのベクトルの角度を、内積を使って取得
4.これら情報を元に、回転用のクォータニオンを生成
5.生成したクォータニオンをオブジェクトに適用

という順番で進めていきます。 [Three.js] クォータニオンを使って任意の方向にオブジェクトを向ける - Qiita

1. 向かせたいベクトルと「上」方向のベクトルを取得

向かせたい座標は[2,3,4]とします。「向かせたいベクトル」を取得するためには,向かせたい座標から回転させたい対象の座標を引く必要があります。これをすることで原点から見た向かせたい座標を見ることができるからではと考えています(数学わからん)。ここでは回転させたい物体の座標を[1,1,1]とします。なので

 \displaystyle \begin{pmatrix} 2, 3, 4\end{pmatrix} -  \begin{pmatrix} 1, 1, 1\end{pmatrix} =  \begin{pmatrix} 1, 2, 3\end{pmatrix}

と表され,[1,2,3]が向かせたいベクトルだとわかります。

「上」方向のベクトルについては[0,1,0]など環境にあった「上」方向を使用します。今回はBlenderのオブジェクトであるアーマチュア(ボーン)の回転を計算したいので,アーマチュアのデフォルトの方向である[0,0,1]を使用しますBlenderではライトのオブジェクトなどのデフォルト方向は[0,0,-1]なため使用するオブジェクトに応じてデフォルトの方向を変更する必要があります。また適宜,各環境のデフォルト方向を割り当ててください。

2. 2つのベクトルとの外積を取る(これが回転軸となる)

numpyを使用するときはnp.cross()で外積を取れます。以下のページを参考にプログラムを書きました。 外積とは何か。ベクトルの外積の定義・意味・大きさについて|アタリマエ!

以下の式がaとbのベクトルの外積を求める式です。a×bが外積を表します。外積は計算する順番が違うと結果が違うことがあるため,順番に気をつけてください。aをデフォルトの方向のベクトル,bを向かせたいベクトルだとすると,

 \displaystyle \boldsymbol{a}=[a_{1}, a_{2}, a_{3}]
 \displaystyle \boldsymbol{b}=[b_{1}, b_{2}, b_{3} ]
 \displaystyle \boldsymbol{a} \times \boldsymbol{b} = \begin{pmatrix} a_{2}b_{3}-a_{3}b_{2}\\ a_{3}b_{1}-a_{1}b_{3}\\a_{1}b_{2}-a_{2}b_{1} \end{pmatrix} =  \begin{pmatrix} \lambda_{x}\\ 
 \lambda_{y}\\ \lambda_{z}\end{pmatrix}

実際に計算してみると以下のようになります。

 \displaystyle \boldsymbol{a}=[0,0,1]
 \displaystyle \boldsymbol{b}=[1, 2, 3]
 \displaystyle \boldsymbol{a} \times \boldsymbol{b} = \begin{pmatrix} 0\times3 - 1\times2 \\ 1\times1 - 0\times3 \\0\times2 - 0\times1 \end{pmatrix} = \begin{pmatrix} -2\\ 1\\ 0 \end{pmatrix}

正規化の話をするため,クォータニオンの式を表します。

 \displaystyle \boldsymbol{q} =\begin{pmatrix} x\\ y\\ z\\ w\\ \end{pmatrix}= \begin{pmatrix} \lambda_{x}\sin\frac{\theta}{2}\\ \lambda_{y}\sin\frac{\theta}{2}\\ \lambda_{z}\sin\frac{\theta}{2}\\ \cos\frac{\theta}{2} \end{pmatrix}

外積をして求められる数値はクォータニオン式の中にある

 \displaystyle \lambda_{x}, \lambda_{y}, \lambda_{z}

に当てはまります。
この3つの数値は正規化しなければいけないという条件があります。 normalize(正規化)する条件は以下のとおりです。

 \displaystyle  \lambda_{x}^2+\lambda_{y}^2+\lambda_{z}^2=1

にならなくてはいけないというルールがあるので正規化します。正規化することによって各座標の2乗の総和を1にすることができます。

外積の結果を正規化する方法は以下の数式で表され

 \begin{pmatrix} \lambda_{x} 
 \lambda_{y} \lambda_{z} \end{pmatrix} /  \displaystyle \sqrt{\sum_{k=1}^{n}}k^2 = \begin{pmatrix} \lambda_{x}  \lambda_{y} \lambda_{z}\end{pmatrix} /  \displaystyle \sqrt{\lambda_{x}^2 + \lambda_{y}^2 + \lambda_{z}^2 }

実際に計算してみると以下のようになります。

 \displaystyle \begin{pmatrix} -2, 1 , 0 \end{pmatrix}  / \sqrt{-2^2 + 1^2 + 0^2} = \begin{pmatrix} -2, 1 , 0 \end{pmatrix}  /  2.23606797749979 \\= \begin{pmatrix} -0.89442719, 0.4472136, 0\end{pmatrix}

Pythonで書くと以下のようになります。

import numpy as np
o=np.array([1,1,1]) #回転させたい物体の座標
point_p = np.array([2,3,4]) #向かせたい座標
p_from_o = point_p - o #向かせたいベクトル
print(f"p_from_o {p_from_o}")
up_vector = np.array([0,0,1]) #blenderのアーマチュアのデフォルト
cross_product = np.array([])

def normalize(v):
 # numpyのnormを使用して計算
    norm = np.linalg.norm(v)
    print(f"norm {norm}")

    # numpyのnorm()を使わず計算
    norm = np.sqrt(sum(v**2))
    print(f"norm {norm}")

    if norm==0: #ゼロ除算を防ぐ
        return v

    return v / norm

# up_vectorが[0,0,-1]で,p_from_oが[1,2,3]
# numpyのnp.cross()を使わず外積計算
cross_product = np.append(cross_product,[up_vector[1]*p_from_o[2] - up_vector[2]*p_from_o[1], up_vector[2]*p_from_o[0] - up_vector[0]*p_from_o[2], up_vector[0]*p_from_o[1] - up_vector[1]*p_from_o[0]])
cross_product = normalize(cross_product)
print(f"cross_product {cross_product}")

# numpyで外積計算
cross_product = np.cross(up_vector, p_from_o)
cross_product = normalize(cross_product)
print(f"cross_product {cross_product}")

np.cross(up_vector, p_from_o)とnp.cross(p_from_o, up_vector)では結果が違うため注意が必要です。 このプログラムはBlender Pythonで方向ベクトルで表される方向をクォータニオンを使って実装する - Qiita を参考にしています。 プログラムを使用する場合はnormを使用する方か,使用しない方,どちらかを選んで使用してください

3. 2つのベクトルの角度を、内積を使って取得

以下のページを参考にプログラムを書きました。内積とは何なのか?ベクトルの内積の2つの求め方とその活用法|アタリマエ!

向かせたいベクトルがまだ正規化されていない場合内積は以下の式で計算できます。
 \vec{a}\cdot\vec{b}=\|a\|\|b\|\cos\theta\displaystyle

上の式を式変形することでcosθを以下の式で計算することができます。

 \displaystyle \cos\theta = \dfrac{\vec{a}\cdot\vec{b}}{\|a\|\|b\|}

cosθからθ(角度)を取得するためにarccosを使用します。

 \displaystyle \theta = \arccos \cos \theta

コードでは以下の式を使用してベクトルaとベクトルbの内積を計算します。

 \displaystyle \vec{a}\cdot\vec{b}=a_{1}b_{1} + a_{2}b_{2} + a_{3}b_{3}

計算してみます。ベクトルaをデフォルトの方向のベクトル,ベクトルbを正規化されていない向かせたいベクトルとします。radはラジアンの意味です。プログラムを実行するとラジアンが返されるため,ここでの計算結果もラジアン表記にしています。

 \displaystyle \vec{a} =[0,0,1 ]
 \displaystyle \vec{b} =[1,2,3 ]
 \displaystyle \vec{a}\cdot\vec{b}=0\times1+0\times2+1\times3 = 3

 \displaystyle 3 = \|a\|\|b\|\cos\theta\\ \quad= \sqrt{0^2+0^2+1^2} \times \sqrt{1^2+2^2+3^2} \times \cos\theta \\
\quad= 3.7416573867739413 \times \cos\theta

 \displaystyle \cos\theta = \dfrac{3
}{3.7416573867739413}

 \displaystyle \theta = \arccos(\dfrac{3
}{3.7416573867739413}) = \arccos(0.8017837257372732) \\= 0.6405223126794245rad


向かせたいベクトルが正規化されている場合内積は計算してみると以下のようになります。
 \displaystyle \theta = \arccos (\vec{a}\cdot\vec{b}) = \arccos(a_{1}b_{1} + a_{2}b_{2} + a_{3}b_{3})


ベクトルaを正規化された向かせたいベクトルとします。
ベクトルbをデフォルトの方向のベクトルとします。
radはラジアンの意味です。プログラムを実行するとラジアンが返されるため,ここでの計算結果もラジアン表記にしています。

 \displaystyle \vec{a} =[0, 0, 1 ]
 \displaystyle \vec{b} = [0.26726124, 0.53452248, 0.80178373 ]

 \displaystyle \theta = \arccos (\vec{a}\cdot\vec{b})\\
=\arccos(0\times0.26726124 + 0\times0.53452248 + 1\times0.80178373)\\ = \arccos(0.80178373) = 0.6405223055465182rad

向かせたい座標へのベクトルを正規化しても,正規化しなくても0.6405223まで一致しているので,好きな方を使うといいと思います。可読性を考えると正規化されたほうを選んだほうがいいと思います。

inner_product = np.array([])

# numpyのnp.dotやnormを使わず内積計算(正規化されている場合もされていない場合も使える)
inner_product=np.append(inner_product, [up_vector[0]*p_from_o[0] + up_vector[1]*p_from_o[1] + up_vector[2]*p_from_o[2]])
print(f"inner_product {inner_product}")
norm_up_vector_p = np.sqrt(sum(up_vector**2)) * np.sqrt(sum(p_from_o**2))
print(f"norm_up_vector_p {norm_up_vector_p}")
cos_theta = inner_product / norm_up_vector_p
print(f"cos_theta {cos_theta}")
theta = np.arccos(cos_theta) #thetaはラジアンで返ってくる
print(f"theta {theta}")

# numpyで内積計算(正規化されている場合もされていない場合も使える)
inner_product = np.dot(up_vector, p_from_o)
print(f"inner_product {inner_product}")
norm_up_vector_p = np.linalg.norm(up_vector) * np.linalg.norm(p_from_o)
print(f"norm_up_vector_p {norm_up_vector_p}")
cos_theta = inner_product / norm_up_vector_p
print(f"cos_theta {cos_theta}")
theta = np.arccos(cos_theta) #thetaはラジアンで返ってくる
print(f"theta {theta}")

#上2つをまとめて短く書いたもの(正規化されていないと使えない)
up_vector = normalize(up_vector)
p_from_o = normalize(p_from_o)
theta = np.arccos(np.dot(up_vector, p_from_o))
print(f"theta {theta}")

4. これら情報を元に、回転用のクォータニオンを生成

クォータニオンは以下の式で表します。

 \displaystyle \boldsymbol{q} =\begin{pmatrix} x\\ y\\ z\\ w\\ \end{pmatrix}= \begin{pmatrix} \lambda_{x}\sin\frac{\theta}{2}\\ \lambda_{y}\sin\frac{\theta}{2}\\ \lambda_{z}\sin\frac{\theta}{2}\\ \cos\frac{\theta}{2} \end{pmatrix}

 \displaystyle (\lambda_{x} \lambda_{y} \lambda_{z}) = (−0.89442719, 0.4472136, 0)

 \displaystyle \theta = 0.6405223055465182rad

 \displaystyle \boldsymbol{q} =\begin{pmatrix} x\\ y\\ z\\ w\\ \end{pmatrix}= \begin{pmatrix} −0.89442719\times\sin(\frac{0.6405223055465182rad}{2})\\ 0.4472136\times\sin(\frac{0.6405223055465182rad}{2})\\ 0\times\sin(\frac{0.6405223055465182rad}{2})\\ \cos\frac{0.6405223055465182rad}{2} \end{pmatrix}\\
= \begin{pmatrix}-0.2815785997243485\\ 0.1407893014362465\\ 0\\ 0.9491532357844017\end{pmatrix}

quaternion = np.array([])
rad_theta_divide2 = theta/2 #ラジアンで返ってくるnp.cosはラジアンを入れる
print(f"rad_theta_divide2 {rad_theta_divide2}")

np.set_printoptions(suppress=True) #指数表記を禁止
#w,x,y,zの順
quaternion = np.append(quaternion, [np.cos(rad_theta_divide2),
                                    cross_product[0]*np.sin(rad_theta_divide2),
                                    cross_product[1]*np.sin(rad_theta_divide2),
                                    cross_product[2]*np.sin(rad_theta_divide2)])
print(f"quaternion {quaternion}")

5. 生成したクォータニオンをオブジェクトに適用

ここは各自で回転させたいオブジェクトにクォータニオンに適用してください。(無慈悲)

プログラムの全体

import numpy as np

# 回転させたい物体の座標
o = np.array([1, 1, 1])
# 向かせたい座標
p = np.array([2, 3, 4])
p_from_o = p - o
print(f"p_from_o {p_from_o}")

up_vector = np.array([0, 0, 1])  # blenderのアーマチュアのデフォルト(デフォルト方向)

# 正規化,配列のリストを1を最大で正規化
def normalize(v):
    # norm = np.linalg.norm(v)
    # print(f"norm {norm}")
    # 同じ意味
    norm = np.sqrt(sum(v**2))
    print(f"normalize(norm) {norm}")
    if norm == 0:  # ゼロ除算を防ぐ
        return v

    return v / norm

cross_product = np.array([])  # 外積用のリスト

# numpyを使わず外積計算
# cross_product = np.append(cross_product, [up_vector[1]*p_from_o[2] - up_vector[2]*p_from_o[1], up_vector[2]
#                           * p_from_o[0] - up_vector[0]*p_from_o[2], up_vector[0]*p_from_o[1] - up_vector[1]*p_from_o[0]])
# print(f"cross_product {cross_product}")
# cross_product = normalize(cross_product)
# print(f"normalize(cross_product) {cross_product}")

# numpyで外積計算
cross_product = np.cross(up_vector, p_from_o)
cross_product = normalize(cross_product)
print(f"cross_product {cross_product}")


inner_product = np.array([])

# 内積から角度を出す

# # numpyのnp.dotやnormを使わず内積計算(正規化されている場合もされていない場合も使える)
# print("numpyを使わず内積計算")
# print(f"up_vector {up_vector}")
# print(f"p_from_o {p_from_o}")
# inner_product=np.append(inner_product, [up_vector[0]*p_from_o[0] + up_vector[1]*p_from_o[1] + up_vector[2]*p_from_o[2]])
# print(f"inner_product {inner_product}")
# norm_up_vector_p = np.sqrt(sum(up_vector**2)) * np.sqrt(sum(p_from_o**2))
# print(f"norm_up_vector_p {norm_up_vector_p}")
# cos_theta = inner_product / norm_up_vector_p
# print(f"cos_theta {cos_theta}")
# theta = np.arccos(cos_theta)
# print(f"theta {theta}")

# # numpyで内積計算(正規化されている場合もされていない場合も使える)
# print("numpyで内積計算")
# inner_product = np.dot(up_vector,p_from_o)
# print(f"inner_product {inner_product}")
# norm_up_vector_p = np.linalg.norm(up_vector) * np.linalg.norm(p_from_o)
# print(f"norm_up_vector_p {norm_up_vector_p}")
# cos_theta = inner_product / norm_up_vector_p
# print(f"cos_theta {cos_theta}")
# theta = np.arccos(cos_theta)
# print(f"theta {theta}")

# 上2つをまとめて短く書いたもの(正規化されていないと使えない)
up_vector = normalize(up_vector)
p_from_o = normalize(p_from_o)
print("上記をまとめて短く書いたもの")
theta = np.arccos(np.dot(up_vector, p_from_o))  # thetaはラジアンで返ってくる
print(f"theta {theta}")


# クォータニオンを求める
quaternion = np.array([])
rad_theta_divide2 = theta/2  # thetaはラジアンで返すためラジアンで変換をしない
print(f"rad_theta_divide2 {rad_theta_divide2}")
np.set_printoptions(suppress=True)  # 指数表記を禁止
quaternion = np.append(quaternion, [np.cos(rad_theta_divide2),
                                    cross_product[0]*np.sin(rad_theta_divide2),
                                    cross_product[1]*np.sin(rad_theta_divide2),
                                    cross_product[2]*np.sin(rad_theta_divide2)])
# w,x,y,z
print(f"quaternion {quaternion}")

まとめ

Pythonとnumpyをフル活用することでかんたんにクォータニオンを求めることが可能。クォータニオンは回転軸と角度を出すことで求められる。回転軸は外積で,角度は内積で出すことができる。クォータニオンを出してオブジェクトに適用して回転!回転!

参考

youtu.be

qiita.com

qiita.com

qiita.com