def interpolate_vec3(op: MVector3D, ow: MVector3D, on: MVector3D, d: MVector3D,
t: float, f: int):
# 念のためコピー
p = op.copy()
w = ow.copy()
n = on.copy()
# 半径は3点間の距離の最長の半分
r = max(p.distanceToPoint(w), p.distanceToPoint(n),
w.distanceToPoint(n)) / 2
logger.test("op: %s, ow: %s, on: %s, d: %s, t: %s, f: %s, r: %s",
p.to_log(), w.to_log(), n.to_log(), d.to_log(), t, f, r)
if r == 0:
# 半径が取れなかった場合、そもそもまったく移動がないので、線分移動
return (p + n) * t
if p == w or p == n or w == n:
# 半径が0の場合か、どれか同じ値の場合、線対称な値を使用する
n = d
# 3点を通る球体の原点を求める
c, radius = calc_sphere_center(p, w, n, r)
if radius == 0:
# 半径が取れなかった場合、そもそもまったく移動がないので、線分移動
return (p + n) * t
# prev -> now の t分の回転量
pn_qq = MQuaternion.rotationTo((p - c).normalized(), (c - c).normalized())
pw_qq = MQuaternion.rotationTo((p - c).normalized(), (w - c).normalized())
# 球形補間の移動量
t_qq = MQuaternion.slerp(pn_qq, pw_qq, t)
logger.test("(p - c): %s, (c - c): %s, (w - c): %s", (p - c).normalized(),
(c - c).normalized(), (w - c).normalized())
logger.test("pn_qq: %s, pw: %s, t: %s", pn_qq, pw_qq, t_qq)
out = t_qq * (p - c) + c
# 値の変化がない場合、上書き
if p.x() == w.x() == n.x():
out.setX(w.x())
if p.y() == w.y() == n.y():
out.setY(w.y())
if p.z() == w.z() == n.z():
out.setZ(w.z())
out.effective()
logger.test(out.to_log())
return out
def test_MQuaternion_rotationTo(self):
# base_v = MVector3D(2.617903093134025, 17.718517382408315, -0.12363630515345259)
from_v = MVector3D(3.1009119396999303, 18.20660094549826, 0.3268970645363245)
new_to_v = MVector3D(3.8692103699906015, 17.718517382408315, -0.12363630515345259)
old_to_v = MVector3D(3.7909166767007814, 17.28083485225695, 1.101139547122786)
new_from_v = new_to_v - from_v
print(new_from_v)
old_from_v = old_to_v - from_v
print(old_from_v)
qq1 = MQuaternion.rotationTo(new_from_v, old_from_v)
print(qq1.toEulerAngles4MMD())
def separate_local_qq(fno: int, bone_name: str, qq: MQuaternion,
global_x_axis: MVector3D):
# ローカル座標系（ボーンベクトルが（1，0，0）になる空間）の向き
local_axis = MVector3D(1, 0, 0)
# グローバル座標系（Ａスタンス）からローカル座標系（ボーンベクトルが（1，0，0）になる空間）への変換
global2local_qq = MQuaternion.rotationTo(global_x_axis, local_axis)
local2global_qq = MQuaternion.rotationTo(local_axis, global_x_axis)
# X成分を抽出する ------------
mat_x1 = MMatrix4x4()
mat_x1.setToIdentity() # 初期化
mat_x1.rotate(qq) # 入力qq
mat_x1.translate(global_x_axis) # グローバル軸方向に伸ばす
mat_x1_vec = mat_x1 * MVector3D()
# YZの回転量（自身のねじれを無視する）
yz_qq = MQuaternion.rotationTo(global_x_axis, mat_x1_vec)
# 除去されたX成分を求める
mat_x2 = MMatrix4x4()
mat_x2.setToIdentity() # 初期化
mat_x2.rotate(qq) # 元々の回転量
mat_x3 = MMatrix4x4()
mat_x3.setToIdentity() # 初期化
mat_x3.rotate(yz_qq) # YZの回転量
x_qq = (mat_x2 * mat_x3.inverted()).toQuaternion()
# YZ回転からZ成分を抽出する --------------
mat_z1 = MMatrix4x4()
mat_z1.setToIdentity() # 初期化
mat_z1.rotate(yz_qq) # YZの回転量
mat_z1.rotate(global2local_qq) # グローバル軸の回転量からローカルの回転量に変換
mat_z1.translate(local_axis) # ローカル軸方向に伸ばす
mat_z1_vec = mat_z1 * MVector3D()
mat_z1_vec.setZ(0) # Z方向の移動量を潰す
# ローカル軸からZを潰した移動への回転量
local_z_qq = MQuaternion.rotationTo(local_axis, mat_z1_vec)
# ボーンローカル座標系の回転をグローバル座標系の回転に戻す
mat_z2 = MMatrix4x4()
mat_z2.setToIdentity() # 初期化
mat_z2.rotate(local_z_qq) # ローカル軸上のZ回転
mat_z2.rotate(local2global_qq) # ローカル軸上からグローバル軸上に変換
z_qq = mat_z2.toQuaternion()
# YZ回転からY成分だけ取り出す -----------
mat_y1 = MMatrix4x4()
mat_y1.setToIdentity() # 初期化
mat_y1.rotate(yz_qq) # グローバルYZの回転量
mat_y2 = MMatrix4x4()
mat_y2.setToIdentity() # 初期化
mat_y2.rotate(z_qq) # グローバルZの回転量
mat_y2_qq = (mat_y1 * mat_y2.inverted()).toQuaternion()
# X成分の捻れが混入したので、XY回転からYZ回転を取り出すことでXキャンセルをかける。
mat_y3 = MMatrix4x4()
mat_y3.setToIdentity()
mat_y3.rotate(mat_y2_qq)
mat_y3.translate(global_x_axis)
mat_y3_vec = mat_y3 * MVector3D()
y_qq = MQuaternion.rotationTo(global_x_axis, mat_y3_vec)
return x_qq, y_qq, z_qq, yz_qq
def convert_target_ik2fk(self, ik_bone: Bone):
motion = self.options.motion
model = self.options.model
bone_name = ik_bone.name
logger.info("-- 腕ＩＫ変換準備:開始【%s】", bone_name)
# モデルのIKボーンのリンク
target_links = model.create_link_2_top_one(bone_name, is_defined=False)
# モデルの人差し指先ボーンのリンク
finger_bone_name = "{0}人指先実体".format(bone_name[0])
finger2_bone_name = "{0}小指先実体".format(bone_name[0])
wrist_bone_name = "{0}手首".format(bone_name[0])
finger_links = model.create_link_2_top_one(finger_bone_name, is_defined=False)
finger2_links = model.create_link_2_top_one(finger2_bone_name, is_defined=False)
# モデルのIKリンク
if not (ik_bone.ik.target_index in model.bone_indexes and model.bone_indexes[ik_bone.ik.target_index] in model.bones):
raise SizingException("{0} のTargetが有効なINDEXではありません。PMXの構造を確認してください。".format(bone_name))
# IKエフェクタ
effector_bone_name = model.bone_indexes[ik_bone.ik.target_index]
effector_links = model.create_link_2_top_one(effector_bone_name, is_defined=False)
ik_links = BoneLinks()
# 末端にエフェクタ
effector_bone = model.bones[effector_bone_name].copy()
effector_bone.degree_limit = math.degrees(ik_bone.ik.limit_radian)
ik_links.append(effector_bone)
for ik_link in ik_bone.ik.link:
# IKリンクを末端から順に追加
if not (ik_link.bone_index in model.bone_indexes and model.bone_indexes[ik_link.bone_index] in model.bones):
raise SizingException("{0} のLinkに無効なINDEXが含まれています。PMXの構造を確認してください。".format(bone_name))
link_bone = model.bones[model.bone_indexes[ik_link.bone_index]].copy()
if link_bone.fixed_axis != MVector3D():
# 捩り系は無視
continue
# 単位角
link_bone.degree_limit = math.degrees(ik_bone.ik.limit_radian)
# # 角度制限
# if ik_link.limit_angle == 1:
# link_bone.limit_min = ik_link.limit_min
# link_bone.limit_max = ik_link.limit_max
ik_links.append(link_bone)
# 回転移管先ボーン
transferee_bone = self.get_transferee_bone(ik_bone, effector_bone)
# 移管先ボーンのローカル軸
transferee_local_x_axis = model.get_local_x_axis(transferee_bone.name)
# 差異の大きい箇所にFKキーフレ追加
fnos = motion.get_differ_fnos(0, [bone_name], limit_degrees=20, limit_length=0.5)
prev_sep_fno = 0
for fno in fnos:
for link_name in list(ik_links.all().keys())[1:]:
bf = motion.calc_bf(link_name, fno)
motion.regist_bf(bf, link_name, fno)
if fno // 500 > prev_sep_fno and fnos[-1] > 0:
logger.count(f"【キーフレ追加 - {bone_name}】", fno, fnos)
prev_sep_fno = fno // 500
logger.info("-- 腕ＩＫ変換準備:終了【%s】", bone_name)
org_motion = motion.copy()
# 元モーションを保持したら、IKキーフレ削除
del motion.bones[bone_name]
for fno in fnos:
# グローバル位置計算(元モーションの位置)
target_ik_global_3ds = MServiceUtils.calc_global_pos(model, target_links, org_motion, fno)
finger_global_3ds = MServiceUtils.calc_global_pos(model, finger_links, org_motion, fno)
finger2_global_3ds = MServiceUtils.calc_global_pos(model, finger2_links, org_motion, fno)
target_effector_pos = target_ik_global_3ds[bone_name]
prev_diff = MVector3D()
org_bfs = {}
for link_name in list(ik_links.all().keys())[1:]:
# 元モーションの角度で保持
bf = org_motion.calc_bf(link_name, fno).copy()
org_bfs[link_name] = bf
# 今のモーションも前のキーフレをクリアして再セット
motion.regist_bf(bf, link_name, fno)
# IK計算実行
for ik_cnt in range(50):
MServiceUtils.calc_IK(model, effector_links, motion, fno, target_effector_pos, ik_links, max_count=1)
# どちらにせよ一旦bf確定
for link_name in list(ik_links.all().keys())[1:]:
ik_bf = motion.calc_bf(link_name, fno)
motion.regist_bf(ik_bf, link_name, fno)
# 現在のエフェクタ位置
now_global_3ds = MServiceUtils.calc_global_pos(model, effector_links, motion, fno)
now_effector_pos = now_global_3ds[effector_bone_name]
# 現在のエフェクタ位置との差分(エフェクタ位置が指定されている場合のみ)
diff_pos = MVector3D() if target_effector_pos == MVector3D() else target_effector_pos - now_effector_pos
if prev_diff == MVector3D() or (prev_diff != MVector3D() and diff_pos.length() < prev_diff.length()):
if diff_pos.length() < 0.1:
logger.debug("☆腕ＩＫ変換成功(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \
target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length())
# org_bfを保持し直し
for link_name in list(ik_links.all().keys())[1:]:
bf = motion.calc_bf(link_name, fno).copy()
org_bfs[link_name] = bf
logger.test("org_bf保持: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log())
# そのまま終了
break
elif prev_diff == MVector3D() or diff_pos.length() < prev_diff.length():
logger.debug("☆腕ＩＫ変換ちょっと失敗採用(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \
target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length())
# org_bfを保持し直し
for link_name in list(ik_links.all().keys())[1:]:
bf = motion.calc_bf(link_name, fno).copy()
org_bfs[link_name] = bf
logger.test("org_bf保持: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log())
# 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了
if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0:
logger.debug("動きがないので終了")
break
# 前回差異を保持
prev_diff = diff_pos
else:
logger.debug("★腕ＩＫ変換ちょっと失敗不採用(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \
target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length())
# 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了
if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0:
break
else:
logger.debug("★腕ＩＫ変換失敗(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \
target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length())
# 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了
if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0:
logger.debug("動きがないので終了")
break
# 最後に成功したところに戻す
for link_name in list(ik_links.all().keys())[1:]:
bf = org_bfs[link_name].copy()
logger.debug("確定bf: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log())
motion.regist_bf(bf, link_name, fno)
# 指先の現在の位置を再計算
if finger_links and finger_links.get(finger_bone_name) and finger_links.get(wrist_bone_name) and \
finger2_links and finger2_links.get(finger2_bone_name) and finger2_links.get(wrist_bone_name) and transferee_bone.name == wrist_bone_name:
# 手首がある場合のみ、再計算
now_finger_global_3ds, now_finger_matrixs = MServiceUtils.calc_global_pos(model, finger_links, motion, fno, return_matrix=True)
# 人指先のローカル位置
finger_initial_local_pos = now_finger_matrixs[wrist_bone_name].inverted() * now_finger_global_3ds[finger_bone_name]
finger_local_pos = now_finger_matrixs[wrist_bone_name].inverted() * finger_global_3ds[finger_bone_name]
finger_rotation = MQuaternion.rotationTo(finger_initial_local_pos, finger_local_pos)
logger.debug("finger_rotation: %s [%s]", finger_bone_name, finger_rotation.toEulerAngles().to_log())
finger_x_qq, finger_y_qq, finger_z_qq, _ = MServiceUtils.separate_local_qq(fno, bone_name, finger_rotation, transferee_local_x_axis)
logger.debug("finger_x_qq: %s [%s]", finger_bone_name, finger_x_qq.toEulerAngles().to_log())
logger.debug("finger_y_qq: %s [%s]", finger_bone_name, finger_y_qq.toEulerAngles().to_log())
logger.debug("finger_z_qq: %s [%s]", finger_bone_name, finger_z_qq.toEulerAngles().to_log())
# 手首の回転は、手首から見た指先の方向
transferee_bf = motion.calc_bf(transferee_bone.name, fno)
transferee_bf.rotation = finger_rotation
# 捩りなしの状態で一旦登録する
motion.regist_bf(transferee_bf, transferee_bone.name, fno)
now_finger2_global_3ds, now_finger2_matrixs = MServiceUtils.calc_global_pos(model, finger2_links, motion, fno, return_matrix=True)
# 小指先のローカル位置
finger2_initial_local_pos = now_finger2_matrixs[wrist_bone_name].inverted() * now_finger2_global_3ds[finger2_bone_name]
finger2_local_pos = now_finger2_matrixs[wrist_bone_name].inverted() * finger2_global_3ds[finger2_bone_name]
finger2_rotation = MQuaternion.rotationTo(finger2_initial_local_pos, finger2_local_pos)
logger.debug("finger2_rotation: %s [%s]", finger2_bone_name, finger2_rotation.toEulerAngles().to_log())
finger2_x_qq = MQuaternion.fromAxisAndAngle(transferee_local_x_axis, finger_x_qq.toDegree() + finger2_rotation.toDegree())
transferee_bf.rotation = finger_y_qq * finger2_x_qq * finger_z_qq
motion.regist_bf(transferee_bf, transferee_bone.name, fno)
logger.debug("transferee_qq: %s [%s], x [%s]", transferee_bone.name, transferee_bf.rotation.toEulerAngles().to_log(), finger2_x_qq.toEulerAngles().to_log())
logger.count("【腕ＩＫ変換 - {0}】".format(bone_name), fno, fnos)
def convert_leg_fk2ik(self, direction: str):
logger.info("足ＩＫ変換 【%s足ＩＫ】",
direction,
decoration=MLogger.DECORATION_LINE)
motion = self.options.motion
model = self.options.model
leg_ik_bone_name = "{0}足ＩＫ".format(direction)
toe_ik_bone_name = "{0}つま先ＩＫ".format(direction)
leg_bone_name = "{0}足".format(direction)
knee_bone_name = "{0}ひざ".format(direction)
ankle_bone_name = "{0}足首".format(direction)
# 足FK末端までのリンク
fk_links = model.create_link_2_top_one(ankle_bone_name,
is_defined=False)
# 足IK末端までのリンク
ik_links = model.create_link_2_top_one(leg_ik_bone_name,
is_defined=False)
# つま先IK末端までのリンク
toe_ik_links = model.create_link_2_top_one(toe_ik_bone_name,
is_defined=False)
# つま先（足首の子ボーン）の名前
ankle_child_bone_name = model.bone_indexes[
model.bones[toe_ik_bone_name].ik.target_index]
# つま先末端までのリンク
toe_fk_links = model.create_link_2_top_one(ankle_child_bone_name,
is_defined=False)
fnos = motion.get_bone_fnos(leg_bone_name, knee_bone_name,
ankle_bone_name)
# まずキー登録
prev_sep_fno = 0
fno = 0
for fno in fnos:
bf = motion.calc_bf(leg_ik_bone_name, fno)
motion.regist_bf(bf, leg_ik_bone_name, fno)
if fno // 1000 > prev_sep_fno and fnos[-1] > 0:
logger.count(f"【準備 - {leg_ik_bone_name}】", fno, fnos)
prev_sep_fno = fno // 1000
logger.info("準備完了 【%s足ＩＫ】",
direction,
decoration=MLogger.DECORATION_LINE)
ik_parent_name = ik_links.get(leg_ik_bone_name, offset=-1).name
# 足IKの移植
prev_sep_fno = 0
# 移植
fno = 0
for fno in fnos:
leg_fk_3ds_dic = MServiceUtils.calc_global_pos(
model, fk_links, motion, fno)
_, leg_ik_matrixs = MServiceUtils.calc_global_pos(
model, ik_links, motion, fno, return_matrix=True)
# 足首の角度がある状態での、つま先までのグローバル位置
leg_toe_fk_3ds_dic = MServiceUtils.calc_global_pos(
model, toe_fk_links, motion, fno)
# IKの親から見た相対位置
leg_ik_parent_matrix = leg_ik_matrixs[ik_parent_name]
bf = motion.calc_bf(leg_ik_bone_name, fno)
# 足ＩＫの位置は、足ＩＫの親から見た足首のローカル位置（足首位置マイナス）
bf.position = leg_ik_parent_matrix.inverted() * (
leg_fk_3ds_dic[ankle_bone_name] -
(model.bones[ankle_bone_name].position -
model.bones[ik_parent_name].position))
bf.rotation = MQuaternion()
# 一旦足ＩＫの位置が決まった時点で登録
motion.regist_bf(bf, leg_ik_bone_name, fno)
# 足ＩＫ回転なし状態でのつま先までのグローバル位置
leg_ik_3ds_dic, leg_ik_matrisxs = MServiceUtils.calc_global_pos(
model, toe_ik_links, motion, fno, return_matrix=True)
[
logger.debug("f: %s, leg_ik_3ds_dic[%s]: %s", fno, k,
v.to_log()) for k, v in leg_ik_3ds_dic.items()
]
# つま先のローカル位置
ankle_child_initial_local_pos = leg_ik_matrisxs[
leg_ik_bone_name].inverted() * leg_ik_3ds_dic[toe_ik_bone_name]
ankle_child_local_pos = leg_ik_matrisxs[leg_ik_bone_name].inverted(
) * leg_toe_fk_3ds_dic[ankle_child_bone_name]
logger.debug("f: %s, ankle_child_initial_local_pos: %s", fno,
ankle_child_initial_local_pos.to_log())
logger.debug("f: %s, ankle_child_local_pos: %s", fno,
ankle_child_local_pos.to_log())
# 足ＩＫの回転は、足首から見たつま先の方向
bf.rotation = MQuaternion.rotationTo(ankle_child_initial_local_pos,
ankle_child_local_pos)
logger.debug("f: %s, ik_rotation: %s", fno,
bf.rotation.toEulerAngles4MMD().to_log())
motion.regist_bf(bf, leg_ik_bone_name, fno)
if fno // 500 > prev_sep_fno and fnos[-1] > 0:
logger.count(f"【足ＩＫ変換 - {leg_ik_bone_name}】", fno, fnos)
prev_sep_fno = fno // 500
logger.info("変換完了 【%s足ＩＫ】",
direction,
decoration=MLogger.DECORATION_LINE)
# IKon
for showik in self.options.motion.showiks:
for ikf in showik.ik:
if ikf.name == leg_ik_bone_name or ikf.name == toe_ik_bone_name:
ikf.onoff = 1
# 不要キー削除処理
if self.options.remove_unnecessary_flg:
self.options.motion.remove_unnecessary_bf(0, leg_ik_bone_name, self.options.model.bones[leg_ik_bone_name].getRotatable(), \
self.options.model.bones[leg_ik_bone_name].getTranslatable())
return True