386 lines
12 KiB
C#
386 lines
12 KiB
C#
|
using System;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Linq;
|
||
|
|
using System.Threading.Tasks;
|
||
|
|
using Godot;
|
||
|
|
using Microsoft.Data.Sqlite;
|
||
|
|
using SqlKata;
|
||
|
|
using SqlKata.Compilers;
|
||
|
|
namespace SJK.Voxels;
|
||
|
|
public interface ISpatialTree<T>// where T : ISpatialEntry
|
||
|
|
{
|
||
|
|
void Insert(T entry);
|
||
|
|
void Remove(T entry);
|
||
|
|
IEnumerable<T> Query(Aabb region);
|
||
|
|
}
|
||
|
|
public interface ISpatialEntry
|
||
|
|
{
|
||
|
|
Guid Guid { get; }
|
||
|
|
Aabb Bounds { get; }
|
||
|
|
}
|
||
|
|
public sealed class SpatialDataLayer<TEntry> : ISpatialTree<TEntry> where TEntry : class, ISpatialEntry
|
||
|
|
{
|
||
|
|
private Dictionary<Guid, TEntry> _entries = new();
|
||
|
|
private record struct Entry(Guid Guid, Aabb Bounds) : ISpatialEntry;
|
||
|
|
GridSpatialIndex<Entry, OctreeEntryStore<Entry>> gridSpatialIndex;
|
||
|
|
public SpatialDataLayer(int gridSize = 128)
|
||
|
|
{
|
||
|
|
gridSpatialIndex = new(() => new OctreeEntryStore<Entry>(new Aabb(Vector3.Zero, Vector3.One * gridSize)), gridSize);
|
||
|
|
}
|
||
|
|
public void Insert(TEntry entry)
|
||
|
|
{
|
||
|
|
gridSpatialIndex.Insert(new(entry.Guid, entry.Bounds));
|
||
|
|
_entries.Add(entry.Guid, entry);
|
||
|
|
|
||
|
|
}
|
||
|
|
// public async ValueTask Save(string @namespace, IDataStore dataStore, IDataSerializer<TEntry[]> serializer)
|
||
|
|
// {
|
||
|
|
// var data = await serializer.SerializeAsync(_entries.Values.ToArray());
|
||
|
|
// await dataStore.WriteAsync(new DataKey(@namespace, nameof(_entries)), data);
|
||
|
|
// }
|
||
|
|
// public async ValueTask Load(string @namespace, IDataStore dataStore, IDataSerializer<TEntry[]> serializer)
|
||
|
|
// {
|
||
|
|
|
||
|
|
// gridSpatialIndex.Clear();
|
||
|
|
// _entries.Clear();
|
||
|
|
// var data = await dataStore.ReadAsync(new DataKey(@namespace, nameof(_entries)));
|
||
|
|
// var e = await serializer.DeserializeAsync(data.Value);
|
||
|
|
|
||
|
|
// e.ToList().ForEach(Insert);
|
||
|
|
// }
|
||
|
|
public IEnumerable<TEntry> Query(Aabb region)
|
||
|
|
{
|
||
|
|
|
||
|
|
return gridSpatialIndex.Query(region).Where(e => _entries.ContainsKey(e.Guid)).Select(e => _entries[e.Guid]);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Remove(TEntry entry)
|
||
|
|
{
|
||
|
|
gridSpatialIndex.Remove(new(entry.Guid, entry.Bounds));
|
||
|
|
_entries.Remove(entry.Guid);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
public class CollectionSpatialTree<TEntry> : ISpatialTree<TEntry> where TEntry : ISpatialEntry
|
||
|
|
{
|
||
|
|
private Dictionary<Guid,TEntry> entries = new();
|
||
|
|
public void Insert(TEntry entry)
|
||
|
|
{
|
||
|
|
entries[entry.Guid] = entry;
|
||
|
|
}
|
||
|
|
|
||
|
|
public IEnumerable<TEntry> Query(Aabb region)
|
||
|
|
{
|
||
|
|
foreach (var item in entries)
|
||
|
|
{
|
||
|
|
if (region.Intersects(item.Value.Bounds))
|
||
|
|
{
|
||
|
|
yield return item.Value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Remove(TEntry entry)
|
||
|
|
{
|
||
|
|
entries.Remove(entry.Guid);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
public class GridSpatialIndex<TEntry, TChunkSpatialTree> : ISpatialTree<TEntry> where TEntry : ISpatialEntry where TChunkSpatialTree : ISpatialTree<TEntry>
|
||
|
|
{
|
||
|
|
private readonly int _cellSize;
|
||
|
|
private readonly Func<TChunkSpatialTree> factory;
|
||
|
|
private readonly Dictionary<Vector3I, TChunkSpatialTree> _cells = new();
|
||
|
|
public GridSpatialIndex(Func<TChunkSpatialTree> factory, int cellSize = 32)
|
||
|
|
{
|
||
|
|
this._cellSize = cellSize;
|
||
|
|
this.factory = factory;
|
||
|
|
}
|
||
|
|
private Vector3I GetCellCoord(Vector3 position)
|
||
|
|
{
|
||
|
|
int cx = (int)MathF.Floor(position.X / _cellSize);
|
||
|
|
int cy = (int)MathF.Floor(position.Y / _cellSize);
|
||
|
|
int cz = (int)MathF.Floor(position.Z / _cellSize);
|
||
|
|
return new(cx, cy, cz);
|
||
|
|
}
|
||
|
|
private Vector3I[] GetOverlappingCells(Aabb bounds)
|
||
|
|
{
|
||
|
|
var minCell = GetCellCoord(bounds.Min);
|
||
|
|
var maxCell = GetCellCoord(bounds.Max);
|
||
|
|
|
||
|
|
var cells = new List<Vector3I>();
|
||
|
|
for (int x = minCell.X; x <= maxCell.X; x++)
|
||
|
|
for (int y = minCell.Y; y <= maxCell.Y; y++)
|
||
|
|
for (int z = minCell.Z; z <= maxCell.Z; z++)
|
||
|
|
cells.Add(new(x, y, z));
|
||
|
|
return cells.ToArray();
|
||
|
|
}
|
||
|
|
public void Insert(TEntry entry)
|
||
|
|
{
|
||
|
|
foreach (var cell in GetOverlappingCells(entry.Bounds))
|
||
|
|
{
|
||
|
|
if (!_cells.TryGetValue(cell, out var tree))
|
||
|
|
{
|
||
|
|
_cells[cell] = tree = factory();
|
||
|
|
}
|
||
|
|
tree.Insert(entry);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
public IEnumerable<TEntry> Query(Aabb region)
|
||
|
|
{
|
||
|
|
var visted = new HashSet<TEntry>();
|
||
|
|
foreach (var cell in GetOverlappingCells(region))
|
||
|
|
{
|
||
|
|
if (_cells.TryGetValue(cell, out var tree))
|
||
|
|
{
|
||
|
|
foreach (var entry in tree.Query(region))
|
||
|
|
{
|
||
|
|
if (visted.Add(entry))
|
||
|
|
{
|
||
|
|
yield return entry;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Remove(TEntry entry)
|
||
|
|
{
|
||
|
|
foreach (var cell in GetOverlappingCells(entry.Bounds))
|
||
|
|
{
|
||
|
|
if (_cells.TryGetValue(cell, out var tree))
|
||
|
|
{
|
||
|
|
tree.Remove(entry);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
public void Clear()
|
||
|
|
{
|
||
|
|
_cells.Clear();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
public readonly record struct Aabb
|
||
|
|
{
|
||
|
|
public readonly Vector3 Min { get; }
|
||
|
|
public readonly Vector3 Max { get; }
|
||
|
|
public Aabb(Vector3 min, Vector3 max)
|
||
|
|
{
|
||
|
|
Min = min.Min(max);
|
||
|
|
Max = min.Max(max);
|
||
|
|
}
|
||
|
|
public bool Intersects(Aabb other)
|
||
|
|
{
|
||
|
|
return !(Max.X < other.Min.X || Min.X > other.Max.X ||
|
||
|
|
Max.Y < other.Min.Y || Min.Y > other.Max.Y ||
|
||
|
|
Max.Z < other.Min.Z || Min.Z > other.Max.Z);
|
||
|
|
}
|
||
|
|
public bool Contains(Vector3 point)
|
||
|
|
{
|
||
|
|
return point.X >= Min.X && point.X <= Max.X &&
|
||
|
|
point.Y >= Min.Y && point.Y <= Max.Y &&
|
||
|
|
point.Z >= Min.Z && point.Z <= Max.Z;
|
||
|
|
}
|
||
|
|
public Aabb Expand(Vector3I value) =>
|
||
|
|
new Aabb(
|
||
|
|
Min.Min(value),
|
||
|
|
Max.Max(value)
|
||
|
|
);
|
||
|
|
public Vector3 Center => (Min + Max) * 0.5f;
|
||
|
|
public Vector3 Size => Max - Min;
|
||
|
|
|
||
|
|
}
|
||
|
|
public sealed class OctreeEntryStore<T> : ISpatialTree<T> where T : ISpatialEntry
|
||
|
|
{
|
||
|
|
private readonly OctreeNode<T> _root;
|
||
|
|
|
||
|
|
public OctreeEntryStore(Aabb worldBounds, int maxDepth = 8, int maxPerNode = 16)
|
||
|
|
{
|
||
|
|
_root = new OctreeNode<T>(worldBounds, maxDepth, maxPerNode);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Insert(T entry) => _root.Insert(entry);
|
||
|
|
public bool Remove(T entry) => _root.Remove(entry);
|
||
|
|
public IEnumerable<T> Query(Aabb bounds) => _root.Query(bounds);
|
||
|
|
// public IEnumerable<T> QuerySphere(Vector3 center, float radius) => _root.QuerySphere(center, radius);
|
||
|
|
|
||
|
|
void ISpatialTree<T>.Remove(T entry)
|
||
|
|
{
|
||
|
|
this.Remove(entry);
|
||
|
|
}
|
||
|
|
|
||
|
|
// --- Internal Octree Node ---
|
||
|
|
private sealed class OctreeNode<TNode> where TNode : ISpatialEntry
|
||
|
|
{
|
||
|
|
private readonly Aabb _bounds;
|
||
|
|
private readonly int _maxDepth;
|
||
|
|
private readonly int _maxPerNode;
|
||
|
|
private readonly int _depth;
|
||
|
|
|
||
|
|
private List<TNode>? _entries;
|
||
|
|
private OctreeNode<TNode>[]? _children;
|
||
|
|
|
||
|
|
public OctreeNode(Aabb bounds, int maxDepth, int maxPerNode, int depth = 0)
|
||
|
|
{
|
||
|
|
_bounds = bounds;
|
||
|
|
_maxDepth = maxDepth;
|
||
|
|
_maxPerNode = maxPerNode;
|
||
|
|
_depth = depth;
|
||
|
|
_entries = new List<TNode>();
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Insert(TNode entry)
|
||
|
|
{
|
||
|
|
// If we have children, try to pass entry down.
|
||
|
|
if (_children != null)
|
||
|
|
{
|
||
|
|
foreach (var child in _children)
|
||
|
|
{
|
||
|
|
if (child._bounds.Intersects(entry.Bounds))
|
||
|
|
{
|
||
|
|
child.Insert(entry);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Otherwise, keep it here.
|
||
|
|
_entries ??= new List<TNode>();
|
||
|
|
_entries.Add(entry);
|
||
|
|
|
||
|
|
// Subdivide if too many and not too deep
|
||
|
|
if (_entries.Count > _maxPerNode && _depth < _maxDepth)
|
||
|
|
Subdivide();
|
||
|
|
}
|
||
|
|
|
||
|
|
public bool Remove(TNode entry)
|
||
|
|
{
|
||
|
|
bool removed = _entries?.Remove(entry) ?? false;
|
||
|
|
if (_children != null)
|
||
|
|
{
|
||
|
|
foreach (var child in _children)
|
||
|
|
removed |= child.Remove(entry);
|
||
|
|
}
|
||
|
|
return removed;
|
||
|
|
}
|
||
|
|
|
||
|
|
public IEnumerable<TNode> Query(Aabb bounds)
|
||
|
|
{
|
||
|
|
if (!_bounds.Intersects(bounds))
|
||
|
|
yield break;
|
||
|
|
|
||
|
|
if (_entries != null)
|
||
|
|
{
|
||
|
|
foreach (var e in _entries)
|
||
|
|
if (e.Bounds.Intersects(bounds))
|
||
|
|
yield return e;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_children != null)
|
||
|
|
{
|
||
|
|
foreach (var child in _children)
|
||
|
|
{
|
||
|
|
foreach (var e in child.Query(bounds))
|
||
|
|
yield return e;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// public IEnumerable<TNode> QuerySphere(Vector3 center, float radius)
|
||
|
|
// {
|
||
|
|
// // Quick reject if AABB doesn't overlap sphere
|
||
|
|
// if (!AabbSphereIntersect(_bounds, center, radius))
|
||
|
|
// yield break;
|
||
|
|
|
||
|
|
// if (_entries != null)
|
||
|
|
// {
|
||
|
|
// foreach (var e in _entries)
|
||
|
|
// {
|
||
|
|
// var distSq = e.Position.DistanceSquaredTo(center);
|
||
|
|
// if (distSq <= radius * radius)
|
||
|
|
// yield return e;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
|
||
|
|
// if (_children != null)
|
||
|
|
// {
|
||
|
|
// foreach (var child in _children)
|
||
|
|
// {
|
||
|
|
// foreach (var e in child.QuerySphere(center, radius))
|
||
|
|
// yield return e;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
|
||
|
|
private void Subdivide()
|
||
|
|
{
|
||
|
|
if (_children != null) return;
|
||
|
|
|
||
|
|
_children = new OctreeNode<TNode>[8];
|
||
|
|
var center = _bounds.Center;
|
||
|
|
var size = _bounds.Size * 0.5f;
|
||
|
|
|
||
|
|
int i = 0;
|
||
|
|
for (int x = 0; x <= 1; x++)
|
||
|
|
for (int y = 0; y <= 1; y++)
|
||
|
|
for (int z = 0; z <= 1; z++)
|
||
|
|
{
|
||
|
|
var min = new Vector3(
|
||
|
|
x == 0 ? _bounds.Min.X : center.X,
|
||
|
|
y == 0 ? _bounds.Min.Y : center.Y,
|
||
|
|
z == 0 ? _bounds.Min.Z : center.Z);
|
||
|
|
var max = min + size;
|
||
|
|
_children[i++] = new OctreeNode<TNode>(new Aabb(min, max), _maxDepth, _maxPerNode, _depth + 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Redistribute current entries
|
||
|
|
var oldEntries = _entries!;
|
||
|
|
_entries = new List<TNode>();
|
||
|
|
foreach (var entry in oldEntries)
|
||
|
|
Insert(entry);
|
||
|
|
}
|
||
|
|
|
||
|
|
private static bool AabbSphereIntersect(Aabb aabb, Vector3 center, float radius)
|
||
|
|
{
|
||
|
|
float sqDist = 0f;
|
||
|
|
for (int i = 0; i < 3; i++)
|
||
|
|
{
|
||
|
|
float v = center[i];
|
||
|
|
if (v < aabb.Min[i]) sqDist += (aabb.Min[i] - v) * (aabb.Min[i] - v);
|
||
|
|
if (v > aabb.Max[i]) sqDist += (v - aabb.Max[i]) * (v - aabb.Max[i]);
|
||
|
|
}
|
||
|
|
return sqDist <= radius * radius;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
public readonly struct SpatialBounds
|
||
|
|
{
|
||
|
|
public readonly Vector3 Center;
|
||
|
|
public readonly Vector3 Extents;
|
||
|
|
|
||
|
|
public SpatialBounds(Vector3 center, Vector3 extents)
|
||
|
|
{
|
||
|
|
Center = center;
|
||
|
|
Extents = extents;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static SpatialBounds Point(Vector3 position) => new(position, Vector3.Zero);
|
||
|
|
|
||
|
|
public Aabb ToAabb() => new(Center - Extents, Center + Extents);
|
||
|
|
|
||
|
|
public bool Intersects(Aabb region) => region.Intersects(ToAabb());
|
||
|
|
}
|
||
|
|
public readonly struct SpatialEntry<T>
|
||
|
|
{
|
||
|
|
public readonly Guid Id;
|
||
|
|
// public readonly Aabb Bounds;
|
||
|
|
public readonly SpatialBounds Bounds;
|
||
|
|
public readonly T Value;
|
||
|
|
|
||
|
|
// public SpatialEntry(Guid id, T value, Aabb bounds)
|
||
|
|
public SpatialEntry(Guid id, T value, SpatialBounds bounds)
|
||
|
|
{
|
||
|
|
Id = id;
|
||
|
|
Value = value;
|
||
|
|
Bounds = bounds;
|
||
|
|
}
|
||
|
|
}
|