ボーン/ボールジョイントのワールド座標での中心位置を計算 (スクリプト)

ボーン/ボールジョイントのワールド座標での中心位置をスクリプトで計算します。

共通部分

import numpy
import math

# --------------------------------------.
# クォータニオンから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

# --------------------------------------.
# translateを4x4行列に変換.
# --------------------------------------.
def translate_matrix4x4 (pos):
    m = numpy.matrix(numpy.identity(4))
    m[3,0] = pos[0]
    m[3,1] = pos[1]
    m[3,2] = pos[2]
    return m

ボールジョイントのワールド座標での中心位置を計算

def calcBallJointWorldCenter(shape):
    cPos = numpy.array([0.0, 0.0, 0.0, 1.0])
    lwMat = numpy.matrix(shape.local_to_world_matrix)
    pos = shape.ball_joint.position
    joint_offset = shape.ball_joint.offset
    joint_rotate = quaternion_matrix4x4(shape.ball_joint.rotation)

    cPos2 = numpy.dot(cPos, joint_rotate)
    cPos2 = numpy.dot(cPos2, translate_matrix4x4(numpy.array(joint_offset) + numpy.array(pos)))
    cPos2 = numpy.dot(cPos2, lwMat)
    return numpy.array([cPos2[0,0], cPos2[0,1], cPos2[0,2]])

この計算は、以下も同じ結果になります。

def calcBallJointWorldCenter(shape):
    lwMat = numpy.matrix(shape.local_to_world_matrix)
    pos = shape.ball_joint.position
    m = numpy.matrix(shape.transformation_matrix).I
    m = numpy.dot(m, shape.transformation)
    cPos2 = numpy.array([pos[0], pos[1], pos[2], 1.0])
    cPos2 = numpy.dot(cPos2, m)
    cPos2 = numpy.dot(cPos2, lwMat)
    return numpy.array([cPos2[0,0], cPos2[0,1], cPos2[0,2]])

ボーンのワールド座標での中心位置を計算

def calcBoneWorldCenter(shape):
    cPos = numpy.array([0.0, 0.0, 0.0, 1.0])
    lwMat = numpy.matrix(shape.local_to_world_matrix)
    m = numpy.matrix(shape.bone_joint.matrix)
    joint_offset = shape.bone_joint.offset
    joint_rotate = quaternion_matrix4x4(shape.bone_joint.rotation)

    cPos2 = numpy.dot(cPos, joint_rotate)
    cPos2 = numpy.dot(cPos2, m)
    cPos2 = numpy.dot(cPos2, translate_matrix4x4(joint_offset))
    cPos2 = numpy.dot(cPos2, lwMat)
    return numpy.array([cPos2[0,0], cPos2[0,1], cPos2[0,2]])

この計算は、以下も同じ結果になります。

def calcBoneWorldCenter(shape):
    cPos = numpy.array([0.0, 0.0, 0.0, 1.0])
    lwMat = numpy.matrix(shape.local_to_world_matrix)
    m = numpy.matrix(shape.transformation)

    cPos2 = numpy.dot(cPos, m)
    cPos2 = numpy.dot(cPos2, lwMat)
    return numpy.array([cPos2[0,0], cPos2[0,1], cPos2[0,2]])

使用方法

上記の関数を使用して、選択形状がボールジョイントまたはボーンの場合、
ワールド座標での中心位置をメッセージウィンドウに表示します。

scene = xshade.scene()

shape = scene.active_shape()
cPos = [0.0, 0.0, 0.0]
if shape.is_ball_joint:     # ボールジョイントの場合.
    cPos = calcBallJointWorldCenter(shape)

if shape.is_bone_joint:     # ボーンの場合.
    cPos = calcBoneWorldCenter(shape)

print "center : " + str(cPos[0]) + ",  " + str(cPos[1]) + ", " + str(cPos[2])