SpatialHash.cs

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

[StructLayout(LayoutKind.Explicit)]
struct SpatialKey
{
    [FieldOffset(0)] public Int32 xz;
    [FieldOffset(0)] public Int16 x;
    [FieldOffset(2)] public Int16 z;
}

public class SpatialHash<T>
{
    internal Dictionary<int, HashSet<T>> index = new Dictionary<int, HashSet<T>>();
    Int16 cellSize;
    readonly int[] keys = new int[9];

    public SpatialHash(int cellSize)
    {
        this.cellSize = (Int16)cellSize;
    }
    
    public bool Move(Vector3 from, Vector3 to, T obj)
    {
        if (CreateKey(from).xz == CreateKey(to).xz)
            return false;
        Remove(from, obj);
        Insert(to, obj);
        return true;
    }
    
    public void Insert(Vector3 v, T obj)
    {
        GetKeys(v, keys);
        for (var i = 0; i < 9; i++)
        {
            var key = keys[i];
            if (!index.TryGetValue(key, out var cell))
                cell = index[key] = new HashSet<T>();
            cell.Add(obj);
        }
    }
    
    public void Remove(Vector3 v, T obj)
    {
        GetKeys(v, keys);
        for (var i = 0; i < 9; i++)
        {
            var key = keys[i];
            if (!index.TryGetValue(key, out var cell))
                cell = index[key] = new HashSet<T>();
            cell.Remove(obj);
        }
    }
    
    public HashSet<T> Query(Vector3 v)
    {
        HashSet<T> cell;
        var key = CreateKey(v);
        if (index.TryGetValue(key.xz, out cell))
            return cell;
        return null;
    }

    public void Clear() => index.Clear();

    SpatialKey CreateKey(Vector3 v)
    {
        //Calculate X and Z quantized to cell size, taking care of int rounding on negative values.
        var x = Mathf.FloorToInt(v.x / cellSize) * cellSize;
        var z = Mathf.FloorToInt(v.z / cellSize) * cellSize;
        //This position hash will only work if X and Z values are += 32767
        Assert.IsTrue(x >= Int16.MinValue && x <= Int16.MaxValue);
        Assert.IsTrue(z >= Int16.MinValue && z <= Int16.MaxValue);
        var key = new SpatialKey() { x = (Int16)x, z = (Int16)z };
        return key;
    }
    
    void GetKeys(Vector3 position, int[] keys)
    {
        //center
        var key = CreateKey(position);
        keys[0] = key.xz;
        //top
        key.z += cellSize;
        keys[1] = key.xz;
        //top left
        key.x -= cellSize;
        keys[2] = key.xz;
        //top right
        key.x += cellSize;
        key.x += cellSize;
        keys[3] = key.xz;
        //right
        key.z -= cellSize;
        keys[4] = key.xz;
        //left
        key.x -= cellSize;
        key.x -= cellSize;
        keys[5] = key.xz;
        //bottom left
        key.z -= cellSize;
        keys[6] = key.xz;
        //bottom
        key.x += cellSize;
        keys[7] = key.xz;
        //bottom right
        key.x += cellSize;
        keys[8] = key.xz;
    }
}