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 test_stance_shoulder_01(self): arm_slope = MVector3D(-1.837494969367981, 18.923479080200195, 0.4923856854438782) shoulder_slope = MVector3D(-0.8141360282897949, 19.6701602935791, 0.4931679964065552) neck_base = MVector3D(0.0, 18.923479080200195, 0.4923856854438782) # 傾きパターン test_slope_param = [arm_slope, shoulder_slope, neck_base] slope_test_params = list(itertools.product(test_slope_param, repeat=2)) # random.shuffle(slope_test_params) # 数値パターン test_number_param = [0, -1, 1] number_test_params = list( itertools.product(test_number_param, repeat=3)) # random.shuffle(number_test_params) target_test_params = list( itertools.product(slope_test_params, number_test_params)) for param in target_test_params: print("------------------") print("param[0][0]: %s" % param[0][0]) print("param[0][1]: %s" % param[0][1]) print("param[1][0]: %s" % param[1][0]) print("param[1][1]: %s" % param[1][1]) print("param[1][2]: %s" % param[1][2]) rep_shoulder_slope = (param[0][0] - param[0][1]).normalized() rep_shoulder_slope_up = MVector3D(param[1][0], param[1][1], param[1][2]) rep_shoulder_slope_cross = MVector3D.crossProduct( rep_shoulder_slope, rep_shoulder_slope_up).normalized() rep_shoulder_initial_slope_qq = MQuaternion.fromDirection( rep_shoulder_slope, rep_shoulder_slope_cross) print("rep_shoulder_slope: %s" % rep_shoulder_slope) print("rep_shoulder_slope_up: %s" % rep_shoulder_slope_up) print("qq: %s" % rep_shoulder_initial_slope_qq.toEulerAngles()) self.assertTrue(True)
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 not_test_stance_shoulder_08(self): new_rep_to_pos = MVector3D(16.638640587237894, 19.455697325211673, 4.067013732312591) # rep_base_pos = MVector3D(15.541596036361701, 18.419343683301417, 2.4565491530944494) rep_from_pos = MVector3D(16.07129122417615, 19.252113744303983, 2.952803072461259) up_pos = MVector3D(0.02597404821369409, -0.6341368197928823, 0.7727844735774392) parent_qq = MQuaternion.fromEulerAngles(4.444080622855673, 131.6889133202979, -6.602768822699441) arm_pos = MVector3D(-1.837494969367981, 18.923479080200195, 0.4923856854438782) shoulder_pos = MVector3D(-0.8141360282897949, 19.6701602935791, 0.4931679964065552) neck_base_pos = MVector3D(0.0, 18.923479080200195, 0.4923856854438782) another_arm_pos = MVector3D(1.837494969367981, 18.923479080200195, 0.4923856854438782) another_shoulder_pos = MVector3D(0.8141360282897949, 19.6701602935791, 0.493167906999588) # 傾きパターン test_slope_param = [arm_pos, shoulder_pos, neck_base_pos] all_slope_test_params = list( itertools.product(test_slope_param, repeat=2)) slope_test_params = [(x00, x01) for (x00, x01) in all_slope_test_params if x00 != x01] # upパターン test_up_param = [ arm_pos, shoulder_pos, neck_base_pos, another_arm_pos, another_shoulder_pos ] all_up_test_params = list(itertools.product(test_up_param, repeat=2)) up_test_params = [(x00, x01) for (x00, x01) in all_up_test_params if x00 != x01] # 数値パターン test_number_param = [0, -1, 1] all_number_test_params = list( itertools.product(test_number_param, repeat=3)) number_test_params = [(x00, x01, x02) for (x00, x01, x02) in all_number_test_params if x00 == 0 or x01 == 0 or x02 == 0] target_test_params = list( itertools.product(slope_test_params, number_test_params, up_test_params)) print(len(target_test_params)) random.shuffle(target_test_params) for params in target_test_params: print("-----------------------") print(params[0][0]) print(params[0][1]) print(params[1]) print(params[2][0]) print(params[2][1]) rep_shoulder_slope = (params[0][0] - params[0][1]).normalized() print("rep_shoulder_slope: %s" % rep_shoulder_slope) rep_shoulder_slope_up = MVector3D(params[1][0], params[1][1], params[1][2]) print("rep_shoulder_slope_up: %s" % rep_shoulder_slope_up) shoulder_cross = MVector3D.crossProduct( rep_shoulder_slope, rep_shoulder_slope_up).normalized() print("shoulder_cross: %s" % shoulder_cross) initial = MQuaternion.fromDirection(rep_shoulder_slope, shoulder_cross) print("initial: %s" % initial.toEulerAngles()) direction = new_rep_to_pos - rep_from_pos up = MVector3D.crossProduct(direction, up_pos) from_orientation = MQuaternion.fromDirection( direction.normalized(), up.normalized()) from_rotation = parent_qq.inverted( ) * from_orientation * initial.inverted() from_rotation.normalize() print("rot %s" % from_rotation.toEulerAngles4MMD()) print("original: %s" % MVector3D( 21.338723875696445, 15.845333083479046, 37.954108757721826))
def not_test_stance_shoulder_06(self): new_rep_to_pos = MVector3D(16.638640587237887, 19.455697325211673, 4.067013732312591) rep_base_pos = MVector3D(15.527995423468052, 19.158781645638516, 2.353690174209908) up_pos = MVector3D(0.02597404821369409, -0.6341368197928823, 0.7727844735774392) parent_qq = MQuaternion.fromEulerAngles(4.444080622855673, 131.6889133202979, -6.602768822699441) arm_pos = MVector3D(-1.837494969367981, 18.923479080200195, 0.4923856854438782) shoulder_pos = MVector3D(-0.8141360282897949, 19.6701602935791, 0.4931679964065552) neck_base_pos = MVector3D(0.0, 19.6701602935791, 0.4931679517030716) # stance_shoulder2neck_qq = MQuaternion.fromEulerAngles(0.015789129707102025, -0.040575112727633096, 42.52534119964952) # stance_shoulder2arm_qq = MQuaternion.fromEulerAngles(0.011536139571057251, 0.03538278693844499, -36.1158910081693) # 傾きパターン test_slope_param = [arm_pos, shoulder_pos, neck_base_pos] all_slope_test_params = list( itertools.product(test_slope_param, repeat=2)) slope_test_params = [(x00, x01) for (x00, x01) in all_slope_test_params if x00 != x01] # 数値パターン test_number_param = [0, -1, 1] all_number_test_params = list( itertools.product(test_number_param, repeat=3)) number_test_params = [(x00, x01, x02) for (x00, x01, x02) in all_number_test_params if x00 == 0 or x01 == 0 or x02 == 0] # qqパターン # test_qq_param = [stance_shoulder2neck_qq, stance_shoulder2neck_qq.inverted(), stance_shoulder2arm_qq, stance_shoulder2arm_qq.inverted(), MQuaternion()] test_qq_param = [MQuaternion()] target_test_params = list( itertools.product(slope_test_params, number_test_params, test_qq_param)) print(len(target_test_params)) random.shuffle(target_test_params) for params in target_test_params: print("-----------------------") print(params[0][0]) print(params[0][1]) print(params[1]) print(params[2].toEulerAngles()) rep_shoulder_slope = (params[0][0] - params[0][1]).normalized() print("rep_shoulder_slope: %s" % rep_shoulder_slope) rep_shoulder_slope_up = MVector3D(params[1][0], params[1][1], params[1][2]) print("rep_shoulder_slope_up: %s" % rep_shoulder_slope_up) initial = MQuaternion.fromDirection(rep_shoulder_slope, rep_shoulder_slope_up) print("initial: %s" % initial.toEulerAngles()) direction = new_rep_to_pos - rep_base_pos up = MVector3D.crossProduct(direction, up_pos) from_orientation = MQuaternion.fromDirection( direction.normalized(), up.normalized()) from_rotation = parent_qq.inverted( ) * from_orientation * initial.inverted() * params[2] from_rotation.normalize() print("rot %s" % from_rotation.toEulerAngles4MMD())