zz
This commit is contained in:
/
986
GlobalShared/OcTree/GenericOcTree.cs
Normal file
986
GlobalShared/OcTree/GenericOcTree.cs
Normal file
@@ -0,0 +1,986 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user