Initial commit
This commit is contained in:
@@ -0,0 +1,299 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.Space;
|
||||
|
||||
/// <summary>
|
||||
/// KD-Tree implementation for efficient 2D spatial search of nodes.
|
||||
/// Thread-safe for read operations after construction.
|
||||
/// Time complexity: O(n log n) build, O(log n) search average case.
|
||||
/// </summary>
|
||||
public class KDTree
|
||||
{
|
||||
private readonly KDTreeNode? _root;
|
||||
private readonly List<GlobalNode> _nodes;
|
||||
|
||||
/// <summary>
|
||||
/// Initializes a new KD-Tree from a collection of nodes.
|
||||
/// </summary>
|
||||
/// <param name="nodes">The nodes to index. Original collection is not modified.</param>
|
||||
/// <exception cref="ArgumentNullException">Thrown when nodes is null.</exception>
|
||||
public KDTree(IEnumerable<GlobalNode> nodes)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(nodes);
|
||||
|
||||
// Create a copy to avoid mutating input
|
||||
_nodes = [.. nodes];
|
||||
|
||||
if (_nodes.Count == 0)
|
||||
{
|
||||
_root = null;
|
||||
return;
|
||||
}
|
||||
|
||||
_root = BuildTree(0, _nodes.Count - 1, 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the total number of nodes in the tree.
|
||||
/// </summary>
|
||||
public int Count => _nodes.Count;
|
||||
|
||||
/// <summary>
|
||||
/// Builds the KD-Tree using index-based recursion to avoid memory allocation overhead.
|
||||
/// Time complexity: O(n log n)
|
||||
/// </summary>
|
||||
private KDTreeNode? BuildTree(int start, int end, int depth)
|
||||
{
|
||||
if (start > end)
|
||||
return null;
|
||||
|
||||
int axis = depth % 2;
|
||||
|
||||
// Use QuickSelect to find median without full sort
|
||||
int medianIndex = QuickSelect(start, end, (start + end) / 2, axis);
|
||||
|
||||
return new KDTreeNode(
|
||||
node: _nodes[medianIndex],
|
||||
axis: axis,
|
||||
left: BuildTree(start, medianIndex - 1, depth + 1),
|
||||
right: BuildTree(medianIndex + 1, end, depth + 1)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// QuickSelect algorithm to find the k-th smallest element.
|
||||
/// Time complexity: O(n) average, O(n²) worst case.
|
||||
/// </summary>
|
||||
private int QuickSelect(int left, int right, int k, int axis)
|
||||
{
|
||||
while (left < right)
|
||||
{
|
||||
int pivotIndex = Partition(left, right, axis);
|
||||
|
||||
if (pivotIndex == k)
|
||||
return k;
|
||||
else if (k < pivotIndex)
|
||||
right = pivotIndex - 1;
|
||||
else
|
||||
left = pivotIndex + 1;
|
||||
}
|
||||
return left;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Partitions the array for QuickSelect using median-of-three pivot selection.
|
||||
/// This is the CORRECTED version that handles all edge cases properly.
|
||||
/// </summary>
|
||||
private int Partition(int left, int right, int axis)
|
||||
{
|
||||
// Handle small subarrays
|
||||
if (right - left < 2)
|
||||
{
|
||||
if (right > left && CompareNodes(_nodes[right], _nodes[left], axis) < 0)
|
||||
Swap(left, right);
|
||||
return left;
|
||||
}
|
||||
|
||||
// Use median-of-three pivot selection for better performance
|
||||
int mid = left + (right - left) / 2; // Overflow-safe
|
||||
|
||||
// Sort left, mid, right to get median as pivot
|
||||
if (CompareNodes(_nodes[mid], _nodes[left], axis) < 0)
|
||||
Swap(left, mid);
|
||||
if (CompareNodes(_nodes[right], _nodes[left], axis) < 0)
|
||||
Swap(left, right);
|
||||
if (CompareNodes(_nodes[right], _nodes[mid], axis) < 0)
|
||||
Swap(mid, right);
|
||||
|
||||
// Now: nodes[left] <= nodes[mid] <= nodes[right]
|
||||
// Use mid as pivot and hide it at right-1
|
||||
GlobalNode pivot = _nodes[mid];
|
||||
Swap(mid, right - 1);
|
||||
|
||||
// Partition with pivot at right-1
|
||||
int i = left + 1; // Start after left (which is already <= pivot)
|
||||
int j = right - 2; // Start before pivot position
|
||||
|
||||
while (i <= j)
|
||||
{
|
||||
// Find element >= pivot from left
|
||||
while (i <= j && CompareNodes(_nodes[i], pivot, axis) < 0)
|
||||
i++;
|
||||
|
||||
// Find element <= pivot from right
|
||||
while (i <= j && CompareNodes(_nodes[j], pivot, axis) > 0)
|
||||
j--;
|
||||
|
||||
if (i < j)
|
||||
{
|
||||
Swap(i, j);
|
||||
i++;
|
||||
j--;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Put pivot in its final position
|
||||
Swap(i, right - 1);
|
||||
return i;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Compares two nodes along the specified axis.
|
||||
/// </summary>
|
||||
private static int CompareNodes(GlobalNode a, GlobalNode b, int axis)
|
||||
{
|
||||
return axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Swaps two elements in the nodes list using tuple deconstruction.
|
||||
/// </summary>
|
||||
private void Swap(int i, int j)
|
||||
{
|
||||
if (i != j)
|
||||
{
|
||||
(_nodes[j], _nodes[i]) = (_nodes[i], _nodes[j]);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finds the nearest node to the given coordinates within the specified distance limit.
|
||||
/// Time complexity: O(log n) average, O(n) worst case.
|
||||
/// </summary>
|
||||
/// <param name="x">X coordinate</param>
|
||||
/// <param name="y">Y coordinate</param>
|
||||
/// <param name="limitDistance">Maximum search distance</param>
|
||||
/// <returns>The nearest node within limit, or null if none found</returns>
|
||||
/// <exception cref="ArgumentOutOfRangeException">Thrown when limitDistance is negative.</exception>
|
||||
public GlobalNode? FindNearest(double x, double y, double limitDistance)
|
||||
{
|
||||
ArgumentOutOfRangeException.ThrowIfNegative(limitDistance);
|
||||
|
||||
if (_root == null)
|
||||
return null;
|
||||
|
||||
double limitDistSquared = limitDistance * limitDistance;
|
||||
var result = FindNearestRecursive(_root, x, y, null, double.MaxValue, limitDistSquared);
|
||||
|
||||
return result.BestNode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Recursively finds the nearest node using squared distances to avoid sqrt operations.
|
||||
/// </summary>
|
||||
private static SearchResult FindNearestRecursive(
|
||||
KDTreeNode? node,
|
||||
double x,
|
||||
double y,
|
||||
GlobalNode? bestNode,
|
||||
double bestDistSquared,
|
||||
double limitDistSquared)
|
||||
{
|
||||
if (node == null)
|
||||
return new SearchResult(bestNode, bestDistSquared);
|
||||
|
||||
// Calculate squared distance (avoid sqrt for performance)
|
||||
double dx = node.Node.X - x;
|
||||
double dy = node.Node.Y - y;
|
||||
double distSquared = dx * dx + dy * dy;
|
||||
|
||||
// Update best if this node is closer and within limit
|
||||
if (distSquared < bestDistSquared && distSquared <= limitDistSquared)
|
||||
{
|
||||
bestNode = node.Node;
|
||||
bestDistSquared = distSquared;
|
||||
}
|
||||
|
||||
// Determine which side to search first
|
||||
double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
|
||||
KDTreeNode? nearSide = delta < 0 ? node.Left : node.Right;
|
||||
KDTreeNode? farSide = delta < 0 ? node.Right : node.Left;
|
||||
|
||||
// Search near side
|
||||
var result = FindNearestRecursive(nearSide, x, y, bestNode, bestDistSquared, limitDistSquared);
|
||||
bestNode = result.BestNode;
|
||||
bestDistSquared = result.BestDistSquared;
|
||||
|
||||
// Only search far side if it could contain a closer point
|
||||
double deltaSquared = delta * delta;
|
||||
if (deltaSquared < bestDistSquared)
|
||||
{
|
||||
result = FindNearestRecursive(farSide, x, y, bestNode, bestDistSquared, limitDistSquared);
|
||||
bestNode = result.BestNode;
|
||||
bestDistSquared = result.BestDistSquared;
|
||||
}
|
||||
|
||||
return new SearchResult(bestNode, bestDistSquared);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finds all nodes within the specified radius from the given coordinates.
|
||||
/// </summary>
|
||||
/// <param name="x">X coordinate</param>
|
||||
/// <param name="y">Y coordinate</param>
|
||||
/// <param name="radius">Search radius</param>
|
||||
/// <returns>List of all nodes within the radius</returns>
|
||||
public List<GlobalNode> FindInRadius(double x, double y, double radius)
|
||||
{
|
||||
ArgumentOutOfRangeException.ThrowIfNegative(radius);
|
||||
|
||||
if (_root == null)
|
||||
return [];
|
||||
|
||||
var result = new List<GlobalNode>();
|
||||
double radiusSquared = radius * radius;
|
||||
FindInRadiusRecursive(_root, x, y, radiusSquared, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Recursively finds all nodes within radius.
|
||||
/// </summary>
|
||||
private static void FindInRadiusRecursive(
|
||||
KDTreeNode? node,
|
||||
double x,
|
||||
double y,
|
||||
double radiusSquared,
|
||||
List<GlobalNode> result)
|
||||
{
|
||||
if (node == null)
|
||||
return;
|
||||
|
||||
double dx = node.Node.X - x;
|
||||
double dy = node.Node.Y - y;
|
||||
double distSquared = dx * dx + dy * dy;
|
||||
|
||||
if (distSquared <= radiusSquared)
|
||||
result.Add(node.Node);
|
||||
|
||||
double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
|
||||
double deltaSquared = delta * delta;
|
||||
|
||||
// Search both sides if sphere intersects splitting plane
|
||||
if (delta < 0)
|
||||
{
|
||||
FindInRadiusRecursive(node.Left, x, y, radiusSquared, result);
|
||||
if (deltaSquared <= radiusSquared)
|
||||
FindInRadiusRecursive(node.Right, x, y, radiusSquared, result);
|
||||
}
|
||||
else
|
||||
{
|
||||
FindInRadiusRecursive(node.Right, x, y, radiusSquared, result);
|
||||
if (deltaSquared <= radiusSquared)
|
||||
FindInRadiusRecursive(node.Left, x, y, radiusSquared, result);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Internal struct to return search results without allocations.
|
||||
/// </summary>
|
||||
private readonly struct SearchResult(GlobalNode? bestNode, double bestDistSquared)
|
||||
{
|
||||
public readonly GlobalNode? BestNode = bestNode;
|
||||
public readonly double BestDistSquared = bestDistSquared;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user