Faster Physics: Part 2 — Spatial Hashing

You might want to read Part 1 first

At this point I had run out of ideas for simple fixes, so the next step was to start ripping out parts of p2 and replacing them with my own. p2 tries to not make too many assumptions about your use case out of the box. It’s a jack-of-all-trades-master-of-none sort of deal, so it’s pretty easy to run up against its limitations. A quick peek at the profiler confirmed my suspicions that collision detection is the place I should start my dismembering.

Note: This might make you think that I should focus on replacing the narrowphase, but what I'm actually gonna do is speed up the broadphase, then shift as much load as I can from the narrowphase to the broadphase.

Collision Detection in P2

Collision detection in p2 happens in 2 phases. First is the Broadphase, which looks at the world and tells you what bodies might be colliding right now. The result of the Broadphase is passed to the Narrowphase, which does some further investigation to tell you which bodies are actually colliding right now. The Broadphase implements two key methods:

AABBQuery(aabb): Body[] — Find all the bodies that overlap a given bounding box
getCollisionPairs(): Body[] — Find all the pairs of bodies that might be colliding right now

The Broadphase that p2 implements uses what’s called the Sweep and Prune algorithm, which keeps a list of all bodies sorted by their lower bounds on one axis. It implements getCollisionPairs by iterating through this sorted list and checking for overlap with adjacent bodies that overlap on the sorted axis. This works fine when you have a small-to-medium number of bodies that are generally spread far apart, but in our use case we have a large number of bodies that are densely packed. P2’s Broadphase implements AABBQuery(aabb) with the naive brute force solution of iterating through all the bodies and testing each one if it overlaps your query. Again, this is gonna be slow with a large number of bodies.

There are a few key insights about our problem that we can use to design a more custom-tailored solution:

We have very few dynamic bodies and a lot of static bodies
Bodies get added and removed from the world infrequently
Everything happens in one fixed-size area. We aren’t wandering around a large open world.
The table is densely packed — there are a lot of shapes very near each other
When a body is created static/kinematic/dynamic, it stays that way for its lifetime
We only really need to check for collions between the ball and the other stuff

Sounds like a job for spatial hashing

The Broadphase technique I implemented is called spatial hashing. It’s a technique I had heard of from Chipmunk. It works by partitioning space up into a grid. For each cell of the grid, you store the set of bodies that overlap that grid cell.

We can start defining our SpatialHashingBroadphase

class SpatialHashingBroadphase extends Broadphase {
  cellSize: number = 8; // The size in world units that each grid cell takes up
  partitions: Set<Body>[] = [];
  constructor(private width: number, private height: number) {
    super();
    for (let i = 0; i < width * height; i++) {
      this.partitions.push(new Set());
    }
  }
  // ...
}

To add a body to the partitions, we figure out which cells its AABB overlaps, and we add it to the set of bodies for each of those cells.

To figure out what cells an AABB overlaps, and to do that we need to do some math. First we need to convert our AABB from world coordinates into grid coordinates.

  // Converts from world coordinates to x,y coordinates on our grid
  worldToGridXy([worldX, worldY]: [number, number]): [number, number] {
    return [
      Math.floor(worldX / this.cellSize),
      Math.floor(worldY / this.cellSize)
    ]
  }

Then we can use our converted grid coordinates to iterate row by row and column by column to get all the cells underneath the AABB.

  // Find the cells that an AABB overlaps
  getCellsForAABB(aabb: AABB): number[] {
    const [lowX, lowY] = this.worldToGridXy(aabb.lowerBound);
    const [highX, highY] = this.worldToGridXy(aabb.upperBound);

    const overlappedCells: number[] = [];
    for (let x = lowX; x <= highX; x++) {
      for (let y = lowY; y <= highY; y++) {
        // Convert these coordinates to the cell's index
        let cell = x + y * this.width;
        // In case we're beyond our grid, we want to wrap back around
        cell = mod(cell, this.partitions.length);
        overlappedCells.push(cell);
      }
    }
    return overlappedCells;
  }

From here, adding a body is simple:

addBodyToPartitions(body: Body) {
  for (const cell of this.getCellsForAABB(body.getAABB()) {
    this.partitions[cell].add(body);
  }
}

To implement AABBQuery we just have to look at the bodies in the cells that our AABB is overlapping.

Spatial hashing AABB Query — The AABB Query overlaps cells `9`, `10` and `11`, so we take the bodies from those cells and get the result `[ B ]`

aabbQuery(aabb: AABB): Body[] {
  const result = new Set<Body>();

  for (const cell of this.getCellsForAABB(aabb)) {
    for (const body of this.partitions[cell]) {
      if (aabb.overlaps(body.getAABB())) {
        result.add(body);
      }
    }
  }

  return result.toArray();
}

Now we know how to add bodies, we need to know when to add bodies. Static bodies are easy: we just add them to the partition as soon as they’re added to the world. Dynamic and Kinematic bodies are a little trickier. If we add them to the partitions and then they move, we’re now storing the incorrect data. What we want to do is add them right before we look for collisions, and remove them again when we’re done. Here’s what that looks like inside the getCollisionPairs method:

  getCollisionPairs(world: World): Body[] {
    const result: Body[] = [];

    this.kinematicBodies.forEach(body => this.addBodyToPartitions(body));
    this.dynamicBodies.forEach(body => this.addBodyToPartitions(body));

    for (const dBody of this.dynamicBodies) {
      // Remove the body so we don't report bodies colliding with themselves,
      // and so we don't double count i.e. [A, B] and [B, A]
      this.removeBodyFromHash(dBody);

      for (const other of this.aabbQuery(world, dBody.getAABB())) {
        if (Broadphase.canCollide(dBody, other)) {
          result.push(dBody, other);
        }
      }
    }

    this.kinematicBodies.forEach(body => this.removeBodyFromPartitions(body));

    return result;
  }

Taking it for a test drive

Now that I had all the pieces in place for a working broadphase, it was time to give it a spin. I refreshed the page and opened up the profiler.

TODO: THIS ISN'T THE RIGHT IMAGE

Performance had improved for sure. Time spent in Broadphase collision was down to practically nothing, but the narrowphase and internalStep were still eating up too many cycles. It looks like I had one more blog post of work left to do.

Continued in Part 3