2つのベクトルが作る回転行列を計算 (スクリプト)
Pythonスクリプトで、2つのベクトルが作る回転行列を計算します。
numpyを使用しています。
numpyについては「Pythonスクリプトでのベクトル/行列計算」にShade3Dでの各機能の利用について記載しています。
import numpy
import math
#---------------------------------------.
# ゼロチェック.
# @param[in] v 実数値.
# @return ゼロの場合はTrue.
#---------------------------------------.
def isZero (v):
minV = 1e-5
if v > -minV and v < minV:
return True
return False
#---------------------------------------.
# ベクトルの長さを計算.
# @param[in] v ベクトル値.
# @return ベクトルの長さ.
#---------------------------------------.
def length_vec3 (v):
vec3 = numpy.array(v)
return numpy.linalg.norm(vec3)
#---------------------------------------.
# 単位ベクトルを計算.
# @param[in] v xyzのベクトル値.
# @return 正規化されたベクトル値.
#---------------------------------------.
def normalize_vec3 (v):
vec3 = numpy.array(v)
len = numpy.linalg.norm(vec3)
if isZero(len):
return [0, 0, 0]
return vec3 / len
#---------------------------------------.
# 単位ベクトルを計算.
# @param[in] v xyzwのベクトル値.
# @return 正規化されたベクトル値.
#---------------------------------------.
def normalize_vec4 (v):
vec4 = numpy.array(v)
len = numpy.linalg.norm(vec4)
if isZero(len):
return [0, 0, 0, 0]
return vec4 / len
#---------------------------------------.
# 内積を計算.
# @param[in] v1 xyzのベクトル1.
# @param[in] v2 xyzのベクトル2.
# @return 内積の値.
#---------------------------------------.
def dot_vec3 (v1, v2):
vec3_1 = numpy.array(v1)
vec3_2 = numpy.array(v2)
return numpy.dot(vec3_1, vec3_2)
#---------------------------------------.
# 外積を計算.
# @param[in] v1 xyzのベクトル1.
# @param[in] v2 xyzのベクトル2.
# @return 外積のベクトル.
#---------------------------------------.
def cross_vec3 (v1, v2):
vec3_1 = numpy.array(v1)
vec3_2 = numpy.array(v2)
return numpy.cross(vec3_1, vec3_2)
#---------------------------------------.
# ベクトルaをbに向かせるquaternionを求める.
# @param[in] a xyzのベクトル1.
# @param[in] b xyzのベクトル2.
# @return Quaternion値.(w,x,y,z)として入っている.
#---------------------------------------.
def quaternion_a_b (a, b):
a = normalize_vec3(a)
b = normalize_vec3(b)
q = [0.0, 0.0, 0.0, 0.0]
c = cross_vec3(b, a)
d = -length_vec3(c)
c = normalize_vec3(c)
epsilon = 0.0002
ip = dot_vec3(a, b)
if -epsilon < d or 1.0 < ip:
if ip < (epsilon - 1.0):
a2 = [-a[1], a[2], a[0]]
c = normalize_vec3(cross_vec3(a2, a))
q[0] = 0.0
q[1] = c[0]
q[2] = c[1]
q[3] = c[2]
else:
q = numpy.array([1.0, 0.0, 0.0, 0.0])
else:
e = c * math.sqrt(0.5 * (1.0 - ip))
q[0] = math.sqrt(0.5 * (1.0 + ip))
q[1] = e[0]
q[2] = e[1]
q[3] = e[2]
return q
#---------------------------------------.
# quaternionから4x4行列を求める.
# @param[in] quaternion値。(w,x,y,z)として入っている.
# @return 4x4の回転行列.
#---------------------------------------.
def quaternion_matrix4x4 (q):
m = numpy.matrix(numpy.identity(4))
# q(w,x,y,z)から(x,y,z,w)に並べ替え.
q2 = range(4)
q2[0] = q[1]
q2[1] = q[2]
q2[2] = q[3]
q2[3] = q[0]
m[0,0] = q2[3]*q2[3] + q2[0]*q2[0] - q2[1]*q2[1] - q2[2]*q2[2]
m[0,1] = 2.0 * q2[0] * q2[1] - 2.0 * q2[3] * q2[2]
m[0,2] = 2.0 * q2[0] * q2[2] + 2.0 * q2[3] * q2[1]
m[0,3] = 0.0
m[1,0] = 2.0 * q2[0] * q2[1] + 2.0 * q2[3] * q2[2]
m[1,1] = q2[3] * q2[3] - q2[0] * q2[0] + q2[1] * q2[1] - q2[2] * q2[2]
m[1,2] = 2.0 * q2[1] * q2[2] - 2.0 * q2[3] * q2[0]
m[1,3] = 0.0
m[2,0] = 2.0 * q2[0] * q2[2] - 2.0 * q2[3] * q2[1]
m[2,1] = 2.0 * q2[1] * q2[2] + 2.0 * q2[3] * q2[0]
m[2,2] = q2[3] * q2[3] - q2[0] * q2[0] - q2[1] * q2[1] + q2[2] * q2[2]
m[2,3] = 0.0
m[3,0] = 0.0
m[3,1] = 0.0
m[3,2] = 0.0
m[3,3] = q2[3] * q2[3] + q2[0] * q2[0] + q2[1] * q2[1] + q2[2] * q2[2]
k = m[3,3]
for i in range(3):
for j in range(3):
m[i,j] /= k
m[3,3] = 1.0
return m
#---------------------------------------.
# ベクトルaをbに向かせる回転行列を求める.
# @param[in] _a xyzのベクトル1.
# @param[in] _b xyzのベクトル2.
# @return 4x4の回転行列.
#---------------------------------------.
def rotate_a_to_b_matrix4x4 (_a, _b):
m = numpy.matrix(numpy.identity(4))
a = numpy.array([_a[0], _a[1], _a[2]])
b = numpy.array([_b[0], _b[1], _b[2]])
return quaternion_matrix4x4( quaternion_a_b(a, b) )
上記の関数を使用し以下のように実行すると、ベクトルaをベクトルbに変換する回転行列mが計算されます。
a = [0, 0.2, 1.0]
b = [1.0, 0.2, 0.0]
m = rotate_a_to_b_matrix4x4(a, b)
print m