private void GetAnkleLeft(Bone b)
{
b.Pos = AnkleLeft;
if (markers[m.leftInnerAnkle].IsNaN())
{
Vector3 up = KneeLeft - AnkleLeft;
b.Orientation = QuaternionHelper2.LookAtUp(AnkleLeft, FootBaseLeft, up);
}
else
{
Vector3 right = markers[m.leftInnerAnkle] - markers[m.leftOuterAnkle];
b.Orientation = QuaternionHelper2.LookAtRight(AnkleLeft, FootBaseLeft, right);
}
}
/// <summary>
/// The function applied to each bone in the skeleton
/// </summary>
/// <param name="bone">The skeleton, a tree of bones</param>
private void TraversFunc(TreeNode <Bone> bone)
{
if (!bone.Data.Exists)
{
if (bone.IsRoot || bone.Parent.IsRoot)
{
return;
}
if (
bone.Data.Name.Equals(Joint.CLAVICLE_L) ||
bone.Data.Name.Equals(Joint.CLAVICLE_R) ||
bone.Data.Name.Equals(Joint.TRAP_L) ||
bone.Data.Name.Equals(Joint.TRAP_R))
{
bone.Data.Pos = new Vector3(bone.Parent.Data.Pos);
bone.Data.Orientation = QuaternionHelper2.LookAtUp(
bone.Data.Pos,
bone.Children.First().Data.Pos,
bone.Parent.Data.GetZAxis());
return;
}
MissingJoint(bone);
}
}
private void UpperLegRight(Bone b)
{
b.Pos = HipJointRight;
b.Orientation = QuaternionHelper2.LookAtRight(HipJointRight, KneeRight, HipJointRight - HipJointLeft);
}
private void MidSpine(Bone b)
{
b.Pos = Spine1;
b.Orientation = QuaternionHelper2.LookAtRight(Spine1, SternumClavicle, HipJointLeft - HipJointRight);
}
private void GetHandRight(Bone b)
{
b.Pos = markers[m.rightHand];
b.Orientation = QuaternionHelper2.LookAtUp(markers[m.rightHand], markers[m.rightIndex], LowerArmForwardRight);
}
private void GetTrapRight(Bone b)
{
b.Pos = WristRight;
b.Orientation = QuaternionHelper2.LookAtUp(WristRight, markers[m.rightThumb], LowerArmForwardRight);
}
private void GetTrapLeft(Bone b)
{
b.Pos = WristLeft;
b.Orientation = QuaternionHelper2.LookAtUp(WristLeft, markers[m.leftThumb], LowerArmForwardLeft);
}
/// <summary>
/// Given a array of bones, and a target bone, solves the chain so that the last bone in the chain is at at the same position as the target
/// </summary>
/// <param name="bones">An array of bones, the chain to be solved by IK</param>
/// <param name="target">The target for the chain</param>
/// <param name="grandparent">the parent of the first bone in the bones chain, is used to ensure constraints</param>
/// <returns>True if target was reached, false if maximum iteration was reached first </returns>
override public bool SolveBoneChain(Bone[] bones, Bone target, Bone grandparent)
{
if (!IsReachable(bones, target))
{
TargetUnreachable(bones, target.Pos, grandparent);
bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W);
return(true);
}
int numberOfBones = bones.Length;
int iter = 0;
int degrees = degreeStep;
bool toggle = false;
bool doneOneLapAroundYAxis = false;
int maxdegrees = 120;
float lastDistanceToTarget = float.MaxValue;
float distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).Length;
// main loop
while (distanceToTarget > threshold && MaxIterations > ++iter)
{
// if CCD is stuck becouse of constraints, we twist the chain
if (distanceToTarget >= lastDistanceToTarget)
{
if (!doneOneLapAroundYAxis && degrees > maxdegrees)
{
doneOneLapAroundYAxis = true;
degrees = degreeStep;
}
else if (doneOneLapAroundYAxis && degrees > maxdegrees)
{
break;
}
Quaternion q = doneOneLapAroundYAxis ?
QuaternionHelper2.RotationX(MathHelper.DegreesToRadians(toggle ? degrees : -degrees))
:
QuaternionHelper2.RotationY(MathHelper.DegreesToRadians(toggle ? degrees : -degrees));
ForwardKinematics(ref bones, q);
if (toggle)
{
degrees += degreeStep;
}
toggle = !toggle;
}
// for each bone, starting with the one closest to the end effector
// (but not the end effector itself)
Vector3 a, b;
Quaternion rotation;
for (int i = numberOfBones - 2; i >= 0; i--)
{
// Get the vectors between the points
a = bones[numberOfBones - 1].Pos - bones[i].Pos;
b = target.Pos - bones[i].Pos;
// Make a rotation quaternion and rotate
// - first the endEffector
// - then the rest of the affected joints
rotation = (a.LengthFast == 0 || b.LengthFast == 0) ? Quaternion.Identity
: QuaternionHelper2.GetRotationBetween(a, b, bones[i].Stiffness);
if (bones[i].HasConstraints)
{
Vector3 res;
Quaternion rot;
if (constraints.CheckRotationalConstraints(
bones[i],
((i > 0) ? bones[i - 1] : grandparent).Orientation, //Reference
bones[i].Pos + Vector3.Transform(bones[i + 1].Pos - bones[i].Pos, rotation), // Target
out res, out rot))
{
rotation = rot * rotation;
}
}
// Move the chain
ForwardKinematics(ref bones, rotation, i);
// Check for twist constraints
if (bones[i].HasTwistConstraints)
{
Quaternion rotation2;
if (constraints.CheckOrientationalConstraint(bones[i], (i > 0) ? bones[i - 1] : grandparent, out rotation2))
{
ForwardKinematics(ref bones, rotation2, i);
}
}
}
lastDistanceToTarget = distanceToTarget;
distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).LengthFast;
}
// Copy the targets rotation so that rotation is consistant
bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W);
return(distanceToTarget <= threshold);
}
private void GetUpperArmRight(Bone b)
{
b.Pos = ShoulderRight;
b.Orientation = QuaternionHelper2.LookAtRight(ShoulderRight, ElbowRight, markers[m.rightOuterElbow] - markers[m.rightInnerElbow]);
}
private void GetUpperArmLeft(Bone b)
{
b.Pos = ShoulderLeft;
b.Orientation = QuaternionHelper2.LookAtRight(ShoulderLeft, ElbowLeft, markers[m.leftInnerElbow] - markers[m.leftOuterElbow]);
}
private void GetShoulderRight(Bone b)
{
b.Pos = SternumClavicle;
b.Orientation = QuaternionHelper2.LookAtUp(SternumClavicle, ShoulderRight, ChestForward);
}
private void GetFootBaseRight(Bone b)
{
b.Pos = FootBaseRight;
b.Orientation = QuaternionHelper2.LookAtUp(b.Pos, markers[m.rightToe2], LowerLegUpRight);
}
private void GetFootBaseLeft(Bone b)
{
b.Pos = FootBaseLeft;
b.Orientation = QuaternionHelper2.LookAtUp(b.Pos, markers[m.leftToe2], LowerLegUpLeft);
}
public void RotateTowards(Vector3 v, float stiffness = 1f)
{
Rotate(QuaternionHelper2.GetRotationBetween(GetYAxis(), v, stiffness = this.stiffness));
}
private void LowerLegLeft(Bone b)
{
b.Pos = KneeLeft;
b.Orientation = QuaternionHelper2.LookAtRight(KneeLeft, AnkleLeft, KneeForwardLeft);
}
public void SetConstraints(BipedSkeleton skeleton)
{
#region Cone constraints
#region Spine too head
skeleton[Joint.SPINE0].Constraints = (Spine);
skeleton[Joint.SPINE1].Constraints = (Spine);
skeleton[Joint.NECK].Constraints = (Neck);
#endregion
#region Legs
skeleton[Joint.HIP_L].Constraints = (SwapXZ(Femur));
skeleton[Joint.HIP_R].Constraints = (Femur);
skeleton[Joint.KNEE_L].Constraints = (SwapXZ(Knee));
skeleton[Joint.KNEE_R].Constraints = (Knee);
skeleton[Joint.ANKLE_L].Constraints = (SwapXZ(Ankle));
skeleton[Joint.ANKLE_R].Constraints = (Ankle);
skeleton[Joint.FOOTBASE_L].Constraints = (SwapXZ(FootBase));
skeleton[Joint.FOOTBASE_R].Constraints = (FootBase);
#endregion
#region Arms
skeleton[Joint.CLAVICLE_L].Constraints = (SwapXZ(Clavicula));
skeleton[Joint.CLAVICLE_R].Constraints = (Clavicula);
skeleton[Joint.SHOULDER_L].Constraints = (SwapXZ(Shoulder));
skeleton[Joint.SHOULDER_R].Constraints = (Shoulder);
skeleton[Joint.ELBOW_L].Constraints = (SwapXZ(Elbow));
skeleton[Joint.ELBOW_R].Constraints = (Elbow);
skeleton[Joint.WRIST_L].Constraints = (SwapXZ(Wrist));
skeleton[Joint.WRIST_R].Constraints = (Wrist);
#endregion
#endregion
#region ParentPointers
skeleton[Joint.CLAVICLE_R].ParentPointer = QuaternionHelper2.RotationZ(-MathHelper.PiOver2);
skeleton[Joint.CLAVICLE_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.PiOver2);
skeleton[Joint.HIP_R].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi);
skeleton[Joint.HIP_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi);
skeleton[Joint.ANKLE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(-MathHelper.PiOver4);
skeleton[Joint.ANKLE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(MathHelper.PiOver4);
skeleton[Joint.FOOTBASE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) *
QuaternionHelper2.RotationZ(-MathHelper.PiOver4);
skeleton[Joint.FOOTBASE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) *
QuaternionHelper2.RotationZ(MathHelper.PiOver4);
#endregion
#region TwistConstraints
#region Spine
skeleton[Joint.SPINE0].TwistLimit = (SpineTwist);
skeleton[Joint.SPINE1].TwistLimit = (SpineTwist);
skeleton[Joint.SPINE3].TwistLimit = (NoTwist);
skeleton[Joint.NECK].TwistLimit = (NeckTwist);
#endregion
#region Legs
skeleton[Joint.HIP_L].TwistLimit = (FemurTwist);
skeleton[Joint.HIP_R].TwistLimit = (FemurTwist);
skeleton[Joint.KNEE_L].TwistLimit = (KneeTwist);
skeleton[Joint.KNEE_R].TwistLimit = (KneeTwist);
skeleton[Joint.ANKLE_L].TwistLimit = (AnkleTwist);
skeleton[Joint.ANKLE_R].TwistLimit = (AnkleTwist);
skeleton[Joint.FOOTBASE_L].TwistLimit = (FootBaseTwist);
skeleton[Joint.FOOTBASE_R].TwistLimit = (FootBaseTwist);
#endregion
#region Arms
skeleton[Joint.CLAVICLE_L].TwistLimit = (ClaviculaTwist);
skeleton[Joint.SHOULDER_L].TwistLimit = (ShoulderTwist);
skeleton[Joint.ELBOW_L].TwistLimit = (ElbowTwist);
skeleton[Joint.WRIST_L].TwistLimit = (WristTwist);
skeleton[Joint.CLAVICLE_R].TwistLimit = (ClaviculaTwist);
skeleton[Joint.SHOULDER_R].TwistLimit = (ShoulderTwist);
skeleton[Joint.ELBOW_R].TwistLimit = (ElbowTwist);
skeleton[Joint.WRIST_R].TwistLimit = (WristTwist);
#endregion
#endregion
#region stiffness
#region Arms
skeleton[Joint.CLAVICLE_L].Stiffness = (verystiff);
skeleton[Joint.CLAVICLE_R].Stiffness = (verystiff);
skeleton[Joint.WRIST_L].Stiffness = (barelymoving);
skeleton[Joint.WRIST_R].Stiffness = (barelymoving);
#endregion
#region Legs
skeleton[Joint.ANKLE_L].Stiffness = (barelymoving);
skeleton[Joint.ANKLE_R].Stiffness = (barelymoving);
skeleton[Joint.FOOTBASE_L].Stiffness = (barelymoving);
skeleton[Joint.FOOTBASE_R].Stiffness = (barelymoving);
#endregion
#endregion
}
private void GetLowerArmLeft(Bone b)
{
b.Pos = ElbowLeft;
b.Orientation = QuaternionHelper2.LookAtUp(ElbowLeft, WristLeft, LowerArmForwardLeft);
}
/// <summary>
/// If any of the hip markers are missing, we predict them using the last position
/// </summary>
/// <param name="markers">The dictionary of markers</param>
private void MissingEssentialMarkers(Dictionary <string, Vector3> markers)
{
Vector3 dirVec1, dirVec2, possiblePos1, possiblePos2,
sacrumLastFrame = lastSACRUMknown,
liasLastFrame = lastLIASknown,
riasLastFrame = lastRIASknown;
Vector3 Sacrum = markers[m.bodyBase],
RIAS = markers[m.rightHip],
LIAS = markers[m.leftHip];
bool s = !Sacrum.IsNaN(),
r = !RIAS.IsNaN(),
l = !LIAS.IsNaN();
if (s) // sacrum exists
{
if (r) // sacrum and rias exist, lias missing
{
dirVec1 = liasLastFrame - sacrumLastFrame; // vector from sacrum too lias in last frame
dirVec2 = liasLastFrame - riasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(RIAS - Sacrum), (riasLastFrame - sacrumLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = Sacrum + transVec1; // add vector from sacrum to lias last frame to this frames sacrum
possiblePos2 = RIAS + transVec2;
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2)); // get mid point of possible positions
}
else if (l) // sacrum and lias exists, rias missing
{
dirVec1 = riasLastFrame - sacrumLastFrame;
dirVec2 = riasLastFrame - liasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(LIAS - Sacrum), (liasLastFrame - sacrumLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = Sacrum + transVec1;
possiblePos2 = LIAS + transVec2;
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2));
}
else // only sacrum exists, lias and rias missing
{
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Sacrum + riasLastFrame - sacrumLastFrame);
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Sacrum + liasLastFrame - sacrumLastFrame);
}
}
else if (r) // rias exists, sacrum missing
{
if (l) // rias and ias exists, sacrum missing
{
dirVec1 = sacrumLastFrame - riasLastFrame;
dirVec2 = sacrumLastFrame - liasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(LIAS - RIAS), (liasLastFrame - riasLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = RIAS + transVec1;
possiblePos2 = LIAS + transVec2;
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], Vector3Helper.MidPoint(possiblePos1, possiblePos2));
}
else // only rias exists, lias and sacrum missing
{
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], RIAS + sacrumLastFrame - riasLastFrame);
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], RIAS + liasLastFrame - riasLastFrame);
}
}
else if (l) // only lias exists, rias and sacrum missing
{
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], LIAS + sacrumLastFrame - liasLastFrame);
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], LIAS + riasLastFrame - liasLastFrame);
}
else // all markers missing
{
markers[m.bodyBase] = markersLastFrame[m.bodyBase];
markers[m.rightHip] = markersLastFrame[m.rightHip];
markers[m.leftHip] = markersLastFrame[m.leftHip];
}
}
private void GetLowerArmRight(Bone b)
{
b.Pos = ElbowRight;
b.Orientation = QuaternionHelper2.LookAtUp(ElbowRight, WristRight, LowerArmForwardRight);
}