def interpolate_rot_circle(self, prev_bf: VmdBoneFrame, now_bf: VmdBoneFrame, next_bf: VmdBoneFrame, target_bf: VmdBoneFrame, local_axis: MVector3D): if local_axis != MVector3D(): # 軸がある場合、その方向に回す p_qq = MQuaternion.fromAxisAndAngle(local_axis, prev_bf.rotation.toDegree()) w_qq = MQuaternion.fromAxisAndAngle(local_axis, now_bf.rotation.toDegree()) n_qq = MQuaternion.fromAxisAndAngle(local_axis, next_bf.rotation.toDegree()) p = p_qq.toEulerAngles() w = w_qq.toEulerAngles() n = n_qq.toEulerAngles() else: p_qq = prev_bf.rotation.copy() w_qq = now_bf.rotation.copy() n_qq = next_bf.rotation.copy() # 軸がない場合、そのまま回転 p = p_qq.toEulerAngles() w = w_qq.toEulerAngles() n = n_qq.toEulerAngles() if target_bf.fno < now_bf.fno: # 変化量 t = (target_bf.fno - prev_bf.fno) / (now_bf.fno - prev_bf.fno) # デフォルト値 d_qq = MQuaternion.slerp(p_qq, w_qq, t) d = d_qq.toEulerAngles() out = interpolate_vec3(p, w, n, d, t, target_bf.fno) else: # 変化量 t = (target_bf.fno - now_bf.fno) / (next_bf.fno - now_bf.fno) # デフォルト値 d_qq = MQuaternion.slerp(w_qq, n_qq, t) d = d_qq.toEulerAngles() out = interpolate_vec3(w, n, p, d, t, target_bf.fno) out_qq = MQuaternion.fromEulerAngles(out.x(), out.y(), out.z()) if local_axis != MVector3D(): # 回転を元に戻す if target_bf.fno < now_bf.fno: d2_qq = MQuaternion.slerp(prev_bf.rotation, now_bf.rotation, t) else: d2_qq = MQuaternion.slerp(now_bf.rotation, next_bf.rotation, t) result_qq = (d_qq.inverted() * out_qq * d2_qq) else: result_qq = out_qq target_bf.rotation = result_qq
def test_separate(self): MLogger.initialize(level=MLogger.TEST, is_file=True) logger = MLogger(__name__, level=MLogger.TEST) # motion = VmdReader("D:\\MMD\\MikuMikuDance_v926x64\\UserFile\\Motion\\ダンス_1人\\桃源恋歌配布用motion moka\\ノーマルTda式用0-2000.vmd").read_data() model = PmxReader( "D:\\MMD\\MikuMikuDance_v926x64\\UserFile\\Model\\VOCALOID\\初音ミク\\Tda式初音ミク・アペンドVer1.10\\Tda式初音ミク・アペンド_Ver1.10.pmx", is_check=False).read_data() bone_axis_dict = {} for bone_name in ["左ひじ", "右ひじ"]: local_x_axis = model.get_local_x_axis("左ひじ") local_z_axis = MVector3D(0, 0, -1) local_y_axis = MVector3D.crossProduct(local_x_axis, local_z_axis).normalized() bone_axis_dict[bone_name] = { "x": local_x_axis, "y": local_y_axis, "z": local_z_axis } new_ik_qq = MQuaternion.fromEulerAngles(24.58152072747821, 135.9182003500461, 56.36785502950723) ik_bone = model.bones["左ひじ"] fno = 394 x_qq, y_qq, z_qq, yz_qq = MServiceUtils.separate_local_qq( fno, ik_bone.name, new_ik_qq, bone_axis_dict[ik_bone.name]["x"]) logger.debug( f"now: {new_ik_qq.toEulerAngles()} -> {(y_qq * x_qq * z_qq).toEulerAngles()}" ) logger.debug( f"now: x: {x_qq.toDegree()}, y: {y_qq.toDegree()}, z: {z_qq.toDegree()}" ) for (x_sign, y_sign, z_sign) in list(itertools.product((1, -1), (1, -1), (1, -1))): new_x_qq = MQuaternion.fromAxisAndAngle(x_qq.vector(), x_qq.toDegree() * x_sign) new_y_qq = MQuaternion.fromAxisAndAngle(y_qq.vector(), y_qq.toDegree() * y_sign) new_z_qq = MQuaternion.fromAxisAndAngle(z_qq.vector(), z_qq.toDegree() * z_sign) logger.debug( f"x: {x_sign}, y: {y_sign}, z: {z_sign} -> {(new_y_qq * new_x_qq * new_z_qq).toEulerAngles()}" ) self.assertTrue(True)
def deform_rotation(model: PmxModel, motion: VmdMotion, bf: VmdBoneFrame): if bf.name not in model.bones: return MQuaternion() bone = model.bones[bf.name] rot = bf.rotation.normalized().copy() if bone.fixed_axis != MVector3D(): # 回転角度を求める if rot != MQuaternion(): # 回転補正 if "右" in bone.name and rot.x() > 0 and bone.fixed_axis.x() <= 0: rot.setX(rot.x() * -1) rot.setScalar(rot.scalar() * -1) elif "左" in bone.name and rot.x() < 0 and bone.fixed_axis.x() >= 0: rot.setX(rot.x() * -1) rot.setScalar(rot.scalar() * -1) # 回転補正(コロン式ミクさん等軸反転パターン) elif "右" in bone.name and rot.x() < 0 and bone.fixed_axis.x() > 0: rot.setX(rot.x() * -1) rot.setScalar(rot.scalar() * -1) elif "左" in bone.name and rot.x() > 0 and bone.fixed_axis.x() < 0: rot.setX(rot.x() * -1) rot.setScalar(rot.scalar() * -1) rot.normalize() # 軸固定の場合、回転を制限する rot = MQuaternion.fromAxisAndAngle(bone.fixed_axis, rot.toDegree()) if bone.getExternalRotationFlag( ) and bone.effect_index in model.bone_indexes: effect_parent_bone = bone effect_bone = model.bones[model.bone_indexes[bone.effect_index]] cnt = 0 while cnt < 100: # 付与親が取得できたら、該当する付与親の回転を取得する effect_bf = motion.calc_bf(effect_bone.name, bf.fno) # 自身の回転量に付与親の回転量を付与率を加味して付与する if effect_parent_bone.effect_factor < 0: # マイナス付与の場合、逆回転 rot = rot * (effect_bf.rotation * abs(effect_parent_bone.effect_factor)).inverted() else: rot = rot * (effect_bf.rotation * effect_parent_bone.effect_factor) if effect_bone.getExternalRotationFlag( ) and effect_bone.effect_index in model.bone_indexes: # 付与親の親として現在のeffectboneを保持 effect_parent_bone = effect_bone # 付与親置き換え effect_bone = model.bones[model.bone_indexes[ effect_bone.effect_index]] else: break cnt += 1 return rot
def calc_IK(model: PmxModel, links: BoneLinks, motion: VmdMotion, fno: int, target_pos: MVector3D, ik_links: BoneLinks, max_count=10): for bone_name in list(ik_links.all().keys())[1:]: # bfをモーションに登録 bf = motion.calc_bf(bone_name, fno) motion.regist_bf(bf, bone_name, fno) local_effector_pos = MVector3D() local_target_pos = MVector3D() for cnt in range(max_count): # 規定回数ループ for ik_idx, joint_name in enumerate(list(ik_links.all().keys())[1:]): # 処理対象IKボーン ik_bone = ik_links.get(joint_name) # 現在のボーングローバル位置と行列を取得 global_3ds_dic, total_mats = calc_global_pos(model, links, motion, fno, return_matrix=True) # エフェクタ(末端) global_effector_pos = global_3ds_dic[ik_links.first_name()] # 注目ノード(実際に動かすボーン) joint_mat = total_mats[joint_name] # ワールド座標系から注目ノードの局所座標系への変換 inv_coord = joint_mat.inverted() # 注目ノードを起点とした、エフェクタのローカル位置 local_effector_pos = inv_coord * global_effector_pos local_target_pos = inv_coord * target_pos # (1) 基準関節→エフェクタ位置への方向ベクトル basis2_effector = local_effector_pos.normalized() # (2) 基準関節→目標位置への方向ベクトル basis2_target = local_target_pos.normalized() # ベクトル (1) を (2) に一致させるための最短回転量(Axis-Angle) # 回転角 rotation_dot = MVector3D.dotProduct(basis2_effector, basis2_target) # 回転角度 rotation_radian = math.acos(max(-1, min(1, rotation_dot))) if abs(rotation_radian) > 0.0001: # 一定角度以上の場合 # 回転軸 rotation_axis = MVector3D.crossProduct( basis2_effector, basis2_target).normalized() # 回転角度 rotation_degree = math.degrees(rotation_radian) # 関節回転量の補正(最大変位量を制限する) correct_qq = MQuaternion.fromAxisAndAngle( rotation_axis, min(rotation_degree, ik_bone.degree_limit)) # ジョイントに補正をかける bf = motion.calc_bf(joint_name, fno) new_ik_qq = correct_qq * bf.rotation # IK軸制限がある場合、上限下限をチェック if ik_bone.ik_limit_min != MVector3D( ) and ik_bone.ik_limit_max != MVector3D(): x_qq, y_qq, z_qq, yz_qq = separate_local_qq( fno, bone_name, new_ik_qq, model.get_local_x_axis(ik_bone.name)) logger.test("new_ik_qq: %s, x_qq: %s, y_qq: %s, z_qq: %s", new_ik_qq.toEulerAngles(), x_qq.toEulerAngles(), y_qq.toEulerAngles(), z_qq.toEulerAngles()) logger.test("new_ik_qq: %s, x_qq: %s, y_qq: %s, z_qq: %s", new_ik_qq.toDegree(), x_qq.toDegree(), y_qq.toDegree(), z_qq.toDegree()) euler_x = min( ik_bone.ik_limit_max.x(), max(ik_bone.ik_limit_min.x(), x_qq.toDegree())) euler_y = min( ik_bone.ik_limit_max.y(), max(ik_bone.ik_limit_min.y(), y_qq.toDegree())) euler_z = min( ik_bone.ik_limit_max.z(), max(ik_bone.ik_limit_min.z(), z_qq.toDegree())) logger.test( "limit_qq: %s -> %s", new_ik_qq.toEulerAngles(), MQuaternion.fromEulerAngles(euler_x, euler_y, euler_z).toEulerAngles()) new_ik_qq = MQuaternion.fromEulerAngles( euler_x, euler_y, euler_z) bf.rotation = new_ik_qq # 位置の差がほとんどない場合、終了 if (local_effector_pos - local_target_pos).lengthSquared() < 0.0001: return return
def convert_target_ik2fk(self, ik_bone: Bone): motion = self.options.motion model = self.options.model bone_name = ik_bone.name logger.info("-- 腕IK変換準備:開始【%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("-- 腕IK変換準備:終了【%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("☆腕IK変換成功(%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("☆腕IK変換ちょっと失敗採用(%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("★腕IK変換ちょっと失敗不採用(%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("★腕IK変換失敗(%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("【腕IK変換 - {0}】".format(bone_name), fno, fnos)