public void ItemsInside(BoundingBox box, ref IList <T> output)
{
if (_SplitDimension != CoordinateAxis.Undefined)
{
double min = box.MinInDimension(_SplitDimension);
double max = box.MaxInDimension(_SplitDimension);
double t0 = (min - _Origin) / _CellSize;
double t1 = (max - _Origin) / _CellSize;
int startIndex = (int)Math.Max(Math.Floor(t0), 0);
int endIndex = (int)Math.Min(Math.Floor(t1), _Branches.Length - 1);
for (int i = startIndex; i <= endIndex; i++)
{
DDTreeNode <T> node = _Branches[i];
if (node != null)
{
node.ItemsInside(box, ref output);
}
}
}
else
{
foreach (T item in _Children)
{
if (_Tree.MaxXOf(item) >= box.MinX && _Tree.MinXOf(item) <= box.MaxX &&
_Tree.MaxYOf(item) >= box.MinY && _Tree.MinYOf(item) <= box.MaxY &&
_Tree.MaxZOf(item) >= box.MinZ && _Tree.MinZOf(item) <= box.MaxZ &&
!output.Contains(item))
{
output.Add(item);
}
}
}
}
/// <summary>
/// Subdivide this node along the principal axis of its child objects
/// </summary>
public void Subdivide()
{
if (_Children != null && _Children.Count > 1)
{
double max;
double min;
CoordinateAxis largest = LargestDimension(_Children, out min, out max);
int divisions = Math.Min(_Children.Count, Math.Min(_Tree.MaxDivisions, (int)Math.Floor(max - min / _Tree.MinCellSize) + 1));
if (divisions > 1)
{
_CellSize = (max - min) / (divisions - 1);
_Origin = min - _CellSize / 2;
_SplitDimension = largest;
_Branches = new DDTreeNode <T> [divisions];
//Add all children to branches:
foreach (T child in _Children)
{
AddToBranch(child, false);
}
//Subdivide children:
for (int i = 0; i < _Branches.Length; i++)
{
DDTreeNode <T> node = _Branches[i];
if (node != null &&
node.Children.Count < Children.Count)
{
node.Subdivide();
}
}
}
}
}
/// <summary>
/// Creates a new DDTree populated with the specified collection of objects
/// </summary>
/// <param name="items">The objects to include within the tree.</param>
/// <param name="maxDivisions">The maximum number of cells into which each
/// level in the tree should be divided</param>
/// <param name="minCellSize">The minimum allowable size of a cell. Once a node
/// reaches this size it will no longer subdivide regardless of how many items are
/// contained within it.</param>
/// <param name="maxLeafPopulation">The maximum population per leaf node. If the
/// number of objects within a cell exceeds this number and the minimum cell size
/// has not yet been reached, the node will subdivide</param>
protected DDTree(IList <T> items, int maxDivisions = 10,
double minCellSize = 1, int maxLeafPopulation = 4)
{
MaxDivisions = maxDivisions;
MinCellSize = minCellSize;
MaxLeafPopulation = maxLeafPopulation;
_RootNode = new DDTreeNode <T>(this);
foreach (T item in items)
{
_RootNode.Children.Add(item);
}
_RootNode.Subdivide();
}
/// <summary>
/// Add an item to a branch node of this node
/// </summary>
/// <param name="item"></param>
protected void AddToBranch(T item, bool autoSubdivide = true)
{
//TODO: As optimisation, re-enable the following for singularity objects?
/*
* double value = _Tree.PositionInDimension(_SplitDimension, item);
* int index = (int)Math.Floor((value - _Origin) / _CellSize);
* if (index < 0) index = 0;
* else if (index >= _Branches.Length) index = _Branches.Length - 1;
* if (_Branches[index] == null) _Branches[index] = new DDTreeNode<T>(_Tree);
* _Branches[index].Add(item);
*/
double min = _Tree.MinInDimension(_SplitDimension, item);
int iMin = (int)Math.Floor((min - _Origin) / _CellSize);
if (iMin < 0)
{
iMin = 0;
}
else if (iMin >= _Branches.Length)
{
iMin = _Branches.Length - 1;
}
double max = _Tree.MaxInDimension(_SplitDimension, item);
int iMax = (int)Math.Floor((max - _Origin) / _CellSize);
if (iMax < 0)
{
iMax = 0;
}
else if (iMax >= _Branches.Length)
{
iMax = _Branches.Length - 1;
}
for (int i = iMin; i <= iMax; i++)
{
if (_Branches[i] == null)
{
_Branches[i] = new DDTreeNode <T>(_Tree);
}
_Branches[i].Add(item, autoSubdivide);
}
}
/// <summary>
/// Remove an item from this node and all branch nodes that contain it
/// </summary>
/// <param name="item"></param>
public void Remove(T item)
{
if (_Children.Contains(item))
{
_Children.Remove(item);
if (_SplitDimension != CoordinateAxis.Undefined)
{
for (int i = 0; i < _Branches.Length; i++)
{
DDTreeNode <T> branch = _Branches[i];
if (branch != null)
{
branch.Remove(item);
}
}
}
}
}
/// <summary>
/// Trace a ray through this node and its children searching for intersections with object geometry.
/// </summary>
/// <param name="ray">The ray to test</param>
/// <param name="hitTest">A method delegate which tests for intersections between an item in the tree and a ray
/// and returns the ray parameter of intersection (if any)</param>
/// <param name="tStart">A clipping value at the start of the section of the ray in which to test for collisions</param>
/// <param name="tEnd">A clipping parameter at the end of the section of the ray in which to test for collisions</param>
/// <returns></returns>
public RayHit <T> RayTrace(Axis ray, Func <T, Axis, double> hitTest, double tStart = 0, double tEnd = 0.0 / 0.0)
{
if (_SplitDimension != CoordinateAxis.Undefined)
{
Vector entryPoint = ray.PointAt(tStart);
// Search through divisions the ray passes through (in order)
double startValue = _Tree.PositionInDimension(_SplitDimension, entryPoint);
double t = (startValue - _Origin) / _CellSize;
int startIndex = (int)Math.Floor(t);
if (startIndex < 0)
{
startIndex = 0;
}
else if (startIndex >= _Branches.Length)
{
startIndex = _Branches.Length - 1;
}
int increment = ray.Direction[_SplitDimension].Sign();
int endIndex;
if (tEnd.IsNaN())
{
if (increment < 0)
{
endIndex = 0;
}
else
{
endIndex = _Branches.Length - 1;
}
}
else
{
Vector exitPoint = ray.PointAt(tEnd);
double t2 = (startValue - _Origin) / _CellSize;
endIndex = (int)Math.Floor(t2);
if (endIndex < 0)
{
endIndex = 0;
}
else if (endIndex >= _Branches.Length)
{
endIndex = _Branches.Length - 1;
}
}
for (int i = startIndex; !i.Exceeded(endIndex, increment); i += increment)
{
DDTreeNode <T> node = _Branches[i];
if (node != null)
{
// Calculate the intersections with the planes either end of this cell to (potentially) restrict
// the range of future checks
double value0 = _Origin + i * _CellSize;
double value1 = _Origin + (i + 1) * _CellSize;
double t0 = i == 0 ? double.NaN : Intersect.LinePlane(ray.Origin, ray.Direction, _SplitDimension, value0);
double t1 = i == _Branches.Length - 1 ? double.NaN : Intersect.LinePlane(ray.Origin, ray.Direction, _SplitDimension, value1);
// The end cells are 'open' and so end at infinity
if (increment < 0)
{
Swap(ref t0, ref t1);
}
double tStartNew = tStart;
if (!t0.IsNaN() && t0 > tStart)
{
tStartNew = t0;
}
double tEndNew = tEnd;
if (!t1.IsNaN() && t1 < tEnd)
{
tEndNew = t1;
}
RayHit <T> result = node.RayTrace(ray, hitTest, tStartNew, tEndNew);
if (result != null)
{
return(result); // Hit something - unwind!
}
}
}
}
else
{
// Search leaf for intersections
double tMin = double.MaxValue;
bool hit = false;
T hitItem = default(T);
foreach (T item in _Children)
{
double t = hitTest.Invoke(item, ray);
if (!t.IsNaN() && t >= 0 && t < tMin)
{
tMin = t;
hitItem = item;
hit = true;
}
}
if (hit)
{
return(new RayHit <T>(hitItem, tMin));
}
}
return(null);
}
/// <summary>
/// Find the closest object to the specified point within the specified distance
/// </summary>
/// <param name="pt"></param>
/// <param name="distanceSquared"></param>
/// <returns></returns>
public T NearestTo(Vector pt, ref double distanceSquared, T ignore)
{
T result = default(T);
if (_SplitDimension != CoordinateAxis.Undefined)
{
double value = _Tree.PositionInDimension(_SplitDimension, pt);
//Check starting cell:
double t = (value - _Origin) / _CellSize;
int startIndex = (int)Math.Floor(t);
if (startIndex < 0)
{
startIndex = 0;
}
else if (startIndex >= _Branches.Length)
{
startIndex = _Branches.Length - 1;
}
DDTreeNode <T> node = _Branches[startIndex];
if (node != null)
{
result = node.NearestTo(pt, ref distanceSquared, ignore);
}
//Check surrounding cells within max distance:
int offsetIndex = 1;
double cellSizeSquared = _CellSize * _CellSize;
double t0 = t - startIndex;
double t1 = -t0;
t0 -= 1;
bool increase = true;
bool decrease = true;
while (increase || decrease)
{
if (increase)
{
double pInd = offsetIndex + t1;
int i = startIndex + offsetIndex;
if (pInd * pInd * cellSizeSquared >= distanceSquared || i >= _Branches.Length)
{
increase = false;
}
else
{
node = _Branches[i];
if (node != null)
{
T result2 = default(T);
result2 = node.NearestTo(pt, ref distanceSquared, ignore);
if (result2 != null)
{
result = result2;
}
}
}
}
if (decrease)
{
double pInd = offsetIndex + t0;
int i = startIndex - offsetIndex;
if (pInd * pInd * cellSizeSquared >= distanceSquared || i < 0)
{
decrease = false;
}
else
{
node = _Branches[i];
if (node != null)
{
T result2 = default(T);
result2 = node.NearestTo(pt, ref distanceSquared, ignore);
if (result2 != null)
{
result = result2;
}
}
}
}
offsetIndex++;
}
}
else
{
foreach (T item in _Children)
{
if (_Tree.CanReturn(item) && !object.ReferenceEquals(item, ignore))
{
double distSquaredTo = _Tree.DistanceSquaredBetween(pt, item);
if (distSquaredTo < distanceSquared)
{
distanceSquared = distSquaredTo;
result = item;
}
}
}
}
return(result);
}
/// <summary>
/// Find all items within the given distance from the given point
/// </summary>
/// <param name="pt"></param>
/// <param name="distanceSquared"></param>
/// <param name="output"></param>
public void CloseTo(Vector pt, double distanceSquared, ref IList <T> output)
{
if (_SplitDimension != CoordinateAxis.Undefined)
{
double value = _Tree.PositionInDimension(_SplitDimension, pt);
//Check starting cell:
double t = (value - _Origin) / _CellSize;
int startIndex = (int)Math.Floor(t);
if (startIndex < 0)
{
startIndex = 0;
}
else if (startIndex >= _Branches.Length)
{
startIndex = _Branches.Length - 1;
}
DDTreeNode <T> node = _Branches[startIndex];
if (node != null)
{
node.CloseTo(pt, distanceSquared, ref output);
}
//Check surrounding cells within max distance:
int offsetIndex = 1;
double cellSizeSquared = _CellSize * _CellSize;
double t0 = t - startIndex;
double t1 = -t0;
t0 -= 1;
bool increase = true;
bool decrease = true;
while (increase || decrease)
{
if (increase)
{
double pInd = offsetIndex + t1;
int i = startIndex + offsetIndex;
if (pInd * pInd * cellSizeSquared >= distanceSquared || i >= _Branches.Length)
{
increase = false;
}
else
{
node = _Branches[i];
if (node != null)
{
node.CloseTo(pt, distanceSquared, ref output);
}
}
}
if (decrease)
{
double pInd = offsetIndex + t0;
int i = startIndex - offsetIndex;
if (pInd * pInd * cellSizeSquared >= distanceSquared || i < 0)
{
decrease = false;
}
else
{
node = _Branches[i];
if (node != null)
{
node.CloseTo(pt, distanceSquared, ref output);
}
}
}
offsetIndex++;
}
}
else
{
foreach (T item in _Children)
{
double distSquaredTo = _Tree.DistanceSquaredBetween(pt, item);
if (distSquaredTo < distanceSquared && !output.Contains(item))
{
output.Add(item);
}
}
}
}
