Rigid Body Physics in C#

jwatte's picture

Let's assume you're trying to simulate a car driving on an uneven terrain (perhaps some sort of heightmap). Let's assume you know how to measure the distance from the car's chassis to the heightmap ground at any position (typically, measure the height of the heightmap, and subtract the height of the car at that point in the XZ plane). The car has four wheels at four different positions on the chassis; these wheels are generally in contact with the ground.

Each contact is a form of physical spring, which affects the car with a force depending on how far the contact is from the chassis. At some length, the force is 0; the wheel lets go of the ground. At some point, the force is 1/4 the weight of the car times gravity. This is the equilibrium point.

Each spring can be treated as attached to the corners of a box. Each spring, each simulation step, applies force to a simulated rigid body. The trick is that the force is off-center, so it applies rotational torque to the body.

In general, you can represent a rigid body as:

  public class RigidBody {
    public Vector3 Position;
    public Quaternion Orientation;
    public Vector3 LinearVelocity;
    public Vector3 AngularVelocity;
    public Vector3 Force;
    public Vector3 Torque;
    public float Mass;
    public void Integrate(float dt) {
      LinearVelocity += Force / Mass * dt;
      Force = Vector3.Zero;
      AngularVelocity += Torque / Mass * dt;
      Torque = Vector3.Zero;
      Position += LinearVelocity * dt;
      Orientation += new Quaternion((AngularVelocity * dt), 0) * Orientation;

This assumes that the mass is evenly distributed (like a sphere) in world space; else you'll have to build an inertia tensor for your mass distribution and apply that to the places that now just divide by Mass.

Now, when you apply an impulse at a (world-space) position to a body, it will apply both force and torque. If I remember the math correctly, the function looks like:

    public void ApplyForce(Vector3 forcePosition, Vector3 directionMagnitude) {
      float lengthSquared = directionMagnitude.LengthSquared();
      if (lengthSquared < 1e-8f) return; // or use your own favorite epsilon
      Force += directionMagnitude;
      Vector3 distance = forcePosition - Position;
      Torque += Vector3.Cross(directionMagnitude, distance);

So, each step, you will run the raycast to detect distance to ground, turn that into a spring force, apply those forces at the positions of the wheels to the body, and then call Integrate() to update position/orientation of the body. Note that tuning physical simulation systems (spring strength, masses, etc) is notoriously tricky and will probably take many tries. You also usually want to dampen the springs; in general by adding a counter-force based on the velocity of each of the points in world space. Velocity of a given point is, again if I remember my math right, something like the following:

    public Vector3 PointVelocity(Vector3 worldPoint) {
      Vector3 distance = worldPoint - Position;
      Vector3 vel = Vector3.Cross(AngularVelocity, distance) + LinearVelocity;
      return vel;

The actual rigid body simulation is pretty simple: each update step, add forces/impulses based on the wheel springs (and any collision detection constraints, etc), then integrate the body for the duration of the time step (typically 1/60th of a second).

However, for a real physics system, you will start doing collision detection, so that the car can't run into walls, or a very hard landing can't push the car underground, etc. At that point, you'll need collision proxies for the shape of the car (typically, a box), and for the ground (a heightmap or mesh). You'll also formulate the collision constraints using some kind of joint, or contact constraint. When you start doing that to a number of movable bodies (boxes, traffic cones, and whatnot) you will come to situations where a number of objects are in contact with each other, and integrating each body in isolation from the others won't be very realistic. At that point, you need a system that solves all the constraints at the same time. Typically, this is formulated as a linear constraint problem solver, also known as a "big matrix" solver.

Collision detection, and constraint solution, are actually the hard parts of physical simulation. Thus, I recommend you get a good reference book if you want to go down that path. Or just download an existing engine, and use that; someone else has already done the work of writing and debugging all that gnarly code :-)

CSharpRigidBody.zip1.55 KB