first commit -push
This commit is contained in:
dungtt
61
RobotNet.RobotManager/Services/Planner/Space/KDTree.cs
Normal file
61
RobotNet.RobotManager/Services/Planner/Space/KDTree.cs
Normal file
@@ -0,0 +1,61 @@
|
||||
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
|
||||
}
|
||||
86
RobotNet.RobotManager/Services/Planner/Space/MapCompute.cs
Normal file
86
RobotNet.RobotManager/Services/Planner/Space/MapCompute.cs
Normal file
@@ -0,0 +1,86 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Space;
|
||||
|
||||
public class MapCompute
|
||||
{
|
||||
public static double DistanceToCurveEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
double dMin = Math.Sqrt(Math.Pow(x - startNode.X, 2) + Math.Pow(y - startNode.Y, 2));
|
||||
var length = MapEditorHelper.GetEdgeLength(new()
|
||||
{
|
||||
X1 = startNode.X,
|
||||
Y1 = startNode.Y,
|
||||
X2 = endNode.X,
|
||||
Y2 = endNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
});
|
||||
double step = 0.1 / (length == 0 ? 0.1 : length);
|
||||
|
||||
for (double t = 0; t <= 1; t += step)
|
||||
{
|
||||
(double curveX, double curveY) = MapEditorHelper.Curve(t, new()
|
||||
{
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y= edge.ControlPoint2Y,
|
||||
X1 = startNode.X,
|
||||
Y1 = startNode.Y,
|
||||
X2 = endNode.X,
|
||||
Y2 = endNode.Y,
|
||||
});
|
||||
double d = Math.Sqrt(Math.Pow(x - curveX, 2) + Math.Pow(y - curveY, 2));
|
||||
if (d < dMin) dMin = d;
|
||||
}
|
||||
|
||||
return dMin;
|
||||
}
|
||||
|
||||
public static MessageResult<double> DistanceToEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
if (edge.TrajectoryDegree == TrajectoryDegree.One)
|
||||
{
|
||||
double time = 0;
|
||||
var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
|
||||
if (edgeLengthSquared > 0)
|
||||
{
|
||||
time = Math.Max(0, Math.Min(1, ((x - startNode.X) * (endNode.X - startNode.X) + (y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
|
||||
}
|
||||
|
||||
double nearestX = startNode.X + time * (endNode.X - startNode.X);
|
||||
double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);
|
||||
|
||||
return new(true) { Data = Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)) };
|
||||
}
|
||||
else
|
||||
{
|
||||
return new(true) { Data = DistanceToCurveEdge(x, y, edge, startNode, endNode) };
|
||||
}
|
||||
}
|
||||
|
||||
public static Direction GetNodeDirection(RobotDirection robotDirection) =>
|
||||
robotDirection switch
|
||||
{
|
||||
RobotDirection.FORWARD => Direction.FORWARD,
|
||||
RobotDirection.BACKWARD => Direction.BACKWARD,
|
||||
_ => Direction.NONE
|
||||
};
|
||||
|
||||
public static RobotDirection GetRobotDirection(Direction nodeDirection) =>
|
||||
nodeDirection switch
|
||||
{
|
||||
Direction.FORWARD => RobotDirection.FORWARD,
|
||||
Direction.BACKWARD => RobotDirection.BACKWARD,
|
||||
_ => RobotDirection.NONE
|
||||
};
|
||||
}
|
||||
206
RobotNet.RobotManager/Services/Planner/Space/RTree.cs
Normal file
206
RobotNet.RobotManager/Services/Planner/Space/RTree.cs
Normal file
@@ -0,0 +1,206 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Space;
|
||||
|
||||
public class Rectangle(double minX, double minY, double maxX, double maxY)
|
||||
{
|
||||
public double MinX { get; set; } = minX;
|
||||
public double MinY { get; set; } = minY;
|
||||
public double MaxX { get; set; } = maxX;
|
||||
public double MaxY { get; set; } = maxY;
|
||||
|
||||
public double DistanceToPoint(double x, double y)
|
||||
{
|
||||
double dx = Math.Max(Math.Max(MinX - x, 0), x - MaxX);
|
||||
double dy = Math.Max(Math.Max(MinY - y, 0), y - MaxY);
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
public bool Contains(double x, double y)
|
||||
{
|
||||
return x >= MinX && x <= MaxX && y >= MinY && y <= MaxY;
|
||||
}
|
||||
}
|
||||
|
||||
public class RTreeNode
|
||||
{
|
||||
public Rectangle Bounds { get; set; } = new Rectangle(0, 0, 0, 0);
|
||||
public List<RTreeNode> Children { get; set; } = [];
|
||||
public List<(EdgeDto Edge, Rectangle Rect)> Entries { get; set; } = [];
|
||||
public bool IsLeaf => Children.Count == 0;
|
||||
}
|
||||
|
||||
public class RTree
|
||||
{
|
||||
private readonly RTreeNode Root;
|
||||
private const int MaxEntries = 4;
|
||||
private readonly List<NodeDto> Nodes;
|
||||
|
||||
public RTree(List<NodeDto> nodes, List<EdgeDto> edges)
|
||||
{
|
||||
Nodes = nodes;
|
||||
Root = new RTreeNode();
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var rect = CalculateMBR(edge);
|
||||
Insert(Root, (edge, rect));
|
||||
}
|
||||
}
|
||||
|
||||
private Rectangle CalculateMBR(EdgeDto edge)
|
||||
{
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (startNode == null || endNode == null) return new Rectangle(0, 0, 0, 0);
|
||||
|
||||
double minX = Math.Min(startNode.X, endNode.X);
|
||||
double maxX = Math.Max(startNode.X, endNode.X);
|
||||
double minY = Math.Min(startNode.Y, endNode.Y);
|
||||
double maxY = Math.Max(startNode.Y, endNode.Y);
|
||||
|
||||
// Mở rộng MBR nếu edge là đường cong
|
||||
if (edge.TrajectoryDegree != TrajectoryDegree.One)
|
||||
{
|
||||
minX = Math.Min(minX, Math.Min(edge.ControlPoint1X, edge.ControlPoint2X));
|
||||
maxX = Math.Max(maxX, Math.Max(edge.ControlPoint1X, edge.ControlPoint2X));
|
||||
minY = Math.Min(minY, Math.Min(edge.ControlPoint1Y, edge.ControlPoint2Y));
|
||||
maxY = Math.Max(maxY, Math.Max(edge.ControlPoint1Y, edge.ControlPoint2Y));
|
||||
}
|
||||
|
||||
return new Rectangle(minX, minY, maxX, maxY);
|
||||
}
|
||||
|
||||
private static void Insert(RTreeNode node, (EdgeDto Edge, Rectangle Rect) entry)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
node.Entries.Add(entry);
|
||||
if (node.Entries.Count > MaxEntries)
|
||||
{
|
||||
SplitNode(node);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var bestChild = ChooseBestChild(node, entry.Rect);
|
||||
Insert(bestChild, entry);
|
||||
AdjustBounds(bestChild);
|
||||
}
|
||||
|
||||
node.Bounds.MinX = Math.Min(node.Bounds.MinX, entry.Rect.MinX);
|
||||
node.Bounds.MinY = Math.Min(node.Bounds.MinY, entry.Rect.MinY);
|
||||
node.Bounds.MaxX = Math.Max(node.Bounds.MaxX, entry.Rect.MaxX);
|
||||
node.Bounds.MaxY = Math.Max(node.Bounds.MaxY, entry.Rect.MaxY);
|
||||
}
|
||||
|
||||
private static RTreeNode ChooseBestChild(RTreeNode node, Rectangle rect)
|
||||
{
|
||||
RTreeNode best = node.Children[0];
|
||||
double minEnlargement = CalculateEnlargement(best.Bounds, rect);
|
||||
foreach (var child in node.Children.Skip(1))
|
||||
{
|
||||
double enlargement = CalculateEnlargement(child.Bounds, rect);
|
||||
if (enlargement < minEnlargement)
|
||||
{
|
||||
minEnlargement = enlargement;
|
||||
best = child;
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
private static double CalculateEnlargement(Rectangle bounds, Rectangle rect)
|
||||
{
|
||||
double newArea = (Math.Max(bounds.MaxX, rect.MaxX) - Math.Min(bounds.MinX, rect.MinX)) *
|
||||
(Math.Max(bounds.MaxY, rect.MaxY) - Math.Min(bounds.MinY, rect.MinY));
|
||||
double oldArea = (bounds.MaxX - bounds.MinX) * (bounds.MaxY - bounds.MinY);
|
||||
return newArea - oldArea;
|
||||
}
|
||||
|
||||
private static void SplitNode(RTreeNode node)
|
||||
{
|
||||
var (group1, group2) = SplitEntries(node.Entries);
|
||||
node.Children.Add(new RTreeNode { Entries = group1 });
|
||||
node.Children.Add(new RTreeNode { Entries = group2 });
|
||||
node.Entries.Clear();
|
||||
foreach (var child in node.Children)
|
||||
{
|
||||
child.Bounds = CalculateBounds(child.Entries);
|
||||
}
|
||||
}
|
||||
|
||||
private static (List<(EdgeDto, Rectangle)>, List<(EdgeDto, Rectangle)>) SplitEntries(List<(EdgeDto Edge, Rectangle Rect)> entries)
|
||||
{
|
||||
entries.Sort((a, b) => a.Rect.MinX.CompareTo(b.Rect.MinX));
|
||||
int mid = entries.Count / 2;
|
||||
return (entries.GetRange(0, mid), entries.GetRange(mid, entries.Count - mid));
|
||||
}
|
||||
|
||||
private static Rectangle CalculateBounds(List<(EdgeDto Edge, Rectangle Rect)> entries)
|
||||
{
|
||||
if (entries.Count == 0) return new(0, 0, 0, 0);
|
||||
var first = entries[0].Rect;
|
||||
double minX = first.MinX, minY = first.MinY, maxX = first.MaxX, maxY = first.MaxY;
|
||||
foreach (var (Edge, Rect) in entries.Skip(1))
|
||||
{
|
||||
minX = Math.Min(minX, Rect.MinX);
|
||||
minY = Math.Min(minY, Rect.MinY);
|
||||
maxX = Math.Max(maxX, Rect.MaxX);
|
||||
maxY = Math.Max(maxY, Rect.MaxY);
|
||||
}
|
||||
return new Rectangle(minX, minY, maxX, maxY);
|
||||
}
|
||||
|
||||
private static void AdjustBounds(RTreeNode node)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
node.Bounds = CalculateBounds(node.Entries);
|
||||
}
|
||||
else
|
||||
{
|
||||
node.Bounds = CalculateBounds(node.Children.SelectMany(c => c.Entries).ToList());
|
||||
}
|
||||
}
|
||||
|
||||
public EdgeDto? FindNearest(double x, double y, double limitDistance)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
EdgeDto? nearestEdge = null;
|
||||
SearchNearest(Root, x, y, ref minDistance, ref nearestEdge);
|
||||
return minDistance <= limitDistance ? nearestEdge : null;
|
||||
}
|
||||
|
||||
private void SearchNearest(RTreeNode node, double x, double y, ref double minDistance, ref EdgeDto? nearestEdge)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
foreach (var (edge, rect) in node.Entries)
|
||||
{
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (startNode == null || endNode == null) continue;
|
||||
|
||||
var distanceResult = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
|
||||
if (distanceResult.IsSuccess && distanceResult.Data < minDistance)
|
||||
{
|
||||
minDistance = distanceResult.Data;
|
||||
nearestEdge = edge;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var sortedChildren = node.Children.OrderBy(c => c.Bounds.DistanceToPoint(x, y));
|
||||
foreach (var child in sortedChildren)
|
||||
{
|
||||
if (child.Bounds.DistanceToPoint(x, y) < minDistance)
|
||||
{
|
||||
SearchNearest(child, x, y, ref minDistance, ref nearestEdge);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user