/// <summary>ボーンの姿勢を更新します。</summary>
public void Update()
{
//子要素の更新
foreach (var child in _childs)
{
child.Update();
}
//このボーン自体の姿勢更新
var lmat = _bone.LocalMatrix;
//ゼロ回転からではなくTポーズからの変化を知りたいので差分を求める
var rotationMat = MatrixRotationDif.CreateDifFrom(lmat, _bone.InitialLocalMatrix);
//クォータニオン表現に修正
var rotation = CreateQuaternion(rotationMat);
//回転軸を取得し、疑似座標系に投影して回転軸がXY(-Z)軸基準になるようにする
Vector3 axis = rotation.Axis;
float angle = rotation.Angle;
Vector3 axisNormalized =
axis.X * xAxis +
axis.Y * yAxis +
axis.Z * zAxis;
Rotation = Quaternion.RotationAxis(axisNormalized, angle);
}
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 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;
}