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 calc_global_camera_pos(cf):
camera_pos = cf.position
# カメラの角度
camera_qq = QQuaternion.fromEulerAngles(degrees(cf.euler.x()),
degrees(cf.euler.y()),
degrees(cf.euler.z()))
logger.info("cf.euler: %s", cf.euler)
logger.info("camera_qq: %s", camera_qq.toEulerAngles())
logger.info("cf.position: %s", cf.position)
logger.info("cf.length: %s", cf.length)
mat = QMatrix4x4()
# カメラの回転
mat.rotate(camera_qq)
# 初期位置
mat.translate(cf.position)
camera_pos = mat * QVector3D()
# # とりあえずZ距離はなし
# camera_pos.setZ(0)
# if cf.position.y() < 0 and camera_pos.y() > 0:
# # 元々のY位置がマイナスで、計算後符号が反転した場合、減算
# camera_pos.setY( cf.position.y() - camera_pos.y() )
logger.info("camera_pos cf.position: %s", cf.position)
logger.info("camera_pos mat: %s", camera_pos)
return camera_pos
def Updategui(self, data): # sets and processes gui data
if self.data['Speed']['Abs']!=[]:
self.ui.sabsval.setText(str(self.data['Speed']['Abs'][-1]))
if self.data['Speed']['Vertical'] !=[]:
self.ui.sverval.setText(str(self.data['Speed']['Vertical'][-1]))
if self.data['Speed']['Horizontal'] !=[]:
self.ui.shorval.setText(str(self.data['Speed']['Horizontal'][-1]))
if self.data['Speed']['Angular'] !=[]:
self.ui.sangval.setText(str(self.data['Speed']['Angular'][-1]))
if self.data['Speed']['Top'] !=[]:
self.ui.stopval.setText(str(self.data['Speed']['Top'][-1]))
if self.data['Acceleration']['Abs'] !=[]:
self.ui.aabsval.setText(str(self.data['Acceleration']['Abs'][-1]))
if self.data['Acceleration']['Vertical'] !=[]:
self.ui.averval.setText(str(self.data['Acceleration']['Vertical'][-1]))
if self.data['Acceleration']['Horizontal'] !=[]:
self.ui.ahorval.setText(str(self.data['Acceleration']['Horizontal'][-1]))
if self.data['Acceleration']['Angular'] !=[]:
self.ui.aangval.setText(str(self.data['Acceleration']['Angular'][-1]))
if self.data['Acceleration']['Top'] !=[]:
self.ui.atopval.setText(str(self.data['Acceleration']['Top'][-1]))
if self.data['Position']['Abs'] !=[]:
self.ui.pabsval.setText(str(self.data['Position']['Abs'][-1]))
if self.data['Position']['Altitude'] !=[]:
self.ui.paltval.setText(str(self.data['Position']['Altitude'][-1]))
if self.data['Position']['Horizontal'] !=[]:
self.ui.phorval.setText(str(self.data['Position']['Horizontal'][-1]))
if self.data['Position']['Latitude'] !=[]:
self.ui.latitudeval.setText(str(self.data['Position']['Latitude'][-1]))
if self.data['Position']['Longitude'] !=[]:
self.ui.longval.setText(str(self.data['Position']['Longitude'][-1]))
if self.data['Orientation']['Roll'] !=[]:
self.ui.orolval.setText(str(self.data['Orientation']['Roll'][-1]))
if self.data['Orientation']['Pitch'] !=[]:
self.ui.opitval.setText(str(self.data['Orientation']['Pitch'][-1]))
if self.data['Orientation']['Yaw'] !=[]:
self.ui.oyawval.setText(str(self.data['Orientation']['Yaw'][-1]))
try:
self.plot.pw.setTitle('T +' + str(data[0]))
except:
pass
try:
self.gyro.rocketTransform.setRotation(QQuaternion.fromEulerAngles(data[0], data[1], data[2]))
except:
pass
for key in list(self.listofplots.keys()):
key = key.split('.')
self.listofplots[key[0] + '.' + key[1]].setData(x=self.time['Time'], y=self.data[key[0]][key[1]],
clear=True)
def rotate(self, angles, center):
"""
Rotate by euler angles at center
-----------------------
In: angle -> QVector3D
center -> QVector3D
-----------------------
"""
eulers = QQuaternion.fromEulerAngles(angles)
R, T = QMatrix4x4(), QMatrix4x4()
R.rotate(eulers)
T.translate(center)
self._modelView = T * R * T.inverted(
)[0] * self.modelView * self.CAMERA_ROT_STEP
def smooth_angle_bone(bone_frame_dic, smooth_times, target_bones):
# 関節の角度円滑化
for bone_name in target_bones:
for n in range(smooth_times):
for frame in range(len(bone_frame_dic[bone_name])):
if frame >= 2:
prev2_bf = bone_frame_dic[bone_name][frame - 2]
prev1_bf = bone_frame_dic[bone_name][frame - 1]
now_bf = bone_frame_dic[bone_name][frame]
if prev2_bf != now_bf.rotation:
# 角度が違っていたら、球形補正開始
euler = QQuaternion.slerp(prev2_bf.rotation, now_bf.rotation, 0.5).toEulerAngles()
euler.setX(0 if np.isnan(euler.x()) else euler.x())
euler.setY(0 if np.isnan(euler.x()) else euler.y())
euler.setZ(0 if np.isnan(euler.x()) else euler.z())
prev1_bf.rotation = QQuaternion.fromEulerAngles(euler)
def calc_limited_rotation(rot, minx, maxx, miny, maxy, minz, maxz):
euler = rot.toEulerAngles()
if euler.x() < minx:
euler.setX(minx)
elif euler.x() > maxx:
euler.setX(maxx)
if euler.y() < miny:
euler.setY(miny)
elif euler.y() > maxy:
euler.setY(maxy)
if euler.z() < minz:
euler.setZ(minz)
elif euler.z() > maxz:
euler.setZ(maxz)
return QQuaternion.fromEulerAngles(euler)
def head_pose_estimation(image_path, shape):
image = cv2.imread(image_path)
pos2d_array = []
for k in [30, 8, 45, 36, 54, 48]:
pos2d_array.append((shape.part(k).x, shape.part(k).y))
pos2d = np.array(pos2d_array, dtype="double")
pos3d_ini = np.array([(0.0, 0.0, 0.0), (0.0, -6.6, -1.3),
(-4.5, 3.4, -2.7), (4.5, 3.4, -2.7),
(-3.0, -3.0, -2.5), (3.0, -3.0, -2.5)])
focal_length = max([image.shape[0], image.shape[1]])
print("focal_length: ", focal_length)
camera = np.array([[focal_length, 0, image.shape[1] / 2],
[0, focal_length, image.shape[0] / 2], [0, 0, 1]],
dtype="double")
distortion = np.zeros((4, 1))
retval, rot_vec, trans_vec = cv2.solvePnP(pos3d_ini,
pos2d,
camera,
distortion,
flags=cv2.SOLVEPNP_ITERATIVE)
# for debug
#print("Rot_Vec: \n ", rot_vec)
#print(type(rot_vec))
#print(rot_vec.shape)
#print("Trans_Vec:\n ", trans_vec)
# 顔の回転を求める
rot_mat = cv2.Rodrigues(rot_vec)[0]
proj_mat = np.array([[rot_mat[0][0], rot_mat[0][1], rot_mat[0][2], 0],
[rot_mat[1][0], rot_mat[1][1], rot_mat[1][2], 0],
[rot_mat[2][0], rot_mat[2][1], rot_mat[2][2], 0]],
dtype="double")
eulerAngles = cv2.decomposeProjectionMatrix(proj_mat)[6]
print("eulerAngles: \n", eulerAngles)
#head_rotation = QQuaternion.fromEulerAngles(eulerAngles[0], eulerAngles[1], eulerAngles[2])
head_rotation = QQuaternion.fromEulerAngles(-eulerAngles[0],
eulerAngles[1],
-eulerAngles[2])
print("head_rotation: ", head_rotation)
return head_rotation
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 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
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
def qFromEuler(self, roll, pitch, yaw):
return QQuaternion.fromEulerAngles(math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw))
def positions_to_rotation(pos, frame=0, xangle=0):
decrease_correctqq = QQuaternion.fromEulerAngles(
QVector3D(xangle * 0.5, 0, 0))
increase_correctqq = QQuaternion.fromEulerAngles(
QVector3D(xangle * 1.5, 0, 0))
normal_correctqq = QQuaternion.fromEulerAngles(QVector3D(xangle, 0, 0))
neck_correctqq = QQuaternion.fromEulerAngles(
QVector3D(xangle * (-0.1), 0, 0))
frame_rotation = []
# 上半身
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_rotation = upper_body_orientation * initial.inverted()
if upper_body_rotation.toEulerAngles().y(
) < 30 and upper_body_rotation.toEulerAngles().y() > -30:
upper_correctqq = increase_correctqq
elif upper_body_rotation.toEulerAngles().y(
) < -120 or upper_body_rotation.toEulerAngles().y() > 120:
upper_correctqq = normal_correctqq
else:
upper_correctqq = decrease_correctqq
upper_body_rotation = upper_body_rotation * upper_correctqq
upper_body_euler = list(upper_body_rotation.getEulerAngles())
frame_rotation.append(upper_body_euler)
# 下半身
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_body_rotation = 0.1 * lower_body_rotation
if lower_body_rotation.toEulerAngles().y(
) < 30 and lower_body_rotation.toEulerAngles().y() > -30:
lower_correctqq = QQuaternion.fromEulerAngles(QVector3D(0, 0, 0))
elif lower_body_rotation.toEulerAngles().y(
) < -120 or lower_body_rotation.toEulerAngles().y() > 120:
lower_correctqq = normal_correctqq
else:
lower_correctqq = decrease_correctqq
lower_body_rotation_scaled = lower_body_rotation * lower_correctqq
lower_body_euler_scaled = list(lower_body_rotation.getEulerAngles())
frame_rotation.append(lower_body_euler_scaled)
# 首
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_correctqq * neck_orientation * initial_orientation.inverted(
)
neck_rotation = upper_body_orientation.inverted() * rotation
rotation_euler = list(neck_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 頭
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 = normal_correctqq * orientation * initial_orientation.inverted()
head_rotation = neck_rotation.inverted() * upper_body_rotation.inverted(
) * rotation
rotation_euler = list(head_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 左肩
direction = pos[11] - pos[8]
up = QVector3D.crossProduct((pos[11] - pos[8]), (pos[14] - pos[11]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(2, -0.8, 0),
QVector3D(0.5, -0.5, -1))
rotation = upper_correctqq * orientation * initial_orientation.inverted()
left_shoulder_rotation = upper_body_rotation.inverted(
) * rotation # 後で使うので保存しておく
rotation_euler = list(left_shoulder_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 左腕
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 = upper_correctqq * orientation * initial_orientation.inverted()
left_arm_rotation = left_shoulder_rotation.inverted(
) * upper_body_rotation.inverted() * rotation
rotation_euler = list(left_arm_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 左ひじ
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 = upper_correctqq * orientation * initial_orientation.inverted()
left_elbow_rotation = left_arm_rotation.inverted(
) * left_shoulder_rotation.inverted() * upper_body_rotation.inverted(
) * rotation
rotation_euler = list(left_elbow_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 右肩
direction = pos[14] - pos[8]
up = QVector3D.crossProduct((pos[14] - pos[8]), (pos[11] - pos[14]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(-2, -0.8, 0),
QVector3D(0.5, 0.5, 1))
rotation = upper_correctqq * orientation * initial_orientation.inverted()
right_shoulder_rotation = upper_body_rotation.inverted(
) * rotation # 後で使うので保存しておく
rotation_euler = list(right_shoulder_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 右腕
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 = upper_correctqq * orientation * initial_orientation.inverted()
right_arm_rotation = right_shoulder_rotation.inverted(
) * upper_body_rotation.inverted() * rotation
rotation_euler = list(right_arm_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
# 右ひじ
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 = upper_correctqq * orientation * initial_orientation.inverted()
right_elbow_rotation = right_arm_rotation.inverted(
) * right_shoulder_rotation.inverted() * upper_body_rotation.inverted(
) * rotation
rotation_euler = list(right_elbow_rotation.getEulerAngles())
frame_rotation.append(rotation_euler)
return frame_rotation
def miku_to_ibuki(rotations):
"""
input data format: 10x3 matrix
output data format: 10x3 matrix
"""
ibuki_rotations = []
# deinitial
# 上半身
ibuki_rotations.append(deorder(rotations[0]))
# 下半身
# 省略
# 首
ibuki_rotations.append(deorder(rotations[2]))
# 頭
# 省略
# ibuki_rotations.append(deorder(rotations[3]))
# 左ひじ
left_elbow_rotation = QQuaternion.fromEulerAngles(*rotations[6])
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0),
QVector3D(1, 1.73, 0))
ibuki_initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(1, 0, 0))
left_elbow_rotation = left_elbow_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = left_elbow_rotation.getEulerAngles()
z = rotation_euler[2]
x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() *
left_elbow_rotation).getEulerAngles()[0]
y = (QQuaternion.fromEulerAngles(x, 0, 0).inverted() *
QQuaternion.fromEulerAngles(0, 0, z).inverted() *
left_elbow_rotation).getEulerAngles()[1]
a = -math.cos(math.radians(y)) + math.sin(math.radians(x)) * math.sin(
math.radians(y)) / math.tan(math.radians(z))
b = -math.cos(math.radians(x)) / math.tan(math.radians(z))
c = -math.sin(math.radians(y)) - math.sin(math.radians(x)) * math.cos(
math.radians(y)) / math.tan(math.radians(z))
if c > 0:
pitch = math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
pitch = -math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
if a > 0:
arm_yaw = math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
arm_yaw = -math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
yaw = y
ibuki_rotations.append(deorder([pitch, 0, yaw]))
# 左腕
left_arm_rotation = QQuaternion.fromEulerAngles(*rotations[5])
initial_orientation = QQuaternion.fromDirection(QVector3D(1.73, -1, 0),
QVector3D(1, 1.73, 0))
ibuki_initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(1, 0, 0))
left_arm_rotation = left_arm_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = left_arm_rotation.getEulerAngles()
z = rotation_euler[2]
x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() *
left_arm_rotation).getEulerAngles()[0]
y = (QQuaternion.fromEulerAngles(x, 0, 0).inverted() *
QQuaternion.fromEulerAngles(0, 0, z).inverted() *
left_arm_rotation).getEulerAngles()[1]
a = -math.cos(math.radians(y)) + math.sin(math.radians(x)) * math.sin(
math.radians(y)) / math.tan(math.radians(z))
b = -math.cos(math.radians(x)) / math.tan(math.radians(z))
c = -math.sin(math.radians(y)) - math.sin(math.radians(x)) * math.cos(
math.radians(y)) / math.tan(math.radians(z))
if c > 0:
pitch = math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
pitch = -math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
if a > 0:
roll = math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
roll = -math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
yaw = arm_yaw
ibuki_rotations.append(deorder([0, roll, yaw]))
ibuki_rotations.append(deorder([pitch, 0, 0]))
# 左肩
left_shoulder_rotation = QQuaternion.fromEulerAngles(*rotations[4])
initial_orientation = QQuaternion.fromDirection(QVector3D(2, -0.8, 0),
QVector3D(0.5, -0.5, -1))
ibuki_initial = QQuaternion.fromDirection(QVector3D(1, 0, 0),
QVector3D(0, 0, -1))
left_shoulder_rotation = left_shoulder_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = left_shoulder_rotation.getEulerAngles()
# z = rotation_euler[2]
# x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() * left_shoulder_rotation).getEulerAngles()[0] + x
# y = (QQuaternion.fromEulerAngles(0, 0, z).inverted() * QQuaternion.fromEulerAngles(x, 0, 0).inverted() * left_shoulder_rotation).getEulerAngles()[1]
# a = -math.cos(math.radians(y))+math.sin(math.radians(x))*math.sin(math.radians(y))/math.tan(math.radians(z))
# b = -math.cos(math.radians(x))/math.tan(math.radians(z))
# c = -math.sin(math.radians(y))-math.sin(math.radians(x))*math.cos(math.radians(y))/math.tan(math.radians(z))
# ibuki_rotations.append(deorder([x,y,z]))
# 右ひじ
right_elbow_rotation = QQuaternion.fromEulerAngles(*rotations[9])
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0),
QVector3D(1, -1.73, 0))
ibuki_initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(1, 0, 0))
right_elbow_rotation = right_elbow_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = right_elbow_rotation.getEulerAngles()
z = rotation_euler[2]
x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() *
right_elbow_rotation).getEulerAngles()[0]
y = (QQuaternion.fromEulerAngles(x, 0, 0).inverted() *
QQuaternion.fromEulerAngles(0, 0, z).inverted() *
right_elbow_rotation).getEulerAngles()[1]
a = math.cos(math.radians(y)) + math.sin(math.radians(x)) * math.sin(
math.radians(y)) / math.tan(math.radians(z))
b = math.cos(math.radians(x)) / math.tan(math.radians(z))
c = math.sin(math.radians(y)) - math.sin(math.radians(x)) * math.cos(
math.radians(y)) / math.tan(math.radians(z))
if c > 0:
pitch = math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
pitch = -math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
if a > 0:
arm_yaw = math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
arm_yaw = -math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
yaw = y
ibuki_rotations.append(deorder([pitch, 0, yaw]))
# 右腕
right_arm_rotation = QQuaternion.fromEulerAngles(*rotations[8])
initial_orientation = QQuaternion.fromDirection(QVector3D(-1.73, -1, 0),
QVector3D(1, -1.73, 0))
ibuki_initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(1, 0, 0))
right_arm_rotation = right_arm_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = right_arm_rotation.getEulerAngles()
z = rotation_euler[2]
x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() *
right_arm_rotation).getEulerAngles()[0]
y = (QQuaternion.fromEulerAngles(x, 0, 0).inverted() *
QQuaternion.fromEulerAngles(0, 0, z).inverted() *
right_arm_rotation).getEulerAngles()[1]
a = math.cos(math.radians(y)) + math.sin(math.radians(x)) * math.sin(
math.radians(y)) / math.tan(math.radians(z))
b = math.cos(math.radians(x)) / math.tan(math.radians(z))
c = math.sin(math.radians(y)) - math.sin(math.radians(x)) * math.cos(
math.radians(y)) / math.tan(math.radians(z))
if c > 0:
pitch = math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
pitch = -math.degrees(
math.acos((a**2 + b**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
if a > 0:
roll = math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
else:
roll = -math.degrees(
math.acos((b**2 + c**2)**0.5 / (a**2 + b**2 + c**2)**0.5))
yaw = arm_yaw
ibuki_rotations.append(deorder([0, roll, yaw]))
ibuki_rotations.append(deorder([pitch, 0, 0]))
# 右肩
right_shoulder_rotation = QQuaternion.fromEulerAngles(*rotations[7])
initial_orientation = QQuaternion.fromDirection(QVector3D(-2, -0.8, 0),
QVector3D(0.5, 0.5, 1))
ibuki_initial = QQuaternion.fromDirection(QVector3D(-1, 0, 0),
QVector3D(0, 0, 1))
right_shoulder_rotation = right_shoulder_rotation * initial_orientation * ibuki_initial.inverted(
)
rotation_euler = right_shoulder_rotation.getEulerAngles()
# z = rotation_euler[2]
# x = (QQuaternion.fromEulerAngles(0, 0, z).inverted() * right_shoulder_rotation).getEulerAngles()[0] + x
# y = (QQuaternion.fromEulerAngles(0, 0, z).inverted() * QQuaternion.fromEulerAngles(x, 0, 0).inverted() * right_shoulder_rotation).getEulerAngles()[1]
# ibuki_rotations.append(deorder([x,y,z]))
# with open('/home/fan/test.txt', 'a+') as f:
# f.write(str(ibuki_rotations))
return ibuki_rotations
def smooth_filter(bone_frame_dic, is_groove, smooth_times):
if smooth_times > 0:
# まず球形補間
smooth_move(bone_frame_dic, is_groove, smooth_times)
smooth_angle(bone_frame_dic, smooth_times)
smooth_IK(bone_frame_dic, smooth_times)
# JSONファイルから設定を読み込む
config_qq = json.load(open("filter/config_qq.json", "r"))
# JSONファイルから設定を読み込む
config_move = json.load(open("filter/config_move.json", "r"))
# 移動用フィルタ
pxfilter = OneEuroFilter(**config_move)
pyfilter = OneEuroFilter(**config_move)
pzfilter = OneEuroFilter(**config_move)
# 回転用フィルタ
rxfilter = OneEuroFilter(**config_qq)
ryfilter = OneEuroFilter(**config_qq)
rzfilter = OneEuroFilter(**config_qq)
rwfilter = OneEuroFilter(**config_qq)
for key in bone_frame_dic.keys():
for n, frame in enumerate(bone_frame_dic[key]):
if key == "グルーブ" and is_groove == False:
continue
if "ＩＫ" in key:
# IKの場合、次のフレームと全く同値の場合、フィルタをかけない
if frame.frame < len(bone_frame_dic[key]) - 1 \
and frame.position == bone_frame_dic[key][n+1].position \
and frame.rotation == bone_frame_dic[key][n+1].rotation:
# 位置と回転が同じ場合、同値とみなす
logger.debug("IK同値: %s %s", n, frame.name)
# 処理をスキップして次に行く
pxfilter.skip(frame.position.x(), frame.frame)
pyfilter.skip(frame.position.y(), frame.frame)
pzfilter.skip(frame.position.z(), frame.frame)
rxfilter.skip(frame.rotation.x(), frame.frame)
ryfilter.skip(frame.rotation.y(), frame.frame)
rzfilter.skip(frame.rotation.z(), frame.frame)
rwfilter.skip(frame.rotation.scalar(), frame.frame)
continue
# XYZそれぞれにフィルターをかける
px = pxfilter(frame.position.x(), frame.frame)
py = pyfilter(frame.position.y(), frame.frame)
pz = pzfilter(frame.position.z(), frame.frame)
frame.position = QVector3D(px, py, pz)
rotation = frame.rotation
# 同じ回転を表すクォータニオンが正負2通りあるので、wの符号が正のほうに統一する
# if rotation.scalar() < 0:
# rotation.setX(rotation.x() * -1)
# rotation.setY(rotation.y() * -1)
# rotation.setZ(rotation.z() * -1)
# rotation.setScalar(rotation.scalar() * -1)
if key != "センター" and key != "グルーブ":
# XYZそれぞれにフィルターをかける(オイラー角)
r = rotation.toEulerAngles()
rx = rxfilter(r.x(), frame.frame)
ry = ryfilter(r.y(), frame.frame)
rz = rzfilter(r.z(), frame.frame)
# rw = rwfilter(rotation.scalar(), frame.frame)
# クォータニオンに戻して保持
frame.rotation = QQuaternion.fromEulerAngles(rx, ry, rz)
def euler2qq(state, frame=0):
# 上半身
bf = VmdBoneFrame(frame)
bf.name = b'\x8f\xe3\x94\xbc\x90\x67' # '上半身'
x, y, z = tuple(state[0])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["上半身"].append(bf)
# 下半身
bf = VmdBoneFrame(frame)
bf.name = b'\x89\xba\x94\xbc\x90\x67' # '下半身'
x, y, z = tuple(state[1])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["下半身"].append(bf)
# 首
bf = VmdBoneFrame(frame)
bf.name = b'\x8e\xf1' # '首'
x, y, z = tuple(state[2])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["首"].append(bf)
# 頭
bf = VmdBoneFrame(frame)
bf.name = b'\x93\xaa' # '頭'
x, y, z = tuple(state[3])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["頭"].append(bf)
# 左肩
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x8C\xA8' # '左肩'
x, y, z = tuple(state[4])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["左肩"].append(bf)
# 左腕
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x98\x72' # '左腕'
x, y, z = tuple(state[5])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["左腕"].append(bf)
rotation_euler = list(bf.rotation.getEulerAngles())
# 左ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb6' # '左ひじ'
x, y, z = tuple(state[6])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["左ひじ"].append(bf)
# 右肩
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x8C\xA8' # '右肩'
x, y, z = tuple(state[7])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["右肩"].append(bf)
# 右腕
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x98\x72' # '右腕'
x, y, z = tuple(state[8])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["右腕"].append(bf)
# 右ひじ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb6' # '右ひじ'
x, y, z = tuple(state[9])
bf.rotation = QQuaternion.fromEulerAngles(QVector3D(x, y, z))
bone_frame_dic["右ひじ"].append(bf)
# 左足
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x91\xab' # '左足'
bone_frame_dic["左足"].append(bf)
# 左ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # '左ひざ'
bone_frame_dic["左ひざ"].append(bf)
# 右足
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x91\xab' # '右足'
bone_frame_dic["右足"].append(bf)
# 右ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x89\x45\x82\xd0\x82\xb4' # '右ひざ'
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 main(csv_bone_path, csv_morph_path, csv_camera_path):
try:
bone_frames = []
morph_frames = []
camera_frames = []
if csv_bone_path:
output_vmd_path = re.sub(
r'\.csv$', "_{0:%Y%m%d_%H%M%S}.vmd".format(datetime.now()),
csv_bone_path)
# ボーンCSV読み込み
with open(csv_bone_path, encoding='cp932', mode='r') as f:
reader = csv.reader(f)
next(reader) # ヘッダーを読み飛ばす
for row in reader:
bf = VmdBoneFrame()
# ボーン名
bf.format_name = row[0]
# ボーン名（バイト）
bf.name = bf.format_name.encode('cp932').decode(
'shift_jis').encode('shift_jis')
# フレーム
bf.frame = int(row[1])
# 位置
bf.position = QVector3D(float(row[2]), float(row[3]),
float(row[4]))
# 回転
bf.rotation = QQuaternion.fromEulerAngles(
float(row[5]),
float(row[6]) * -1,
float(row[7]) * -1)
# 補間曲線
bf.complement = [
int(row[8]),
int(row[9]),
int(row[10]),
int(row[11]),
int(row[12]),
int(row[13]),
int(row[14]),
int(row[15]),
int(row[16]),
int(row[17]),
int(row[18]),
int(row[19]),
int(row[20]),
int(row[21]),
int(row[22]),
int(row[23]),
int(row[24]),
int(row[25]),
int(row[26]),
int(row[27]),
int(row[28]),
int(row[29]),
int(row[30]),
int(row[31]),
int(row[32]),
int(row[33]),
int(row[34]),
int(row[35]),
int(row[36]),
int(row[37]),
int(row[38]),
int(row[39]),
int(row[40]),
int(row[41]),
int(row[42]),
int(row[43]),
int(row[44]),
int(row[45]),
int(row[46]),
int(row[47]),
int(row[48]),
int(row[49]),
int(row[50]),
int(row[51]),
int(row[52]),
int(row[53]),
int(row[54]),
int(row[55]),
int(row[56]),
int(row[57]),
int(row[58]),
int(row[59]),
int(row[60]),
int(row[61]),
int(row[62]),
int(row[63]),
int(row[64]),
int(row[65]),
int(row[66]),
int(row[67]),
int(row[68]),
int(row[69]),
int(row[70]),
int(row[71])
]
bone_frames.append(bf)
# logger.debug("bf: %s %s", bf.name, bf)
if csv_morph_path:
output_vmd_path = re.sub(
r'\.csv$', "_{0:%Y%m%d_%H%M%S}.vmd".format(datetime.now()),
csv_morph_path)
# モーフCSV読み込み
with open(csv_morph_path, encoding='cp932', mode='r') as f:
reader = csv.reader(f)
next(reader) # ヘッダーを読み飛ばす
for row in reader:
mf = VmdMorphFrame()
# ボーン名
mf.format_name = row[0]
# ボーン名（バイト）
mf.name = mf.format_name.encode('cp932').decode(
'shift_jis').encode('shift_jis')
# フレーム
mf.frame = int(row[1])
# 位置
mf.ratio = float(row[2])
morph_frames.append(mf)
writer = VmdWriter()
if len(morph_frames) > 0 or len(bone_frames) > 0:
# ボーン・モーフモーション生成
writer.write_vmd_file(output_vmd_path, "CSV Convert Model",
bone_frames, morph_frames, [], [], [], [])
print("ボーン・モーフVMD出力成功: %s" % output_vmd_path)
if csv_camera_path:
# カメラCSV読み込み
with open(csv_camera_path, encoding='cp932', mode='r') as f:
reader = csv.reader(f)
next(reader) # ヘッダーを読み飛ばす
for row in reader:
cf = VmdCameraFrame()
# フレーム
cf.frame = int(row[0])
# 位置
cf.position = QVector3D(float(row[1]), float(row[2]),
float(row[3]))
# 回転（オイラー角）
cf.euler = QVector3D(float(row[4]), float(row[5]),
float(row[6]))
# 距離
cf.length = -(float(row[7]))
# 視野角
cf.angle = int(row[8])
# パース
cf.perspective = int(row[9])
# 補間曲線
cf.complement = [
int(row[10]),
int(row[11]),
int(row[12]),
int(row[13]),
int(row[14]),
int(row[15]),
int(row[16]),
int(row[17]),
int(row[18]),
int(row[19]),
int(row[20]),
int(row[21]),
int(row[22]),
int(row[23]),
int(row[24]),
int(row[25]),
int(row[26]),
int(row[27]),
int(row[28]),
int(row[29]),
int(row[30]),
int(row[31]),
int(row[32]),
int(row[33])
]
camera_frames.append(cf)
# logger.debug("bf: %s %s", bf.name, bf)
if len(camera_frames) > 0:
# カメラモーション生成
output_vmd_path = re.sub(
r'\.csv$', "_{0:%Y%m%d_%H%M%S}.vmd".format(datetime.now()),
csv_camera_path)
writer.write_vmd_file(output_vmd_path, "CSV Convert Model", [], [],
camera_frames, [], [], [])
print("カメラVMD出力成功: %s" % output_vmd_path)
except Exception:
print("■■■■■■■■■■■■■■■■■")
print("■ **ERROR** ")
print("■ CSV解析処理が意図せぬエラーで終了しました。")
print("■■■■■■■■■■■■■■■■■")
print(traceback.format_exc())
def __init__(self, parent=None):
super(AppWindow, self).__init__(parent)
self.time = {'Time': [0]}
self.speed = {'Abs': [0], 'Horizontal': [0], 'Vertical': [0], 'Angular': [0], 'Top': [0]}
self.acceleration = {'Abs': [0], 'Horizontal': [0], 'Vertical': [0], 'Angular': [0], 'Top': [0]}
self.position = {'Abs': [0], 'Horizontal': [0], 'Altitude': [0], 'Latitude': [0], 'Longitude': [0]}
self.health = {'Coms': [0], 'Parachute': [0], 'T1': [0], 'T2': [0], 'Pressure': [0]}
self.orientation = {'Pitch': [0], 'Roll': [0], 'Yaw': [0]}
self.data = {'Speed': self.speed, 'Acceleration': self.acceleration, 'Position': self.position,
'Health': self.health, 'Time': self.time, 'Orientation': self.orientation}
self.stages = {'Ignition': False, 'TF': False, 'FF': False, 'Apogee': False, 'FD': False, 'Parachute': False,
'PD': False, 'Ground': False}
self.receiverdata = {}
self.receiverdata_index = {}
self.numberofplots = 0
self.analysisplotsnames = []
self.plotposition = {'1': [0, 0, False], '2': [1, 0, False], '3': [0, 1, False], '4': [1, 1, False],
'5': [0, 2, False], '6': [1, 2, False]}
for key in list(self.data.keys()):
for subkey in list(self.data[key].keys()):
self.analysisplotsnames.append(str(key) + '.' + str(subkey))
self.serial = None
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.gyro = GyroApp(self.ui.gyrolayout)
self.gyro.rocketTransform.setRotation(QQuaternion.fromEulerAngles(-90, 0, 0))
self.Connect()
self.plot = plot(self.ui.plotlayout)
self.ui.xaxisname.addItems(self.analysisplotsnames)
self.ui.yaxisname.addItems(self.analysisplotsnames)
self.analysisplots = {}
self.listofplots = {}
self.listoflegends = {}
self.ui.sabsch.stateChanged.connect(partial(self.Plotitem, self.ui.sabsch, 'Speed', 'Abs'))
self.ui.sverch.stateChanged.connect(partial(self.Plotitem, self.ui.sverch, 'Speed', 'Vertical'))
self.ui.sangch.stateChanged.connect(partial(self.Plotitem, self.ui.sangch, 'Speed', 'Angular'))
self.ui.shorch.stateChanged.connect(partial(self.Plotitem, self.ui.shorch, 'Speed', 'Horizontal'))
self.ui.baddplot.clicked.connect(self.createplot)
self.ui.bdelplot.clicked.connect(self.deleteplot)
self.ui.bopenlog.clicked.connect(self.reorganizeplots)
self.ui.data1.addItem('None')
self.ui.data2.addItem('None')
self.ui.data3.addItem('None')
self.ui.data4.addItem('None')
self.ui.data5.addItem('None')
self.ui.data6.addItem('None')
self.ui.data7.addItem('None')
self.ui.data8.addItem('None')
self.ui.data9.addItem('None')
self.ui.data10.addItem('None')
self.ui.data11.addItem('None')
self.ui.data12.addItem('None')
for key in list(self.data.keys()):
for subkey in list(self.data[key].keys()):
self.ui.data1.addItem(key+"."+subkey)
self.ui.data2.addItem(key + "." + subkey)
self.ui.data3.addItem(key + "." + subkey)
self.ui.data4.addItem(key + "." + subkey)
self.ui.data5.addItem(key + "." + subkey)
self.ui.data6.addItem(key + "." + subkey)
self.ui.data7.addItem(key + "." + subkey)
self.ui.data8.addItem(key + "." + subkey)
self.ui.data9.addItem(key + "." + subkey)
self.ui.data10.addItem(key + "." + subkey)
self.ui.data11.addItem(key + "." + subkey)
self.ui.data12.addItem(key + "." + subkey)
self.ui.data1.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data1, 0))
self.ui.data2.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data2, 1))
self.ui.data3.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data3, 2))
self.ui.data4.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data4, 3))
self.ui.data5.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data5, 4))
self.ui.data6.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data6, 5))
self.ui.data7.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data7, 6))
self.ui.data8.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data8, 7))
self.ui.data9.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data9, 8))
self.ui.data10.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data10, 9))
self.ui.data11.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data11, 10))
self.ui.data12.currentTextChanged.connect(partial(self.setDataIndex, self.ui.data12, 11))
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)