/// <summary>
/// IKTo get the link based effector 1 position
/// </summary>
/// <param name="ikLink">IKLink</param>
/// <param name="effector">Effector</param>
/// <returns>IKLink based effector 1 position</returns>
private Vector3 GetLink2Effector(IkLink ikLink, PMXBone effector)
{
var ToLinkLocal = Matrix.Invert(ikLink.ikLinkBone.GlobalPose);
var effectorPos = Vector3.TransformCoordinate(effector.Position, effector.GlobalPose * ToLinkLocal); //●
return(Vector3.Normalize(effectorPos - ikLink.ikLinkBone.Position));
}
/// <summary>
/// To limit the amount of rotation
/// </summary>
/// <param name="ikLink">IKLink</param>
private void RestrictRotation(IkLink ikLink)
{
if (!ikLink.isLimited)
{
return;
}
float xRotation, yRotation, zRotation;
var type = SplitRotation(ikLink.ikLinkBone.Rotation, out xRotation, out yRotation, out zRotation);
var clamped = Vector3.Clamp(new Vector3(xRotation, yRotation, zRotation).NormalizeEular(), ikLink.minRot, ikLink.maxRot);
xRotation = clamped.X;
yRotation = clamped.Y;
zRotation = clamped.Z;
switch (type)
{
case 0:
ikLink.ikLinkBone.Rotation = Quaternion.RotationMatrix(Matrix.RotationX(xRotation) * Matrix.RotationY(yRotation) * Matrix.RotationZ(zRotation));
break;
case 1:
ikLink.ikLinkBone.Rotation = Quaternion.RotationMatrix(Matrix.RotationY(yRotation) * Matrix.RotationZ(zRotation) * Matrix.RotationX(xRotation));
break;
case 2:
ikLink.ikLinkBone.Rotation = Quaternion.RotationYawPitchRoll(yRotation, xRotation, zRotation);
break;
}
}
/// <summary>
/// IKTo get the link relative target position
/// </summary>
/// <param name="ikLink">IKLink</param>
/// <param name="TargetGlobalPos">Global relative target position</param>
/// <returns>IKTarget position of a standard link</returns>
private Vector3 GetLink2Target(IkLink ikLink, Vector3 TargetGlobalPos)
{
var ToLinkLocal = Matrix.Invert(ikLink.ikLinkBone.GlobalPose);
Vector3 targetPos;
Vector3.TransformCoordinate(ref TargetGlobalPos, ref ToLinkLocal, out targetPos);
return(Vector3.Normalize(targetPos - ikLink.ikLinkBone.Position));
}
/*
* CCD-IKのIKLink計算の仕組み
*-◎-◎-◎-●
* ↑関節 ▲←IKターゲット(MMD内ではIKBoneと呼ぶ)。関節を近づけたい目標
* (関節内の◎にあたる、IKで曲げる途中の関節をIKリンク、●にあたる関節の終点をエフェクタ(MMD内ではターゲットボーン)
* とする。
* 1)まず、◎-●の方向ベクタを求める。ベクトルA
* 2)次に、◎-▲の方向ベクタを求める。ベクトルB
* 3)∠●◎▲における最短回転角度を求める。arccos(A・B)よりラジアン単位で求まる
* 4)２つの方向ベクトルの外積から回転軸を求める
*/
/// <summary>
/// IKLinkCalculation
/// </summary>
/// <param name="ikLink">IKLink</param>
/// <param name="link2Effector">IKLink based effector position</param>
/// <param name="link2Target">IKTarget position of a standard link</param>
/// <param name="RotationLimited">The absolute value of the upper and lower limits for the angle of rotation</param>
private void IKLinkCalc(IkLink ikLink, Vector3 link2Effector, Vector3 link2Target, float RotationLimited)
{
//To determine the rotation angle
var dot = Vector3.Dot(link2Effector, link2Target);
if (dot > 1f)
{
dot = 1f;
}
var rotationAngle = ClampFloat((float)Math.Acos(dot), RotationLimited);
if (float.IsNaN(rotationAngle))
{
return;
}
if (rotationAngle <= 1.0e-3f)
{
return;
}
//Ask the axis
var rotationAxis = Vector3.Cross(link2Effector, link2Target);
ikLink.loopCount++;
//Builds a matrix that rotates around axis。
var rotation = Quaternion.RotationAxis(rotationAxis, rotationAngle);
rotation.Normalize();
ikLink.ikLinkBone.Rotation = rotation * ikLink.ikLinkBone.Rotation;
#region Rotation amount limit
RestrictRotation(ikLink);
#endregion
ikLink.ikLinkBone.UpdateGrobalPose();
}
/*
* CCD-IKのIKLink計算の仕組み
*-◎-◎-◎-●
* ↑関節 ▲←IKターゲット(MMD内ではIKBoneと呼ぶ)。関節を近づけたい目標
* (関節内の◎にあたる、IKで曲げる途中の関節をIKリンク、●にあたる関節の終点をエフェクタ(MMD内ではターゲットボーン)
* とする。
* 1)まず、◎-●の方向ベクタを求める。ベクトルA
* 2)次に、◎-▲の方向ベクタを求める。ベクトルB
* 3)∠●◎▲における最短回転角度を求める。arccos(A・B)よりラジアン単位で求まる
* 4)２つの方向ベクトルの外積から回転軸を求める
*/
/// <summary>
/// IKLinkを計算
/// </summary>
/// <param name="ikLink">IKリンク</param>
/// <param name="link2Effector">IKリンク基準のエフェクタ位置</param>
/// <param name="link2Target">IKリンク基準のターゲット位置</param>
/// <param name="RotationLimited">回転角度の上下限値の絶対値</param>
private void IKLinkCalc(IkLink ikLink, Vector3 link2Effector, Vector3 link2Target, float RotationLimited)
{
//回転角度を求める
var dot = Vector3.Dot(link2Effector, link2Target);
if (dot > 1f)
{
dot = 1f;
}
var rotationAngle = ClampFloat((float)Math.Acos(dot), RotationLimited);
if (float.IsNaN(rotationAngle))
{
return;
}
if (rotationAngle <= 1.0e-3f)
{
return;
}
//回転軸を求める
var rotationAxis = Vector3.Cross(link2Effector, link2Target);
ikLink.loopCount++;
//軸を中心として回転する行列を作成する。
var rotation = Quaternion.RotationAxis(rotationAxis, rotationAngle);
rotation.Normalize();
ikLink.ikLinkBone.Rotation = rotation * ikLink.ikLinkBone.Rotation;
#region 回転量制限
RestrictRotation(ikLink);
#endregion
ikLink.ikLinkBone.UpdateGrobalPose();
}