Exemple #1
0
 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)
Exemple #2
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))
Exemple #3
0
    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()
Exemple #4
0
    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()
Exemple #5
0
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
Exemple #6
0
 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
Exemple #7
0
 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)
Exemple #9
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
Exemple #10
0
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
Exemple #11
0
    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)
Exemple #13
0
 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()
Exemple #14
0
    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
Exemple #15
0
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
Exemple #16
0
    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()
Exemple #17
0
 def right(self):
     self._right = QVector3D.crossProduct(self._up, self.direction)
     return self._right
Exemple #18
0
 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)
Exemple #21
0
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()
Exemple #24
0
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
Exemple #25
0
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)
Exemple #26
0
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)
Exemple #27
0
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)

    # 左足IK
    bf = VmdBoneFrame(frame)
    bf.name = b'\x8d\xb6\x91\xab\x82\x68\x82\x6a'  # '左足IK'
    bone_frame_dic["左足IK"].append(bf)

    # 右足IK
    bf = VmdBoneFrame(frame)
    bf.name = b'\x89\x45\x91\xab\x82\x68\x82\x6a'  # '右足IK'
    bone_frame_dic["右足IK"].append(bf)
Exemple #28
0
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
Exemple #29
0
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())
Exemple #30
0
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
Exemple #31
0
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