def smooth_bf(self, data_set_no: int, bone_name: str, is_rot: bool, is_mov: bool, limit_degrees: float, start_fno=-1, end_fno=-1, is_show_log=True): # キーフレを取得する if start_fno < 0 and end_fno < 0: # 範囲指定がない場合、全範囲 fnos = self.get_bone_fnos(bone_name) else: # 範囲指定がある場合はその範囲内だけ fnos = self.get_bone_fnos(bone_name, start_fno=start_fno, end_fno=end_fno) prev_sep_fno = 0 if len(fnos) > 2: for fno in fnos: prev_bf = self.calc_bf(bone_name, fno - 1) now_bf = self.calc_bf(bone_name, fno) next_bf = self.calc_bf(bone_name, fno + 1) if is_rot and now_bf.key: # 前後の内積 prev_next_dot = MQuaternion.dotProduct(prev_bf.rotation, next_bf.rotation) # 自分と後の内積 now_next_dot = MQuaternion.dotProduct(now_bf.rotation, next_bf.rotation) # 内積差分 diff = np.abs(np.diff([prev_next_dot, now_next_dot])) logger.test("set: %s, %s, f: %s, diff: %s, prev_next_dot: %s, now_next_dot: %s", data_set_no, bone_name, fno, diff, prev_next_dot, now_next_dot) # 前後と自分の内積の差が一定以上の場合、円滑化 if prev_next_dot > now_next_dot and diff > math.radians(limit_degrees): logger.debug("★ 円滑化 set: %s, %s, f: %s, diff: %s, prev_next_dot: %s, now_next_dot: %s", \ data_set_no, bone_name, fno, diff, prev_next_dot, now_next_dot) now_bf.rotation = MQuaternion.slerp(prev_bf.rotation, next_bf.rotation, ((now_bf.fno - prev_bf.fno) / (next_bf.fno - prev_bf.fno))) if is_show_log and data_set_no > 0 and fno // 500 > prev_sep_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - 円滑化 - %s】", fno, round((fno / fnos[-1]) * 100, 3), data_set_no, bone_name) prev_sep_fno = fno // 500
def get_differ_fnos(self, data_set_no: int, bone_name_list: str, limit_degrees: float, limit_length: float): limit_radians = math.cos(math.radians(limit_degrees)) fnos = [0] for bone_name in bone_name_list: prev_sep_fno = 0 # 有効キーを取得 bone_fnos = self.get_bone_fnos(bone_name, is_key=True) if len(bone_fnos) <= 0: continue before_bf = self.calc_bf(bone_name, 0) # 比較対象bf for fno in range(1, bone_fnos[-1]): bf = self.calc_bf(bone_name, fno) if bf.read: # 読み込みキーである場合、必ず処理対象に追加 fnos.append(fno) # 前回キーとして保持 before_bf = bf.copy() else: # 読み込みキーではない場合、処理対象にするかチェック # 読み込みキーとの差 dot = MQuaternion.dotProduct(before_bf.rotation, bf.rotation) if dot < limit_radians: # 前と今回の内積の差が指定度数より離れている場合、追加 logger.test("★ 追加 set: %s, %s, f: %s, dot: %s", data_set_no, bone_name, fno, dot) fnos.append(fno) # 前回キーとして保持 before_bf = bf.copy() # 読み込みキーとの差 diff = before_bf.position.distanceToPoint(bf.position) if diff > limit_length: # 前と今回の移動量の差が指定値より離れている場合、追加 logger.test("★ 追加 set: %s, %s, f: %s, dot: %s", data_set_no, bone_name, fno, dot) fnos.append(fno) # 前回キーとして保持 before_bf = bf.copy() if data_set_no > 0 and fno // 500 > prev_sep_fno and bone_fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - キーフレ追加準備 - %s】", fno, round((fno / bone_fnos[-1]) * 100, 3), data_set_no, bone_name) prev_sep_fno = fno // 500 # 重複を除いて再計算 return sorted(list(set(fnos)))
def convert_multi_split(self, bone_name: str, rrxbn: str, rrybn: str, rrzbn: str, rmxbn: str, rmybn: str, rmzbn: str, center_mx: str, center_my: str, center_mz: str): logger.info("多段分割【%s】", bone_name, decoration=MLogger.DECORATION_LINE) motion = self.options.motion model = self.options.model # 事前に変化量全打ち if bone_name == "センター" or bone_name == "グルーブ": fnos = self.prev_motion.get_differ_fnos(0, ["センター", "グルーブ"], limit_degrees=70, limit_length=1) else: fnos = self.prev_motion.get_differ_fnos(0, [bone_name], limit_degrees=70, limit_length=1) if len(fnos) == 0: return prev_sep_fno = 0 for fno in fnos: # 一度そのままキーを登録 motion.regist_bf(motion.calc_bf(bone_name, fno), bone_name, fno) # 補間曲線のため、もう一度取得しなおし bf = motion.calc_bf(bone_name, fno) if model.bones[bone_name].getRotatable(): rx_bf = motion.calc_bf(rrxbn, fno) motion.copy_interpolation(bf, rx_bf, MBezierUtils.BZ_TYPE_R) motion.regist_bf(rx_bf, rx_bf.name, fno, copy_interpolation=True) ry_bf = motion.calc_bf(rrybn, fno) motion.copy_interpolation(bf, ry_bf, MBezierUtils.BZ_TYPE_R) motion.regist_bf(ry_bf, ry_bf.name, fno, copy_interpolation=True) rz_bf = motion.calc_bf(rrzbn, fno) motion.copy_interpolation(bf, rz_bf, MBezierUtils.BZ_TYPE_R) motion.regist_bf(rz_bf, rz_bf.name, fno, copy_interpolation=True) if model.bones[bone_name].getTranslatable(): mx_bf = motion.calc_bf(rmxbn, fno) motion.copy_interpolation(bf, mx_bf, MBezierUtils.BZ_TYPE_MX) motion.regist_bf(mx_bf, mx_bf.name, fno, copy_interpolation=True) my_bf = motion.calc_bf(rmybn, fno) motion.copy_interpolation(bf, my_bf, MBezierUtils.BZ_TYPE_MY) motion.regist_bf(my_bf, my_bf.name, fno, copy_interpolation=True) mz_bf = motion.calc_bf(rmzbn, fno) motion.copy_interpolation(bf, mz_bf, MBezierUtils.BZ_TYPE_MZ) motion.regist_bf(mz_bf, mz_bf.name, fno, copy_interpolation=True) if fno // 500 > prev_sep_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【キーフレ追加 - %s】", fno, round((fno / fnos[-1]) * 100, 3), bone_name) prev_sep_fno = fno // 500 logger.info("分割準備完了【%s】", bone_name, decoration=MLogger.DECORATION_LINE) # ローカルX軸 local_x_axis = model.bones[bone_name].local_x_vector if local_x_axis == MVector3D(1, 0, 0) or local_x_axis == MVector3D(): # 指定が無い場合、腕系はローカルX軸、それ以外はノーマル if "腕" in bone_name or "ひじ" in bone_name or "手首" in bone_name: local_x_axis = model.get_local_x_axis(bone_name) else: local_x_axis = None logger.debug(f"{bone_name}, local_x_axis: {local_x_axis}") prev_sep_fno = 0 for fno in fnos: bf = motion.calc_bf(bone_name, fno) # 多段分割 self.split_bf(fno, bf, local_x_axis, bone_name, rrxbn, rrybn, rrzbn, rmxbn, rmybn, rmzbn) if fno // 500 > prev_sep_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【多段分割 - %s】", fno, round((fno / fnos[-1]) * 100, 3), bone_name) prev_sep_fno = fno // 500 check_fnos = [] check_prev_next_fnos = {} # 分離後に乖離起こしてないかチェック for fno_idx, (prev_fno, next_fno) in enumerate(zip(fnos[:-1], fnos[1:])): fno = int(prev_fno + ((next_fno - prev_fno) / 2)) if fno not in fnos: check_fnos.append(fno) check_prev_next_fnos[fno] = { "prev": prev_fno, "next": next_fno } check_fnos = list(sorted(list(set(check_fnos)))) logger.debug("bone_name: %s, check_fnos: %s", bone_name, check_fnos) prev_sep_fno = 0 for fno in check_fnos: is_subdiv = False prev_motion_bf = self.prev_motion.calc_bf(bone_name, fno).copy() if model.bones[bone_name].getRotatable(): # 回転を分ける if local_x_axis: # ローカルX軸がある場合 x_qq, y_qq, z_qq, _ = MServiceUtils.separate_local_qq( fno, bone_name, prev_motion_bf.rotation, local_x_axis) else: # ローカルX軸の指定が無い場合、グローバルで分ける euler = prev_motion_bf.rotation.toEulerAngles() x_qq = MQuaternion.fromEulerAngles(euler.x(), 0, 0) y_qq = MQuaternion.fromEulerAngles(0, euler.y(), 0) z_qq = MQuaternion.fromEulerAngles(0, 0, euler.z()) if len(rrxbn) > 0: rx_bf = motion.calc_bf(rrxbn, fno) dot = MQuaternion.dotProduct(x_qq.normalized(), rx_bf.rotation.normalized()) if dot < 0.98: is_subdiv = True if len(rrybn) > 0: ry_bf = motion.calc_bf(rrybn, fno) dot = MQuaternion.dotProduct(y_qq.normalized(), ry_bf.rotation.normalized()) if dot < 0.98: is_subdiv = True if len(rrzbn) > 0: rz_bf = motion.calc_bf(rrzbn, fno) dot = MQuaternion.dotProduct(z_qq.normalized(), rz_bf.rotation.normalized()) if dot < 0.98: is_subdiv = True if model.bones[bone_name].getTranslatable(): if len(center_mx) > 0 or len(center_my) > 0 or len( center_mz) > 0: # センターとグルーブを両方分割してる場合 prev_center_motion_bf = self.prev_motion.calc_bf( "センター", fno).copy() if len(center_mx) > 0 and rmxbn == center_mx: prev_motion_bf.position.setX( prev_motion_bf.position.x() + prev_center_motion_bf.position.x()) if len(center_my) > 0 and rmybn == center_my: prev_motion_bf.position.setY( prev_motion_bf.position.y() + prev_center_motion_bf.position.y()) if len(center_mz) > 0 and rmzbn == center_mz: prev_motion_bf.position.setZ( prev_motion_bf.position.z() + prev_center_motion_bf.position.z()) # 移動を分ける if len(rmxbn) > 0: mx_bf = motion.calc_bf(rmxbn, fno) if np.diff( [mx_bf.position.x(), prev_motion_bf.position.x()]) > 0.1: is_subdiv = True if len(rmybn) > 0: my_bf = motion.calc_bf(rmybn, fno) if np.diff( [my_bf.position.y(), prev_motion_bf.position.y()]) > 0.1: is_subdiv = True if len(rmzbn) > 0: mz_bf = motion.calc_bf(rmzbn, fno) if np.diff( [mz_bf.position.z(), prev_motion_bf.position.z()]) > 0.1: is_subdiv = True if is_subdiv: # 細分化ONの場合、更に分割する if model.bones[bone_name].getRotatable(): if len(rrxbn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rrxbn, fno), rrxbn, fno) if len(rrybn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rrybn, fno), rrybn, fno) if len(rrzbn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rrzbn, fno), rrzbn, fno) if model.bones[bone_name].getTranslatable(): if len(rmxbn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rmxbn, fno), rmxbn, fno) if len(rmybn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rmybn, fno), rmybn, fno) if len(rmzbn) > 0: motion.regist_bf(self.prev_motion.calc_bf(rmzbn, fno), rmzbn, fno) # 分割前の値を再登録 motion.regist_bf(self.prev_motion.calc_bf(bone_name, fno), bone_name, fno) subdiv_bf = motion.calc_bf(bone_name, fno) if bone_name == "グルーブ" and (len(center_mx) > 0 or len(center_my) > 0 or len(center_mz) > 0): prev_center_motion_bf = self.prev_motion.calc_bf( "センター", fno) if len(center_mx) > 0 and rmxbn == center_mx: subdiv_bf.position.setX( subdiv_bf.position.x() + prev_center_motion_bf.position.x()) if len(center_my) > 0 and rmybn == center_my: subdiv_bf.position.setY( subdiv_bf.position.y() + prev_center_motion_bf.position.y()) if len(center_mz) > 0 and rmzbn == center_mz: subdiv_bf.position.setZ( subdiv_bf.position.z() + prev_center_motion_bf.position.z()) # 多段分割 self.split_bf(fno, subdiv_bf, local_x_axis, bone_name, rrxbn, rrybn, rrzbn, rmxbn, rmybn, rmzbn) # prev_fno = check_prev_next_fnos[fno]["prev"] # next_fno = check_prev_next_fnos[fno]["next"] # logger.info(f"-- 軌跡ズレ防止のため、「{bone_name}」の{prev_fno}F~{next_fno}F間を細分化・不要キー除去します") # for f in range(prev_fno, next_fno + 1): # # 区間内を初期登録 # if model.bones[bone_name].getRotatable(): # # 回転を分ける # if local_x_axis: # # ローカルX軸がある場合 # x_qq, y_qq, z_qq, _ = MServiceUtils.separate_local_qq(f, bone_name, prev_motion_bf.rotation, local_x_axis) # else: # # ローカルX軸の指定が無い場合、グローバルで分ける # euler = prev_motion_bf.rotation.toEulerAngles() # x_qq = MQuaternion.fromEulerAngles(euler.x(), 0, 0) # y_qq = MQuaternion.fromEulerAngles(0, euler.y(), 0) # z_qq = MQuaternion.fromEulerAngles(0, 0, euler.z()) # if len(rrxbn) > 0: # prev_rx_bf = self.prev_motion.calc_bf(rrxbn, f).copy() # prev_rx_bf.rotation = x_qq # motion.regist_bf(prev_rx_bf, rrxbn, f) # if len(rrybn) > 0: # prev_ry_bf = self.prev_motion.calc_bf(rrybn, f).copy() # prev_ry_bf.rotation = y_qq # motion.regist_bf(prev_ry_bf, rrybn, f) # if len(rrzbn) > 0: # prev_rz_bf = self.prev_motion.calc_bf(rrzbn, f).copy() # prev_rz_bf.rotation = z_qq # motion.regist_bf(prev_rz_bf, rrzbn, f) # if model.bones[bone_name].getTranslatable(): # if len(center_mx) > 0 or len(center_my) > 0 or len(center_mz) > 0: # # センターとグルーブを両方分割してる場合 # prev_center_motion_bf = self.prev_motion.calc_bf("センター", fno).copy() # if len(center_mx) > 0 and rmxbn == center_mx: # prev_motion_bf.position.setX(prev_motion_bf.position.x() + prev_center_motion_bf.position.x()) # if len(center_my) > 0 and rmybn == center_my: # prev_motion_bf.position.setY(prev_motion_bf.position.y() + prev_center_motion_bf.position.y()) # if len(center_mz) > 0 and rmzbn == center_mz: # prev_motion_bf.position.setZ(prev_motion_bf.position.z() + prev_center_motion_bf.position.z()) # if len(rmxbn) > 0: # prev_mx_bf = self.prev_motion.calc_bf(rmxbn, f).copy() # prev_mx_bf.position.setX(prev_motion_bf.position.x()) # motion.regist_bf(prev_mx_bf, rmxbn, f) # if len(rmybn) > 0: # prev_my_bf = self.prev_motion.calc_bf(rmybn, f).copy() # prev_my_bf.position.setY(prev_motion_bf.position.y()) # motion.regist_bf(prev_my_bf, rmybn, f) # if len(rmzbn) > 0: # prev_mz_bf = self.prev_motion.calc_bf(rmzbn, f).copy() # prev_mz_bf.position.setZ(prev_motion_bf.position.z()) # motion.regist_bf(prev_mz_bf, rmzbn, f) # # 不要キー削除 # futures = [] # with ThreadPoolExecutor(thread_name_prefix="remove", max_workers=self.options.max_workers) as executor: # if model.bones[bone_name].getRotatable(): # if len(rrxbn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rrxbn, start_fno=prev_fno, end_fno=next_fno)) # if len(rrybn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rrybn, start_fno=prev_fno, end_fno=next_fno)) # if len(rrzbn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rrzbn, start_fno=prev_fno, end_fno=next_fno)) # if model.bones[bone_name].getTranslatable(): # if len(rmxbn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rmxbn, start_fno=prev_fno, end_fno=next_fno)) # if len(rmybn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rmybn, start_fno=prev_fno, end_fno=next_fno)) # if len(rmzbn) > 0: # futures.append(executor.submit(self.remove_unnecessary_bf, rmzbn, start_fno=prev_fno, end_fno=next_fno)) # concurrent.futures.wait(futures, timeout=None, return_when=concurrent.futures.FIRST_EXCEPTION) # for f in futures: # if not f.result(): # return False if fno // 1000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【分割後チェック - {bone_name}】", fno, fnos) prev_sep_fno = fno // 1000 logger.info("分割完了【%s】", bone_name, decoration=MLogger.DECORATION_LINE) # 元のボーン削除 if rrxbn != bone_name and rrybn != bone_name and rrzbn != bone_name and rmxbn != bone_name and rmybn != bone_name and rmzbn != bone_name: del motion.bones[bone_name] # # 跳ねてるの除去 # futures = [] # with ThreadPoolExecutor(thread_name_prefix="smooth", max_workers=self.options.max_workers) as executor: # if model.bones[bone_name].getRotatable(): # if len(rrxbn) > 0: # futures.append(executor.submit(self.smooth_bf, rrxbn)) # if len(rrybn) > 0: # futures.append(executor.submit(self.smooth_bf, rrybn)) # if len(rrzbn) > 0: # futures.append(executor.submit(self.smooth_bf, rrzbn)) # if model.bones[bone_name].getTranslatable(): # if len(rmxbn) > 0: # futures.append(executor.submit(self.smooth_bf, rmxbn)) # if len(rmybn) > 0: # futures.append(executor.submit(self.smooth_bf, rmybn)) # if len(rmzbn) > 0: # futures.append(executor.submit(self.smooth_bf, rmzbn)) # concurrent.futures.wait(futures, timeout=None, return_when=concurrent.futures.FIRST_EXCEPTION) # for f in futures: # if not f.result(): # return False return True
def test_MQuaternion_dotProduct(self): qq1 = MQuaternion.fromEulerAngles(0, 0, 0) qq2 = MQuaternion.fromEulerAngles(160, 0, 0) dot = MQuaternion.dotProduct(qq1, qq2) print(dot)
def execute_avoidance(self, data_set_idx: int, direction: str, all_avoidance_axis: dict): logger.info("接触回避処理【No.%s - %s】", (data_set_idx + 1), direction) logger.copy(self.options) # 処理対象データセット data_set = self.options.data_set_list[data_set_idx] # 回避用オプション avoidance_options = self.avoidance_options[(data_set_idx, direction)] arm_bone_name = "{0}腕".format(direction) elbow_bone_name = "{0}ひじ".format(direction) wrist_bone_name = "{0}手首".format(direction) target_bone_names = [ arm_bone_name, "{0}腕捩".format(direction), elbow_bone_name, "{0}手捩".format(direction), wrist_bone_name ] avoidance_axis = {} prev_block_fno = 0 fnos = data_set.motion.get_bone_fnos(*target_bone_names) # 一度全部キーを追加する(キー自体は無効化のまま) for fno in fnos: for bone_name in target_bone_names: if bone_name not in data_set.motion.bones: data_set.motion.bones[bone_name] = {} data_set.motion.bones[bone_name][ fno] = data_set.motion.calc_bf(bone_name, fno) while len(fnos) > 0: fno = fnos[0] if fno in all_avoidance_axis: # 回避ブロックが始まったら、保持 avoidance_axis = all_avoidance_axis[fno] for ((avoidance_name, avodance_link), avoidance) in zip(avoidance_options.avoidance_links.items(), avoidance_options.avoidances.values()): # 剛体の現在位置をチェック rep_avbone_global_3ds, rep_avbone_global_mats = \ MServiceUtils.calc_global_pos(data_set.rep_model, avodance_link, data_set.motion, fno, return_matrix=True) obb = avoidance.get_obb( fno, avodance_link.get(avodance_link.last_name()).position, rep_avbone_global_mats, self.options.arm_options.alignment, direction == "左") # # 剛体の原点 --------------- # debug_bone_name = "原点" # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = obb.origin # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # -------------- for arm_link in avoidance_options.arm_links: # 先モデルのそれぞれのグローバル位置 rep_global_3ds = MServiceUtils.calc_global_pos( data_set.rep_model, arm_link, data_set.motion, fno) # [logger.test("f: %s, k: %s, v: %s", fno, k, v) for k, v in rep_global_3ds.items()] # 衝突情報を取る collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(rep_global_3ds[arm_link.last_name()], rep_global_3ds[arm_bone_name], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if collision or near_collision: logger.debug("f: %s(%s-%s:%s), c[%s], nc[%s], xd[%s], zd[%s], xv[%s], zv[%s]", \ fno, (data_set_idx + 1), arm_link.last_name(), avoidance_name, collision, near_collision, \ x_distance, z_distance, rep_x_collision_vec.to_log(), rep_z_collision_vec.to_log()) is_success = [] failured_last_names = [] # 衝突単位のbf情報 org_bfs = {} for ik_links_list in avoidance_options.ik_links_list.values( ): for ik_links in ik_links_list: for link_name in ik_links.all().keys(): if link_name not in org_bfs: bf = data_set.motion.calc_bf( link_name, fno) bf.org_rotation = bf.rotation.copy() data_set.motion.regist_bf( bf, link_name, fno) org_bfs[link_name] = bf.copy() # 既定の回避方向(なければとりあえずZ) axis = "z" if "axis" not in avoidance_axis else avoidance_axis[ "axis"] # 回避方向 rep_collision_vec = rep_x_collision_vec if axis == "x" else rep_z_collision_vec if collision: logger.info("○接触あり: f: %s(%s-%s:%s:%s), 元: %s, 回避: %s", fno, \ (data_set_idx + 1), arm_link.last_display_name(), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) else: logger.info("-近接あり: f: %s(%s-%s:%s:%s), 元: %s, 回避: %s", fno, \ (data_set_idx + 1), arm_link.last_display_name(), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}A".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = rep_collision_vec # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # IK処理実行 for ik_cnt, (ik_links, ik_max_count) in enumerate(zip(avoidance_options.ik_links_list[arm_link.last_name()], \ avoidance_options.ik_count_list[arm_link.last_name()])): prev_rep_diff = MVector3D() # ひじを含んでいるか is_in_elbow = elbow_bone_name in list( ik_links.all().keys()) for now_ik_max_count in range(1, ik_max_count + 1): logger.debug("IK計算開始(%s): f: %s(%s:%s:%s), axis: %s, now[%s], new[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) # 修正角度がない場合、IK計算実行 MServiceUtils.calc_IK(data_set.rep_model, arm_link, data_set.motion, fno, rep_collision_vec, ik_links, max_count=1) # 現在のエフェクタ位置 now_rep_global_3ds = MServiceUtils.calc_global_pos( data_set.rep_model, arm_link, data_set.motion, fno) now_rep_effector_pos = now_rep_global_3ds[ arm_link.last_name()] # 現在のエフェクタ位置との差分(エフェクタ位置が指定されている場合のみ) rep_diff = MVector3D( ) if rep_collision_vec == MVector3D( ) else rep_collision_vec - now_rep_effector_pos # IKの関連ボーンの内積チェック dot_dict = {} dot_limit_dict = {} for link_name, link_bone in ik_links.all( ).items(): dot_dict[ link_name] = MQuaternion.dotProduct( org_bfs[link_name].rotation, data_set.motion.calc_bf( link_name, fno).rotation) dot_limit_dict[ link_name] = link_bone.dot_limit # まずは一旦確定 for link_name in list( ik_links.all().keys())[1:]: now_bf = data_set.motion.calc_bf( link_name, fno) data_set.motion.regist_bf( now_bf, link_name, fno) if np.count_nonzero( np.where( np.array(list(dot_dict.values())) < np.array( list(dot_limit_dict.values())), 1, 0)) == 0: if (prev_rep_diff == MVector3D() or np.sum(np.abs(rep_diff.data())) < np.sum(np.abs(prev_rep_diff.data()))) and \ np.count_nonzero(np.where(np.abs(rep_diff.data()) > (0.2 if data_set.original_xz_ratio > 0.5 else 0.1), 1, 0)) == 0: logger.debug("☆接触回避実行成功(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}B".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = now_rep_effector_pos # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # org_bfを保持し直し for link_name in ik_links.all().keys(): org_bfs[ link_name] = data_set.motion.calc_bf( link_name, fno).copy() # 回避方向保持 for link_name in list( ik_links.all().keys())[1:]: data_set.motion.bones[link_name][ fno].avoidance = axis # 大体同じ位置にあって、角度もそう大きくズレてない場合、OK(全部上書き) is_success = [True] # 衝突を計り直す collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(now_rep_global_3ds[arm_link.last_name()], now_rep_global_3ds[arm_link.first_name()], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if ( not collision and not near_collision ) or prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: # 衝突していなければこのIKターンは終了 # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 break prev_rep_diff = rep_diff elif (prev_rep_diff == MVector3D() or (prev_rep_diff != MVector3D() and np.sum(np.abs(rep_diff.data())) < np.sum(np.abs(prev_rep_diff.data())))) and \ (np.count_nonzero(np.where(np.abs(rep_diff.data()) > (1 if data_set.original_xz_ratio > 0.5 else 0.5), 1, 0)) == 0): logger.debug("☆接触回避実行ちょっと失敗採用(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}B".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = now_rep_effector_pos # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # org_bfを保持し直し for link_name in ik_links.all().keys(): org_bfs[ link_name] = data_set.motion.calc_bf( link_name, fno).copy() # 回避方向保持 for link_name in list( ik_links.all().keys())[1:]: data_set.motion.bones[link_name][ fno].avoidance = axis # 採用されたらOK is_success.append(True) # 衝突を計り直す collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(now_rep_global_3ds[arm_link.last_name()], now_rep_global_3ds[arm_link.first_name()], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if ( not collision and not near_collision ) or prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: # 衝突していなければこのIKターンは終了 # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 break # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options. ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 is_success.append(False) failured_last_names.append( arm_link.last_name()) prev_rep_diff = rep_diff else: logger.debug("★接触回避実行ちょっと失敗不採用(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) is_success.append(False) # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options. ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 failured_last_names.append( arm_link.last_name()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: break if prev_rep_diff == MVector3D(): # 初回失敗の場合、とりあえず設定 prev_rep_diff = rep_diff else: logger.debug("★接触回避実行失敗(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) is_success.append(False) # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options.ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 failured_last_names.append( arm_link.last_name()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: break if is_success == [True]: # 成功していたらそのまま終了 break if len(is_success) > 0: if is_success.count(True) == 0: # 全てのパターンで失敗してる場合、失敗ログ logger.info("×接触回避失敗: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) # 元々の値に戻す for ik_links in avoidance_options.ik_links_list[ arm_link.last_name()]: for link_name in list( ik_links.all().keys())[1:]: data_set.motion.regist_bf( org_bfs[link_name].copy(), link_name, fno) else: # どっか成功していたら、最後に成功したトコまで戻す is_in_elbow = False for ik_links in avoidance_options.ik_links_list[ arm_link.last_name()]: is_in_elbow = is_in_elbow or elbow_bone_name in list( ik_links.all().keys()) for link_name in list( ik_links.all().keys())[1:]: data_set.motion.regist_bf( org_bfs[link_name].copy(), link_name, fno) if not is_in_elbow: # IKリストの中にひじが含まれていない場合、キャンセル arm_bf = data_set.motion.calc_bf( arm_bone_name, fno) elbow_bf = data_set.motion.calc_bf( elbow_bone_name, fno) # 腕の変化量YZのみをひじに加算 elbow_adjust_qq = arm_bf.rotation.inverted( ) * arm_bf.org_rotation * elbow_bf.rotation logger.debug("◆手首調整: f: %s(%s:%s), arm_bf.org_rotation [%s], arm_bf.rotation[%s], elbow_bf.rotation[%s], adjust_qq[%s]", \ fno, (data_set_idx + 1), avoidance_name, arm_bf.org_rotation.toEulerAngles().to_log(), \ arm_bf.rotation.toEulerAngles().to_log(), elbow_bf.rotation.toEulerAngles().to_log(), elbow_adjust_qq.toEulerAngles().to_log()) # 再設定 elbow_bf.rotation = elbow_adjust_qq data_set.motion.regist_bf( elbow_bf, elbow_bone_name, fno) if len(is_success) > 1 and is_success.count( False) > 0: # どこかのパターンで失敗している場合、一部成功ログ logger.info("△接触回避一部成功: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) else: # 全部成功している場合、成功ログ logger.info("○接触回避成功: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) if fno // 500 > prev_block_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - 接触回避 - %s】", fno, round((fno / fnos[-1]) * 100, 3), data_set_idx + 1, direction) prev_block_fno = fno // 500 # キーの登録が増えているかもなので、ここで取り直す fnos = data_set.motion.get_bone_fnos(*target_bone_names, start_fno=(fno + 1)) for bone_name in target_bone_names: # 非活性キー削除 data_set.motion.remove_unkey_bf(data_set_idx + 1, bone_name) return True