Philip Diffenderfer's grid spatial partitioning implementation

GridMain.cpp

#include <cstdlib>
#include <cmath>
#include <iostream>

#ifdef _WIN32
#include "sys\time.h"
#include <windows.h>
#else
#include "sys/time.h"
#endif

using namespace std;

namespace steerio
{

   /**
    * An axis-aligned bounding box.
    */
   struct AABB {
      float l, t, r, b;
      AABB() { }
      AABB(float l, float t, float r, float b) {
         set(l, t, r, b);
      }
      void set(float left, float top, float right, float bottom) {
         l = left; t = top; r = right; b = bottom;
      }
      bool intersects(const AABB &y) const {
         return !(l >= y.r || r < y.l || t >= y.b || b < y.t);
      }
      bool contains(const AABB &y) const {
         return !(y.l < l || y.r >= r || y.t < t || y.b >= b);
      }
   };

   /**
    * A body in 2d space that has an axis-aligned bounding box, a set of groups
    * it's in, can collide with, can resolve collisions with, and can be inert.
    */
   struct Body {
      AABB aabb;
      long groups = -1;
      long collisionGroups = -1;
      long resolveGroups = -1;
      bool inert = false;
      bool freed = false;
      bool canCollideWith(const Body &b) const {
         return (collisionGroups & b.groups);
      }
      bool canResolveWith(const Body &b) const {
         return (resolveGroups & b.groups);
      }
   };

   /**
    * A callback interface that listens for collisions from a BroadphaseHandler.
    */
   class BroadphaseCallback {
   public:

      /**
       * A method called before the BroadphaseHandler starts looking for potential collisions.
       */
      virtual void onBroadphaseStart() = 0;

      /**
       * A method called for every collision detected by a BroadphaseHandler.
       *
       * @param a The current body.
       * @param b The body that has collided with a.
       * @param index The number of collisions found already.
       * @param mutual True if b has already been notified of a collision with a.
       */
      virtual void onBroadphaseCollision(const Body *a, const Body *b, int index, bool mutual) = 0;

      /**
       * A method called when the BroadphaseHandler is finished looking for potential collisions.
       * 
       * @param collisionCount The number of collisions detected.
       */
      virtual void onBroadphaseEnd(int collisionCount) = 0;
   };

   /**
    * An interface that you can add bodies to and check for collisions between bodies.
    * Bodies added to a BroadphaseHandler will not be deleted from memory after the
    * body becomes inert or the BroadphaseHandler is deallocated. Bodies can not be
    * explicitly removed from the Handler, their inert flag must be set to true and
    * after the next refresh they will be removed and are available for deletion. Never
    * delete a body that has not been implicitly removed from a BroadphaseHandler.
    */
   class BroadphaseHandler {
   public:

      /**
       * Adds a body to the BroadphaseHandler so that it can now collide with other bodies.
       * Bodies can not be explicitly removed
       *
       * @param body The body to add to the BroadphaseHandler.
       */
      virtual void add(Body *body) = 0;

      /**
       * Refreshes the handler by removing any inert bodies. This should be called
       * after every time a body is updated (and has had their AABB updated) in the
       * event that the BroadphaseHandler implementation uses the body's AABB to 
       * acceleration collision detection.
       *
       * @return The number of live (not inert) bodies that exist in this BroadphaseHandler.
       */
      virtual int refresh() = 0;


      /**
       * Finds all potential collisions between all bodies in this BroadphaseHandler. The
       * implementation is expected to never report a collision between two entities more
       * than once. The AABBs of the bodies in this BroadphaseHandler are used to determine
       * collision, the callback should perform a more detailed collision check if one is
       * necessary.
       * 
       * @param callback The callback interface to notify when the collision checking starts, 
       *        has found a collision pair, and when it ends.
       * @return The number of collisions handled.
       */
      virtual int handle(BroadphaseCallback *callback) = 0;
   };
   
   /**
    * A node for a singly-linked list.
    */
   template<class V> class LinkedNode {
   public:
      V *value;
      LinkedNode *next;

      LinkedNode() 
      {
      }

      LinkedNode(V *value, LinkedNode *next) 
      {
         this->value = value;
         this->next = next;
      }
   };

   /**
    * A singly-linked list implementation that can have nodes added to it and can
    * have nodes removed from it through a cleaning process.
    */
   template<class V> class LinkedList {
   public:
      LinkedNode<V> *head;

      LinkedList() 
      {
         head = new LinkedNode<V>();
      }

      ~LinkedList() 
      {
         LinkedNode<V> *next = NULL;
         while (head != NULL) {
            next = head->next;
            delete head;
            head = next;
         }
      }

      /**
       * Adds a node to this LinkedList. The node must not already exist in another LinkedList.
       *
       * @param node The node to add to the LinkedList.
       */
      void add(LinkedNode<V> *node) 
      {
         node->next = head->next;
         head->next = node;
      }

      /**
       * Cleans this linked list removing any nodes determined dirty by the callback function
       * and possibly deleting the node altogether if the callback function sets autoDelete 
       * to true.
       *
       * @param metadata A pointer to pass along to the isDirty callback.
       * @param isDirty A callback function to invoke to determine if a given node is dirty
       *        and should be removed from this list, even possibly deleted forever.
       * @return The number of clean nodes in this list, essentially the new size of the list.
       */
      int clean(void *metadata, bool (*isDirty)(LinkedNode<V> *node, LinkedList<V> *list, void *metadata, bool &autoDelete)) 
      {
         LinkedNode<V> *prev = head, *curr = head->next, *next = NULL;
         int cleanCount = 0;
         bool autoDelete;
         while (curr != NULL) {
            next = curr->next;
            autoDelete = false;
            if (isDirty(curr, this, metadata, autoDelete)) {
               prev->next = next;
               if (autoDelete) {
                  delete curr;
               }
            } else {
               cleanCount++;
               prev = curr;
            }
            curr = next;         
         }
         return cleanCount;
      }
   };

   // GRID IMPLEMENTATION

   /**
    * A BroadphaseHandler implementation that is a grid of cells where every cell
    * is a list of bodies that exist in that cell. A body exists in a cell if the
    * top-left corner of the body's AABB lies within the cell's boundaries. This
    * implementation maintains a separate list for all bodies outside the grid.
    */
   class BroadphaseGrid : public BroadphaseHandler {
   public:

      /**
       * Allocates a BroadphaseHandler implementation that is a grid of cells where
       * every cell is a list of bodies that exist in that cell.
       * 
       * @param x The left side of the grid.
       * @param y The top side of the grid.
       * @param columns The number of cells there are across the grid on the x-axis.
       * @param rows The number of cells there are down the grid on the y-axis.
       * @param cellWidth The width of a cell.
       * @param cellHeight The height of a cell.
       */
      BroadphaseGrid(float x, float y, int columns, int rows, float cellWidth, float cellHeight) 
      {
         this->boundary.set( x, y, x + ((columns + 1) * cellWidth), y + ((rows + 1) * cellHeight) );
         this->columns = columns;
         this->rows = rows;
         this->cellWidth = cellWidth;
         this->cellHeight = cellHeight;
         this->cells = new LinkedList<Body>*[rows];
         for (int cy = 0; cy < rows; cy++) {
            this->cells[cy] = new LinkedList<Body>[columns];
         }
      }

      ~BroadphaseGrid() 
      {
         delete [] *cells;
         delete [] cells;
      }

      void add(Body *body) 
      {
         LinkedNode<Body> *node = new LinkedNode<Body>(body, NULL);
         int cx, cy;
         if (getCell(body->aabb, cx, cy)) {
            cells[cy][cx].add(node);
         } else {
            outsiders.add(node);
         }
      }

      int refresh() 
      {
         int alive = 0;

         // Remove inert bodies.
         for (int y = 0; y < rows; y++) {
            for (int x = 0; x < columns; x++) {
               alive += cells[y][x].clean(this, &BroadphaseGrid::cleanRefresh);
            }
         }
         alive += outsiders.clean(this, &BroadphaseGrid::cleanRefresh);

         // Relocate bodies to new cells
         for (int y = 0; y < rows; y++) {
            for (int x = 0; x < columns; x++) {
               cells[y][x].clean(this, &BroadphaseGrid::cleanUpdate);
            }
         }
         outsiders.clean(this, &BroadphaseGrid::cleanUpdate);

         return alive;
      }

      int handle(BroadphaseCallback *callback)
      {
         int collisionCount = 0;
         callback->onBroadphaseStart();
         for (int y = 0; y < rows; y++) {
            for (int x = 0; x < columns; x++) {
               handleCollisionList( cells[y][x], x, y, collisionCount, callback );
            }
         }
         handleCollisionList( outsiders, -1, -1, collisionCount, callback );
         callback->onBroadphaseEnd(collisionCount);
         return collisionCount;
      }

   private:
      AABB boundary;   
      int rows, columns;
      float cellWidth, cellHeight;
      LinkedList<Body> **cells;
      LinkedList<Body> outsiders;

      /**
       * A callback method to the LinkedList.clean method which will notify the list to
       * remove the node and delete it if the body is inert.
       *
       * @param node The current node being cleaned.
       * @param list The list being cleaned.
       * @param autoDelete Set to true when the list should delete the current node from memory.
       * @return True if the list should remove and delete the node, otherwise false.
       */
      static bool cleanRefresh(LinkedNode<Body> *node, LinkedList<Body> *list, void *metadata, bool &autoDelete) 
      {
         // Remove and delete node if the body is inert, mark it as freed.
         return (node->value->freed = autoDelete = node->value->inert);
      }

      /**
       * A callback method to the LinkedList.clean method which will notify the list to
       * remove the node if the body no longer belongs in the given list.
       * 
       * @param node The current node being cleaned.
       * @param list The list being cleaned.
       * @param autoDelete Set to true when the list should delete the current node from memory.
       * @return True if the list should remove and delete the node, otherwise false.
       */
      static bool cleanUpdate(LinkedNode<Body> *node, LinkedList<Body> *list, void *metadata, bool &autoDelete) 
      {
         BroadphaseGrid *grid = (BroadphaseGrid*)metadata;
         LinkedList<Body> *destination = list;
         bool changed = false;
         int cx = 0, cy = 0;
         // If the body is inside the grid...
         if (grid->getCell(node->value->aabb, cx, cy)) {
            destination = &grid->cells[cy][cx];
         // If the body is outside the grid...
         } else {
            destination = &grid->outsiders;
         }
         if ((changed = (list != destination))) {
            destination->add(node);
         }
         return changed;
      }

      /**
       * Handles collisions for all bodies in the given list. This will check for collisions
       * for every body in the list against all other bodies in the list as well as any bodies
       * that exist in the cells the current body overlaps.
       * 
       * @param list The list of Bodies to handle collisions for.
       * @param skipX The column of a cell to skip.
       * @param skipY The row of a cell to skip.
       * @param collisionCount The counter used to keep track of number of collisions.
       * @param callback The callback to notifiy when a collision is detected.
       */
      void handleCollisionList(const LinkedList<Body> &list, const int skipX, const int skipY, int &collisionCount, BroadphaseCallback *callback) const 
      {
         int sx, sy, ex, ey;
         LinkedNode<Body> *curr = list.head->next;
         while (curr != NULL) {
            Body *body = curr->value;
            // Handle collisions with this body and all other bodies in this list
            handleCollision(body, curr, collisionCount, callback);
            // Get the span of cells this body covers...
            getCellSpan(body->aabb, sx, sy, ex, ey);
            // For each cell this body covers, handle collisions between this body and all bodies in the cell.
            if ((ex - sx) > 1 || (ey - sy) > 1) {
               for (int cy = sy; cy < ey; cy++) {
                  for (int cx = sx; cx < ex; cx++) {
                     if (cx != skipX || cy != skipY) {
                        handleCollision(body, cells[cy][cx].head, collisionCount, callback);
                     }
                  }
               }
            }
            curr = curr->next;
         }
      } 

      /**
       * Handles collision checks between a and all bodies on a list starting with node.
       * 
       * @param a The body to check for collisions.
       * @param node The head of a linked chain of bodies to check for collisions against a.
       * @param collisionCount The counter used to keep track of number of collisions.
       * @param callback The callback to notifiy when a collision is detected.
       */
      void handleCollision(Body *a, LinkedNode<Body> *node, int &collisionCount, BroadphaseCallback *callback) const 
      {
         node = node->next;
         while (node != NULL) {
            // Avoid checking for collisions for inert bodies.
            if (a->inert) {
               return;
            }
            Body *b = node->value;
            // Avoid checking for collisions for inert bodies.
            if (b->inert) {
               node = node->next;
               continue;
            }
            // Based on their groups, determine if they are applicable for collision
            bool applicableA = a->canCollideWith(*b);
            bool applicableB = b->canCollideWith(*a);
            // At least one needs to be...
            if (applicableA | applicableB) {
               // If they are intersecting...
               if ( a->aabb.intersects( b->aabb ) ) {
                  // If they both can intersect with each other, make sure to 
                  // let the callback know that it's a duplicate collision
                  // notification, it's just going the other way.
                  bool mutual = false;
                  // If A can collide with B, notify A of a collision.
                  if (applicableA) {
                     callback->onBroadphaseCollision(a, b, collisionCount, mutual);
                     mutual = true;
                  }
                  // If B can collide with A, notify B of a collision if it is not inert
                  if (applicableB && !a->inert && !b->inert) {
                     callback->onBroadphaseCollision(b, a, collisionCount, mutual);
                  }
                  collisionCount++;                  
               }               
            }
            node = node->next;
         }
      }
      
      /**
       * Given an AABB, set {cx,cy} to the cell that the AABB belongs in based on upper
       * left corner. Returns false if the AABB is not in a cell.
       * 
       * @param aabb The AABB to find a cell for.
       * @param cx The column of the cell aabb exists in.
       * @param cy The row of the cell aabb exists in.
       * @return True if the AABB's top-left corner lies in a cell, otherwise false.
       */
      inline bool getCell(const AABB &aabb, int &cx, int &cy) const 
      {
         cx = floor((aabb.l - boundary.l) / cellWidth);
         cy = floor((aabb.t - boundary.t) / cellHeight);
         return isColumn(cx) && isRow(cy);
      }

      /**
       * Given an AABB, set {sx,sy}->{ex,ey} to the span of cells that the AABB
       * intersects with based on all four sides of the AABB, where the left and 
       * top are inclusive and the right and bottom are exclusive.
       * 
       * @param aabb The AABB to find a span of cells for.
       * @param sx The left-most cell the aabb intersects with (inclusive). 
       * @param sy The top-most cell the aabb intersects with (inclusive).
       * @param ex The right-most cell the aabb intersects with (exclusive).
       * @param ey The bottom-most cell the aabb intersects with (exclusive).
       */
      inline void getCellSpan(const AABB &aabb, int &sx, int &sy, int &ex, int &ey) const 
      {
         sx = clamp( (int)floor((aabb.l - boundary.l) / cellWidth), 0, columns);
         sy = clamp( (int)floor((aabb.t - boundary.t) / cellHeight), 0, rows);
         ex = clamp( (int)ceil((aabb.r - boundary.l) / cellWidth), 0, columns);
         ey = clamp( (int)ceil((aabb.b - boundary.t) / cellHeight), 0, rows);
      }

      /**
       * @return True if x is a valid column, otherwise false.
       */
      inline bool isColumn(const int x) const 
      {
         return (x >= 0 && x < columns);
      }

      /**
       * @return True if y is a valid row, otherwise false.
       */
      inline bool isRow(const int y) const 
      {
         return (y >= 0 && y < rows);
      }

      /**
       * Calculates the value for a given inclusive minimum and maximum bounds.
       * 
       * @param p The value to claim between the min and the max.
       * @param min The minimum value to return.
       * @param max The maximum value to return.
       * @return The clamped value.
       */
      inline int clamp(const int a, const int min, const int max) const
      {
         return (a < min ? min : (a > max ? max : a));
      }
   };

}


class Timer {

private:
   ulong frequency, time;
   
   void getFrequency(ulong &frequency) {
   #if defined _WIN32 || defined _WIN64
      LARGE_INTEGER windowsFrequency;
      if (QueryPerformanceFrequency(&windowsFrequency)) {
         frequency = windowsFrequency.QuadPart;
      } else {
         frequency = 1000;
      }
   #else
      frequency = 1000000;
   #endif
   }
   
   void getTime(ulong &time) {
   #if defined _WIN32 || defined _WIN64
      LARGE_INTEGER windowsTime;
      if (QueryPerformanceCounter(&windowsTime)) {
         time = windowsTime.QuadPart;
      } else {
         time = GetTickCount();
      }
   #else
      struct timeval timex;
      gettimeofday(&timex, NULL);
      time = timex.tv_usec + (timex.tv_sec * 1000000);
   #endif
   }
   
public:
   Timer() {
      getFrequency(frequency);
   }
   void start() {
      getTime(time);
   }
   float elapsed() {
      ulong start = time;
      getTime(time);
      return (time - start) / (float)frequency;
   }
   ulong getTime() {
      getTime(time);
      return time;
   }
   ulong getFrequency() {
      return frequency;
   }
};

using namespace steerio;

class Ball {
public:
   Body body;
   float x, y, r, vx, vy;
   void update(float dt, const AABB &bounds) {
      x += vx * dt;
      y += vy * dt;
      if (x + r > bounds.r) { 
         x = bounds.r - r;
         vx = -vx;
      }
      if (x - r < bounds.l) {
         x = bounds.l + r;
         vx = -vx;
      }
      if (y + r > bounds.b) {  
         y = bounds.b - r;
         vy = -vy;
      }
      if (y - r < bounds.t) {
         y = bounds.t + r;
         vy = -vy;
      }
      body.aabb.set(x - r, y - r, x + r, y + r);
   }
};

class BallCallback : public BroadphaseCallback {
public:
   void onBroadphaseStart() {};
   void onBroadphaseCollision(const Body *a, const Body *b, int index, bool mutual) {
      if (!mutual) { // we only care about the first collision between a and b (opposed to b and a).
         // TODO notify of collision
         if ((a->groups & b->resolveGroups) || (a->resolveGroups & b->groups)) {
            // TODO resolve collision
         }
      }   
   };
   void onBroadphaseEnd(int collisionCount) {};
};

float randomFloat() {
   return (float)rand() / (float)RAND_MAX;
}
float randomFloat(float min, float max) {
   return (max - min) * randomFloat() + min;
}
long randomLong(long min, long max) {
   return (long)((max - min + 1) * randomFloat() + min);
}
int randomInt(int min, int max) {
   return (int)((max - min + 1) * randomFloat() + min);
}

int main(int argc, char **argv)
{
   const int BALL_COUNT = 10000;
   const AABB WORLD_BOUNDS ( 0.0f, 0.0f, 12800.0f, 12800.0f );
   const float VELOCITY_MAX = 300.0f;
   const float RADIUS_MIN = 0.5f;
   const float RADIUS_MAX = 3.0f;
   const float DELTATIME = 0.01f;
   const int ITERATIONS = 1000;
   const int GRID_ROWS = 64;
   const int GRID_COLUMNS = 64;
   const long COLLISION_MIN = 0;
   const long COLLISION_MAX = 15; 
   const int STATIC_PERCENT = 5;

   // Initialize balls
   Ball balls[BALL_COUNT];
   for (int i = 0; i < BALL_COUNT; i++) {
      Ball *b = &balls[i];
      b->r = randomFloat( RADIUS_MIN, RADIUS_MAX );
      b->x = randomFloat( WORLD_BOUNDS.l + b->r, WORLD_BOUNDS.r - b->r );
      b->y = randomFloat( WORLD_BOUNDS.t + b->r, WORLD_BOUNDS.b - b->r );
      if (randomInt(0, 100) < STATIC_PERCENT) {
         b->vx = b->vy = 0.0f;
      } else {
         b->vx = randomFloat( -VELOCITY_MAX, VELOCITY_MAX );
         b->vy = randomFloat( -VELOCITY_MAX, VELOCITY_MAX );
      }
      b->body.groups = randomLong( COLLISION_MIN, COLLISION_MAX );
      b->body.collisionGroups = randomLong( COLLISION_MIN, COLLISION_MAX );
      b->update( 0.0f, WORLD_BOUNDS );
   }

   // Broadphase callback
   BallCallback *callback = new BallCallback();

   // Broadphase handler
   BroadphaseHandler *handler = new BroadphaseGrid(WORLD_BOUNDS.l, WORLD_BOUNDS.t, GRID_COLUMNS, GRID_ROWS, (WORLD_BOUNDS.r - WORLD_BOUNDS.l) / GRID_COLUMNS, (WORLD_BOUNDS.b - WORLD_BOUNDS.t) / GRID_ROWS);
   for (int i = 0; i < BALL_COUNT; i++) {
      handler->add( &(balls[i].body) );
   }

   // Simulation
   Timer refreshTimer, collisionTimer;

   float refreshTotal = 0.0f;
   float collisionTotal = 0.0f;

   for (int i = 0; i < ITERATIONS; i++) {
      
      for (int k = 0; k < BALL_COUNT; k++) {
        balls[k].update( DELTATIME, WORLD_BOUNDS );
      }

      refreshTimer.start();
      handler->refresh();
      refreshTotal += refreshTimer.elapsed();

      collisionTimer.start();
      handler->handle(callback);
      collisionTotal += collisionTimer.elapsed();
   }

   // Statistics
   cout << "Refresh Average: " << (refreshTotal / ITERATIONS) << endl;
   cout << "Collision Detection Average: " << (collisionTotal / ITERATIONS) << endl;

   return 0;
}