private void ApplyIK(Model model)
{
//追加: 加速度センサONの場合に回転不変性取るよう改善
var boneHip = QumarionModel.Bones[StandardPSBones.Hips];
var initPosHip = boneHip.InitialWorldMatrix.Translate;
var posHip = boneHip.WorldMatrix.Translate;
//NOTE:
var rotHipInverse = MatrixRotationDif.CreateDifFrom(
boneHip.WorldMatrix.Rotate,
boneHip.InitialWorldMatrix.Rotate
).Transpose();
foreach (var t in _targetIKBones)
{
var src = QumarionModel.Bones[QumaBone2MMDBone.GetQumaBone(t)];
var dest = MMDStandardBones.GetBone(model, t);
if (dest == null)
{
continue;
}
//1: Tポーズでの Hip -> t のベクトル取得(これは直立)
var initPosWorld = src.InitialWorldMatrix.Translate;
var initPos = new Vector3f(
initPosWorld.X - initPosHip.X,
initPosWorld.Y - initPosHip.Y,
initPosWorld.Z - initPosHip.Z
);
//2: 現在の Hip -> t のベクトルを回転除去しつつ取得
var posWorld = src.WorldMatrix.Translate;
var pos = new Vector3f(
posWorld.X - posHip.X,
posWorld.Y - posHip.Y,
posWorld.Z - posHip.Z
);
//ここ大事: posのベクトルは体の回転込みの値なので逆回転して相殺
pos = MatrixUtil.Multiply(rotHipInverse, pos);
//NOTE: MMMではZ軸が後ろ向き正だがQumarionだと前向き正
var dif = new Vector3(
pos.X - initPos.X,
pos.Y - initPos.Y,
-pos.Z + initPos.Z
);
var motion = dest.CurrentLocalMotion;
motion.Move = dif * LegIKScaleFactor;
dest.CurrentLocalMotion = motion;
}
}
private void ApplyFK(Model model)
{
var childBonesQumarion = _rootBone.ChildBones;
foreach (var mmdBone in _targetBones)
{
var src = childBonesQumarion.FirstOrDefault(
bone => bone.IsValidBone && bone.MMDBone == mmdBone
);
var dest = MMDStandardBones.GetBone(model, mmdBone);
if (src == null || dest == null)
{
continue;
}
if (_armRollPair.Keys.Contains(mmdBone))
{
var rollBone = MMDStandardBones.GetBone(model, _armRollPair[mmdBone]);
//追加ロジック: 回転をもとのボーンと「捩」ボーンに分割する
Quaternion roll, nonRoll;
QuaternionDecomposition.DecompositeStandardEuler(src.Rotation, out roll, out nonRoll);
nonRoll = GetRotation(nonRoll, mmdBone);
roll = GetRotation(roll, mmdBone);
//本体側
var motion = dest.CurrentLocalMotion;
motion.Rotation = nonRoll;
dest.CurrentLocalMotion = motion;
//追加: 対応する「捩」ボーン
var rollMotion = rollBone.CurrentLocalMotion;
rollMotion.Rotation = roll;
rollBone.CurrentLocalMotion = rollMotion;
}
else
{
//普通のボーン
//Qumarion側の正規化回転をMMDモデルのローカル回転に直す
var rotation = GetRotation(src.Rotation, mmdBone);
var motion = dest.CurrentLocalMotion;
motion.Rotation = rotation;
dest.CurrentLocalMotion = motion;
}
}
}
//モデルを接地させる(接地の定義は実装を見よ。)
private void BoundCenterToGround(Model model)
{
//追加: 加速度センサONの場合に回転不変性取るよう改善
var boneHip = QumarionModel.Bones[StandardPSBones.Hips];
var initPosHip = boneHip.InitialWorldMatrix.Translate;
var posHip = boneHip.WorldMatrix.Translate;
//NOTE:
var rotHipInverse = MatrixRotationDif.CreateDifFrom(
boneHip.WorldMatrix.Rotate,
boneHip.InitialWorldMatrix.Rotate
).Transpose();
var heights = new List <float>();
foreach (var t in _groundingIKBones)
{
var src = QumarionModel.Bones[QumaBone2MMDBone.GetQumaBone(t)];
var dest = MMDStandardBones.GetBone(model, t);
if (dest == null)
{
continue;
}
//1: Tポーズでの Hip -> t のベクトル取得(これは直立)
var initPosWorld = src.InitialWorldMatrix.Translate;
var initPos = new Vector3f(
initPosWorld.X - initPosHip.X,
initPosWorld.Y - initPosHip.Y,
initPosWorld.Z - initPosHip.Z
);
//2: 現在の Hip -> t のベクトルを回転除去しつつ取得
var posWorld = src.WorldMatrix.Translate;
var pos = new Vector3f(
posWorld.X - posHip.X,
posWorld.Y - posHip.Y,
posWorld.Z - posHip.Z
);
//ここ大事: posのベクトルは体の回転込みの値なので逆回転して相殺
pos = MatrixUtil.Multiply(rotHipInverse, pos);
//NOTE: MMMではZ軸が後ろ向き正だがQumarionだと前向き正
var dif = new Vector3(
pos.X - initPos.X,
pos.Y - initPos.Y,
-pos.Z + initPos.Z
);
heights.Add(dif.Y * LegIKScaleFactor);
}
if (heights.Count == 0)
{
return;
}
//移動差分のうちもっとも低い値を取る
float lowestPosDif = heights.Min();
var centerBone = MMDStandardBones.GetBone(model, MMDStandardBone.Center);
if (centerBone == null)
{
return;
}
var motion = centerBone.CurrentLocalMotion;
//CAUTION: X, Zを勝手に書き換えないように！
motion.Move = new Vector3(motion.Move.X, -lowestPosDif, motion.Move.Z);
centerBone.CurrentLocalMotion = motion;
}