/// <summary>
/// Test wheter a bone has moved unatural much since last frame
/// </summary>
/// <param name="bones">The skeleton, a tree of bones</param>
/// <returns>True if any changes has been applied to the skeleton</returns>
private bool JerkingTest(TreeNode <Bone> bones, bool pos = true, bool rot = true)
{
bool hasChanges = false;
foreach (TreeNode <Bone> bone in bones)
{
if (bone.IsRoot || bone.Data.HasNaN)
{
continue;
}
Bone lastFrameBone = lastSkel.Root.FindTreeNode(tn => tn.Data.Name == bone.Data.Name).Data;
#region Poss
if (pos && bone != bones)
{
Vector3 posInitial = lastFrameBone.Pos;
Vector3 diffInitToFinalVec = (bone.Data.Pos - posInitial);
if (diffInitToFinalVec.Length > 0.025f)
{
diffInitToFinalVec.NormalizeFast();
diffInitToFinalVec *= 0.025f;
Quaternion rotToNewPos =
QuaternionHelper2.RotationBetween(
bone.Parent.Data.GetYAxis(),
((posInitial + diffInitToFinalVec) - bone.Parent.Data.Pos));
FK(bone.Parent, rotToNewPos);
hasChanges = true;
}
}
#endregion
#region Rots
if (rot)
{
Quaternion oriFinal = bone.Data.Orientation;
Quaternion oriInitial = lastFrameBone.Orientation;
if (!bone.IsLeaf)
{
float quatDiff = QuaternionHelper2.DiffrenceBetween(oriFinal, oriInitial);
if (quatDiff > 0.03f)
{
float slerp = (1 - quatDiff) - (Mathf.Cos((MathHelper.Pi * quatDiff) / 2) - (1 - quatDiff * 0.8f));
Quaternion qTrans = Quaternion.Invert(
Quaternion.Slerp(oriInitial, oriFinal, slerp)
* Quaternion.Invert(oriInitial));
FK(bone, qTrans);
hasChanges = true;
}
}
}
#endregion
}
return(hasChanges);
}