How to simulate bones and joints

My custom physics engine had collisions working great with velocities and gravities and such, however I needed a way to give "bones" and "joints" to creatures.

At the moment my creatures are composed of spheres. Here is an example of a "spider":

 

Each line you see is a bone, which you can think of as a rod between those two spheres. I can mark a bone as "usable" by the creature, which allows the creature to vary the length of that rod over time. Otherwise they are a fixed length.

I can also add a "joint" to any sphere, which restricts the ways in which two different bones attached to that sphere interact with each other. The intent here is to model simplified versions of joints that are occurring in real life biology: ball joints, hinge joints, saddle joints, etc.

Now, simulating bones and joints is hard, mostly because if you don't do it right they can explode under harsh conditions. Basically what happens is that if you run your simulation one step, then try and "correct" for where bones want their attached spheres' to be, if they are too far off the correction will be too harsh and the next frame more correction will be needed and this gets worse and worse until eventually your objects are accidentally at infinity. Sometimes it is not that severe, but you can still get "stuttering" if not programmed right which would be nice to avoid.

After about a week of trying various things, I found a simple method that works fairly well for simulating bones, collisions, and any kind of joints that I think is worth sharing here.

First, we need to talk about Vector Projections and Vector rejections. These are very general concepts that make a large amount of physics become very simple.

Graphic is from Wikipedia

In this picture, our two vectors are $a$ and $b$. The projection of $a$ onto $b$ is the part of $a$ that goes along $b$, which in this case is $a_1$. The rejection of $a$ from $b$ is the part of $a$ that is perpendicular to $b$ which is $a_2$.

Luckily, these are very easy to compute in any dimension. The projection is simply:

$$ a \text{ project onto } b = b (a \circ \frac{b}{\vert b \vert}) $$

Where $ \frac{b}{\vert b \vert}$ is simply the normalized vector in the direction of $b$ (with magnitude of 1), and $\circ$ is the dot product.

Because the projection plus the rejection is the original vector, to find the rejection we can simply do:

$$ a \text{ reject from } b = a - (a \text{ project onto } b)$$

To see an application of these, imagine that we have a ball that ran into the floor with a diagonal velocity:

The arrow pointing down is the velocity, the arrow pointing up is the normal. The normal is just a fancy word for the vector that is pointing away from the surface that you hit. One way to think of a normal is that if you accidentally went in the ground, that is the direction that you would have to be pushed in to get back out of the ground.

So if we know our ball is about to hit the ground, we don't want its velocity to keep pushing it into the ground. Instead, we have two choices: bounce, in which case the y component of the velocity is flipped, or don't bounce: in which case the y component of the velocity is removed and the ball rolls to the side.

With a surface that is flat this is easy to do. However lets say you hit a floor that is tilted. How do you find the new velocity?

The answer is very simple: use the vector rejection. Specifically, given a normal $n$ (we assume is normalized) and a velocity $v$, we first compute $n \circ v$. If this value is negative, $v$ isn't even going towards the surface so we can just return $v$. If this value is positive, we can simply return 

$$ v \text{ reject from } n$$

if we don't want to bounce, or

$$ v \text{ reject from } n - v \text{ project to } n$$

if we do want to bounce.

The reason this works is because the vector rejection gives us the component of our velocity that is not along the normal, which is the only part that is left if we don't bounce. If we do bounce, we want to add the negated component of the velocity that is going along the normal (the projection).

So that's pretty great that you have such a clean formula for any collision.

Now we want to simulate a bone. To do this, I first run my physics simulation for one tick assuming the bone doesn't exist, then for every bone (and joint - this technique is very general) I do the following four steps:

*Edit: Turns out this doesn't quite work as intended - it is not energy conserving, and as a result things are able to "fly" by manipulating the added energy. While cool, this isn't what I intended and removes quite a bit of cool behavior cause the strategy is usually "fly to the goal".

Because that didn't work, this is a new method that is working great for simulating bones. I haven't been able to figure out joints yet, but this is a start and you can still do quite a bit with bones.

1. Each bone has a "desired distance". If the bones are close enough to that desired distance apart (I use 0.05 or less), do nothing. Otherwise, if the actual distance between the two spheres connected by the bone is too big, pretend that they hit a surface outside the rod that is pushing them in. If it is too small, pretend that they hit a surface inside the rod that is pushing them out.

2. Use the non bouncing collision algorithm above with those normals.

3. The non bouncing collision algorithm might have caused some velocity to be lost. Compute the lost velocity of each sphere (simply by taking original velocity - new velocity), then average these lost velocities together. Now each sphere's actual resulting velocity is new velocity + average of lost velocities.

This works great with fixed bone sizes, assuming the spheres start at the desired distance apart. However, we would like the creature to be able to change a few bone's sizes to act as muscles instead of bones. The problem is that 1-3 doesn't add velocity to correct for this changed size. For example, if both spheres are sitting on the ground, they don't have any velocity so the collision algorithm doesn't do anything to pull them together. To fix this, we can do:

4. Use a variable named "prevDistance". Initially this is set to the desired distance. Now, if the desired distance is changed, prevDistance no longer equals the desired distance. If they aren't equal, add a velocity of desired distance - prevDistance to each sphere, pointed towards away from each other (if desired distance - prevDistance is negative this will add a velocity that is pointing towards each other). Now, set prevDistance to the current distance between spheres.

What this does is continue to add velocity until the spheres reach their new desired position. Once that happens they run as normal, using only steps 1-3, because prevDistance is approximately equal to desired distance.

In code (C#) we have:

public static Vector3 VectorProjection(Vector3 a, Vector3 b)
{
    return b*Vector3.Dot(a, b.normalized);
}

public static Vector3 VectorRejection(Vector3 a, Vector3 b)
{
    return a - VectorProjection(a, b);
}

public static Vector3 VectorAfterNormalForce(Vector3 force, Vector3 normal)
{
    Vector3 rejection = VectorRejection(force, normal);
    Vector3 projection = VectorProjection(force, normal);
    float pointingInSameDirection = Vector3.Dot(normal, force);
    // Not pointing in same direction
    if (pointingInSameDirection <= 0.0f)
    {
        return rejection;
    }
    // Pointing in same direction
    else
    {
        return rejection + projection;
    }
}

public static void ApplyBone(Bone bone)
{
    float curDist = Vector3.Distance(bone.me.pos, bone.other.pos);

    if (Math.Abs(bone.desiredDist - curDist) >= 0.05f)
    {
        Vector3 collisionNormal = ((curDist - bone.desiredDist) / Math.Abs(bone.desiredDist - curDist)) * (bone.me.pos - bone.other.pos).normalized;

        Vector3 myNotAlongNormal = VectorAfterNormalForce(bone.me.velocity, -collisionNormal);
        Vector3 otherNotAlongNormal = VectorAfterNormalForce(bone.other.velocity, collisionNormal);

        Vector3 myAlongNormal = bone.me.velocity - myNotAlongNormal;
        Vector3 otherAlongNormal = bone.other.velocity - otherNotAlongNormal;

        Vector3 averageAlongNormal = (myAlongNormal + otherAlongNormal) / 2.0f;

        bone.me.velocity = myNotAlongNormal + averageAlongNormal;
        bone.other.velocity = otherNotAlongNormal + averageAlongNormal;

        Vector3 normalPointingAway = -(bone.other.pos - bone.me.pos).normalized;

        if (bone.prevDistance != bone.desiredDist )
        {
            bone.other.velocity += -normalPointingAway* (bone.desiredDist - bone.prevDistance);
            bone.me.velocity += normalPointingAway* (bone.desiredDist - bone.prevDistance);
        }
        bone.prevDistance = curDist;
    }
}