QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

281
srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/AStar/AStarPlanner.cs Normal file
View File

@@ -0,0 +1,281 @@
using RobotNet10.GlobalPathPlanner.Model;
using RobotNet10.GlobalPathPlanner.Space;
namespace RobotNet10.GlobalPathPlanner.AStar;
public class AStarPlanner(List<GlobalNode> Nodes, List<GlobalEdge> Edges)
{
private GlobalEdge[]? GetClosesEdges(GlobalNode nodeRef, double limitDistance)
{
double minDistance = double.MaxValue;
List<GlobalEdge> edgesResult = [];
foreach (var edge in Edges)
{
var startNode = Nodes.FirstOrDefault(node => node.Id == edge.StartNodeId);
var endNode = Nodes.FirstOrDefault(node => node.Id == edge.EndNodeId);
if (startNode is null || endNode is null) continue;
var distance = MathExtensions.DistanceToEdge(nodeRef, startNode, endNode, edge);
if (distance < minDistance)
{
minDistance = distance;
edgesResult = [edge];
var reverseEdge = Edges.FirstOrDefault(e => e.EndNodeId == startNode.Id && e.StartNodeId == endNode.Id);
if (reverseEdge != null) edgesResult = [.. edgesResult, reverseEdge];
}
}
if (minDistance <= limitDistance) return [.. edgesResult];
else return null;
}
private GlobalNode? GetOnNode(double x, double y, double limitDistance)
{
KDTree KDTree = new(Nodes);
return KDTree.FindNearest(x, y, limitDistance);
}
private List<GlobalNode> GetNegativeNodes(Guid nodeId)
{
var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
if (node is null) return [];
var ListNegativeNodes = new List<GlobalNode>();
var ListPaths = Edges.Where(p => p.StartNodeId == nodeId);
foreach (var path in ListPaths)
{
var negativeNode = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
if (negativeNode is not null) ListNegativeNodes.Add(negativeNode);
}
return ListNegativeNodes;
}
private double GetNegativeCost(AStarNode currenNode, AStarNode negativeNode)
{
var negativeEdges = Edges.Where(e => e.StartNodeId == currenNode.Id && e.EndNodeId == negativeNode.Id || e.StartNodeId == negativeNode.Id && e.EndNodeId == currenNode.Id).ToList();
double minDistance = double.MaxValue;
foreach (var edge in negativeEdges)
{
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
if (startNode is null || endNode is null) return 0;
var distance = MathExtensions.GetEdgeLength(startNode, endNode, edge);
if (distance < minDistance) minDistance = distance;
}
return minDistance != double.MaxValue ? minDistance : 0;
}
private List<AStarNode> GetNegativeAStarNodes(AStarNode nodeCurrent, GlobalNode endNode)
{
var possiblePointNegative = new List<AStarNode>();
foreach (var nodeNegative in nodeCurrent.NegativeNodes)
{
nodeNegative.Parent = nodeCurrent;
var ListNodesNegative = GetNegativeNodes(nodeNegative.Id);
foreach (var item in ListNodesNegative)
{
nodeNegative.NegativeNodes.Add(new AStarNode()
{
Id = item.Id,
X = item.X,
Y = item.Y,
Name = item.Name,
});
}
var cost = GetNegativeCost(nodeCurrent, nodeNegative);
nodeNegative.Cost = (cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2))) + nodeCurrent.Cost;
nodeNegative.Heuristic = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
possiblePointNegative.Add(nodeNegative);
}
return possiblePointNegative;
}
private List<AStarNode> Find(AStarNode startNode, GlobalNode endNode, CancellationToken? cancellationToken = null)
{
try
{
var activeNodes = new PriorityQueue<AStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
var visitedNodes = new HashSet<AStarNode>();
List<AStarNode> Path = [];
activeNodes.Enqueue(startNode);
while (activeNodes.Count != 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
{
var checkNode = activeNodes.Dequeue();
if (checkNode.Id == endNode.Id)
{
var node = checkNode;
while (node != null)
{
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
Path.Add(node);
node = node.Parent;
}
return Path;
}
visitedNodes.Add(checkNode);
var ListNodeNegative = GetNegativeAStarNodes(checkNode, endNode);
foreach (var node in ListNodeNegative)
{
if (visitedNodes.TryGetValue(node, out AStarNode? value) && value is not null)
{
if (value.TotalCost > node.TotalCost)
{
visitedNodes.Remove(value);
activeNodes.Enqueue(node);
}
continue;
}
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id);
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
{
activeNodes.Items.Remove(activeNode);
activeNodes.Enqueue(node);
}
else if (activeNode is null)
{
activeNodes.Enqueue(node);
}
}
}
return [];
}
catch (OperationCanceledException)
{
return [];
}
}
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) Planning(double x, double y, GlobalNode goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
{
var Path = new List<GlobalNode>();
AStarNode RobotNode = new()
{
Id = Guid.NewGuid(),
X = x,
Y = y,
Name = "RobotCurrentNode",
};
GlobalEdge[] closesEdges = [];
var closesNode = GetOnNode(x, y, maxDistanceToNode);
if (closesNode is not null)
{
if (closesNode.Id == goal.Id) return ([goal], null);
RobotNode.Name = closesNode.Name;
RobotNode.Id = closesNode.Id;
RobotNode.X = closesNode.X;
RobotNode.Y = closesNode.Y;
foreach (var negativeNode in GetNegativeNodes(RobotNode.Id))
{
var cost = GetNegativeCost(RobotNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
RobotNode.NegativeNodes.Add(new()
{
Id = negativeNode.Id,
X = negativeNode.X,
Y = negativeNode.Y,
Name = negativeNode.Name,
Cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(RobotNode.X - negativeNode.X, 2) + Math.Pow(RobotNode.Y - negativeNode.Y, 2)),
Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
});
}
}
else
{
closesEdges = GetClosesEdges(new() { X = x, Y = y }, maxDistanceToEdge) ?? [];
if (closesEdges is null || closesEdges.Length == 0) throw new Exception("The robot is too far from the route");
var edgeToGoal = closesEdges.FirstOrDefault(e => e.EndNodeId == goal.Id);
if (edgeToGoal != null)
{
var endNode = Nodes.FirstOrDefault(n => n.Id == edgeToGoal.EndNodeId);
if (endNode != null) return ([new() {
Id = RobotNode.Id,
X = RobotNode.X,
Y = RobotNode.Y,
Name = RobotNode.Name,
MapId = endNode.MapId,
}, endNode], edgeToGoal);
}
foreach (var edge in closesEdges)
{
var node = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
if (node == null) continue;
RobotNode.NegativeNodes.Add(new()
{
Id = node.Id,
X = node.X,
Y = node.Y,
Name = node.Name,
Cost = Math.Sqrt(Math.Pow(RobotNode.X - node.X, 2) + Math.Pow(RobotNode.Y - node.Y, 2)),
Heuristic = Math.Abs(goal.X - node.X) + Math.Abs(goal.Y - node.Y),
});
}
}
if (RobotNode.NegativeNodes.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
var path = Find(RobotNode, goal, cancellationToken);
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
path.Reverse();
foreach (var node in path)
{
if (node.Name == "RobotCurrentNode")
{
Path.Add(new()
{
Id = RobotNode.Id,
Name = RobotNode.Name,
X = RobotNode.X,
Y = RobotNode.Y,
});
continue;
}
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
Path.Add(nodedb);
}
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
if (startEdge is null && closesEdges is not null && closesEdges.Length > 0)
{
startEdge = path.Count > 1 ? closesEdges.FirstOrDefault(e => e.EndNodeId == path[1].Id) : null;
}
Console.WriteLine($"AStar Planner Found: {string.Join(",", Path.Select(n => $"({n.X} - {n.Y})"))}");
return ([.. Path], startEdge);
}
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) Planning(GlobalNode startNode, GlobalNode goal, CancellationToken? cancellationToken = null)
{
var Path = new List<GlobalNode>();
var currentNode = new AStarNode
{
Id = startNode.Id,
X = startNode.X,
Y = startNode.Y,
NegativeNodes = [..GetNegativeNodes(startNode.Id).Select(n => new AStarNode
{
Id = n.Id,
Name = n.Name,
X = n.X,
Y = n.Y,
Cost = Math.Sqrt(Math.Pow(startNode.X - n.X, 2) + Math.Pow(startNode.Y - n.Y, 2)),
Heuristic = Math.Abs(goal.X - n.X) + Math.Abs(goal.Y - n.Y),
})],
};
var path = Find(currentNode, goal, cancellationToken);
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
path.Reverse();
foreach (var node in path)
{
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
Path.Add(nodedb);
}
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
return ([.. Path], startEdge);
}
}