Spatial hash implementation for Unity.

SpatialHash.cs

// The contents of this file is free and unencumbered software released into the
// public domain. For more information, please refer to <http://unlicense.org/>

using System;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using UnityEngine;
using UnityEngine.Assertions;

public class SpatialHash<T>
{
    public readonly struct HashCell : IEquatable<HashCell>
    {
        public readonly int x;
        public readonly int y;
        public readonly int z;

        private HashCell(int x, int y, int z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public bool Equals(HashCell other)
        {
            return x == other.x && y == other.y && z == other.z;
        }

        public override bool Equals(object obj)
        {
            return obj is HashCell other && Equals(other);
        }

        public override int GetHashCode()
        {
            return HashCode.Combine(x, y, z);
        }

        public override string ToString()
        {
            return $"{x:000} {y:000} {z:000}";
        }

        public static HashCell GetCell(Vector3 position, float cellSize)
        {
            (int x, int y, int z) = GetCellCoords(position, cellSize);

            return new HashCell(x, y, z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static (int x, int y, int z) GetCellCoords(Vector3 position, float cellSize)
        {
            var x = (int)Math.Round(position.x / cellSize, MidpointRounding.AwayFromZero);
            var y = (int)Math.Round(position.y / cellSize, MidpointRounding.AwayFromZero);
            var z = (int)Math.Round(position.z / cellSize, MidpointRounding.AwayFromZero);

            return (x, y, z);
        }

        public static HashCell GetCell(int x, int y, int z)
        {
            return new HashCell(x, y, z);
        }

        public static HashCell GetCell(Vector3Int v)
        {
            return new HashCell(v.x, v.y, v.z);
        }
    }

    private readonly Dictionary<HashCell, HashSet<T>> m_HashTable = new Dictionary<HashCell, HashSet<T>>();
    private float m_CellSize;

    public int Count => m_HashTable.Count;
    public float CellSize => m_CellSize;
    public IReadOnlyCollection<HashCell> Cells => m_HashTable.Keys;

    public SpatialHash(float cellSize)
    {
        Assert.IsTrue(cellSize > 0.0f);
        m_CellSize = cellSize;
    }

    /// <summary>
    /// Adds an item to the spatial hash
    /// </summary>
    /// <param name="position"></param>
    /// <param name="item"></param>
    public void Add(Vector3 position, T item)
    {
        var cellIndex = HashCell.GetCell(position, m_CellSize);
        if (!m_HashTable.ContainsKey(cellIndex))
        {
            m_HashTable[cellIndex] = new HashSet<T>();
        }
        m_HashTable[cellIndex].Add(item);
    }

    /// <summary>
    /// Attempts to remove an item from the spatial hash
    /// </summary>
    /// <param name="position"></param>
    /// <param name="item"></param>
    /// <returns></returns>
    public bool Remove(Vector3 position, T item)
    {
        var cellIndex = HashCell.GetCell(position, m_CellSize);

        if (!m_HashTable.TryGetValue(cellIndex, out var hashSet))
        {
            return false;
        }

        if (!hashSet.Remove(item))
        {
            return false;
        }

        m_HashTable[cellIndex] = hashSet;
        return true;
    }

    /// <summary>
    /// Attempts to empty the cell that contains point.
    /// </summary>
    /// <param name="position"></param>
    /// <returns></returns>
    public bool Clear(Vector3 position)
    {
        var cellIndex = HashCell.GetCell(position, m_CellSize);

        if (!m_HashTable.TryGetValue(cellIndex, out var points))
        {
            return false;
        }

        points.Clear();
        m_HashTable[cellIndex] = points;
        return true;
    }

    /// <summary>
    /// Get a collection of points that share a cell with position.
    /// </summary>
    /// <param name="position"></param>
    /// <param name="items"></param>
    /// <returns></returns>
    public bool TryGetCell(Vector3 position, out IReadOnlyCollection<T> items)
    {
        var cellIndex = HashCell.GetCell(position, m_CellSize);

        if (!m_HashTable.TryGetValue(cellIndex, out var result))
        {
            items = null;
            return false;
        }

        items = result;
        return true;
    }

    public Bounds GetCellBounds(Vector3 position)
    {
        (int x, int y, int z) = HashCell.GetCellCoords(position, m_CellSize);

        var cellPosition = new Vector3(x, y, z) * m_CellSize;
        var size = Vector3.one * m_CellSize;
        var bounds = new Bounds(cellPosition, size);

        return bounds;
    }

    public Vector3 CellToWorld(in HashCell cell)
    {
        return new Vector3(cell.x, cell.y, cell.z) * m_CellSize;
    }

    /// <summary>
    /// Get list of points that are within the nine cells near position
    /// </summary>
    /// <param name="position"></param>
    /// <param name="nearbyItems"></param>
    /// <returns></returns>
    public int GetNearby(Vector3 position, HashSet<T> nearbyItems)
    {
        var cellIndex = HashCell.GetCell(position, m_CellSize);
        nearbyItems.Clear();

        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                for (int z = -1; z <= 1; z++)
                {
                    var neighborIndex = HashCell.GetCell(cellIndex.x + x, cellIndex.y + y, cellIndex.z + z);
                    if (m_HashTable.TryGetValue(neighborIndex, out var hashSet))
                    {
                        foreach (var item in hashSet)
                        {
                            nearbyItems.Add(item);
                        }
                    }
                }
            }
        }

        return nearbyItems.Count;
    }
}