Uniform-dimension cells in a sparse grid, for acceleration of 2D bounding rectangle overlap tests.

Space.cs

#if WINDOWS && !RETAIL
#define PROFILE_COLLISION
#endif

#define NO_CELL_MASK

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using Microsoft.Xna.Framework;

namespace Physics
{
    public class Space
    {
        #region Data
        
        public struct Coord
        {
            public static readonly Coord PosMax = new Coord()
            {
                X = int.MaxValue,
                Y = int.MaxValue,
            };

            public static readonly Coord NegMax = new Coord()
            {
                X = -int.MaxValue,
                Y = -int.MaxValue,
            };

            public int X;
            public int Y;

            public override int GetHashCode()
            {
                unchecked
                {
                    return (X * 397) ^ Y; ;
                }
            }

            public bool Equals(Coord other)
            {
                return other.X == X && other.Y == Y;
            }

            public static bool operator ==(Coord a, Coord b)
            {
                return a.X == b.X && a.Y == b.Y;
            }

            public static bool operator !=(Coord a, Coord b)
            {
                return a.X != b.X && a.Y != b.Y;
            }
        }

        /*
        public class CoordComparer : IComparer<Coord>
        {
            public int Compare(Coord a, Coord b)
            {
                long aa = a.X << 32 | a.Y << 32;
                long bb = b.X << 32 | b.Y <, 32;
                return aa.CompareTo(bb);
            }
        }
        */

        public class Cell
        {
            public Coord Coord;
            public readonly List<RigidBody> Objects;
#if !NO_CELL_MASK
            public int Mask;
#endif

            public Cell(Coord coord)
            {
                Coord = coord;          
                Objects = new List<RigidBody>();
            }
        }

        public struct SpaceMetrics
        {
            public int ObjectCount;
            public int CellCount;
            public int QueryCount;
            public int CellsPerObject;
            public int CellsPerQuery;
            public int ObjectsPerQuery;

            public int _totalCellsQueried;
            public int _totalObjectsCompared;
        }

        private readonly int _originX;
        private readonly int _originY;
        private readonly int _cellDim;
        //private readonly IComparer<Coord> _comparer = new CoordComparer();
        private readonly Dictionary<long, Cell> _cells;// = new Dictionary<long, Cell>();
        private readonly List<Cell> _unusedCells;// = new List<Cell>();
        private readonly HashSet<RigidBody> _objects;
        private readonly List<RigidBody> _temp;
        public SpaceMetrics Metrics;

        #endregion

        #region Public API 

        public Space(int originX, int originY, int cellDim)
        {
            _originX = originX;
            _originY = originY;
            _cellDim = cellDim;
            _cells = new Dictionary<long, Cell>();
            _unusedCells = new List<Cell>();
            _objects = new HashSet<RigidBody>();
            _temp = new List<RigidBody>();
        }

        public void Query(Rectangle queryBounds, List<RigidBody> results)
        {
#if PROFILE_COLLISION
            Metrics.QueryCount++;
#endif

            Coord min = new Coord();
            Coord max = new Coord();
            PositionToCoord(queryBounds.X, queryBounds.Y, ref min);
            PositionToCoord(queryBounds.X + queryBounds.Width, queryBounds.Y + queryBounds.Height, ref max);

            // iterate all cells in coord range testing objects therein for collision
            Cell cell = null;
            Coord coord = new Coord();
            bool hit;
            for (coord.Y = min.Y; coord.Y <= max.Y; coord.Y++)
            {
                for (coord.X = min.X; coord.X <= max.X; coord.X++)
                {
                    long key = coord.X << 32 | coord.Y;
                    if (_cells.TryGetValue(key, out cell))
                    {
#if PROFILE_COLLISION
                        Metrics._totalCellsQueried++;
#endif
#if !NO_CELL_MASK
                        if ((cell.Mask & queryMask) == 0)
                            continue;                  
#endif
#if PROFILE_COLLISION
                        Metrics._totalObjectsCompared += cell.Objects.Count;
#endif
                        foreach (var obj in cell.Objects)
                        {
                            if (results.Contains(obj))
                            {
                                continue;
                            }

                            Rectangle objectBounds = obj._quadRect;
                            hit =
                                queryBounds.X < (objectBounds.X + objectBounds.Width) &&
                                objectBounds.X < (queryBounds.X + queryBounds.Width) &&
                                queryBounds.Y < (objectBounds.Y + objectBounds.Height) &&
                                objectBounds.Y < (queryBounds.Y + queryBounds.Height);

                            if (hit)
                                results.Add(obj);
                        }
                    }
                }
            }
        }

        public void Query(Rectangle queryBounds, int queryMask, List<RigidBody> results)
        {
#if PROFILE_COLLISION
            Metrics.QueryCount++;
#endif

            Coord min = new Coord();
            Coord max = new Coord();
            PositionToCoord(queryBounds.X, queryBounds.Y, ref min);
            PositionToCoord(queryBounds.X + queryBounds.Width, queryBounds.Y + queryBounds.Height, ref max);

            // iterate all cells in coord range testing objects therein for collision
            Cell cell = null;
            Coord coord = new Coord();
            bool hit;
            for (coord.Y = min.Y; coord.Y <= max.Y; coord.Y++)
            {
                for (coord.X = min.X; coord.X <= max.X; coord.X++)
                {
                    long key = coord.X << 32 | coord.Y;
                    if (_cells.TryGetValue(key, out cell))
                    {
#if PROFILE_COLLISION
                        Metrics._totalCellsQueried++;
#endif
#if !NO_CELL_MASK
                        if ((cell.Mask & queryMask) == 0)
                            continue;                  
#endif                        
#if PROFILE_COLLISION
                        Metrics._totalObjectsCompared += cell.Objects.Count;
#endif

                        foreach (var obj in cell.Objects)
                        {
                            if (((int)obj.CollisionType & queryMask) == 0)
                            {
                                continue;
                            }

                            if (results.Contains(obj))
                            {
                                continue;
                            }

                            Rectangle objectBounds = obj._quadRect;
                            hit =
                                queryBounds.X < (objectBounds.X + objectBounds.Width) &&
                                objectBounds.X < (queryBounds.X + queryBounds.Width) &&
                                queryBounds.Y < (objectBounds.Y + objectBounds.Height) &&
                                objectBounds.Y < (queryBounds.Y + queryBounds.Height);

                            if (hit)
                                results.Add(obj);
                        }
                    }
                }
            }
        }

        public void Add(RigidBody obj)
        {
            Rectangle rect = obj._quadRect;
            rect.Inflate(1, 1);

            Coord min = new Coord();
            Coord max = new Coord();
            PositionToCoord(rect.X, rect.Y, ref min);
            PositionToCoord(rect.X + rect.Width, rect.Y + rect.Height, ref max);

            _Add(obj, min, max);
        }

        public void Remove(RigidBody obj)
        {
            //bool wasRemoved = _objects.Remove(obj);
            //if (wasRemoved)
            //{
                foreach (var c in obj._cells)
                {
                    c.Objects.Remove(obj);

                    if (c.Objects.Count == 0)
                    {
                        long key = c.Coord.X << 32 | c.Coord.Y;
                        bool wasRemoved = _cells.Remove(key);
                        if (wasRemoved)
                            _unusedCells.Add(c);
                    }
                    else
                    {
#if !NO_CELL_MASK
                        c.Mask = 0;
                        foreach (var o in c.Objects)
                        {
                            c.Mask |= (int)o.CollisionType;
                        }
#endif
                    }
                }

                obj._cells.Clear();
                obj._minCoord = Coord.PosMax;
                obj._maxCoord = Coord.NegMax;
                //obj._quadAdded = false;
                //obj._space = null;
            //}

            _objects.Remove(obj);

//#if !RETAIL
            //if (obj._cells.Count != 0)
                //throw new Exception("Space._objects and RigidBody._Cells out of sync!");
//#endif
        }

        public void Clear()
        {
            _temp.Clear();
            _temp.AddRange(_objects);
            
            foreach (var obj in _temp)
            {
                Remove(obj);

                obj._quadAdded = false;
                obj._space = null;
                obj._minCoord = Coord.PosMax;
                obj._maxCoord = Coord.NegMax;
            }

            _temp.Clear();

#if !RETAIL
            if (_objects.Count != 0)
                throw new Exception(string.Format("There are still {0} objects after Space.Clear, expected there to be zero!", _objects.Count));
            if (_cells.Count != 0)
                throw new Exception(string.Format("There are still {0} cells after Space.Clear, expected there to be zero!", _cells.Count));
#endif
        }

        #endregion

        private void PositionToCoord(int posx, int posy, ref Coord coord)
        {
            coord.X = (posx - _originX) / _cellDim;
            coord.Y = (posy - _originY) / _cellDim;

            /*
            // convert bounds to a range of cell coordinates

            // x-min
            int x = queryBounds.X - _originX;
            int xmin = x / _cellDim;
            // x-max
            x = bounds.X + queryBounds.Width;
            float fx = (float)x / (float)_cellDim;
            x = (int)fx;
            if (fx != (float)x)
                x += 1;
            int xmax = x;
            // y-min
            int y = queryBounds.Y - _originY;
            int ymin = y / _cellDim;
            // y-max
            y = bounds.Y + queryBounds.Height;
            float fy = (float)y / (float)_cellDim;
            y = (int)fy;
            if (fy != (float)y)
                y += 1;
            int ymax = y;
            */
        }

        public void OnBoundsChanged(RigidBody obj)
        {
            var rect = obj._quadRect;
            rect.Inflate(1, 1);

            Coord min = new Coord();
            Coord max = new Coord();
            PositionToCoord(rect.X, rect.Y, ref min);
            PositionToCoord(rect.X + rect.Width, rect.Y + rect.Height, ref max);

            if (obj._minCoord == min && obj._maxCoord == max)
                return;

            _Add(obj, min, max);
        }

        public void _Add(RigidBody obj, Coord min, Coord max)
        {
            // remove obj if already present in any cells
            //if (_objects.Contains(obj))
                Remove(obj);

            // iterate overlapped cells, adding the obj to them
            Cell cell = null;
            Coord coord = new Coord();
            for (coord.Y = min.Y; coord.Y <= max.Y; coord.Y++)
            {
                for (coord.X = min.X; coord.X <= max.X; coord.X++)
                {
                    long key = coord.X << 32 | coord.Y;
                    if (!_cells.TryGetValue(key, out cell))
                    {
                        int c = _unusedCells.Count;
                        if (c > 0)
                        {
                            c--;
                            cell = _unusedCells[c];
                            _unusedCells.RemoveAt(c);
                            cell.Coord = coord;
#if !NO_CELL_MASK
                            cell.Mask = 0;
#endif
                        }
                        else
                        {
                            cell = new Cell(coord);
                        }

                        _cells.Add(key, cell);
                    }

                    cell.Objects.Add(obj);
                    obj._cells.Add(cell);

#if !NO_CELL_MASK
                    cell.Mask |= (int)obj.CollisionType;
#endif
                }
            }

            bool wasAdded = _objects.Add(obj);
#if !RETAIL
            if (!wasAdded)
                throw new Exception("Space._objects out of sync!");
#endif

            obj._minCoord = min;
            obj._maxCoord = max;
        }

        public void MetricsBegin()
        {
            Metrics._totalCellsQueried = 0;
            Metrics._totalObjectsCompared = 0;

            Metrics.ObjectCount = 0;
            Metrics.CellCount = 0;
            Metrics.QueryCount = 0;
            Metrics.CellsPerObject = 0;
            Metrics.CellsPerQuery = 0;
            Metrics.ObjectsPerQuery = 0;            
        }

        public void MetricsEnd()
        {
            Metrics.ObjectCount = _objects.Count;
            Metrics.CellCount = _cells.Count;
            
            int cellsPerObject = 0;
            if (_objects.Count > 0)
            {
                foreach (var obj in _objects)
                {
                    cellsPerObject += obj._cells.Count;
                }            
                cellsPerObject /= _objects.Count;
            }
            Metrics.CellsPerObject = cellsPerObject;

            int cellsPerQuery = 0;
            if (Metrics.QueryCount > 0)
                cellsPerQuery = Metrics._totalCellsQueried / Metrics.QueryCount;
            Metrics.CellsPerQuery = cellsPerQuery;

            int objectsPerQuery = 0;
            if (Metrics.QueryCount > 0)
                objectsPerQuery = Metrics._totalObjectsCompared / Metrics.QueryCount;
            Metrics.ObjectsPerQuery = objectsPerQuery;            
        }
    }
}