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
Translate »