MoonTools.Bonk/Bonk/BroadPhase/SpatialHash.cs

102 lines
3.4 KiB
C#
Raw Normal View History

2019-12-30 06:19:10 +00:00
using System;
2019-09-06 08:11:58 +00:00
using System.Collections.Generic;
2019-12-30 06:19:10 +00:00
using System.Numerics;
2019-09-06 08:11:58 +00:00
using MoonTools.Core.Structs;
namespace MoonTools.Core.Bonk
{
2019-10-25 21:01:36 +00:00
/// <summary>
/// Used to quickly check if two shapes are potentially overlapping.
/// </summary>
/// <typeparam name="T">The type that will be used to uniquely identify shape-transform pairs.</typeparam>
2019-09-06 08:11:58 +00:00
public class SpatialHash<T> where T : IEquatable<T>
{
private readonly int cellSize;
private readonly Dictionary<long, HashSet<T>> hashDictionary = new Dictionary<long, HashSet<T>>();
private readonly Dictionary<T, (IShape2D, Transform2D)> IDLookup = new Dictionary<T, (IShape2D, Transform2D)>();
2019-09-06 08:11:58 +00:00
public SpatialHash(int cellSize)
{
this.cellSize = cellSize;
}
2019-12-30 06:19:10 +00:00
private (int, int) Hash(Vector2 position)
2019-09-06 08:11:58 +00:00
{
2019-12-30 06:19:10 +00:00
return ((int)Math.Floor(position.X / cellSize), (int)Math.Floor(position.Y / cellSize));
2019-09-06 08:11:58 +00:00
}
2019-10-25 21:01:36 +00:00
/// <summary>
/// Inserts an element into the SpatialHash.
/// </summary>
/// <param name="id">A unique ID for the shape-transform pair.</param>
/// <param name="shape"></param>
/// <param name="transform2D"></param>
public void Insert(T id, IShape2D shape, Transform2D transform2D)
2019-09-06 08:11:58 +00:00
{
2019-12-30 06:19:10 +00:00
var box = shape.TransformedAABB(transform2D);
var minHash = Hash(box.Min);
var maxHash = Hash(box.Max);
2019-09-06 08:11:58 +00:00
2019-12-30 06:19:10 +00:00
for (var i = minHash.Item1; i <= maxHash.Item1; i++)
2019-09-06 08:11:58 +00:00
{
2019-12-30 06:19:10 +00:00
for (var j = minHash.Item2; j <= maxHash.Item2; j++)
2019-09-06 08:11:58 +00:00
{
2020-01-01 01:57:38 +00:00
var key = MakeLong(i, j);
if (!hashDictionary.ContainsKey(key))
2019-09-06 08:11:58 +00:00
{
hashDictionary.Add(key, new HashSet<T>());
2019-09-06 08:11:58 +00:00
}
hashDictionary[key].Add(id);
IDLookup[id] = (shape, transform2D);
2019-09-06 08:11:58 +00:00
}
}
}
2019-10-25 21:01:36 +00:00
/// <summary>
/// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID.
/// </summary>
public IEnumerable<(T, IShape2D, Transform2D)> Retrieve(T id, IShape2D shape, Transform2D transform2D)
2019-09-06 08:11:58 +00:00
{
2019-12-30 06:19:10 +00:00
AABB box = shape.TransformedAABB(transform2D);
var minHash = Hash(box.Min);
var maxHash = Hash(box.Max);
2019-09-06 08:11:58 +00:00
for (int i = minHash.Item1; i <= maxHash.Item1; i++)
{
for (int j = minHash.Item2; j <= maxHash.Item2; j++)
{
2020-01-01 01:57:38 +00:00
var key = MakeLong(i, j);
if (hashDictionary.ContainsKey(key))
2019-09-06 08:11:58 +00:00
{
foreach (var t in hashDictionary[key])
2019-09-06 08:11:58 +00:00
{
var (otherShape, otherTransform) = IDLookup[t];
if (!id.Equals(t)) { yield return (t, otherShape, otherTransform); }
2019-09-06 08:11:58 +00:00
}
}
}
}
}
2019-10-25 21:01:36 +00:00
/// <summary>
/// Removes everything that has been inserted into the SpatialHash.
/// </summary>
2019-09-06 08:11:58 +00:00
public void Clear()
{
foreach (var hash in hashDictionary.Values)
2019-09-06 08:11:58 +00:00
{
hash.Clear();
2019-09-06 08:11:58 +00:00
}
IDLookup.Clear();
}
2020-01-01 01:57:38 +00:00
private static long MakeLong(int left, int right)
{
return ((long)left << 32) | ((uint)right);
}
2019-09-06 08:11:58 +00:00
}
2019-12-30 06:19:10 +00:00
}