2つの長さが同じ3次元実数値ベクトル(空間上の位置)xとyが与えられた時、
xからy(あるいはyからx)へ回転する行列Rを求めたいです。
numpyやscipyを使って、どのようなコードでこれを実現できるでしょうか?
1 件の回答
まず、Mathematics Stack Exchange 上の同様の質問に対する Nico Schlömer さんの回答を和訳します:
ロドリゲスの回転公式を使うと、ベクトル
a
を回転軸k
まわりに角θ
だけ回転したときのベクトルが得られます。ここで単位ベクトルa
をb
に回転するにはa
を(a + b) / 2
まわりにπ
だけ回転すれば良いことを考えると、この公式から以下の美しい式を得ます:
この式は a
, b
が単位ベクトルのときに限らず、a
と b
の長さが等しく a ≠ -b
であれば使えます。このことから、上の式をそのまま実装してみました。
import numpy as np
def rotation_matrix_3d(frm, to):
if np.array_equal(frm, to):
return np.identity(3)
if np.array_equal(frm, -to):
return -np.identity(3)
s = frm + to
return 2.0 * np.outer(s, s) / np.dot(s, s) - np.identity(3)
以下は動作テストの様子です。
def generate(r):
theta = np.random.uniform(0, 2.0 * np.pi)
cosphi = np.random.uniform(-1.0, 1.0)
sinphi = np.sqrt(1.0 - cosphi * cosphi)
v = r * np.array([np.cos(theta) * sinphi, np.sin(theta) * sinphi, cosphi])
return v
if __name__ == '__main__':
r = np.random.random()
a = generate(r)
b = generate(r)
r = rotation_matrix_3d(a, b)
# test
print(" R =", r)
print()
print(" a =", a)
print("Ra =", np.dot(r, a))
print(" b =", b)
$ python test.py
R = [[-0.33930792 0.7584119 0.55649036]
[ 0.7584119 -0.129415 0.6387982 ]
[ 0.55649036 0.6387982 -0.53127708]]
a = [0.83254887 0.9255484 0.65935197]
Ra = [0.78637951 0.93282798 0.70424549]
b = [0.78637951 0.93282798 0.70424549]