///***************************************************************************************
/// CalcIK_2D_ConstraintRelaxation_ConvertToWorld
/// This is a helper function to generate a world space bone chain for relaxation given
/// a local space bone chain.
///***************************************************************************************
public static void CalcIK_2D_ConstraintRelaxation_ConvertToWorld
(
out List <Bone_2D_ConstraintRelaxation_World> worldBones, // Output world space bones
List <Bone_2D_ConstraintRelaxation> localBones // Input local space bones
)
{
int numBones = localBones.Count;
//Debug.Assert (numBones > 0);
// create the list to output
worldBones = new List <Bone_2D_ConstraintRelaxation_World> ();
// Start with the root bone.
Bone_2D_ConstraintRelaxation_World rootWorldBone
= new Bone_2D_ConstraintRelaxation_World();
rootWorldBone.x = 0;
rootWorldBone.y = 0;
rootWorldBone.length = localBones [0].length;
rootWorldBone.weight = localBones [0].weight;
worldBones.Add(rootWorldBone);
double prevAngle = localBones [0].angle;
double prevAngleCos = Math.Cos(prevAngle);
double prevAngleSin = Math.Sin(prevAngle);
// Convert child bones to world space.
for (int boneIdx = 1; boneIdx < numBones; ++boneIdx)
{
Bone_2D_ConstraintRelaxation_World prevWorldBone = worldBones [boneIdx - 1];
Bone_2D_ConstraintRelaxation prevLocalBone = localBones [boneIdx - 1];
Bone_2D_ConstraintRelaxation_World newWorldBone
= new Bone_2D_ConstraintRelaxation_World();
newWorldBone.x = prevWorldBone.x + prevAngleCos * prevLocalBone.length;
newWorldBone.y = prevWorldBone.y + prevAngleSin * prevLocalBone.length;
newWorldBone.length = localBones [boneIdx].length;
newWorldBone.weight = localBones [boneIdx].weight;
worldBones.Add(newWorldBone);
prevAngle = prevAngle + localBones [boneIdx].angle;
prevAngleCos = Math.Cos(prevAngle);
prevAngleSin = Math.Sin(prevAngle);
}
}
///***************************************************************************************
/// CalcIK_2D_ConstraintRelaxation
/// Given a bone chain located at the origin, this function will perform a single
/// relaxation iteration. This finds a solution of bone angles that places the final bone
/// in the given chain at a target position. The supplied bone angles are used to prime
/// the iteration. If a valid solution does not exist, the angles will move as close to
/// the target as possible. The user should resupply the updated angles until a valid
/// solution is found (or until an iteration limit is met).
///***************************************************************************************
public static void CalcIK_2D_ConstraintRelaxation
(
ref List <Bone_2D_ConstraintRelaxation_World> bones, // Bone values to update
double targetX, // Target x position for the end effector
double targetY // Target y position for the end effector
)
{
// Set an epsilon value to prevent division by small numbers.
const double epsilon = 0.0001;
int numBones = bones.Count;
//Debug.Assert (numBones > 0);
//===
// Constrain the end bone to the target.
{
int boneIdx = numBones - 1;
double toTargetX = targetX - bones [boneIdx].x;
double toTargetY = targetY - bones [boneIdx].y;
double toTargetLenSqr = toTargetX * toTargetX + toTargetY * toTargetY;
if (toTargetLenSqr > epsilon)
{
double toTargetLen = Math.Sqrt(toTargetLenSqr);
double toTargetScale = (bones [boneIdx].length / toTargetLen) - 1.0;
bones [boneIdx].x -= toTargetScale * toTargetX;
bones [boneIdx].y -= toTargetScale * toTargetY;
}
}
//===
// Perform relaxation on the bones in a loop from the final bone to the first child
// bone.
for (int boneIdx = numBones - 2; boneIdx >= 1; --boneIdx)
{
Bone_2D_ConstraintRelaxation_World curBone = bones [boneIdx];
Bone_2D_ConstraintRelaxation_World childBone = bones [boneIdx + 1];
// Get the vector from the current bone to the child bone.
double toChildX = childBone.x - curBone.x;
double toChildY = childBone.y - curBone.y;
double toChildLenSqr = toChildX * toChildX + toChildY * toChildY;
double totalWeight = curBone.weight + childBone.weight;
if (toChildLenSqr > epsilon && totalWeight > epsilon)
{
double toChildLen = Math.Sqrt(toChildLenSqr);
double toChildScale =
((bones [boneIdx].length / toChildLen) - 1.0) / totalWeight;
double curBoneScale = toChildScale * curBone.weight;
double childBoneScale = toChildScale * childBone.weight;
curBone.x -= curBoneScale * toChildX;
curBone.y -= curBoneScale * toChildY;
childBone.x += childBoneScale * toChildX;
childBone.y += childBoneScale * toChildY;
}
}
//===
// Constrain the first child joint to the root joint
if (numBones > 1)
{
int boneIdx = 0;
// Get the vector from the current bone to the child bone.
double toChildX = bones [boneIdx + 1].x - bones [boneIdx].x;
double toChildY = bones [boneIdx + 1].y - bones [boneIdx].y;
double toChildLenSqr = toChildX * toChildX + toChildY * toChildY;
if (toChildLenSqr > epsilon)
{
double toChildLen = Math.Sqrt(toChildLenSqr);
double toChildScale = (bones [boneIdx].length / toChildLen) - 1.0;
bones [boneIdx + 1].x += toChildScale * toChildX;
bones [boneIdx + 1].y += toChildScale * toChildY;
}
}
}
///***************************************************************************************
/// CalcIK_2D_ConstraintRelaxation_ConvertToWorld
/// This is a helper function to generate a world space bone chain for relaxation given
/// a local space bone chain.
///***************************************************************************************
public static void CalcIK_2D_ConstraintRelaxation_ConvertToWorld(
out List<Bone_2D_ConstraintRelaxation_World> worldBones, // Output world space bones
List<Bone_2D_ConstraintRelaxation> localBones // Input local space bones
)
{
int numBones = localBones.Count;
//Debug.Assert (numBones > 0);
// create the list to output
worldBones = new List<Bone_2D_ConstraintRelaxation_World> ();
// Start with the root bone.
Bone_2D_ConstraintRelaxation_World rootWorldBone
= new Bone_2D_ConstraintRelaxation_World ();
rootWorldBone.x = 0;
rootWorldBone.y = 0;
rootWorldBone.length = localBones [0].length;
rootWorldBone.weight = localBones [0].weight;
worldBones.Add (rootWorldBone);
double prevAngle = localBones [0].angle;
double prevAngleCos = Math.Cos (prevAngle);
double prevAngleSin = Math.Sin (prevAngle);
// Convert child bones to world space.
for (int boneIdx = 1; boneIdx < numBones; ++boneIdx) {
Bone_2D_ConstraintRelaxation_World prevWorldBone = worldBones [boneIdx - 1];
Bone_2D_ConstraintRelaxation prevLocalBone = localBones [boneIdx - 1];
Bone_2D_ConstraintRelaxation_World newWorldBone
= new Bone_2D_ConstraintRelaxation_World ();
newWorldBone.x = prevWorldBone.x + prevAngleCos * prevLocalBone.length;
newWorldBone.y = prevWorldBone.y + prevAngleSin * prevLocalBone.length;
newWorldBone.length = localBones [boneIdx].length;
newWorldBone.weight = localBones [boneIdx].weight;
worldBones.Add (newWorldBone);
prevAngle = prevAngle + localBones [boneIdx].angle;
prevAngleCos = Math.Cos (prevAngle);
prevAngleSin = Math.Sin (prevAngle);
}
}
