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