Rigid Body Dynamics, part 2

My rigid-body dynamics program now handles discs in addition to boxes. Since there is still no friction, there is no way yet to affect a disc's spin.



Boxes and discs share much in common: they have mass, rotational inertia, momentum, position, orientation, color, and elasticity. They differ only in their geometry, which affects how they are drawn and how they collide.

I created a simple class hierarchy to represent boxes and discs. Both of them derive from a class called Body which contains all of the common state. The Box class adds width and height, while the Disc class has a radius instead. Here is how the classes stand now:

class Box;
class Disc;

class Body
{
public:

    virtual void draw() const = 0;

    virtual void integrate(float dt);

    virtual void compute_contacts(Body &, Contacts &) = 0;
    virtual void compute_contacts(Box &,  Contacts &) = 0;
    virtual void compute_contacts(Disc &, Contacts &) = 0;

    vec2 pt_velocity(vec2 p) const;

    vec2 pos; // position
    vec2 vel; // linear velocity
    float angle; // heading
    float angle_vel; // angular velocity
    float inv_mass; // inverse mass
    float inv_rot; // inverse of rotational inertia tensor
    float life; // remaining lifetime, in seconds
    float bounciness; // 0 to 1
    vec3 color;
};

class Box : public Body
{
public:
    virtual void draw() const;

    virtual void integrate(float dt);

    virtual void compute_contacts(Body &, Contacts &);
    virtual void compute_contacts(Box &,  Contacts &);
    virtual void compute_contacts(Disc &, Contacts &);

    vec2 radius; // half width, height in local space
    vec2 x_axis; // computed, cached axes
    vec2 y_axis;
};

class Disc : public Body
{
public:
    virtual void draw() const;

    virtual void compute_contacts(Body &, Contacts &);
    virtual void compute_contacts(Box &,  Contacts &);
    virtual void compute_contacts(Disc &, Contacts &);

    float radius;
};

The draw method is overridden by Box and Disc to draw the appropriate geometry. integrate is implemented at the Body level since it is the same for both kinds of bodies. The Box version call's Body's, then recalculates its axes based on the new box orientation. (The axes are used a bunch during contact determination, so it's good to have them computed only once per frame.)

The first step of collision handling is to collect a list of contacts between pairs of objects. (Last week, I handled each collision as I found it, but I'm trying to move toward the creation of forces to handle resting contact, so I need the whole set of contacts at once.)

To compute contacts, the program has to consider pairs of objects and decide:

• If they are touching
• Where they are touching
• What the surface normal is at the contact
• How deeply they are interpenetrating

As in the tutorials, I have a Contact structure that contains this information:

struct Contact
{
    Body * a;
    Body * b;
    vec2 pos; // world-space contact position
    vec2 normal; // world-space contact normal (points away from body a)
    float depth; // interpenetration distance (positive when interpenetrating)
};

The central loop that compares each pair of bodies does not know whether a particular body is a box or a disc. I use a two-stage virtual method dispatch to resolve the types of the two bodies. Here's the top-level loop (at the moment I am not doing any culling by bounding-box or otherwise, so all pairs are considered):

static void find_contacts(const Bodies & bodies, Contacts & contacts)
{
    contacts.clear();

    Bodies::const_iterator body_end = bodies.end();
    Bodies::const_iterator body1 = bodies.begin();
    for (; body1 != body_end; ++body1)
    {
        Bodies::const_iterator body2 = body1;
        for (++body2; body2 != body_end; ++body2)
        {
            if ((*body1)->inv_mass == 0 && (*body1)->inv_rot == 0 &&
                (*body2)->inv_mass == 0 && (*body2)->inv_rot == 0)
                continue;

            (*body1)->compute_contacts(**body2, contacts);
        }
    }
}

This method is overridden for boxes and discs, so the type of body1 is established. These methods then call the appropriate methods on body2, passing the first object as an argument (but with known type now). Here's the code:

void Box::compute_contacts(Body & body, Contacts & contacts)
{
    body.compute_contacts(*this, contacts);
}

void Box::compute_contacts(Box & box, Contacts & contacts)
{
    box_box_contacts(*this, box, contacts);
}

void Box::compute_contacts(Disc & disc, Contacts & contacts)
{
    disc_box_contacts(disc, *this, contacts);
}

void Disc::compute_contacts(Body & body, Contacts & contacts)
{
    body.compute_contacts(*this, contacts);
}

void Disc::compute_contacts(Box & box, Contacts & contacts)
{
    disc_box_contacts(*this, box, contacts);
}

void Disc::compute_contacts(Disc & disc, Contacts & contacts)
{
    disc_disc_contacts(*this, disc, contacts);
}

Once both body types are known, the correct function for computing contacts can be called. It's one of these three:

void box_box_contacts(Box & box1, Box & box2, Contacts &);
void disc_box_contacts(Disc & disc, Box & box, Contacts &);
void disc_disc_contacts(Disc & disc1, Disc & disc2, Contacts &);

As you can see, this is sort of a hack to get the functionality of multimethods, which most object-oriented languages don't support. With multimethods, we could just write four functions that took all the permutations of box and disc

The mechanics of detecting contacts between objects is dealt with to some extent in the tutorials, although they don't cover round objects. Round objects are pretty straightforward to add, although I had to work at it a bit. Some time I will write an entry about sphere vs. convex polyhedron collision; it's an interesting problem.

My contact handling hasn't changed from last week. I start by moving the two objects apart so they don't interpenetrate; then apply impulses to them (if they are moving into each other) so they aren't moving toward each other.

This coming week I hope to look into friction and/or resting contact.