986 lines
37 KiB
C#
986 lines
37 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using Global.Shared.OcTree.Data;
|
|
using Global.Shared.Plugin;
|
|
using Global.Shared.Util;
|
|
using VRageMath;
|
|
|
|
namespace Global.Shared.OcTree
|
|
{
|
|
public class GenericOcTree<T> : IPoolable where T : IOcTreeData
|
|
{
|
|
private readonly GenericContainer<T> _bounds;
|
|
|
|
private readonly List<GenericContainer<T>> _containersToAdd = new List<GenericContainer<T>>();
|
|
|
|
private readonly Pool<GenericContainer<T>> _cPool =
|
|
new Pool<GenericContainer<T>>(() => new GenericContainer<T>());
|
|
|
|
private readonly GenericOcTree<T>[] _nodes = new GenericOcTree<T>[8];
|
|
|
|
private readonly Pool<GenericOcTree<T>> _otPool = new Pool<GenericOcTree<T>>(() => new GenericOcTree<T>());
|
|
|
|
private int _capacity;
|
|
private Bag<GenericContainer<T>> _containers;
|
|
|
|
private Dictionary<long, GenericContainer<T>> _idToContainer;
|
|
private int _maxDepth;
|
|
|
|
private GenericOcTree<T> _parent;
|
|
|
|
private int treeDepth;
|
|
|
|
private GenericOcTree() : this(0, 0, 0, 0, 0, 0, 0, 0)
|
|
{
|
|
}
|
|
|
|
public GenericOcTree(float x, float y, float z, float width, float height, float depth, int capacity,
|
|
int maxDepth)
|
|
{
|
|
_bounds = new GenericContainer<T>().Set(x, y, z, width, height, depth);
|
|
_capacity = capacity;
|
|
_maxDepth = maxDepth;
|
|
_containers = new Bag<GenericContainer<T>>(capacity);
|
|
_idToContainer = new Dictionary<long, GenericContainer<T>>();
|
|
}
|
|
|
|
public T this[long entityId] => Get(entityId);
|
|
|
|
public void Reset()
|
|
{
|
|
for (var i = _containers.Size() - 1; i >= 0; i--) _cPool.Release(_containers.Remove(i));
|
|
for (var i = 0; i < _nodes.Length; i++)
|
|
if (_nodes[i] != null)
|
|
{
|
|
_otPool.Release(_nodes[i]);
|
|
_nodes[i] = null;
|
|
}
|
|
}
|
|
|
|
private GenericOcTree<T> Init(int currentTreeDepth, float x, float y, float z, float width, float height,
|
|
float depth,
|
|
GenericOcTree<T> parent)
|
|
{
|
|
treeDepth = currentTreeDepth;
|
|
_parent = parent;
|
|
_capacity = parent?._capacity ?? 1;
|
|
_maxDepth = parent?._maxDepth ?? 6;
|
|
_bounds.Set(x, y, z, width, height, depth);
|
|
_idToContainer = parent?._idToContainer ?? new Dictionary<long, GenericContainer<T>>();
|
|
_containers = new Bag<GenericContainer<T>>(_capacity);
|
|
|
|
return this;
|
|
}
|
|
|
|
private int IndexOf(float x, float y, float z, float width, float height, float depth)
|
|
{
|
|
var midX = _bounds.X + _bounds.Width / 2;
|
|
var midY = _bounds.Y + _bounds.Height / 2;
|
|
var midZ = _bounds.Z + _bounds.Depth / 2;
|
|
|
|
var res = 0;
|
|
if (x > midX) res |= E;
|
|
else if (x < midX && x + width < midX) res |= W;
|
|
else return OUTSIDE;
|
|
|
|
if (y > midY) res |= U;
|
|
else if (y < midY && y + height < midY) res |= D;
|
|
else return OUTSIDE;
|
|
|
|
if (z > midZ) res |= S;
|
|
else if (z < midZ && z + depth < midZ) res |= N;
|
|
else return OUTSIDE;
|
|
|
|
return res;
|
|
}
|
|
|
|
private bool ShouldMerge()
|
|
{
|
|
// If has no children, has nothing to merge
|
|
if (_nodes[0] == null) return false;
|
|
// If has any children with children, cannot merge
|
|
if (_nodes.Any(n => n._nodes[0] != null)) return false;
|
|
// If children combined contain less than capacity, can merge
|
|
return _containers.Size() + _nodes.Sum(n => n._containers.Size()) <= _capacity;
|
|
}
|
|
|
|
private bool ShouldSplit()
|
|
{
|
|
var halfWidth = _bounds.Width / 2;
|
|
var halfHeight = _bounds.Height / 2;
|
|
var halfDepth = _bounds.Depth / 2;
|
|
var temporaryBounds = new GenericContainer<T>();
|
|
temporaryBounds.Set(_bounds.X, _bounds.Y, _bounds.Z, halfWidth, halfHeight, halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y, _bounds.Z, halfWidth, halfHeight, halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X, _bounds.Y + halfHeight, _bounds.Z, halfWidth, halfHeight, halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y + halfHeight, _bounds.Z, halfWidth, halfHeight,
|
|
halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X, _bounds.Y, _bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y, _bounds.Z + halfDepth, halfWidth, halfHeight,
|
|
halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X, _bounds.Y + halfHeight, _bounds.Z + halfDepth, halfWidth, halfHeight,
|
|
halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y + halfHeight, _bounds.Z + halfDepth, halfWidth,
|
|
halfHeight, halfDepth);
|
|
if (_containers.All(e => temporaryBounds.Contains(e))) return false;
|
|
return true;
|
|
}
|
|
|
|
private void Split()
|
|
{
|
|
// if (!ShouldSplit()) return; TODO: See if necessary
|
|
|
|
var halfWidth = _bounds.Width / 2;
|
|
var halfHeight = _bounds.Height / 2;
|
|
var halfDepth = _bounds.Depth / 2;
|
|
|
|
_nodes[DNW] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y, _bounds.Z, halfWidth, halfHeight,
|
|
halfDepth, this);
|
|
_nodes[DNE] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y, _bounds.Z, halfWidth,
|
|
halfHeight, halfDepth, this);
|
|
_nodes[DSW] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y, _bounds.Z + halfDepth, halfWidth,
|
|
halfHeight, halfDepth, this);
|
|
_nodes[DSE] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y,
|
|
_bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);
|
|
|
|
_nodes[UNW] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y + halfHeight, _bounds.Z,
|
|
halfWidth, halfHeight, halfDepth, this);
|
|
_nodes[UNE] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y + halfHeight,
|
|
_bounds.Z, halfWidth, halfHeight, halfDepth, this);
|
|
_nodes[USW] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y + halfHeight,
|
|
_bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);
|
|
_nodes[USE] = _otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y + halfHeight,
|
|
_bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);
|
|
}
|
|
|
|
private void HandleMerge()
|
|
{
|
|
if (!ShouldMerge()) return;
|
|
|
|
for (var index = _nodes.Length - 1; index >= 0; index--)
|
|
{
|
|
var ocTree = _nodes[index];
|
|
for (var i = ocTree._containers.Size() - 1; i >= 0; i--)
|
|
{
|
|
var ocTreeContainer = ocTree._containers[i];
|
|
_containersToAdd.Add(ocTreeContainer);
|
|
ocTree._containers.Remove(i);
|
|
}
|
|
}
|
|
|
|
for (var index = _nodes.Length - 1; index >= 0; index--)
|
|
{
|
|
var ocTree = _nodes[index];
|
|
_nodes[index] = null;
|
|
_otPool.Release(ocTree);
|
|
ocTree.Reset();
|
|
}
|
|
|
|
foreach (var container in _containersToAdd) Insert(container);
|
|
|
|
_containersToAdd.Clear();
|
|
}
|
|
|
|
public IEnumerable<T> GetAll()
|
|
{
|
|
if (_nodes[0] != null)
|
|
foreach (var data in _nodes.SelectMany(e => e.GetAll()))
|
|
yield return data;
|
|
foreach (var genericContainer in _containers)
|
|
if (genericContainer.Value != null)
|
|
yield return genericContainer.Value;
|
|
}
|
|
|
|
#region Node Bitwise
|
|
|
|
private const int OUTSIDE = -1;
|
|
private const int D = 0b000;
|
|
private const int U = 0b100;
|
|
private const int S = 0b000;
|
|
private const int N = 0b010;
|
|
private const int W = 0b000;
|
|
private const int E = 0b001;
|
|
private const int DSW = D | S | W; // 0
|
|
private const int DSE = D | S | E; // 1
|
|
private const int DNW = D | N | W; // 2
|
|
private const int DNE = D | N | E; // 3
|
|
private const int USW = U | S | W; // 4
|
|
private const int USE = U | S | E; // 5
|
|
private const int UNW = U | N | W; // 6
|
|
private const int UNE = U | N | E; // 7
|
|
|
|
#endregion
|
|
|
|
#region Modifiers
|
|
|
|
public void Update(long entityId)
|
|
{
|
|
if (!Contains(entityId)) return;
|
|
var c = _idToContainer[entityId];
|
|
try
|
|
{
|
|
var (removedChildren, addedChildren) = c.Update();
|
|
if (c.EntityId != entityId)
|
|
{
|
|
var old = entityId;
|
|
entityId = c.EntityId;
|
|
// already contains new id.. just give up
|
|
if (Contains(entityId)) return;
|
|
// Entity id changed, update the octree
|
|
_idToContainer.Remove(old);
|
|
_idToContainer.Add(entityId, c);
|
|
}
|
|
|
|
// Attempting to update subgrid
|
|
if (removedChildren != null)
|
|
foreach (var removedChild in removedChildren)
|
|
_idToContainer.Remove(removedChild);
|
|
if (addedChildren != null)
|
|
foreach (var addedChild in addedChildren)
|
|
_idToContainer.Add(addedChild, c);
|
|
|
|
var parentTree = c.Parent;
|
|
if (parentTree._bounds.Contains(c)) return;
|
|
|
|
parentTree._containers.Remove(c);
|
|
while (parentTree._parent != null && !parentTree._bounds.Contains(c)) parentTree = parentTree._parent;
|
|
|
|
parentTree.Insert(c);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
GlobalInstance.Log.Critical(e, "Failed to update octree");
|
|
}
|
|
}
|
|
|
|
public void Insert(T value)
|
|
{
|
|
Insert(_cPool.Obtain().Set(value));
|
|
}
|
|
|
|
private void Insert(GenericContainer<T> c)
|
|
{
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(c.X, c.Y, c.Z, c.Width, c.Height, c.Depth);
|
|
if (index != OUTSIDE)
|
|
{
|
|
_nodes[index].Insert(c);
|
|
return;
|
|
}
|
|
}
|
|
|
|
c.Parent = this;
|
|
_idToContainer[c.EntityId] = c;
|
|
if (c.ChildIds != null)
|
|
foreach (var childId in c.ChildIds)
|
|
_idToContainer[childId] = c;
|
|
_containers.Add(c);
|
|
|
|
// If under capacity or at/over max depth, we're done.
|
|
if (_containers.Size() <= _capacity || treeDepth >= _maxDepth) return;
|
|
|
|
if (_nodes[0] == null) Split();
|
|
if (_nodes[0] == null) return;
|
|
|
|
var items = _containers.data;
|
|
for (var i = _containers.Size() - 1; i >= 0; i--)
|
|
{
|
|
var next = items[i];
|
|
if (next == null) continue;
|
|
var index = IndexOf(next.X, next.Y, next.Z, next.Width, next.Height, next.Depth);
|
|
if (index == OUTSIDE) continue;
|
|
_nodes[index].Insert(next);
|
|
_containers.Remove(i);
|
|
}
|
|
}
|
|
|
|
public void Remove(long entityId)
|
|
{
|
|
if (!_idToContainer.ContainsKey(entityId)) return;
|
|
|
|
var container = _idToContainer[entityId];
|
|
var parent = container.Parent;
|
|
parent?._containers.Remove(container);
|
|
_idToContainer.Remove(entityId);
|
|
if (container.ChildIds != null)
|
|
foreach (var childId in container.ChildIds)
|
|
_idToContainer.Remove(childId);
|
|
container.Value?.Dispose();
|
|
container.Value = default;
|
|
_cPool.Release(container);
|
|
parent?._parent?.HandleMerge();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Getters
|
|
|
|
public bool Contains(long entityId)
|
|
{
|
|
return _idToContainer.ContainsKey(entityId);
|
|
}
|
|
|
|
public T Get(long entityId)
|
|
{
|
|
return !Contains(entityId) ? default : _idToContainer[entityId].Value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns entityIds of entities that are inside the OcTrees that contain given point.
|
|
/// </summary>
|
|
/// <param name="entityIds">List to fill</param>
|
|
/// <param name="x">X position</param>
|
|
/// <param name="y">Y position</param>
|
|
/// <param name="z">Z position</param>
|
|
public List<long> Get(List<long> entityIds, float x, float y, float z)
|
|
{
|
|
if (!_bounds.Contains(x, y, z)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, 0f, 0f, 0f);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, x, y, z);
|
|
}
|
|
|
|
entityIds.AddRange(_containers.Select(container => container.EntityId));
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> Get(List<long> entityIds, float x, float y, float z, long flags)
|
|
{
|
|
if (flags == 0L) return Get(entityIds, x, y, z);
|
|
if (!_bounds.Contains(x, y, z)) return entityIds;
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, 0f, 0f, 0f);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, x, y, z, flags);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags)
|
|
select container.EntityId
|
|
);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> Get(List<long> entityIds, BoundingSphereD sphereD)
|
|
{
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, sphereD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.Get(entityIds, sphereD);
|
|
}
|
|
|
|
entityIds.AddRange(_containers.Select(container => container.EntityId));
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> Get(List<long> entityIds, long flags, BoundingSphereD sphereD)
|
|
{
|
|
if (flags == 0L) return Get(entityIds, sphereD);
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, flags, sphereD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.Get(entityIds, flags, sphereD);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags)
|
|
select container.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
//TODO: Make exact check sphere rather than box
|
|
public List<long> GetExact(List<long> entityIds, BoundingSphereD sphereD)
|
|
{
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, sphereD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, sphereD);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.Overlaps(x, y, z, width, height, depth) && sphereD.Intersects(
|
|
new BoundingBoxD(new Vector3D(container.X, container.Y, container.Z),
|
|
new Vector3D(
|
|
container.X + container.Width,
|
|
container.Y + container.Height,
|
|
container.Z + container.Depth
|
|
)))
|
|
select container.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> GetExact(List<long> entityIds, long flags, BoundingSphereD sphereD)
|
|
{
|
|
if (flags == 0L) return GetExact(entityIds, sphereD);
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, flags, sphereD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, flags, sphereD);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags) &&
|
|
container.Overlaps(x, y, z, width, height, depth) &&
|
|
sphereD.Intersects(
|
|
new BoundingBoxD(new Vector3D(container.X, container.Y, container.Z),
|
|
new Vector3D(
|
|
container.X + container.Width,
|
|
container.Y + container.Height,
|
|
container.Z + container.Depth
|
|
)))
|
|
select container.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> Get(List<long> entityIds, BoundingBoxD boxD)
|
|
{
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.Get(entityIds, boxD);
|
|
}
|
|
|
|
entityIds.AddRange(_containers.Select(container => container.EntityId));
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> Get(List<long> entityIds, long flags, BoundingBoxD boxD)
|
|
{
|
|
if (flags == 0L) return Get(entityIds, boxD);
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].Get(entityIds, flags, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.Get(entityIds, flags, boxD);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags) && container.Overlaps(x, y, z, width, height, depth)
|
|
select container.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> GetExact(List<long> entityIds, MyOrientedBoundingBoxD boxBoundingBox)
|
|
{
|
|
var boxD = boxBoundingBox.GetAABB();
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, boxD);
|
|
}
|
|
|
|
foreach (var container in _containers)
|
|
{
|
|
if (!container.Overlaps(x, y, z, width, height, depth)) continue;
|
|
|
|
var containerBox = container.BoundingBox;
|
|
if (boxBoundingBox.Intersects(ref containerBox)) entityIds.Add(container.EntityId);
|
|
}
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> GetExact(List<long> entityIds, long flags, MyOrientedBoundingBoxD boxBoundingBox)
|
|
{
|
|
if (flags == 0L) return GetExact(entityIds, boxBoundingBox);
|
|
var boxD = boxBoundingBox.GetAABB();
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, boxD);
|
|
}
|
|
|
|
foreach (var container in _containers)
|
|
{
|
|
if (!container.MatchesFlags(flags) ||
|
|
!container.Overlaps(x, y, z, width, height, depth)) continue;
|
|
|
|
var containerBox = container.BoundingBox;
|
|
if (boxBoundingBox.Intersects(ref containerBox)) entityIds.Add(container.EntityId);
|
|
}
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> GetExact(List<long> entityIds, BoundingBoxD boxD)
|
|
{
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, boxD);
|
|
}
|
|
|
|
entityIds.AddRange(from container in _containers
|
|
where container.Overlaps(x, y, z, width, height, depth)
|
|
select container.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
public List<long> GetExact(List<long> entityIds, long flags, BoundingBoxD boxD)
|
|
{
|
|
if (flags == 0L) return GetExact(entityIds, boxD);
|
|
var x = (float)boxD.Min.X;
|
|
var y = (float)boxD.Min.Y;
|
|
var z = (float)boxD.Min.Z;
|
|
var width = (float)boxD.Max.X - x;
|
|
var height = (float)boxD.Max.Y - y;
|
|
var depth = (float)boxD.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(entityIds, flags, boxD);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(entityIds, flags, boxD);
|
|
}
|
|
|
|
entityIds.AddRange(from GenericContainer in _containers
|
|
where GenericContainer.MatchesFlags(flags) && GenericContainer.Overlaps(x, y, z, width, height, depth)
|
|
select GenericContainer.EntityId);
|
|
|
|
return entityIds;
|
|
}
|
|
|
|
|
|
public void GetExact(List<T> outList, BoundingSphereD sphereD, long flags = 0L)
|
|
{
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(outList, sphereD, flags);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(outList, sphereD, flags);
|
|
}
|
|
|
|
if (_containers.Any())
|
|
outList.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags) &&
|
|
container.Overlaps(x, y, z, width, height, depth) &&
|
|
sphereD.Intersects(container.BoundingBox)
|
|
select container.Value);
|
|
}
|
|
|
|
public void GetExact(List<T> outList, BoundingBoxD box, long flags = 0L)
|
|
{
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(outList, box, flags);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(outList, box, flags);
|
|
}
|
|
|
|
if (_containers.Any())
|
|
outList.AddRange(from container in _containers
|
|
where container.MatchesFlags(flags) &&
|
|
container.Overlaps(x, y, z, width, height, depth) &&
|
|
box.Intersects(container.BoundingBox)
|
|
select container.Value);
|
|
}
|
|
|
|
public void GetExact(List<T> outList, MyOrientedBoundingBoxD boxD, long flags = 0L)
|
|
{
|
|
var box = boxD.GetAABB();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) return;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE) _nodes[index].GetExact(outList, box, flags);
|
|
else
|
|
foreach (var ocTree in _nodes)
|
|
ocTree.GetExact(outList, box, flags);
|
|
}
|
|
|
|
if (_containers.Any())
|
|
// ReSharper disable once LoopCanBeConvertedToQuery
|
|
foreach (var container in _containers)
|
|
{
|
|
if (!container.MatchesFlags(flags) || !container.Overlaps(x, y, z, width, height, depth)) continue;
|
|
var containerBox = container.BoundingBox;
|
|
if (boxD.Intersects(ref containerBox)) outList.Add(container.Value);
|
|
}
|
|
}
|
|
|
|
|
|
public IEnumerable<T> GetEnumerable(BoundingSphereD sphereD, long flags = 0L)
|
|
{
|
|
var box = sphereD.GetBoundingBox();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) yield break;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE)
|
|
foreach (var data in _nodes[index].GetEnumerable(sphereD, flags))
|
|
yield return data;
|
|
else
|
|
foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(sphereD, flags)))
|
|
yield return data;
|
|
}
|
|
|
|
if (!_containers.Any()) yield break;
|
|
|
|
foreach (var c in _containers)
|
|
if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) &&
|
|
sphereD.Intersects(c.BoundingBox))
|
|
yield return c.Value;
|
|
}
|
|
|
|
|
|
public IEnumerable<T> GetEnumerable(BoundingBoxD box, long flags = 0L)
|
|
{
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) yield break;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE)
|
|
foreach (var data in _nodes[index].GetEnumerable(box, flags))
|
|
yield return data;
|
|
else
|
|
foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(box, flags)))
|
|
yield return data;
|
|
}
|
|
|
|
if (!_containers.Any()) yield break;
|
|
|
|
foreach (var c in _containers)
|
|
if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) && box.Intersects(c.BoundingBox))
|
|
yield return c.Value;
|
|
}
|
|
|
|
|
|
public IEnumerable<T> GetEnumerable(MyOrientedBoundingBoxD boxD, long flags = 0L)
|
|
{
|
|
var box = boxD.GetAABB();
|
|
var x = (float)box.Min.X;
|
|
var y = (float)box.Min.Y;
|
|
var z = (float)box.Min.Z;
|
|
var width = (float)box.Max.X - x;
|
|
var height = (float)box.Max.Y - y;
|
|
var depth = (float)box.Max.Z - z;
|
|
|
|
if (!_bounds.Overlaps(x, y, z, width, height, depth)) yield break;
|
|
|
|
if (_nodes[0] != null)
|
|
{
|
|
var index = IndexOf(x, y, z, width, height, depth);
|
|
if (index != OUTSIDE)
|
|
foreach (var data in _nodes[index].GetEnumerable(boxD, flags))
|
|
yield return data;
|
|
else
|
|
foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(boxD, flags)))
|
|
yield return data;
|
|
}
|
|
|
|
if (!_containers.Any()) yield break;
|
|
|
|
foreach (var c in _containers)
|
|
{
|
|
var cBox = c.BoundingBox;
|
|
if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) && boxD.Intersects(ref cBox))
|
|
yield return c.Value;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
|
|
public class GenericContainer<T> : IPoolable, IContainer where T : IOcTreeData
|
|
{
|
|
private static readonly BoundingBoxD ReusableBox = new BoundingBoxD();
|
|
public List<long> ChildIds;
|
|
public long EntityId;
|
|
|
|
public long Flags;
|
|
public GenericOcTree<T> Parent;
|
|
|
|
public T Value;
|
|
|
|
public BoundingBoxD BoundingBox => ReusableBox.SetToContainer(this);
|
|
public float X { get; private set; }
|
|
public float Y { get; private set; }
|
|
public float Z { get; private set; }
|
|
public float Width { get; private set; }
|
|
public float Height { get; private set; }
|
|
public float Depth { get; private set; }
|
|
|
|
public void Reset()
|
|
{
|
|
ChildIds = null;
|
|
EntityId = -1;
|
|
Flags = 0;
|
|
X = 0;
|
|
Y = 0;
|
|
Z = 0;
|
|
Width = 0;
|
|
Height = 0;
|
|
Depth = 0;
|
|
Value = default;
|
|
}
|
|
|
|
public GenericContainer<T> Set(T value)
|
|
{
|
|
Value = value;
|
|
ChildIds = value.ChildIds;
|
|
EntityId = value.Id;
|
|
Flags = value.Flags;
|
|
var box = value.BoundingBox;
|
|
X = (float)box.Min.X;
|
|
Y = (float)box.Min.Y;
|
|
Z = (float)box.Min.Z;
|
|
Width = (float)box.Max.X - X;
|
|
Height = (float)box.Max.Y - Y;
|
|
Depth = (float)box.Max.Z - Z;
|
|
return this;
|
|
}
|
|
|
|
public KeyValuePair<IEnumerable<long>, IEnumerable<long>> Update()
|
|
{
|
|
if (Value == null) return new KeyValuePair<IEnumerable<long>, IEnumerable<long>>(null, null);
|
|
Value.Update();
|
|
var previousChildIds = ChildIds;
|
|
ChildIds = Value.ChildIds;
|
|
Flags = Value.Flags;
|
|
var box = Value.BoundingBox;
|
|
X = (float)box.Min.X;
|
|
Y = (float)box.Min.Y;
|
|
Z = (float)box.Min.Z;
|
|
Width = (float)box.Max.X - X;
|
|
Height = (float)box.Max.Y - Y;
|
|
Depth = (float)box.Max.Z - Z;
|
|
return new KeyValuePair<IEnumerable<long>, IEnumerable<long>>(
|
|
previousChildIds.Where(e => !ChildIds.Contains(e)),
|
|
ChildIds.Where(e => !previousChildIds.Contains(e)));
|
|
}
|
|
|
|
public GenericContainer<T> Set(float x, float y, float z, float width, float height, float depth)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
Width = width;
|
|
Height = height;
|
|
Depth = depth;
|
|
return this;
|
|
}
|
|
|
|
public bool MatchesFlags(long flags)
|
|
{
|
|
return flags == 0 || (Flags & flags) > 0;
|
|
}
|
|
|
|
public bool Contains(float x, float y, float z)
|
|
{
|
|
return X <= x && x <= X + Width &&
|
|
Y <= y && y <= Y + Height &&
|
|
Z <= z && z <= Z + Depth;
|
|
}
|
|
|
|
public bool Contains(float x, float y, float z, float width, float height, float depth)
|
|
{
|
|
var xMax = x + width;
|
|
var yMax = x + height;
|
|
var zMax = x + depth;
|
|
|
|
return x > X && x < X + Width && xMax > X && xMax < X + Width &&
|
|
y > Y && y < Y + Height && yMax > Y && yMax < Y + Height &&
|
|
z > Z && z < Z + Depth && zMax > Z && zMax < Z + Depth;
|
|
}
|
|
|
|
public bool Overlaps(float x, float y, float z, float width, float height, float depth)
|
|
{
|
|
return !(X > x + width || x > X + Width || Y > y + height || y > Y + Height || Z > z + depth ||
|
|
z > Z + Depth);
|
|
}
|
|
|
|
public bool IntersectsRay(Vector3 start, Vector3 end)
|
|
{
|
|
var lineD = new LineD(start, end);
|
|
var box = BoundingBox;
|
|
return box.Intersects(ref lineD);
|
|
}
|
|
|
|
public bool Contains(GenericContainer<T> c)
|
|
{
|
|
return Contains(c.X, c.Y, c.Z, c.Width, c.Height, c.Depth);
|
|
}
|
|
}
|
|
} |