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) # グローバル座標系(Aスタンス)からローカル座標系(ボーンベクトルが(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("-- 腕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)
def convert_leg_fk2ik(self, direction: str): logger.info("足IK変換 【%s足IK】", direction, decoration=MLogger.DECORATION_LINE) motion = self.options.motion model = self.options.model leg_ik_bone_name = "{0}足IK".format(direction) toe_ik_bone_name = "{0}つま先IK".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足IK】", 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) # 足IKの位置は、足IKの親から見た足首のローカル位置(足首位置マイナス) 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() # 一旦足IKの位置が決まった時点で登録 motion.regist_bf(bf, leg_ik_bone_name, fno) # 足IK回転なし状態でのつま先までのグローバル位置 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()) # 足IKの回転は、足首から見たつま先の方向 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"【足IK変換 - {leg_ik_bone_name}】", fno, fnos) prev_sep_fno = fno // 500 logger.info("変換完了 【%s足IK】", 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