private bool UpdateChildBones()
{
for (int i = 0; i < Childs.Length; i++)
{
var child = Childs[i];
Matrix4 invMat = ~child.ChildBone.WorldMat;
Vector3 ikPos = (Vector4)IKBone.WorldMat.Translation * invMat;
Vector3 targetPos = (Vector4)TargetBone.WorldMat.Translation * invMat;
if ((ikPos - targetPos).Length() < 0.01F)
{
return(true);
}
var scaling = new Matrix3(0, 1, 1);
var ikVec = +(ikPos * scaling);
var targetVec = +(targetPos * scaling);
float angle = (float)Math.Acos(ikVec * targetVec);
angle = MathUtil.Clamp(angle, -LimitAngle, LimitAngle);
var axis = targetVec ^ ikVec;
if (Math.Abs(angle) < 0.0001F || axis.Length() < 0.0001F)
{
continue;
}
Quaternion rot = Quaternion.RotationAxis(axis, angle);
if (child.Limit)
{
rot = ClampEular(rot, child.AngleMin, child.AngleMax);
}
child.ChildBone.Rot = rot * child.ChildBone.Rot;
child.ChildBone.UpdateLocalMatrix();
for (int j = i; j >= 0; j--)
{
Childs[j].ChildBone.UpdateWorldMatrix();
}
TargetBone.UpdateWorldMatrix();
}
return(false);
}
public void Update()
{
IKBone.UpdateWorldMatrix();
for (int j = Childs.Length - 1; j >= 0; j--)
{
Childs[j].ChildBone.UpdateWorldMatrix();
}
TargetBone.UpdateWorldMatrix();
for (int i = 0; i < 255; i++)
{
if (UpdateChildBones())
{
return;
}
}
}