|
Page 6 of 6
Writing the Code
We can now program a constraint relaxation based IK solver. The code is sourced from the RJ_Demo_IK application that can be downloaded to see a sample implementation. In RJ_Demo_IK, the bones are natively stored as pairs of parent relative angles and lengths. Because of this, the code is broken down into a three step process. An initial function is used to convert from the native bone layout to the world space positions used by the constraint relaxation process. This is followed by a function that performs a single iteration of constraint relaxation. This function is called multiple times depending on the desired number of iterations. A final function is used to convert back to the RJ_Demo_IK native format.
These code samples are released under the following license.
| License |
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
|
/******************************************************************************
Copyright (c) 2008-2009 Ryan Juckett
http://www.ryanjuckett.com/
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
******************************************************************************/
|
When working with two dimensional rotations, it is often useful to wrap angles outside of the ![[-\pi,\pi]](http://tex.ryanjuckett.com/mimetex.cgi?[-\pi,\pi] "[-\pi,\pi]") range back into the range. The constraint relaxation functions make use of this helper function.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
12
13
|
///***************************************************************************************
/// SimplifyAngle
/// This function will convert an angle to the equivalent rotation in the range [-pi,pi]
///***************************************************************************************
private static double SimplifyAngle(double angle)
{
angle = angle % (2.0 * Math.PI);
if( angle < -Math.PI )
angle += (2.0 * Math.PI);
else if( angle > Math.PI )
angle -= (2.0 * Math.PI);
return angle;
}
|
This class is used to represent the local space bones used natively by the application.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
|
///***************************************************************************************
/// Bone_2D_ConstraintRelaxation
/// This class is used to supply the constraint relaxation functions with a bone's
/// representation relative to its parent in the kinematic chain.
///***************************************************************************************
public class Bone_2D_ConstraintRelaxation
{
public double angle; // angle in parent space
public double length; // length of the bone (relaxation constraint)
public double weight; // weight of the bone during relaxation
};
|
This class is used to represent the world space bones used by the constraint relaxation.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
12
|
///***************************************************************************************
/// Bone_2D_ConstraintRelaxation_World
/// This class is used to supply the constraint relaxation functions with a bone's
/// representation in world space.
///***************************************************************************************
public class Bone_2D_ConstraintRelaxation_World
{
public double x; // x position in world space
public double y; // y position in world space
public double length; // length of the bone (relaxation constraint)
public double weight; // weight of the bone during relaxation
};
|
At the start of each update, the application uses this function to convert its local space bones into world space bones that the constraint relaxation algorithm can process.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
|
///***************************************************************************************
/// 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 );
}
}
|
This function is called in a loop to update the world space bones. Each call brings the bones closer to a valid solution.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
///***************************************************************************************
/// 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;
}
}
}
|
After enough iterations have been performed, the following function is used to convert the bones back to the local space format native to the application.
| C# Code |
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
|
///***************************************************************************************
/// CalcIK_2D_ConstraintRelaxation_ConvertToLocal
/// This is a helper function to update a local space bone chain after relaxation has been
/// performed on the corresponding world space bone chain.
///***************************************************************************************
public static void CalcIK_2D_ConstraintRelaxation_ConvertToLocal
(
ref List<Bone_2D_ConstraintRelaxation> localBones, // Update local space bones
List<Bone_2D_ConstraintRelaxation_World> worldBones, // Input world space bones
double targetX, // Target x position for the end effector
double targetY // Target y position for the end effector
)
{
Debug.Assert(localBones.Count == worldBones.Count);
int numBones = localBones.Count;
Debug.Assert(numBones > 0);
// Extract bone angles for all bones but the end bone
double prevWorldAngle = 0.0;
for( int boneIdx = 0; boneIdx < numBones-1; ++boneIdx )
{
double toNextBoneX = worldBones[boneIdx+1].x - worldBones[boneIdx].x;
double toNextBoneY = worldBones[boneIdx+1].y - worldBones[boneIdx].y;
double worldAngle = Math.Atan2(toNextBoneY,toNextBoneX);
double newAngle = SimplifyAngle(worldAngle - prevWorldAngle);
localBones[boneIdx].angle = newAngle;
prevWorldAngle = worldAngle;
}
// Point the end bone towards the target
{
int boneIdx = numBones-1;
double toTargetX = targetX - worldBones[boneIdx].x;
double toTargetY = targetY - worldBones[boneIdx].y;
double worldAngle = Math.Atan2(toTargetY,toTargetX);
double newAngle = SimplifyAngle(worldAngle - prevWorldAngle);
localBones[boneIdx].angle = newAngle;
}
}
|
|
