# Game Creation with XNA/Mathematics Physics/Inverse Kinematics

## Inverse Kinematics

Inverse Kinematics (IK) is related to skeletal animation. Examples are the motion of a robotic arm or the motion of animated characters. Inverse Kinematics for Humanoid Skeletons Tutorial and Inverse kinematics on Wikipedia.

An example could be the simulation of a robotic arm with the XNA framework. This chapter should worry more about the mathematical background, whereas the chapter Character Animation will deal more with the models coming from 3D modellers.

If you want to move an arm of robotics or animated characters to a certain direction this entity is mostly modeled as a rigid multibody system consisting of a set of rigid objects which are called links. These links are connected by joints. To control the movement of this rigid multibody and get it into the destined direction inverse kinematics is often used.

The goal of inverse kinematics is to place each joint at its target. For that the right settings for the joint angles need to be found. The angles are represented by a vector .

Inverse kinematics is very challenging since there may be several possible solutions for the angle or none. In case of a solution complex and expensive computations could be required to find it . Many different approaches for solving that problem exist:

• Jacobian transpose method
• Pseudoinverse method
• Damped Least Squares (DLS)
• Selectively Damped Least Square (SDLS)
• Cyclic Coordinate Descent

It is a big effort to implement the Jacobian based methods because they require enormous mathematical knowledge and many prerequisites like classes for matrices with m columns and n rows or singular value decomposition. An Example for implementation can be found here. It was created by Samuel R. Buss and Jin-Su Kim.

All methods mentioned above except the Cyclic Coordinate Descent are based upon the Jacobian matrix which is a function of the joint angle values and used to determine the end position. They discuss the question of how to choose the angle. The values of the angles need to be altered until a value approximately equal to the target value is reached.

Updating the values of the joint angles can be used in two ways:

1) Each step perform a single update of the angle values (using equation) so that joint follows the target position.
2) The angles are updated iteratively until it is close to a solution 

The Jacobian can only be used as an approximation near a position. The process of calculating the Jacobian must therefore be repeated in small steps until the desired end position is reached.

Pseudo Code:

```while (e is too far from g) {

Compute J(e,Φ) for the current pose Φ

Compute J-1	                        // invert the Jacobian matrix

Δe = β(g - e)		// pick approximate step to take

ΔΦ = J-1 • Δe	// compute change in joint DOFs

Φ = Φ + ΔΦ	// apply change to DOFs

Compute new e vector	// apply forward kinematics to see where we ended up

}
```

The following methods deal with the issue of choosing the appropriate angle value.

### Jacobian transpose method

The idea of the Jacobian transpose method is to update the angles by equation using the transpose instead of the inverse or pseudoinverse (since an inversion is not always possible) . With this method the change to an angle can be computed directly by looping through it. It avoids expensive inversion and singularity problems but converges towards a solution very slowly. The motion of this method closely matches the physics unlike other inverse kinematics solutions which can result in an unnatural motion .

### Pseudoinverse method

This method sets the angle values to the pseudoinverse of the Jacobian. It tries to find a matrix which effectively inverts a non-square matrix. It has singularity issues which tend to the fact that certain directions are not reachable. The problem is that the method first loops through all angles and then needs to compute and store the Jacobian, pseudoinvert it, calculate the changes in the angle and last apply the changes .

### Damped Least Squares (DLS)

This method avoids certain problems of the pseudoinverse method. It finds the value of the angle that minimizes the quantity rather than just the one finding the minimum vector. The damping constant must be chosen carefully to make the equation stable .

### Selectively Damped Least Square (SDLS)

This method is a refinement of the DLS method and needs fewer iterations.

### Cyclic Coordinate Descent

The algorithms based on the inverse Jacobian Matrix are sometimes unstable and fail to converge. Therefore another method exists. The Cyclic Coordinate Descent adjusts one joint angle at a time. It starts at the last link in the chain and works backwards iteratively through all of the adjustable angles until the desired position is reached or the loop has repeated a set number of times. The algorithm uses two vectors to determine the angle in order to rotate the model to the desired spot. This is solved by the inverse cosine of the dot product. Additionally, to define the rotation direction the cross product is used . A concept demonstration of the method can be watched here

Here is a sample implementation:

First we need an object that represents a joint.

```using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace InverseKinematics
{
/// <summary>
/// Represents a chain link of the class BoneChain
/// </summary>
public class Bone
{
/// <summary>
/// the bone's appearance
/// </summary>
private Cuboid cuboid;

/// <summary>
/// the bone's last calculated angle if errors occure like not a number
/// this will be used instead
/// </summary>
public float lastAngle = 0;

private Vector3 worldCoordinate, destination;

/// <summary>
/// where the bone does point at
/// </summary>
public Vector3 Destination
{
get { return destination; }
set { destination = value; }
}

/// <summary>
/// the bone's source position
/// </summary>
public Vector3 WorldCoordinate
{
get { return worldCoordinate; }
set { worldCoordinate = value; }
}

/// <summary>
/// Generates a bone by another bone's end
/// </summary>
/// <param name="lastBone">the bone's end for this bone's source</param>
/// <param name="destination"></param>
public Bone(Bone lastBone, Vector3 destination) : this(lastBone.Effector, destination)
{
}

/// <summary>
/// Generates a bone at a coordinate in
/// </summary>
/// <param name="worldCoordinate"></param>
/// <param name="destination"></param>
public Bone(Vector3 worldCoordinate, Vector3 destination)
{
cuboid = new Cuboid();
this.worldCoordinate = worldCoordinate;
this.destination = destination;
}
```

These are the fields and constructors which we need for our bone class. The field cuboid is the 3D model which represents our bone. The destination and worldCoordinate describe the joints. The worldCoordinate shows the position of the bone. The destination is the targeted position. The first constructor contains the settings for both vectors. The second constructor takes the world position and the target position (also called end effector) and generates a new world position for the new bone from them.

```        /// <summary>
/// calculate's the bone's appearance appropiate to its world position
/// and its destination
/// </summary>
public void Update()
{

Vector3 direction = new Vector3(destination.Length() / 2, 0, 0);

cuboid.Scale(new Vector3(destination.Length() / 2, 5f, 5f));
cuboid.Translate(direction);

cuboid.Rotate(SphereCoordinateOrientation(destination));
cuboid.Translate(worldCoordinate);

cuboid.Update();
}
```

The update method scales the cuboid with the length of the destination vector with the width of 5 and depth of 5. It translates the cuboid by its half length to get the rotation pivot and rotates it by the sphere coordinate angles of the destination vector and translates it to its world coordinate.

```        /// <summary>
/// Draws the bone's appearance
/// </summary>
/// <param name="device">the device to draw the bone's appearance</param>
public void Draw(GraphicsDevice device)
{
cuboid.Draw(device);
}
```

The draw method draws the updated vector.

```        /// <summary>
/// generates the bone's rotation by unsing sphere coordinates
/// </summary>
/// <param name="position"></param>
/// <returns></returns>
private Vector3 SphereCoordinateOrientation(Vector3 position)
{
float alpha = 0;
float beta = 0;
if (position.Z != 0.0 || position.X != 0.0)
alpha = (float)Math.Atan2(position.Z, position.X);

if (position.Y != 0.0)
beta = (float)Math.Atan2(position.Y, Math.Sqrt(position.X * position.X + position.Z * position.Z));

return new Vector3(0, -alpha, beta);
}
```
```        /// <summary>
/// the bone's destination is local and points to the world's destination
/// so this function just subtract's the bone's world coordinate from the world's destination
/// and gets the bone's local destination vector
/// </summary>
/// <param name="destination">The destination in the world coordinate system</param>
public void SetLocalDestinationbyAWorldDestination(Vector3 destination)
{
this.destination = destination - worldCoordinate;
}

/// <summary>
/// the bone's source plus the bone's destination vector
/// </summary>
/// <returns></returns>
public Vector3 Effector
{
get
{
return worldCoordinate + destination;
}
}
}
}
```

The rest of the bone class is getters and setters.

```using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace InverseKinematics
{
/// <summary>
/// The BoneChain class repressents a list of bones which are always connected once.
/// On the one hand you can add new bones and every bone's source is the last bone's destination
/// on the other hand you can use the cyclic coordinate descent to change the bones' positions.
/// </summary>
public class BoneChain
{
/// <summary>
/// The last bone that were created
/// </summary>
private Bone lastBone;

/// <summary>
/// All the concatenated bones
/// </summary>
private List<Bone> bones;

/// <summary>
/// Creates an empty bone chain
/// Added Bones will be affected by inverse kinematics
/// </summary>
public BoneChain()
{
this.bones = new List<Bone>();
}
```

The BoneChain class repressents a list of bones which are always connected once. On the one hand you can add new bones and every bone's source is the last bone's destination on the other hand you can use the cyclic coordinate descent to change the bones' positions. The class works with a list which contains the bones their coordinates. The class has two modes. The first is the creation mode where one bone is created after another and they keep connected. The other mode is the CCD (described further below).

```        /// <summary>
/// Draws all the bones in this chain
/// </summary>
/// <param name="device"></param>
public void Draw(GraphicsDevice device)
{
foreach (Bone bone in bones) bone.Draw(device);
}
```

```        /// <summary>
/// Creates a bone
/// Every bone's destination is the next bone's source
/// </summary>
/// <param name="v">the bone's destination</param>
/// <param name="click">if true it sets the bone with its coordinate and adds the next bone</param>
public void CreateBone(Vector3 v, bool click)
{
if (click)
{
//if it is the first bone it will create the bone's source at the destination point
//so it need not to start at the coordinates(0/0/0)
if (bones.Count == 0)
{
lastBone = new Bone(v, Vector3.Zero);
}
else
{
Bone temp = new Bone(lastBone, v);
lastBone = temp;
}
}
if (lastBone != null)
{
lastBone.SetLocalDestinationbyAWorldDestination(v);
}

}
```

This is the method for creating the bones (creation mode)

```        /// <summary>
/// The Cyclic Coordinate Descent
/// </summary>
/// <param name="destination">Where the bones should be adjusted</param>
/// <param name="gameTime"></param>
public void CalculateCCD(Vector3 destination, GameTime gameTime)
{

// iterating the bones reverse
int index = bones.Count - 1;
while (index >= 0)
{
//getting the vector between the new destination and the joint's world position
Vector3 jointWorldPositionToDestination = destination - bones.ElementAt(index).WorldCoordinate;

//getting the vector between the end effector and the joint's world position
Vector3 boneWorldToEndEffector = bones.Last().Effector - bones.ElementAt(index).WorldCoordinate;

//calculate the rotation axis which is the cross product of the destination
Vector3 cross = Vector3.Cross(jointWorldPositionToDestination, boneWorldToEndEffector);

//normalizing that rotation axis
cross.Normalize();
//check if there occured divisions by 0
if (float.IsNaN(cross.X) || float.IsNaN(cross.Y) || float.IsNaN(cross.Z))
//take a temporary vector
cross = Vector3.UnitZ;

// calculate the angle between jointWorldPositionToDestination and boneWorldToEndEffector
// in regard of the rotation axis
float angle = CalculateAngle(jointWorldPositionToDestination, boneWorldToEndEffector, cross);
if (float.IsNaN(angle)) angle = 0;

//create a matrix for the roation of this bone's destination
Matrix m = Matrix.CreateFromAxisAngle(cross, angle);

// rotate the destination
bones.ElementAt(index).Destination = Vector3.Transform(bones.ElementAt(index).Destination, m);

// update all bones which are affected by this bone
UpdateBones(index);
index--;
}
}
```

This is one possible version of the CCD Algorithm.

```        /// <summary>
/// While CalculateCCD changes the destinations of all the bones,
/// every affected adjacent bone's WorldCoordinate must be updated to keep the bone chain together.
/// </summary>
/// <param name="index">when the bones should updated, because CalculateCCD changed their destinations</param>
private void UpdateBones(int index)
{
for (int j = index; j < bones.Count - 1; j++)
{
bones.ElementAt(j + 1).WorldCoordinate = (bones.ElementAt(j).Effector);
}
}

/// <summary>
/// Updates all the representation parameters for every bone
/// including orienations and positionsin this bonechain
/// </summary>
public void Update()
{
foreach (Bone bone in bones) bone.Update();
}

/// <summary>
/// This function calculates an angle between two vectors
/// the cross product which is orthogonal to the two vectors is the most common orientation vector
/// for specifing the angle's direction.
/// </summary>
/// <param name="v0">the first vector </param>
/// <param name="v1">the second vector </param>
/// <param name="crossProductOfV0andV1">the cross product of the first and second vector </param>
/// <returns>the angle between the two vectors in radians</returns>
private float CalculateAngle(Vector3 v0, Vector3 v1, Vector3 crossProductOfV0andV1)
{
Vector3 n0 = Vector3.Normalize(v0);
Vector3 n1 = Vector3.Normalize(v1);
Vector3 NCross = Vector3.Cross(n1, n0);
NCross.Normalize();
float NDot = Vector3.Dot(n0, n1);
if (float.IsNaN(NDot)) NDot = 0;
if (NDot > 1) NDot = 1;
if (NDot < -1) NDot = -1;
float a = (float)Math.Acos(NDot);
if ((n0 + n1).Length() < 0.01f) return (float)Math.PI;
return Vector3.Dot(NCross, crossProductOfV0andV1) >= 0 ? a : -a;
}

}
}
```