After studying Box2D, Bullet and some slides an alumnus (thanks to Nathan Carlson!) from DigiPen created, I put together a dynamic AABB (axis aligned bounding box) tree broadphase. A Dynamic AABB Tree is a binary search tree for spatial partitioning. Special thanks to Nathanael Presson for the original authoring of the tree within the Bullet engine source code (see comments of this post).

A broadphase in a physics simulation has the job of reporting when bodies are potentially colliding. Usually this is done through cheap intersection tests between simple bounding volumes (AABBs in this case).

There are some interesting properties of a dynamic AABB tree that make the data structure rather simple in terms of implementation.

There are a couple main functions to implement outlined here:

- Insert
- Remove
- Update

The underlying data structure ought to be a huge array of nodes. This is much more optimal in terms of cache performance than many loose heap-allocated nodes. This is very important as the entire array of nodes is going to be fetched from memory every single time the broadphase is updated.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 |
struct Node { bool IsLeaf( void ) const { // The right leaf does not use the same memory as the userdata, // and will always be Null (no children) return right == Null; } // Fat AABB for leafs, bounding AABB for branches AABB aabb; union { int32 parent; int32 next; // free list }; union { // Child indices struct { int32 left; int32 right; }; // Since only leaf nodes hold userdata, we can use the // same memory used for left/right indices to store // the userdata void pointer void *userData; }; // leaf = 0, free nodes = -1 int32 height; static const int32 Null = -1; }; |

The node of the AABB tree can be carefully constructed to take up minimal space, as nodes are always in one of two states: branches and leaves. Since nodes are stored in an array this allows for the nodes to be referenced by integral index instead of pointer. This allows the internal array to grow or shrink as necessary without fear of leaving dangling pointers anywhere.

The idea of the tree is to allow user data to be stored only within leaf nodes. All branch nodes contain just a single AABB enveloping both of its children. This leads to a short description of invariants for the data structure:

- All branches must have two valid children
- Only leaves contain user data

The first rule allows for some simplification of operations like insert/remove. No additional code branching is required to check for NULL children, increases code performance.

## Insert

Insertion involves creating a fat AABB to bound the userdata associated with the new leaf node. A fat AABB is just an AABB that is slightly larger than a tight fitting AABB. Once a new leaf node is created with its fat AABB a tree traversal is required in order to find a sibling to pair up with, and a new parent branch is constructed.

Traversing the tree should be done by following some sort of cost heuristic. The best seems to be a cost involving the surface area of involved nodes. See Box2D for a resource in cost heuristics.

After a new parent is created and a sibling found, the bounding volume hierarchy of all parent nodes are invalid. A traversal up the tree, correcting all the bounding AABBs and node heights is required. This hierarchy correction can be abstracted into a simple function:

1 2 3 4 5 6 7 8 9 10 11 12 13 |
void DynamicAABBTree::SyncHeirarchy( int32 index ) { while(index != Null) { int32 left = m_nodes[index].left; int32 right = m_nodes[index].right; m_nodes[index].height = 1 + Max( m_nodes[left].height, m_nodes[right].height ); m_nodes[index].aabb = Combine( m_nodes[left].aabb, m_nodes[right].aabb ); index = m_nodes[index].parent; } } |

## Remove

Removing nodes from binary search trees can involve a stack to trace back a traversal path, however due to the few invariants of the dynamic AABB tree removal quite simple. Since all branches must contain two valid children, and only leaves contain userdata, the only nodes that require deletion are leaves.

Remove the leaf’s parent and replace it with the leaf’s sibling. After this operation the parent hierarchy needs to be recalculated (just as in insert).

## Update

Since this tree is dynamic it needs a way to handle moving objects, and not just static level geometry. Since fat AABBs are created upon insertion, objects can move around a little bit before the AABB within the tree is invalid. The fatness factor, or how much to fatten each AABB, can be fine-tuned for performance. I myself use an arbitrary distance (about 5% of the average scale of objects). It is also possible to fatten the AABB based upon the object’s previous frame’s displacement (see Box2D for details on displacement predictions).

The update function of the tree checks to make sure that the current tight-fitting AABB of a shape is still contained by the AABB within the tree. In order for this operation to be constant time the node index in the tree needs to be intrusively stored within the shape itself. This will allow a shape to provide its node index along with an up to date tight fitting AABB to the tree, in order to see if its AABB is still within the fat AABB’s bounds.

If the tight AABB is not contained by the fat AABB the shape needs to be removed and reinserted into the tree.

## Balancing the Tree

Since this sort of spatial partitioning involves a binary search tree, the tree will perform optimally (in terms of collision queries) so long as the tree is balanced.

However, how you balance the tree matters. I’ve heard (through rumors) that dynamic AABB trees ought to be balanced in terms of surface area and not tree height. Though this makes sense conceptually, I was not sure how to balance the tree based of surface area. Knowing this, I just went with something I’m more comfortable with, which is balancing based upon tree height.

Balancing a single node involves taking a look at its two children. The height of each child should be compared, and if one is higher than the other by a height of two or more a rotation should be performed. In other words, the child with a larger height ought to be promoted, wherein promotion is a way of rotating a node up the tree hierarchy.

This sort of balancing (AVL balancing) can be performed at the beginning of the hierarchy synchronization loop. This allows the tree to self-balance itself upon insertion and removal of leaves.

## Free List

The tree needs to maintain an internal free list of unused nodes. Constructing a free list involves looping over all the nodes of the tree, upon initial tree construction, linking each node to the next with indices. This process is really simple, though readers may be unfamiliar with lists of indices as opposed to lists of pointers.

Here is a helper function I’ve constructed for myself to setup free lists (useful upon construction of the tree, and growing of the internal array):

1 2 3 4 5 6 7 8 9 10 11 12 |
inline void DynamicAABBTree::AddToFreeList( int32 index ) { for(int32 i = index; i < m_capacity - 1; ++i) { m_nodes[i].next = i + 1; m_nodes[i].height = Node::Null; } m_nodes[m_capacity - 1].next = Node::Null; m_nodes[m_capacity - 1].height = Node::Null; m_freeList = index; } |

Taking nodes from the free list involves setting the current m_freeList index to the next node in the list. Inserting nodes is just as trivial. You can think of this sort of list as a singly linked list, except the links are through indices rather than pointers.

## Queries

Querying volumes against the tree is highly efficient, so long as collision checks against an AABB are fast. Sphere, AABB and raycasts are all very fast.

Querying the tree involves detecting collisions with each node in the tree, starting with the root, traversing to all children until the tree is exhausted. Traversal to a child should be performed if there is a collision with the parent.

This sort of query can easily be done with recursion (note: the callback returns bool, signifying continue or termination of the query):

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
template <typename T> inline void DynamicAABBTree::Query( T *cb, const AABB& aabb, int32 id ) const { const Node *n = m_nodes + id; if(Prim::AABBtoAABB( aabb, n->aabb )) { if(n->IsLeaf( )) { // Report, via callback, collision with a leaf if(!cb->TreeCallBack( id )) return; } else { Query( cb, aabb, n->left ); Query( cb, aabb, n->right ); } } } |

However it may be more favorable to do so with an iterative algorithm. Iterative algorithms are generally a little harder to implement, but take much less memory (and thus are more efficient):

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 |
template <typename T> inline void DynamicAABBTree::Query( T *cb, const AABB& aabb ) const { const int32 k_stackCapacity = 256; int32 stack[k_stackCapacity]; uint32 sp = 1; *stack = m_root; while(sp) { assert( sp < k_stackCapacity ); // k_stackCapacity too small! int32 id = stack[--sp]; const Node *n = m_nodes + id; if(TestAABBOverlap( aabb, n->aabb )) { if(n->IsLeaf( )) { // Report, via callback, a collision with leaf if(!cb->TreeCallBack( id )) return; } else { stack[sp++] = n->left; stack[sp++] = n->right; } } } } |

## Culling Queries Further

It might be a good idea to only query rigid bodies that moved last frame. This is much better than querying all dynamic bodies unconditionally. If you have a contact graph or constraint graph, or any sort of islanding implementation then you can easily cull objects from queries. The idea is that if a rigid body is static or not placed within an island, then it didn’t move at all the previous frame. This is very simple and elegant.

Usually only active (awake) dynamic bodies are placed into an island (and any objects they have contact constraints with). This is because objects that are sleeping don’t need to be considered at all when forming new islands, as they are asleep. Knowing this a small flag can be set within a shape when it is placed into an island. This flag can later be checked to know whether or not it needs to be queried at all within the broadphase.

Though this step is not specific to the dynamic AABB tree, it is a nice trick to know. One caveat is that when contact constraints are updated, they must first have each involved shape’s bounding volume checked against each other. This will allow old contact constraints to be removed once there is no potential for the narrow phase to return true.

## Conclusion

The dynamic AABB tree is an extremely fast spatial partitioning data structure, and is ideal for both large unbounded worlds, and lots of dynamic rigid bodies. Querying the tree has the time complexity of a binary search. I can’t imagine much better of a broadphase for rigid body simulation. There’s an old saying of “no broadphase is perfect, and neither is this one.” when speaking of broadphases. However the dynamic AABB tree has completely impressed me.

In terms of performance, my simulation would grind to a halt and bottleneck with an N^2 broadphase at just over 100 dynamic bodies. Now I can easily simulate around 5000 rigid bodies with the broadphase taking about 5% of the simulation time. My current bottleneck is cache misses during constraint solving.