Roll Your Own Physics Engine 1

Visualization of a rigid body physics simulation of some cubes interacting under the influence of gravity.

So you want to write a physics engine. Nevermind quality open-source offerings such as Bullet or PhysX. If you want absolute control over and a deep understanding of the inner workings of your game's physics, then this article will show you how.

First, some assumptions:

Now, with that out of the way, let's get on to the physics.

Integration

Let's begin by reviewing the equations of unconstrained motion.

Linear Motion

Let us consider the simplest kind of physical object, the point mass, which has no volume and whose state is fully specified by its position \(\vb{p}\) and mass \(m\). Given this description, we can define the velocity \(\vb{v}\) and acceleration \(\vb{a}\) of the point mass as \[\vb{v}=\dv{\vb{p}}{t}\] and \[\vb{a}=\dv{\vb{v}}{t}\]

From Newton's Second Law, we know that a body's acceleration is related to the net force \(\vb{F}_\text{net}\) on the body by the equation \[\vb{F}_\text{net}=m\vb{a}\] But what is a force? For the purposes of this article, let's just say that a force is something that causes a body to accelerate (e.g., due to gravity or magnetism), the sum of the forces on a body is called the net force \(\vb{F}_\text{net}\), and a body's acceleration is related to the net force on the body by the equation above.

Rearranging these equations, we get: \[\vb{a}=m^{-1}\vb{F}_\text{net}\] \[\dd{\vb{v}}=\vb{a}\dd{t}\] \[\dd{\vb{p}}=\vb{v}\dd{t}\] which we will solve numerically to simulate motion.

Angular Motion

The equations used above for point masses can be reused to describe the motion of a rigid body's center of mass. However, because rigid bodies can rotate, we must also keep track of their orientation \(\vb{q}\), angular velocity \(\vb*{\omega}\), angular acceleration \(\vb*{\alpha}\), and the net torque about their center of mass \(\vb*{\tau}_\text{net}\).

Orientation

We could technically use any 3D rotation representation for \(\vb{q}\), but for our engine we will let \(\vb{q}\) be a unit quaternion representing the rotation of the body relative to its default orientation.

Angular Velocity

\(\vb*{\omega}\) is a 3D vector* where \(\norm{\vb*{\omega}}\) is the angular speed of the body's rotation in radians/second and \(\vb*{\omega}\) points along the axis about which the body is rotating:

Angular Acceleration

\(\vb*{\alpha}\) is the time derivative of \(\vb*{\omega}\): \[\vb*{\alpha}=\dv{\vb*{\omega}}{t}\]

Torque

Torque is the angular analog of force and causes angular acceleration. Like force, torque is a 3D vector*, and, like forces, torques add together. If we think of a torque as a twisting force, then \(\norm{\vb*{\tau}_\text{net}}\) is the strength of the total twisting force acting on a body and \(\vb*{\tau}_\text{net}\) points along the axis about which the total twisting force acts.

Although in a simulation we can apply torques directly, in real life torque is a result of force(s). Imagine twisting a bottle cap with just one finger. You apply a force \(\vb{F}\) at position \(\vb{r}\) relative to the center of mass. This imparts a torque \(\vb*{\tau}=\) \(\vb{r}\cross\vb{F}\):

There is an angular version of Newton's Second Law that relates \(\vb*{\tau}_\text{net}\) and \(\vb*{\alpha}\): \[\vb*{\tau}_\text{net} = \vb{I}\vb*{\alpha}\] where \(\vb{I}\), is the inertia tensor, the angular analog of mass.

The Inertia Tensor

The inertia tensor is a symmetric 3-by-3 matrix that determines a body's response to torque. This means that torques of the same magnitude applied along different axes may produce angular accelerations of different magnitudes: It also means that the angular acceleration produced by a torque may not always point along the same direction as the torque itself.

One thing to note about \(\vb{I}\) is that it depends on the body's orientation. For example, imagine holding a heavy rod in your hand and applying a clockwise torque. You will produce a greater angular acceleration for the same torque if the rod is aligned with the torque axis than if the rod is perpendicular to the torque axis:

To calculate \(\vb{I}\) for a given orientation, we must first calculate \(\vb{I}_0\), the inertia tensor of the body in its default orientation (i.e., when \(\vb{q}\) represents the identity rotation). Calculating \(\vb{I}_0\) in general is outside the scope of this article. However, Wikipedia provides a list of inertia tensors for some common shapes. If you use an inertia tensor provided by someone else, be sure that it was calculated relative to the center of mass. In this article I assume that all torques and inertia tensors are calculated relative to the center of mass because it makes the math easier.

Once you have \(\vb{I}_0\), you can calculate \(\vb{I}\) by the following equation: \[\vb{I}=\vb{R}\vb{I_0}\vb{R}^{T}\] where \(\vb{R}\) is the rotation matrix corresponding to \(\vb{q}\).

Equations

The equations we need to simulate angular motion are: \[\vb*{\alpha}=\vb{I}^{-1}\vb*{\tau}_\text{net}\] \[\dd{\vb*{\omega}}=\vb*{\alpha}\dd{t}\] \[\dd{\vb{q}}=\frac{\dd{t}}{2}(0 + \vb*{\omega}_x\vb{i} + \vb*{\omega}_y\vb{j} + \vb*{\omega}_z\vb{k})\vb{q}\] Note the similarity between the first two of these equations and the corresponding equations for linear motion.

Semi-implicit Euler Method

Now that we know the equations that govern motion, we can use numerical methods to solve them. We will use the semi-implicit Euler method because it's simple and it works. The semi-implicit Euler method discretizes the equations of motion given a time step \(h\). Recall the equations of motion: \[\vb{a}=m^{-1}\vb{F}_\text{net}\] \[\dd{\vb{v}}=\vb{a}\dd{t}\] \[\dd{\vb{p}}=\vb{v}\dd{t}\] \[\vb*{\alpha}=\vb{I}^{-1}\vb*{\tau}_\text{net}\] \[\dd{\vb*{\omega}}=\vb*{\alpha}\dd{t}\] \[\dd{\vb{q}}=\frac{\dd{t}}{2}(0 + \vb*{\omega}_x\vb{i} + \vb*{\omega}_y\vb{j} + \vb*{\omega}_z\vb{k})\vb{q}\]

The semi-implicit Euler versions are: \[\vb{a}\leftarrow m^{-1}\vb{F}_\text{net}\] \[\vb{v}\leftarrow\vb{v} + \vb{a}h\] \[\vb{p}\leftarrow\vb{p} + \vb{v}h\] \[\vb*{\alpha}\leftarrow\vb{I}^{-1}\vb*{\tau}_\text{net}\] \[\vb*{\omega}\leftarrow\vb*{\omega} + \vb*{\alpha}h\] \[ \vb{q}\leftarrow\small{\frac {\vb{q}+\frac{h}{2}(0 + \vb*{\omega}_x\vb{i} + \vb*{\omega}_y\vb{j} + \vb*{\omega}_z\vb{k})\vb{q}} {\norm{\vb{q}+\frac{h}{2}(0 + \vb*{\omega}_x\vb{i} + \vb*{\omega}_y\vb{j} + \vb*{\omega}_z\vb{k})\vb{q}}}} \] We normalize \(\vb{q}\) because the discretization causes error.

In C++ that looks like:


void Particle::integrate(float h) {
  // We store the inverse mass because multiplication is faster than division
  // and we almost always want to divide by mass
  auto const a = _inverse_mass * _net_force;
  _velocity += a * h;
  _position += _velocity * h; 
}

void Rigid_body::integrate(float h) {
  auto const a = _inverse_mass * _net_force;
  _velocity += a * h;
  _position += _velocity * h;
  auto const R = Mat3x3f::rotation(_orientation);
  // We also store the inverse of the reference inertia tensor because inverting
  // a matrix is slow.
  auto const I_inv = R * _inverse_inertia_tensor * transpose(R);
  auto const alpha = I_inv * _net_torque;
  _angular_velocity += alpha * h;
  _orientation += 0.5f * h * Quatf{0.0f, _angular_velocity} * _orientation;
  // We normalize because the discrete approximation creates error.
  _orientation = normalize(_orientation);
}

\(h\) does not have to be the same as your game tick. In fact, a game tick in the ballpark of 60hz may be too slow for stable physics, so we will subdivide the time step inside the physics engine. For example, if the game asks the physics engine to simulate 16ms, the physics engine might simulate 16 1ms slices of time. For this reason, I will differentiate the physics step of duration \(\Delta t\) (16ms in this example) from the physics substep of duration \(h\) (1ms in this example).

With our \(\Delta t\) and \(h\) chosen, we can simulate the world with the semi-implicit Euler method: 


void World::simulate(float dt, int substep_count) {
  auto const h = dt / substep_count;
  for (auto i = 0; i < substep_count; ++i) {
    auto const integrate = [&](auto &object) { object.integrate(h); };
    std::ranges::for_each(_particles, integrate);
    std::ranges::for_each(_rigid_bodies, integrate);
  }
  auto const reset_accumulators = [](auto &object) {
    object.reset_accumulators();
  };
  std::ranges::for_each(_particles, reset_accumulators);
  std::ranges::for_each(_rigid_bodies, reset_accumulators);
}

void Particle::reset_accumulators() {
  _net_force = Vec3f::zero();
}

void Rigid_body::reset_accumulators() {
  _net_force = Vec3f::zero();
  _net_torque = Vec3f::zero();
}

To apply a force to a particle over the next simulation step, we simply add to the net force: 


void Particle::apply_force(Vec3f const &f) {
  _net_force += f;
}

To apply a force to the center of mass of a rigid body is the same: 


void Rigid_body::apply_force(Vec3f const &f) {
  _net_force += f;
}

A force applied at a point other than the center of mass will also impart a torque: 


void Rigid_body::apply_force(Vec3f const &f, Vec3f const &r) {
  // r is the vector from the center of mass to the point where the force is
  // applied (in world space)
  _net_force += f;
  apply_torque(cross(r, f));
}

We can also apply a torque directly: 


void Rigid_body::apply_torque(Vec3f const &t) {
  _net_torque += t;
}

A couple notes about the code above:

Rigidity

Now that we have objects in motion, we must enforce rigidity. In other words, we need to constrain the objects' motion in a physically realistic way such that no two objects overlap. If instead we were to leave the engine as described, simulated objects would move through each other without resistance. To enforce rigidity, we take two steps after performing integration:

  1. Collision detection: we detect violations of rigidity (cases where two objects interpenetrate)
  2. Collision response: we enforce rigidity by applying impulses (instantaneous changes in position or velocity)

Collision Detection

Collision detection proceeds in two phases: a broad phase then a narrow phase. Let's look at the narrow phase first.

Narrow Phase

In order to enforce rigidity, we will search for contacts between pairs of objects after integration. Each contact has a position, a normal, and a penetration depth. Let's visualize this:

A diagram of two spheres in contact.

I should also mention that there are libaries like libccd and XenoCollide which can find the position, normal, and penetration depth of a contact between arbitrary convex shapes.

My contact struct looks something like this:


  struct Contact {
    std::array<Vec3f, 2> local_positions;
    Vec3f normal;
    float separation;
  };

As you can see, I store the local position of the contact in each body's coordinate frame rather than the global position. This makes it easy for me to calculate some other quantities down the line. I also store separation, the negation of penetration, rather than penetration because it makes more sense to me.

Broad Phase

If we simply ran a narrow-phase collision check for each pair of objects, we'd have to do \(O(n^2)\) checks for \(n\) objects. These checks can be time consuming, so the naive approach significantly reduces the size of simulation we can run in real time. The standard solution to this problem is to employ a broad phase of collsion detection before the narrow phase.

The goal of the broad phase is to produce a reduced list of potentially touching pairs of objects such that the time saved by performing fewer narrow-phase checks exceeds the time spent producing the list. There are several ways to do this. Most if not all of them involve bounding volumes.

For one volume to bound another means that the bounding volume contains every point in the bounded volume. By definition, if two volumes do not overlap, then any volumes they bound cannot overlap. This means that if we wrap every object in a bounding volume, then we can skip narrow-phase collision detection for a pair of objects when their bounding volumes don't overlap. For this to be a performance win, the bounding volumes should be fast to compute, have a fast intersection test, and not contain too much extra space. We will use an axis-aligned bounding box (AABB) to bound our objects.

To be continued...

*Specifically, angular velocity, angular acceleration, and torque are pseudovectors.