62 lines
2.0 KiB
C#
62 lines
2.0 KiB
C#
using RobotNet.MapShares.Dtos;
|
|
|
|
namespace RobotNet.RobotManager.Services.Planner.Space;
|
|
|
|
public class KDTree(List<NodeDto> nodes)
|
|
{
|
|
private readonly KDTreeNode? Root = BuildTree(nodes, 0);
|
|
|
|
private static KDTreeNode? BuildTree(List<NodeDto> nodes, int depth)
|
|
{
|
|
if (nodes.Count == 0) return null;
|
|
int axis = depth % 2;
|
|
nodes.Sort((a, b) => axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y));
|
|
int median = nodes.Count / 2;
|
|
|
|
return new KDTreeNode
|
|
{
|
|
Node = nodes[median],
|
|
Axis = axis,
|
|
Left = BuildTree(nodes.GetRange(0, median), depth + 1),
|
|
Right = BuildTree(nodes.GetRange(median + 1, nodes.Count - median - 1), depth + 1)
|
|
};
|
|
}
|
|
|
|
public NodeDto? FindNearest(double x, double y, double limitDistance)
|
|
{
|
|
if (Root is null) return null;
|
|
var (node, dist) = Nearest(Root, x, y, null, double.MaxValue);
|
|
return dist <= limitDistance ? node : null;
|
|
}
|
|
|
|
private static (NodeDto?, double) Nearest(KDTreeNode? node, double x, double y, NodeDto? best, double bestDist)
|
|
{
|
|
if (node == null) return (best, bestDist);
|
|
|
|
double d = Math.Sqrt(Math.Pow(node.Node.X - x, 2) + Math.Pow(node.Node.Y - y, 2));
|
|
if (d < bestDist)
|
|
{
|
|
best = node.Node;
|
|
bestDist = d;
|
|
}
|
|
|
|
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;
|
|
|
|
(best, bestDist) = Nearest(nearSide, x, y, best, bestDist);
|
|
if (Math.Abs(delta) < bestDist)
|
|
(best, bestDist) = Nearest(farSide, x, y, best, bestDist);
|
|
|
|
return (best, bestDist);
|
|
}
|
|
}
|
|
|
|
public class KDTreeNode
|
|
{
|
|
public NodeDto Node { get; set; } = new();
|
|
public KDTreeNode? Left { get; set; }
|
|
public KDTreeNode? Right { get; set; }
|
|
public int Axis { get; set; } // 0 for X, 1 for Y
|
|
}
|