| public static RotationX ( float radians ) : Quaternion | ||
| radians | float | rotation amount |
| return | Quaternion | |
/// <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);
}
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
}