def lookAt(self, direction, up):
"""Calculate new camera orientation based on camera direction and up vectors"""
z = -direction
x = QVector3D.crossProduct(up, z)
y = QVector3D.crossProduct(z, x)
self._orientation = QMatrix4x4(x[0], x[1], x[2], 0.0, y[0], y[1], y[2],
0.0, z[0], z[1], z[2], 0.0, 0.0, 0.0,
0.0, 1.0)
def rayCastStack(self, i, j, width, height):
direction = self.direction
right = QVector3D.crossProduct(direction, self._up).normalized()
up = QVector3D.crossProduct(direction, right) * -1.0
center = self.position + direction
print(up)
# normalizedI = (i/width)- 0.5
# normalizedJ = (j /height) - 0.5
s = center + i * right + j * up
# print((self.position - center).length())
return Camera.Ray(self.position, (s - self.position))
def updateCameraVectors(self):
front = QVector3D();
front.setX(cos(radians(self.yaw)) * cos(radians(self.pitch)))
front.setY(sin(radians(self.pitch)))
front.setZ(sin(radians(self.yaw)) * cos(radians(self.pitch)))
front.normalize()
self.Front = front
self.Right = QVector3D.crossProduct(self.Front, self.WORLD_UP)
self.Right.normalize()
self.Up = QVector3D.crossProduct(self.Right, self.Front)
self.Up.normalize()
def __updateCameraVectors(self):
self.front = QVector3D(
math.cos(math.radians(self.yaw)) *
math.cos(math.radians(self.pitch)),
math.sin(math.radians(self.pitch)),
math.sin(math.radians(self.yaw)) *
math.cos(math.radians(self.pitch)))
self.front.normalize()
self.right = QVector3D.crossProduct(self.front,
self.worldUp).normalized()
self.up = QVector3D.crossProduct(self.right, self.front).normalized()
def position_to_frame_leg_one_side_calc(frame, pos, lower_correctqq,
lower_body_rotation, points,
slope_motion, direction_name):
# 足
direction = pos[points['Knee']] - pos[points['Hip']]
up = QVector3D.crossProduct((pos[points['Knee']] - pos[points['Hip']]),
(pos[points['Foot']] - pos[points['Knee']]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = lower_correctqq * orientation * initial_orientation.inverted()
# 足の傾きデータ
leg_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
leg_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}足".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
leg_rotation = leg_correctqq * lower_body_rotation.inverted() * rotation
# ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # 'ひざ'
direction = pos[points['Foot']] - pos[points['Knee']]
up = QVector3D.crossProduct((pos[points['Knee']] - pos[points['Hip']]),
(pos[points['Foot']] - pos[points['Knee']]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = lower_correctqq * orientation * initial_orientation.inverted()
# ひざの傾きデータ
knee_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
knee_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}ひざ".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
knee_rotation = knee_correctqq * leg_rotation.inverted(
) * lower_body_rotation.inverted() * rotation
return leg_rotation, knee_rotation
def __init__(self, position=QVector3D(0, 0, 3),
look_at=QVector3D(0, 0, 0),
up=QVector3D(0, 1, 0),
viewing_plane_distance = 1):
super(Camera, self).__init__()
self.position = position
self.look_at = look_at
self.viewing_plane_distance = viewing_plane_distance
#compute uvw
self._dir = (look_at - self.position).normalized()
self._back = (self.position - look_at).normalized()
self._right = QVector3D.crossProduct(up, self._back)
self._up = QVector3D.crossProduct(self._back, self._right)
self._center = self.position + self.direction
def __init__(self, position, heightMap):
self.Up = QVector3D(0., 1., 0.)
self.position = position
self.viewType = 0 #(0 : 1st Person, 1: 3rd Person, 3: Free)
self.Front = QVector3D(0., 0., 1.)
self.Right = QVector3D.crossProduct(self.Front, self.WORLD_UP)
self.heightMap = heightMap
self.position.setY(self.heightMap.getY(self.position.x(),self.position.z()))
self.updateCameraVectors()
def move(self, x, y, width, height):
self.currentPos = self.mapToSphere(x, y, width, height)
self._axis = QVector3D.crossProduct(self.lastPos, self.currentPos)
length = math.sqrt(QVector3D.dotProduct(self.axis, self.axis))
self.angle = QVector3D.dotProduct(self.lastPos, self.currentPos)
self.lastPos = self.currentPos
if length > EPSILON:
return QQuaternion.fromAxisAndAngle(self._axis, self.angle)
return QQuaternion.fromAxisAndAngle(0, 0, 0, 0)
def mouseMoveEvent(self, event):
"""
Called by the Qt libraries whenever the window receives a mouse
move/drag event.
"""
btns = event.buttons()
# pixel coordinates relative to the window
x = event.localPos().x()
y = event.localPos().y()
if btns & Qt.LeftButton:
# Rotation via emulated trackball using quaternions.
# For method employed see:
# https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
# get current vector from sphere/trackball center to surface
mouse_rotation_current_vec = self._find_trackball_vector(x, y)
# get the angle between the vector which was stored at the
# time of mouse click and the current vector
angle_between = PainterWidget.angle_between(
mouse_rotation_current_vec,
self._mouse_rotation_start_vec)
angle_between *= 20 # arbitrary amplification for faster rotation
# get the rotation axis which is perpendicular to both vectors
rotation_axis = QVector3D.crossProduct(
self._mouse_rotation_start_vec,
mouse_rotation_current_vec)
# create a rotated normalized quaternion corresponding to the
# drag distance travelled by the mouse since the click
delta = QQuaternion.fromAxisAndAngle(rotation_axis, angle_between)
delta.normalize()
# rotate self._rotation_quat (used to rotate the View matrix)
self._rotation_quat = delta * self._rotation_quat_start
self._rotation_quat.normalize()
elif btns & Qt.MidButton:
# Translation left/right and up/down depending on camera orientation
diff = QVector3D(x, y, 0) - self._mouse_translation_start_vec
diff_x = diff[0]
diff_y = diff[1]
self._translation_vec = self._translation_vec_start - self.cam_right * diff_x * 2 + self.cam_up * diff_y * 2
elif btns & Qt.RightButton:
# Translation forward/backward depending on camera orientation
diff_y = y - self._mouse_camforward_start
self._translation_vec = self._translation_vec_start - self.cam_look * diff_y * 2
# re-draw at next timer tick
self.dirty = True
def normalsPerTriangle(vertices=None, indices=None):
triangleNormals = []
i = 0
for index in range(0, len(indices) - 1, 1):
if indices[index] != indices[index - 1] and indices[index] != indices[
index + 1] and indices[index - 1] != indices[index + 1]:
a = (vertices[indices[index - 1]] - vertices[indices[index]])
b = (vertices[indices[index + 1]] - vertices[indices[index]])
if index % 2 == 0:
normal = QVector3D.crossProduct(b, a)
else:
normal = QVector3D.crossProduct(a, b)
triangleNormals.append([
i, (indices[index], indices[index - 1], indices[index + 2]),
normal
])
i += 1
return triangleNormals
def rayCast(self, i, j, width, height):
# Eye Coordinate System
origin = self.position
n = (origin - QVector3D(0, 0, 0)).normalized()
u = QVector3D.crossProduct(self._up, n).normalized()
v = QVector3D.crossProduct(n, u).normalized()
# Image Plane setup
planeCenter = origin + n
d = (origin - planeCenter).length()
aspectRatio = width / height
H = math.tan(aspectRatio * self.fov) * 2 * d
W = H * aspectRatio
C = origin - n * d
L = C - u * W / 2 - v * H / 2
pixelWidth = W / width
pixelHeight = H / height
s = L + u * i * pixelWidth + v * j * pixelHeight
print(s)
return Camera.Ray(origin, (s - origin).normalized())
def rotateUpDown(self, angle):
""" Rotates camera up/down """
axis = QVector3D.crossProduct(self.up, self.center - self.eye)
transform = QMatrix4x4()
transform.translate(self.center)
transform.rotate(angle, axis)
transform.translate(-self.center)
self.eye = transform.map(self.eye)
transform = QMatrix4x4()
transform.rotate(angle, axis)
self.up = transform.map(self.up)
def __init__(self,
position=QVector3D(0, 0, 1),
lookAt=QVector3D(0, 0, 0),
up=QVector3D(0, 1, 0),
fov=90):
super(Camera, self).__init__()
self._pos = position
self._lookAt = lookAt
self._dir = (lookAt - self._pos).normalized()
# self._dir = direction.normalized()
self._up = up
self._fov = fov
self._right = QVector3D.crossProduct(self.direction, self.position)
self._center = self.position + self.direction
self._projectionMatrix = QMatrix4x4()
self._modelViewMatrix = QMatrix4x4()
self._normalMatrix = QMatrix4x4()
def move(self, point, quat):
"""Move trackball"""
if not self._pressed:
return
currentTime = QTime.currentTime()
msecs = self._lastTime.msecsTo(currentTime)
if msecs <= 20:
return
if self._mode == self.TrackballMode.Planar:
delta = QLineF(self._lastPos, point)
self._angularVelocity = 180.0 * delta.length() / (math.pi * msecs)
self._axis = QVector3D(-delta.dy(), delta.dx(), 0.0).normalized()
self._axis = quat.rotatedVector(self._axis)
self._rotation = QQuaternion.fromAxisAndAngle(self._axis, 180.0 / math.pi * delta.length()) * self._rotation
elif self._mode == self.TrackballMode.Spherical:
lastPos3D = QVector3D(self._lastPos.x(), self._lastPos.y(), 0.0)
sqrZ = 1.0 - QVector3D.dotProduct(lastPos3D, lastPos3D)
if sqrZ > 0:
lastPos3D.setZ(math.sqrt(sqrZ))
else:
lastPos3D.normalize()
currentPos3D = QVector3D(point.x(), point.y(), 0.0)
sqrZ = 1.0 - QVector3D.dotProduct(currentPos3D, currentPos3D)
if sqrZ > 0:
currentPos3D.setZ(math.sqrt(sqrZ))
else:
currentPos3D.normalize()
self._axis = QVector3D.crossProduct(lastPos3D, currentPos3D)
angle = 180.0 / math.pi * math.asin(math.sqrt(QVector3D.dotProduct(self._axis, self._axis)))
self._angularVelocity = angle / msecs
self._axis.normalize()
self._axis = quat.rotatedVector(self._axis)
self._rotation = QQuaternion.fromAxisAndAngle(self._axis, angle) * self._rotation
self._lastPos = point
self._lastTime = currentTime
def position_to_frame_lower_calc(frame, pos, slope_motion):
direction = pos[0] - pos[7]
up = QVector3D.crossProduct(direction, (pos[4] - pos[1]))
lower_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(0, 0, 1))
lower_body_rotation = lower_body_orientation * initial.inverted()
# 傾き補正
lower_correctqq = QQuaternion()
# 傾きデータがある場合、補正をかける
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(lower_body_rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
lower_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["下半身"][0].rotation.toEulerAngles() *
y_degree).inverted()
lower_body_rotation = lower_correctqq * lower_body_rotation
return lower_body_rotation, lower_correctqq
def pick(self, clicked_x, clicked_y, modifiers = None):
# http://schabby.de/picking-opengl-ray-tracing/
aspect_ratio = self.aspect_ratio()
cam_origin = self._camera.eye
cam_direction = (self._camera.center - cam_origin).normalized()
# The coordinate system we chose has x pointing right, y pointing down, z pointing into the screen
# in screen coordinates, the vertical axis points down, this coincides with our 'y' axis.
v = -self._camera._up # our y axis points down
# in screen coordinates, the horizontal axis points right, this coincides with our x axis
h = QVector3D.crossProduct(cam_direction, self._camera._up).normalized() # cam_direction points into the screen
# in InFobRenderer::render(), we use Viewport::perspective_matrix(), where self._camera.fov is used
# as QMatrix4x4::perspective()'s verticalAngle parameter, so near clipping plane's vertical scale is given by:
v_scale = math.tan( math.radians(self._camera.vfov) / 2 ) * self._camera.near
h_scale = v_scale * aspect_ratio
# translate mouse coordinates so that the origin lies in the center
# of the viewport (screen coordinates origin is top, left)
x = clicked_x - self.width() / 2
y = clicked_y - self.height() / 2
# scale mouse coordinates so that half the view port width and height
# becomes 1 (to be coherent with v_scale, which takes half of fov)
x /= (self.width() / 2)
y /= (self.height() / 2)
# the picking ray origin: corresponds to the intersection of picking ray with
# near plane (we don't want to pick actors behind the near plane)
world_origin = cam_origin + cam_direction * self._camera.near + h * h_scale * x + v * v_scale * y
# the picking ray direction
world_direction = (world_origin - cam_origin).normalized()
if self._debug and modifiers is not None and (modifiers & Qt.ShiftModifier):
np_origin = utils.to_numpy(world_origin)
np_v = utils.to_numpy(v)
np_h = utils.to_numpy(h)
self._debug_actors.clearActors()
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_h, 0.01, QColor('red')))
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_v, 0.01, QColor('green')))
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + utils.to_numpy(world_direction) * 100, 0.01, QColor('magenta')))
min_t = float("inf")
min_result = None
for actor in self.renderer.sorted_actors:
if actor._geometry:
if actor._geometry.indices is not None\
and actor._geometry.attribs.vertices is not None:
bvh = actor._geometry.goc_bvh()
if bvh is None:
continue
bvh.update()
if bvh.bvh is None:
continue
# bring back the actor at the origin
m = actor.transform.worldTransform() if actor.transform else QMatrix4x4()
m_inv = m.inverted()[0]
# bring the ray in the actor's referential
local_origin, local_direction = m_inv.map(world_origin), m_inv.mapVector(world_direction)
local_origin_np, local_direction_np = utils.to_numpy(local_origin), utils.to_numpy(local_direction)
# try to intersect the actor's geometry!
if bvh.primitiveType == BVH.PrimitiveType.TRIANGLES or bvh.primitiveType == BVH.PrimitiveType.LINES:
ids, tuvs = bvh.bvh.intersect_ray(local_origin_np, local_direction_np, True)
if ids.size > 0:
actor_min_t = tuvs[:,0].min()
if actor_min_t < min_t:
min_t = actor_min_t
min_result = (actor, ids, tuvs, world_origin, world_direction, local_origin, local_direction)
elif bvh.primitiveType == BVH.PrimitiveType.POINTS:
object_id, distance, t = bvh.bvh.ray_distance(local_origin_np, local_direction_np)
real_distance = math.sqrt(t**2 + distance**2)
if real_distance < min_t:
min_t = real_distance
min_result = (actor, bvh.indices.ndarray[object_id, None], np.array([[t, distance, real_distance]]), world_origin, world_direction, local_origin, local_direction)
if min_result is not None:
return min_result
raise NothingToPickException()
def right(self):
self._right = QVector3D.crossProduct(self._up, self.direction)
return self._right
def up(self):
self._up = QVector3D.crossProduct(self.direction, self._right)
return self._up.normalized()
def translate(self, _eye, _center, dx, dy):
_left = QVector3D.crossProduct((_center - _eye), self._up).normalized()
d = _left * dx + self._up * dy
d *= (_eye - _center).length()
self.eye = _eye + d
self.center = _center + d
def pan_tilt(self, _eye, _up, dx, dy):
_front = self._center - _eye
_right = QVector3D.crossProduct(_front, _up).normalized()
q = QQuaternion.fromAxisAndAngle(_up, dx) * QQuaternion.fromAxisAndAngle(_right, dy)
self.eye = self._center - q.rotatedVector(_front)
self.up = q.rotatedVector(_up)
def _ortho_look_at_custom(eye_x: float, eye_y: float, eye_z: float,
center_x: float, center_y: float, center_z: float,
up_x: float, up_y: float, up_z: float):
"""
Return the ortho projection matrix
:param eye_x:
:param eye_y:
:param eye_z:
:param center_x:
:param center_y:
:param center_z:
:param up_x:
:param up_y:
:param up_z:
:return: the matrix
"""
eye_base = QVector3D(eye_x, eye_y, eye_z)
up = QVector3D(up_x, up_y, up_z)
center = QVector3D(center_x, center_y, center_y)
eye = eye_base - center
right = QVector3D.crossProduct(up, eye)
up = QVector3D.crossProduct(eye, right)
z_axis = QVector3D(0, 0, 1)
r, theta, phi = cart2spher(eye_x, eye_y, eye_z)
angle = math.pi / 2 - theta
angle2 = phi + math.pi
base_up = QVector3D(*spher2cart(r, angle, angle2))
# print("up", up)
# print("base_up",base_up)
dot_up_base_up = QVector3D.dotProduct(up, base_up)
if math.isclose(dot_up_base_up, 0):
angle_up_base_up = 0
else:
cos_up_base_up = round(dot_up_base_up / base_up.length() / up.length(),
5)
# print("cos_up_base_up",cos_up_base_up)
cross_up_base_up = QVector3D.crossProduct(up, base_up)
if QVector3D.dotProduct(cross_up_base_up, eye) < 0:
angle_up_base_up = math.acos(cos_up_base_up)
else:
angle_up_base_up = math.pi * 2 - math.acos(cos_up_base_up)
cos_x = math.cos(angle_up_base_up)
sin_x = math.sin(angle_up_base_up)
m_x = QMatrix4x4(1, 0, 0, 0, 0, cos_x, -sin_x, 0, 0, sin_x, cos_x, 0, 0, 0,
0, 1)
# print("angle",math.degrees(angle_up_base_up))
# print(m_x)
# 求视线矢量到z轴的投影
eye_project_z_factor = QVector3D.dotProduct(
eye, z_axis) / z_axis.lengthSquared()
eye_project_z = z_axis * eye_project_z_factor
# 视线矢量在xy平面上的投影
eye_project_xy = eye - eye_project_z
# 求视线矢量与xy平面的夹角
cos_eye_and_xy = QVector3D.dotProduct(
eye, eye_project_xy) / eye_project_xy.length() / eye.length()
angle_eye_and_xy = math.acos(cos_eye_and_xy)
if eye.z() < 0:
angle_eye_and_xy = -angle_eye_and_xy
# 求 视线矢量在xy平面上的投影 与 x轴夹角
# x_axis = QVector3D(1, 0, 0)
# cos_project_xy_and_x = QVector3D.dotProduct(x_axis, eye_project_xy) / x_axis.length() / eye_project_xy.length()
# angle_project_xy_and_x = math.acos(cos_project_xy_and_x)
# if eye_project_xy.y() < 0:
# angle_project_xy_and_x = 2 * math.pi - angle_project_xy_and_x
angle_project_xy_and_x = phi
# print("eye", eye)
# print("eye_project_z", eye_project_z)
# print("eye_project_xy", eye_project_xy)
# print(angle_eye_and_xy)
# print(angle_project_xy_and_x)
# 绕y轴逆时针旋转 angle_eye_and_xy 度
cos_1 = math.cos(angle_eye_and_xy)
sin_1 = math.sin(angle_eye_and_xy)
m_y = QMatrix4x4(cos_1, 0, sin_1, 0, 0, 1, 0, 0, sin_1, 0, cos_1, 0, 0, 0,
0, 1)
# 绕z轴顺时针转 angle_project_xy_and_x 度
cos_2 = math.cos(-angle_project_xy_and_x)
sin_2 = math.sin(-angle_project_xy_and_x)
m_z = QMatrix4x4(-cos_2, sin_2, 0, 0, sin_2, cos_2, 0, 0, 0, 0, 1, 0, 0, 0,
0, 1)
m_ortho = QMatrix4x4(0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 1)
return m_ortho * m_x.transposed() * m_y.transposed() * m_z.transposed()
def normal(self):
a = self.vertices[0]
b = self.vertices[1]
c = self.vertices[2]
return QVector3D.crossProduct(c - a, b - a)
def normal(self):
return QVector3D.crossProduct(self.c - self.a,
self.b - self.a).normalized()
def positions_to_frames(pos,
vis,
frame_num=0,
center_enabled=False,
head_rotation=None):
"""convert positions to bone frames"""
frames = []
if len(pos) < 17:
return frames
# センター
if center_enabled:
bf = VmdBoneFrame()
bf.name = b'\x83\x5a\x83\x93\x83\x5e\x81\x5b' # 'センター'
bf.frame = frame_num
bf.position = pos[7]
frames.append(bf)
# 上半身
bf = VmdBoneFrame()
bf.name = b'\x8f\xe3\x94\xbc\x90\x67' # '上半身'
bf.frame = frame_num
direction = pos[8] - pos[7]
up = QVector3D.crossProduct(direction, (pos[14] - pos[11])).normalized()
upper_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0), QVector3D(0, 0, 1))
bf.rotation = upper_body_orientation * initial.inverted()
frames.append(bf)
upper_body_rotation = bf.rotation
# 下半身
bf = VmdBoneFrame()
bf.name = b'\x89\xba\x94\xbc\x90\x67' # '下半身'
bf.frame = frame_num
direction = pos[0] - pos[7]
up = QVector3D.crossProduct(direction, (pos[4] - pos[1]))
lower_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(0, 0, 1))
bf.rotation = lower_body_orientation * initial.inverted()
lower_body_rotation = bf.rotation
frames.append(bf)
# 首は回転させず、頭のみ回転させる
# 頭
bf = VmdBoneFrame()
bf.name = b'\x93\xaa' # '頭'
bf.frame = frame_num
if head_rotation is None:
# direction = pos[10] - pos[9]
direction = pos[10] - pos[8]
up = QVector3D.crossProduct((pos[9] - pos[8]), (pos[10] - pos[9]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(
QVector3D(0, 1, 0), QVector3D(1, 0, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = upper_body_rotation.inverted() * rotation
else:
bf.rotation = upper_body_rotation.inverted() * head_rotation
frames.append(bf)
# 左腕
bf = VmdBoneFrame()
bf.name = b'\x8d\xb6\x98\x72' # '左腕'
bf.frame = frame_num
direction = pos[12] - pos[11]
up = QVector3D.crossProduct((pos[12] - pos[11]), (pos[13] - pos[12]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0),
QVector3D(1, 1.73, 0))
rotation = orientation * initial_orientation.inverted()
# 左腕ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# upper_body_rotation * bf.rotation = rotation なので、
bf.rotation = upper_body_rotation.inverted() * rotation
left_arm_rotation = bf.rotation # 後で使うので保存しておく
if vis[6]: # 左ひじが見えているなら
frames.append(bf)
# 左ひじ
bf = VmdBoneFrame()
bf.name = b'\x8d\xb6\x82\xd0\x82\xb6' # '左ひじ'
bf.frame = frame_num
direction = pos[13] - pos[12]
up = QVector3D.crossProduct((pos[12] - pos[11]), (pos[13] - pos[12]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0),
QVector3D(1, 1.73, 0))
rotation = orientation * initial_orientation.inverted()
# 左ひじポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# upper_body_rotation * left_arm_rotation * bf.rotation = rotation なので、
bf.rotation = left_arm_rotation.inverted() * upper_body_rotation.inverted(
) * rotation
# bf.rotation = (upper_body_rotation * left_arm_rotation).inverted() * rotation # 別の表現
if vis[6] and vis[7]: # 左ひじと左手首が見えているなら
frames.append(bf)
# 右腕
bf = VmdBoneFrame()
bf.name = b'\x89\x45\x98\x72' # '右腕'
bf.frame = frame_num
direction = pos[15] - pos[14]
up = QVector3D.crossProduct((pos[15] - pos[14]), (pos[16] - pos[15]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0),
QVector3D(1, -1.73, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = upper_body_rotation.inverted() * rotation
right_arm_rotation = bf.rotation
if vis[3]: # 右ひじが見えているなら
frames.append(bf)
# 右ひじ
bf = VmdBoneFrame()
bf.name = b'\x89\x45\x82\xd0\x82\xb6' # '右ひじ'
bf.frame = frame_num
direction = pos[16] - pos[15]
up = QVector3D.crossProduct((pos[15] - pos[14]), (pos[16] - pos[15]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0),
QVector3D(1, -1.73, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = right_arm_rotation.inverted() * upper_body_rotation.inverted(
) * rotation
if vis[3] and vis[4]: # 右ひじと右手首が見えているなら
frames.append(bf)
# 左足
bf = VmdBoneFrame()
bf.name = b'\x8d\xb6\x91\xab' # '左足'
bf.frame = frame_num
direction = pos[5] - pos[4]
up = QVector3D.crossProduct((pos[5] - pos[4]), (pos[6] - pos[5]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = lower_body_rotation.inverted() * rotation
left_leg_rotation = bf.rotation
if vis[12]: # 左ひざが見えているなら
frames.append(bf)
# 左ひざ
bf = VmdBoneFrame()
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # '左ひざ'
bf.frame = frame_num
direction = pos[6] - pos[5]
up = QVector3D.crossProduct((pos[5] - pos[4]), (pos[6] - pos[5]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = left_leg_rotation.inverted() * lower_body_rotation.inverted(
) * rotation
if vis[12] and vis[13]: # 左ひざと左足首が見えているなら
frames.append(bf)
# 右足
bf = VmdBoneFrame()
bf.name = b'\x89\x45\x91\xab' # '右足'
bf.frame = frame_num
direction = pos[2] - pos[1]
up = QVector3D.crossProduct((pos[2] - pos[1]), (pos[3] - pos[2]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = lower_body_rotation.inverted() * rotation
right_leg_rotation = bf.rotation
if vis[9]: # 右ひざが見えているなら
frames.append(bf)
# 右ひざ
bf = VmdBoneFrame()
bf.name = b'\x89\x45\x82\xd0\x82\xb4' # '右ひざ'
bf.frame = frame_num
direction = pos[3] - pos[2]
up = QVector3D.crossProduct((pos[2] - pos[1]), (pos[3] - pos[2]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = orientation * initial_orientation.inverted()
bf.rotation = right_leg_rotation.inverted() * lower_body_rotation.inverted(
) * rotation
if vis[9] and vis[10]: # 右ひざと右足首が見えているなら
frames.append(bf)
return frames
def convert_vmd(template_vmd_path, pts3d_path, output_path):
print('Converting VMD file...')
print('Source: %s Pose3D:%s Target: %s' %
(template_vmd_path, pts3d_path, output_path))
r = VMD.VMDReader(template_vmd_path)
r.bone_record = r.bone_record[:1]
joints = [[8, 9], [9, 10], [8, 14], [14, 15], [15, 16], [8, 11], [11, 12],
[12, 13], [8, 7], [7, 0], [4, 5], [5, 6], [0, 4], [0, 1], [1, 2],
[2, 3]]
pts3d = pickle.load(open(pts3d_path, 'rb'))
time = 0
for i in tqdm(range(len(pts3d))):
time += 1
pts = pts3d[i]
# print(pts.shape)
buf = []
for i in range(17):
buf.append(QVector3D(pts[i][0], -pts[i][1], pts[i][2]))
pts = buf
# lowerbody
bonename = b'\x89\xba\x94\xbc\x90\x67'
direction = pts[0] - pts[7]
up = QVector3D.crossProduct(direction, (pts[4] - pts[1])).normalized()
ori_lower = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(0, 0, 1))
rotation_lower = ori_lower * initial.inverted()
frame_lower_body = poseFrame(bonename, rotation_lower)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_lower_body)
# upperbody
bonename = b'\x8f\xe3\x94\xbc\x90\x67'
direction = pts[8] - pts[7]
up = QVector3D.crossProduct(direction,
(pts[14] - pts[11])).normalized()
ori_upper = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0),
QVector3D(0, 0, 1))
rotation_upper = ori_upper * initial.inverted()
frame_upper_body = poseFrame(bonename, rotation_upper)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_upper_body)
# F**k i dont want to use english
# atama
bonename = b'\x93\xaa'
direction = pts[10] - pts[8]
up = QVector3D.crossProduct((pts[9] - pts[8]), (pts[10] - pts[9]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0),
QVector3D(1, 0, 0))
rotation = ori * initial.inverted()
rotation = rotation_upper.inverted() * rotation
frame_head = poseFrame(bonename, rotation)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_head)
# hidari ude
bonename = b'\x8d\xb6\x98\x72'
direction = pts[12] - pts[11]
up = QVector3D.crossProduct((pts[12] - pts[11]), (pts[13] - pts[12]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(1.0, -1, 0),
QVector3D(1, 1.0, 0))
rotation = ori * initial.inverted()
rotation_larm = rotation_upper.inverted() * rotation
frame_hidariude = poseFrame(bonename, rotation_larm)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_hidariude)
# hidari hiji
bonename = b'\x8d\xb6\x82\xd0\x82\xb6'
direction = pts[13] - pts[12]
up = QVector3D.crossProduct((pts[12] - pts[11]), (pts[13] - pts[12]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(1.0, -1, 0),
QVector3D(1, 1.0, 0))
rotation = ori * initial.inverted()
rotation = rotation_larm.inverted() * rotation_upper.inverted(
) * rotation
frame_hidarihiji = poseFrame(bonename, rotation)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_hidarihiji)
# migi ude
bonename = b'\x89\x45\x98\x72'
direction = pts[15] - pts[14]
up = QVector3D.crossProduct((pts[15] - pts[14]), (pts[16] - pts[15]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(-1.0, -1, 0),
QVector3D(1, -1.0, 0))
rotation = ori * initial.inverted()
rotation_rarm = rotation_upper.inverted() * rotation
frame_migiude = poseFrame(bonename, rotation_rarm)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_migiude)
# migi hiji
bonename = b'\x89\x45\x82\xd0\x82\xb6'
direction = pts[16] - pts[15]
up = QVector3D.crossProduct((pts[15] - pts[14]), (pts[16] - pts[15]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(-1.0, -1, 0),
QVector3D(1, -1.0, 0))
rotation = ori * initial.inverted()
rotation = rotation_rarm.inverted() * rotation_upper.inverted(
) * rotation
frame_migihiji = poseFrame(bonename, rotation)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_migihiji)
# hidari ashi
bonename = b'\x8d\xb6\x91\xab'
direction = pts[5] - pts[4]
up = QVector3D.crossProduct((pts[5] - pts[4]), (pts[6] - pts[5]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = ori * initial.inverted()
rotation_lleg = rotation_lower.inverted() * rotation
frame_hidariashi = poseFrame(bonename, rotation_lleg)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_hidariashi)
# hidari hiza
bonename = b'\x8d\xb6\x82\xd0\x82\xb4'
direction = pts[6] - pts[5]
up = QVector3D.crossProduct((pts[5] - pts[4]), (pts[6] - pts[5]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = ori * initial.inverted()
rotation = rotation_lleg.inverted() * rotation_lower.inverted(
) * rotation
frame_hidarihiza = poseFrame(bonename, rotation)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_hidarihiza)
# migi ashi
bonename = b'\x89\x45\x91\xab'
direction = pts[2] - pts[1]
up = QVector3D.crossProduct((pts[2] - pts[1]), (pts[3] - pts[2]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = ori * initial.inverted()
rotation_rleg = rotation_lower.inverted() * rotation
frame_migiashi = poseFrame(bonename, rotation_rleg)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_migiashi)
# migi hiza
bonename = b'\x89\x45\x82\xd0\x82\xb4'
direction = pts[3] - pts[2]
up = QVector3D.crossProduct((pts[2] - pts[1]), (pts[3] - pts[2]))
ori = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = ori * initial.inverted()
rotation = rotation_rleg.inverted() * rotation_lower.inverted(
) * rotation
frame_migihiza = poseFrame(bonename, rotation)
record = addOneMore(r)
record['FrameTime'] = time
assign(record, frame_migihiza)
r.bone_record = r.bone_record[1:]
w = VMD.VMDWriter(r)
w.write(output_path)
def positions_to_frames(pos, frame=0, xangle=0):
logger.debug("角度計算 frame={0}".format(str(frame)))
# 補正角度のクォータニオン
correctqq = QQuaternion.fromEulerAngles(QVector3D(xangle, 0, 0))
"""convert positions to bone frames"""
# 上半身
bf = VmdBoneFrame(frame)
bf.name = b'\x8f\xe3\x94\xbc\x90\x67' # '上半身'
direction = pos[8] - pos[7]
up = QVector3D.crossProduct(direction, (pos[14] - pos[11])).normalized()
upper_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0), QVector3D(0, 0, 1))
# 補正をかけて回転する
bf.rotation = correctqq * upper_body_orientation * initial.inverted()
upper_body_rotation = bf.rotation
bone_frame_dic["上半身"].append(bf)
# 下半身
bf = VmdBoneFrame(frame)
bf.name = b'\x89\xba\x94\xbc\x90\x67' # '下半身'
direction = pos[0] - pos[7]
up = QVector3D.crossProduct(direction, (pos[4] - pos[1]))
lower_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0), QVector3D(0, 0, 1))
bf.rotation = correctqq * lower_body_orientation * initial.inverted()
lower_body_rotation = bf.rotation
bone_frame_dic["下半身"].append(bf)
# 首
bf = VmdBoneFrame(frame)
bf.name = b'\x8e\xf1' # '首'
direction = pos[9] - pos[8]
up = QVector3D.crossProduct((pos[14] - pos[11]), direction).normalized()
neck_orientation = QQuaternion.fromDirection(up, direction)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, 0, -1), QVector3D(0, -1, 0))
rotation = correctqq * neck_orientation * initial_orientation.inverted()
bf.rotation = upper_body_orientation.inverted() * rotation
neck_rotation = bf.rotation
bone_frame_dic["首"].append(bf)
# 頭
bf = VmdBoneFrame(frame)
bf.name = b'\x93\xaa' # '頭'
direction = pos[10] - pos[9]
up = QVector3D.crossProduct((pos[9] - pos[8]), (pos[10] - pos[9]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, 1, 0), QVector3D(1, 0, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = neck_rotation.inverted() * upper_body_rotation.inverted() * rotation
bone_frame_dic["頭"].append(bf)
# FIXME 一旦コメントアウト
# 左肩
# bf = VmdBoneFrame(frame)
# bf.name = b'\x8d\xb6\x8C\xA8' # '左肩'
# direction = pos[11] - pos[8]
# up = QVector3D.crossProduct((pos[11] - pos[8]), (pos[12] - pos[11]))
# orientation = QQuaternion.fromDirection(direction, up)
# # TODO パラメータ要調整
# initial_orientation = QQuaternion.fromDirection(QVector3D(2, -0.5, 0), QVector3D(0, 0.5, -1.5))
# rotation = correctqq * orientation * initial_orientation.inverted()
# # 左肩ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# # upper_body_rotation * bf.rotation = rotation なので、
# left_shoulder_rotation = upper_body_rotation.inverted() * rotation # 後で使うので保存しておく
# bf.rotation = left_shoulder_rotation
# bone_frame_dic["左肩"].append(bf)
# 左腕
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x98\x72' # '左腕'
direction = pos[12] - pos[11]
up = QVector3D.crossProduct((pos[12] - pos[11]), (pos[13] - pos[12]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0), QVector3D(1, 1.73, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
# 左腕ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# left_shoulder_rotation * upper_body_rotation * bf.rotation = rotation なので、
bf.rotation = upper_body_rotation.inverted() * rotation
left_arm_rotation = bf.rotation # 後で使うので保存しておく
bone_frame_dic["左腕"].append(bf)
# 左ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb6' # '左ひじ'
direction = pos[13] - pos[12]
up = QVector3D.crossProduct((pos[12] - pos[11]), (pos[13] - pos[12]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0), QVector3D(1, 1.73, 0))
rotation = orientation * initial_orientation.inverted()
# ひじはX軸補正しない(Y軸にしか曲がらないから)
# 左ひじポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# upper_body_rotation * left_arm_rotation * bf.rotation = rotation なので、
bf.rotation = left_arm_rotation.inverted() * upper_body_rotation.inverted() * rotation
# bf.rotation = (upper_body_rotation * left_arm_rotation).inverted() * rotation # 別の表現
bone_frame_dic["左ひじ"].append(bf)
# FIXME 一旦コメントアウト
# 右肩
# bf = VmdBoneFrame(frame)
# bf.name = b'\x89\x45\x8C\xA8' # '右肩'
# direction = pos[14] - pos[8]
# up = QVector3D.crossProduct((pos[14] - pos[8]), (pos[15] - pos[14]))
# orientation = QQuaternion.fromDirection(direction, up)
# # TODO パラメータ要調整
# initial_orientation = QQuaternion.fromDirection(QVector3D(-2, -0.5, 0), QVector3D(0, 0.5, 1.5))
# rotation = correctqq * orientation * initial_orientation.inverted()
# # 左肩ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# # upper_body_rotation * bf.rotation = rotation なので、
# right_shoulder_rotation = upper_body_rotation.inverted() * rotation # 後で使うので保存しておく
# bf.rotation = right_shoulder_rotation
# bone_frame_dic["右肩"].append(bf)
# 右腕
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x98\x72' # '右腕'
direction = pos[15] - pos[14]
up = QVector3D.crossProduct((pos[15] - pos[14]), (pos[16] - pos[15]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0), QVector3D(1, -1.73, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = upper_body_rotation.inverted() * rotation
right_arm_rotation = bf.rotation
bone_frame_dic["右腕"].append(bf)
# 右ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb6' # '右ひじ'
direction = pos[16] - pos[15]
up = QVector3D.crossProduct((pos[15] - pos[14]), (pos[16] - pos[15]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0), QVector3D(1, -1.73, 0))
# ひじはX軸補正しない(Y軸にしか曲がらないから)
rotation = orientation * initial_orientation.inverted()
bf.rotation = right_arm_rotation.inverted() * upper_body_rotation.inverted() * rotation
bone_frame_dic["右ひじ"].append(bf)
# 左足
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x91\xab' # '左足'
direction = pos[5] - pos[4]
up = QVector3D.crossProduct((pos[5] - pos[4]), (pos[6] - pos[5]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0), QVector3D(-1, 0, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = lower_body_rotation.inverted() * rotation
left_leg_rotation = bf.rotation
bone_frame_dic["左足"].append(bf)
# 左ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # '左ひざ'
direction = pos[6] - pos[5]
up = QVector3D.crossProduct((pos[5] - pos[4]), (pos[6] - pos[5]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0), QVector3D(-1, 0, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = left_leg_rotation.inverted() * lower_body_rotation.inverted() * rotation
bone_frame_dic["左ひざ"].append(bf)
# 右足
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x91\xab' # '右足'
direction = pos[2] - pos[1]
up = QVector3D.crossProduct((pos[2] - pos[1]), (pos[3] - pos[2]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0), QVector3D(-1, 0, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = lower_body_rotation.inverted() * rotation
right_leg_rotation = bf.rotation
bone_frame_dic["右足"].append(bf)
# 右ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb4' # '右ひざ'
direction = pos[3] - pos[2]
up = QVector3D.crossProduct((pos[2] - pos[1]), (pos[3] - pos[2]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0), QVector3D(-1, 0, 0))
rotation = correctqq * orientation * initial_orientation.inverted()
bf.rotation = right_leg_rotation.inverted() * lower_body_rotation.inverted() * rotation
bone_frame_dic["右ひざ"].append(bf)
# センター(箱だけ作る)
bf = VmdBoneFrame(frame)
bf.name = b'\x83\x5A\x83\x93\x83\x5E\x81\x5B' # 'センター'
bone_frame_dic["センター"].append(bf)
# グルーブ(箱だけ作る)
bf = VmdBoneFrame(frame)
bf.name = b'\x83\x4F\x83\x8B\x81\x5B\x83\x75' # 'グルーブ'
bone_frame_dic["グルーブ"].append(bf)
def position_to_frame(bone_frame_dic, pos, pos_gan, smoothed_2d, frame,
is_upper2_body, slope_motion):
logger.debug("角度計算 frame={0}".format(str(frame)))
# 上半身の方向の安定化のため脊椎を20mm後ろへ動かす(LSld, RSld, Hipでできる平面の垂直方向へ動かす)
up = QVector3D.crossProduct((pos[0] - pos[14]),
(pos[14] - pos[11])).normalized()
pos[7] += up * 20
# 体幹の回転
upper_body_rotation1, upper_body_rotation2, upper_correctqq, lower_body_rotation, lower_correctqq, is_gan \
= position_to_frame_trunk(bone_frame_dic, frame, pos, pos_gan, is_upper2_body, slope_motion)
# 上半身
bf = VmdBoneFrame(frame)
bf.name = b'\x8f\xe3\x94\xbc\x90\x67' # '上半身'
bf.rotation = upper_body_rotation1
bone_frame_dic["上半身"].append(bf)
# 上半身2(角度がある場合のみ登録)
if upper_body_rotation2 != QQuaternion():
bf = VmdBoneFrame(frame)
bf.name = b'\x8f\xe3\x94\xbc\x90\x67\x32' # '上半身2'
bf.rotation = upper_body_rotation2
bone_frame_dic["上半身2"].append(bf)
# 下半身
bf = VmdBoneFrame(frame)
bf.name = b'\x89\xba\x94\xbc\x90\x67' # '下半身'
bf.rotation = lower_body_rotation
bone_frame_dic["下半身"].append(bf)
# 頭の方向の安定化のためNeck/NoseとHeadを500mm後ろへ動かす(LSld, RSld, Hipでできる平面の垂直方向へ動かす)
up = QVector3D.crossProduct((pos[0] - pos[14]),
(pos[14] - pos[11])).normalized()
pos[9] += up * 500
pos[10] += up * 500
neck_rotation, head_rotation = \
position_to_frame_head(frame, pos, pos_gan, upper_body_rotation1, upper_body_rotation2, upper_correctqq, is_gan, slope_motion)
# 首
bf = VmdBoneFrame(frame)
bf.name = b'\x8e\xf1' # '首'
bf.rotation = neck_rotation
bone_frame_dic["首"].append(bf)
# 頭
bf = VmdBoneFrame(frame)
bf.name = b'\x93\xaa' # '頭'
bf.rotation = head_rotation
bone_frame_dic["頭"].append(bf)
# 左肩の初期値
gan_left_shoulder_initial_orientation = QQuaternion.fromDirection(
QVector3D(1, 0, 0), QVector3D(0, 1, 0))
left_shoulder_initial_orientation = QQuaternion.fromDirection(
QVector3D(2, -0.8, 0), QVector3D(0.5, -0.5, -1))
left_arm_initial_orientation = QQuaternion.fromDirection(
QVector3D(1.73, -1, 0), QVector3D(1, 1.73, 0))
# 左手系の回転
left_shoulder_rotation, left_arm_rotation, left_elbow_rotation = \
position_to_frame_arm_one_side(frame, pos, pos_gan, upper_correctqq, upper_body_rotation1, upper_body_rotation2, gan_left_shoulder_initial_orientation, left_shoulder_initial_orientation, left_arm_initial_orientation, LEFT_POINT, is_gan, slope_motion, "左")
# 左肩
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x8C\xA8' # '左肩'
bf.rotation = left_shoulder_rotation
bone_frame_dic["左肩"].append(bf)
# 左腕
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x98\x72' # '左腕'
bf.rotation = left_arm_rotation
bone_frame_dic["左腕"].append(bf)
# 左ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb6' # '左ひじ'
bf.rotation = left_elbow_rotation
bone_frame_dic["左ひじ"].append(bf)
# 右肩の初期値
gan_right_shoulder_initial_orientation = QQuaternion.fromDirection(
QVector3D(-1, 0, 0), QVector3D(0, -1, 0))
right_shoulder_initial_orientation = QQuaternion.fromDirection(
QVector3D(-2, -0.8, 0), QVector3D(0.5, 0.5, 1))
right_arm_initial_orientation = QQuaternion.fromDirection(
QVector3D(-1.73, -1, 0), QVector3D(1, -1.73, 0))
# 右手系の回転
right_shoulder_rotation, right_arm_rotation, right_elbow_rotation = \
position_to_frame_arm_one_side(frame, pos, pos_gan, upper_correctqq, upper_body_rotation1, upper_body_rotation2, gan_right_shoulder_initial_orientation, right_shoulder_initial_orientation, right_arm_initial_orientation, RIGHT_POINT, is_gan, slope_motion, "右")
# 右肩
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x8C\xA8' # '右肩'
bf.rotation = right_shoulder_rotation
bone_frame_dic["右肩"].append(bf)
# 右腕
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x98\x72' # '右腕'
bf.rotation = right_arm_rotation
bone_frame_dic["右腕"].append(bf)
# 右ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb6' # '右ひじ'
bf.rotation = right_elbow_rotation
bone_frame_dic["右ひじ"].append(bf)
# 左足と左ひざの回転
left_leg_rotation, left_knee_rotation = \
position_to_frame_leg_one_side(frame, pos, pos_gan, lower_correctqq, lower_body_rotation, LEFT_POINT, ["左足", "左ひざ"], is_gan, slope_motion, "左")
# 膝がまっすぐのときつま先が不自然に回転することがあり、対策のため膝を20mmから100mm前へ移動する
leg_v = left_leg_rotation.toVector4D()
leg_x = leg_v.x()
leg_y = leg_v.y()
leg_z = leg_v.z()
leg_w = leg_v.w()
m20 = 2.0 * leg_x * leg_z + 2.0 * leg_w * leg_y
m22 = 1.0 - 2.0 * leg_x * leg_x - 2.0 * leg_y * leg_y
ty = -math.degrees(math.atan2(m20, m22)) # 左脚の角度y
# RHip, LHip, LFootでできる平面の垂直方向へ移動
up = QVector3D.crossProduct((pos[6] - pos[4]),
(pos[4] - pos[1])).normalized()
# 左足の回転が大きいほど膝の移動量を増やす(20mmから100mm)
pos[5] -= up * (20 + 80 * abs(ty) / 180.0)
# 左足と左ひざの回転の再計算
left_leg_rotation, left_knee_rotation = \
position_to_frame_leg_one_side(frame, pos, pos_gan, lower_correctqq, lower_body_rotation, LEFT_POINT, ["左足", "左ひざ"], is_gan, slope_motion, "左")
# 左足
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x91\xab' # '左足'
bf.rotation = left_leg_rotation
bone_frame_dic["左足"].append(bf)
# 左ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # '左ひざ'
bf.rotation = left_knee_rotation
bone_frame_dic["左ひざ"].append(bf)
# 右足と右ひざの回転
right_leg_rotation, right_knee_rotation = \
position_to_frame_leg_one_side(frame, pos, pos_gan, lower_correctqq, lower_body_rotation, RIGHT_POINT, ["右足", "右ひざ"], is_gan, slope_motion, "右")
# 膝がまっすぐのときつま先が不自然に回転することがあり、対策のため膝を20mmから100mm前へ移動する
leg_v = right_leg_rotation.toVector4D()
leg_x = leg_v.x()
leg_y = leg_v.y()
leg_z = leg_v.z()
leg_w = leg_v.w()
m20 = 2.0 * leg_x * leg_z + 2.0 * leg_w * leg_y
m22 = 1.0 - 2.0 * leg_x * leg_x - 2.0 * leg_y * leg_y
# 右足の角度y
ty = -math.degrees(math.atan2(m20, m22))
# LHip, RHip, RFootでできる平面の垂直方向へ移動
up = QVector3D.crossProduct((pos[3] - pos[1]),
(pos[1] - pos[4])).normalized()
# 右足の回転が大きいほど膝の移動量を増やす(20mmから100mm)
pos[2] += up * (20 + 80 * abs(ty) / 180.0)
# 右足と右ひざの回転の再計算
right_leg_rotation, right_knee_rotation = \
position_to_frame_leg_one_side(frame, pos, pos_gan, lower_correctqq, lower_body_rotation, RIGHT_POINT, ["右足", "右ひざ"], is_gan, slope_motion, "右")
# 右足
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x91\xab' # '右足'
bf.rotation = right_leg_rotation
bone_frame_dic["右足"].append(bf)
# 右ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb4' # '右ひざ'
bf.rotation = right_knee_rotation
bone_frame_dic["右ひざ"].append(bf)
# センター(箱だけ作る)
bf = VmdBoneFrame(frame)
bf.name = b'\x83\x5A\x83\x93\x83\x5E\x81\x5B' # 'センター'
bone_frame_dic["センター"].append(bf)
# グルーブ(箱だけ作る)
bf = VmdBoneFrame(frame)
bf.name = b'\x83\x4F\x83\x8B\x81\x5B\x83\x75' # 'グルーブ'
bone_frame_dic["グルーブ"].append(bf)
# 左足ＩＫ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x91\xab\x82\x68\x82\x6a' # '左足ＩＫ'
bone_frame_dic["左足ＩＫ"].append(bf)
# 右足ＩＫ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x91\xab\x82\x68\x82\x6a' # '右足ＩＫ'
bone_frame_dic["右足ＩＫ"].append(bf)
def position_to_frame_head(frame, pos, pos_gan, upper_body_rotation1,
upper_body_rotation2, upper_correctqq, is_gan,
slope_motion):
if is_gan:
# 体幹が3dpose-ganで決定されている場合
# 首
direction = pos[9] - pos[8]
up = QVector3D.crossProduct((pos[14] - pos[11]),
direction).normalized()
neck_orientation = QQuaternion.fromDirection(up, direction)
initial_orientation = QQuaternion.fromDirection(
QVector3D(0, 0, -1), QVector3D(0, 1, 0))
rotation = neck_orientation * initial_orientation.inverted()
neck_rotation = upper_body_rotation2.inverted(
) * upper_body_rotation1.inverted() * rotation
# 頭
direction = pos[10] - pos[9]
up = QVector3D.crossProduct((pos[14] - pos[11]), (pos[10] - pos[9]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(
QVector3D(0, 1, 0), QVector3D(0, 0, 0))
rotation = upper_correctqq * orientation * initial_orientation.inverted(
)
head_rotation = neck_rotation.inverted(
) * upper_body_rotation2.inverted() * upper_body_rotation1.inverted(
) * rotation
else:
# 体幹が 3d-pose-baseline で決定されている場合
# 首
direction = pos[9] - pos[8]
up = QVector3D.crossProduct((pos[14] - pos[11]),
direction).normalized()
neck_orientation = QQuaternion.fromDirection(up, direction)
initial_orientation = QQuaternion.fromDirection(
QVector3D(0, -1, 0), QVector3D(0, 0, -1))
rotation = neck_orientation * initial_orientation.inverted()
neck_rotation = upper_body_rotation2.inverted(
) * upper_body_rotation1.inverted() * rotation
# 頭
direction = pos[10] - pos[9]
up = QVector3D.crossProduct((pos[9] - pos[8]), (pos[10] - pos[9]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(
QVector3D(0, 1, 0), QVector3D(1, 0, 0))
rotation = upper_correctqq * orientation * initial_orientation.inverted(
)
head_rotation = neck_rotation.inverted(
) * upper_body_rotation2.inverted() * upper_body_rotation1.inverted(
) * rotation
# 首の傾きデータ
neck_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(neck_rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
neck_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["首"][0].rotation.toEulerAngles() *
y_degree).inverted()
neck_rotation = neck_correctqq * neck_rotation
# 頭の傾きデータ
head_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(head_rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
head_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["頭"][0].rotation.toEulerAngles() *
y_degree).inverted()
head_rotation = head_correctqq * head_rotation
return neck_rotation, head_rotation
points_3d.append((0, 0, 0))
points_3d.append((1, 0, 0))
points_3d.append((0, 1, 0))
points_3d.append((1, 1, 0))
points_3d.append((0, 0, 1))
points_3d.append((1, 0, 1))
points_3d.append((0, 1, 1))
points_3d.append((1, 1, 1))
points = []
degree = -1
length = 1
eye = QVector3D(1, 1, 1)
right = QVector3D(-1, 1, 0)
up_base = QVector3D.crossProduct(eye, right)
# print(up_base)
while is_run():
clear()
degree += 1
rotate_m = QMatrix4x4()
rotate_m.rotate(degree, eye)
up = rotate_m.map(up_base)
# up = up_base
# print(degree)
matrix_ortho = ortho_look_at(eye.x(), eye.y(), eye.z(), 0, 0, 0, up.x(),
up.y(), up.z())
def position_to_frame_shoulder_one_side_calc(frame, pos, upper_correctqq,
upper_body_rotation1,
upper_body_rotation2,
shoulder_initial_orientation,
arm_initial_orientation, points,
slope_motion, direction_name):
# 肩
direction = pos[points['Shoulder']] - pos[points['Thorax']]
up = QVector3D.crossProduct(
(pos[points['Shoulder']] - pos[points['Thorax']]),
(pos[points['AnotherShoulder']] - pos[points['Shoulder']]))
orientation = QQuaternion.fromDirection(direction, up)
rotation = upper_correctqq * orientation * shoulder_initial_orientation.inverted(
)
# 肩の傾きデータ
shoulder_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
shoulder_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}肩".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
# 肩ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
shoulder_rotation = shoulder_correctqq * upper_body_rotation2.inverted(
) * upper_body_rotation1.inverted() * rotation # 後で使うので保存しておく
# 腕
direction = pos[points['Elbow']] - pos[points['Shoulder']]
up = QVector3D.crossProduct(
(pos[points['Elbow']] - pos[points['Shoulder']]),
(pos[points['Wrist']] - pos[points['Elbow']]))
orientation = QQuaternion.fromDirection(direction, up)
rotation = upper_correctqq * orientation * arm_initial_orientation.inverted(
)
# 腕の傾きデータ
arm_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
arm_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}腕".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
# 腕ポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
arm_rotation = arm_correctqq * shoulder_rotation.inverted(
) * upper_body_rotation2.inverted() * upper_body_rotation1.inverted(
) * rotation # 後で使うので保存しておく
# ひじ
direction = pos[points['Wrist']] - pos[points['Elbow']]
up = QVector3D.crossProduct(
(pos[points['Elbow']] - pos[points['Shoulder']]),
(pos[points['Wrist']] - pos[points['Elbow']]))
orientation = QQuaternion.fromDirection(direction, up)
rotation = upper_correctqq * orientation * arm_initial_orientation.inverted(
)
# ひじの傾きデータ
elbow_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
elbow_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}ひじ".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
# ひじポーンの回転から親ボーンの回転を差し引いてbf.rotationに格納する。
# upper_body_rotation * left_shoulder_rotation * left_arm_rotation * bf.rotation = rotation なので、
elbow_rotation = elbow_correctqq * arm_rotation.inverted(
) * shoulder_rotation.inverted() * upper_body_rotation2.inverted(
) * upper_body_rotation1.inverted() * rotation
# bf.rotation = (upper_body_rotation * left_arm_rotation).inverted() * rotation # 別の表現
return shoulder_rotation, arm_rotation, elbow_rotation
def position_to_frame_upper_calc(frame, pos, is_upper2_body, slope_motion):
if is_upper2_body == True:
# 上半身2がある場合、分割して登録する
# 上半身
direction = pos[7] - pos[0]
up = QVector3D.crossProduct(direction,
(pos[14] - pos[11])).normalized()
upper_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0),
QVector3D(0, 0, 1))
upper_body_rotation1 = upper_body_orientation * initial.inverted()
# 傾き補正
upper_correctqq = QQuaternion()
# 傾きデータがある場合、補正をかける
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 -
abs(upper_body_rotation1.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
upper_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["上半身"][0].rotation.toEulerAngles() *
y_degree).inverted()
upper_body_rotation1 = upper_correctqq * upper_body_rotation1
# 上半身2
direction = pos[8] - pos[7]
up = QVector3D.crossProduct(direction,
(pos[14] - pos[11])).normalized()
upper_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0),
QVector3D(0, 0, 1))
upper_body_rotation2 = upper_body_orientation * initial.inverted()
# 傾き補正
upper_correctqq = QQuaternion()
# 傾きデータがある場合、補正をかける
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 -
abs(upper_body_rotation1.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
upper_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["上半身2"][0].rotation.toEulerAngles() *
y_degree).inverted()
upper_body_rotation2 = upper_correctqq * upper_body_rotation1.inverted(
) * upper_body_rotation2
else:
# 上半身2は初期クォータニオン
upper_body_rotation2 = QQuaternion()
"""convert positions to bone frames"""
# 上半身
direction = pos[8] - pos[7]
up = QVector3D.crossProduct(direction,
(pos[14] - pos[11])).normalized()
upper_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, 1, 0),
QVector3D(0, 0, 1))
upper_body_rotation1 = upper_body_orientation * initial.inverted()
# 傾き補正
upper_correctqq = QQuaternion()
# 傾きデータがある場合、補正をかける
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 -
abs(upper_body_rotation1.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
upper_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["上半身"][0].rotation.toEulerAngles() *
y_degree).inverted()
upper_body_rotation1 = upper_correctqq * upper_body_rotation1
return upper_body_rotation1, upper_body_rotation2, upper_correctqq
