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); } } } }