Initial commit
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,202 @@
|
||||
using RobotNet10.GlobalPathPlanner.AStar;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.Differential;
|
||||
|
||||
public class DifferentialPlanner : IPathPlanner
|
||||
{
|
||||
private List<GlobalNode> Nodes = [];
|
||||
private List<GlobalEdge> Edges = [];
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private PathPlannerOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 2,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
ChangeOrientationAngle = 89
|
||||
};
|
||||
public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlannerOptions options)
|
||||
{
|
||||
Options = options;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if(Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
|
||||
try
|
||||
{
|
||||
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
||||
(var path, var closesEdge) = AStarPathPlanner.Planning(x, y,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}, {theta}]");
|
||||
if (path.Length == 1) return (path, []);
|
||||
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
|
||||
try
|
||||
{
|
||||
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
||||
(var path, var closesEdge) = AStarPathPlanner.Planning(startNode, goal, cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
|
||||
if (path.Length == 1) return (path, []);
|
||||
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch(OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
|
||||
foreach (var node in basicPath.Nodes)
|
||||
{
|
||||
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,471 @@
|
||||
using RobotNet10.GlobalPathPlanner.AStar;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.Forklift;
|
||||
|
||||
public class ForkliftPathPlanner : IPathPlanner
|
||||
{
|
||||
private List<GlobalNode> Nodes = [];
|
||||
private List<GlobalEdge> Edges = [];
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private PathPlannerOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
ChangeOrientationAngle = 89
|
||||
};
|
||||
public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlannerOptions options)
|
||||
{
|
||||
Options = options;
|
||||
}
|
||||
|
||||
private static bool TStructureExisted(List<TStructure> TStructures, GlobalNode node1, GlobalNode node2, GlobalNode node3)
|
||||
{
|
||||
var TStructureExistedStep1 = TStructures.Where(ts => ts.Node1 == node1 || ts.Node2 == node1 || ts.Node3 == node1).ToList();
|
||||
if (TStructureExistedStep1.Count != 0)
|
||||
{
|
||||
var TStructureExistedStep2 = TStructureExistedStep1.Where(ts => ts.Node1 == node2 || ts.Node2 == node2 || ts.Node3 == node2).ToList();
|
||||
if (TStructureExistedStep2.Count != 0)
|
||||
{
|
||||
var TStructureExistedStep3 = TStructureExistedStep2.Where(ts => ts.Node1 == node3 || ts.Node2 == node3 || ts.Node3 == node3).ToList();
|
||||
if (TStructureExistedStep3.Count != 0) return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private TStructure[] GetTStructure()
|
||||
{
|
||||
List<TStructure> TStructures = [];
|
||||
foreach (var node in Nodes)
|
||||
{
|
||||
var inEdges = Edges.Where(edge => edge.StartNodeId == node.Id || edge.EndNodeId == node.Id).ToList();
|
||||
if (inEdges.Count < 2) continue;
|
||||
List<GlobalNode> inNodes = [];
|
||||
foreach (var edge in inEdges)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == (node.Id == edge.EndNodeId ? edge.StartNodeId : edge.EndNodeId));
|
||||
if (endNode is null) continue;
|
||||
inNodes.Add(endNode);
|
||||
}
|
||||
for (int i = 0; i < inNodes.Count - 1; i++)
|
||||
{
|
||||
for (int j = i + 1; j < inNodes.Count; j++)
|
||||
{
|
||||
if (TStructureExisted(TStructures, node, inNodes[i], inNodes[j])) continue;
|
||||
var edgeT = Edges.FirstOrDefault(e => (e.StartNodeId == inNodes[i].Id && e.EndNodeId == inNodes[j].Id) ||
|
||||
(e.EndNodeId == inNodes[i].Id && e.StartNodeId == inNodes[j].Id));
|
||||
var edge1 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[i].Id || edge.EndNodeId == inNodes[i].Id);
|
||||
var edge2 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[j].Id || edge.EndNodeId == inNodes[j].Id);
|
||||
if (edgeT is null || edge1 is null || edge2 is null) continue;
|
||||
if (edgeT.Degree == 1 &&
|
||||
edge1.Degree == 1 &&
|
||||
edge2.Degree == 1) continue;
|
||||
|
||||
TStructures.Add(new()
|
||||
{
|
||||
Node1 = node,
|
||||
Node2 = inNodes[i],
|
||||
Node3 = inNodes[j],
|
||||
Edge12 = edge1,
|
||||
Edge13 = edge2,
|
||||
Edge23 = edgeT,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
return [.. TStructures];
|
||||
}
|
||||
|
||||
private (bool IsSuccess, GlobalNode? intraNode, TStructure? tstructure) IsReverse(GlobalNode currentNode, GlobalNode olderNode, GlobalNode? futureNode, GlobalEdge olderedge, GlobalEdge? futureedge, double startAngle, List<TStructure> tstructures)
|
||||
{
|
||||
var tstructures1 = tstructures.Where(t => t.Node1.Id == currentNode.Id || t.Node2.Id == currentNode.Id || t.Node3.Id == currentNode.Id).ToList();
|
||||
if (tstructures1 is null || tstructures1.Count < 1) return (false, null, null);
|
||||
var tstructures2 = tstructures1.Where(t => t.Node1.Id == olderNode.Id || t.Node2.Id == olderNode.Id || t.Node3.Id == olderNode.Id).ToList();
|
||||
if (tstructures2 is null || tstructures2.Count < 1) return (false, null, null);
|
||||
foreach (var ts in tstructures2)
|
||||
{
|
||||
var midleReverse = ts.IsDriectionReverse(currentNode, olderNode, Options.ChangeOrientationAngle);
|
||||
var intraNode = ts.GetIntraNode(currentNode, olderNode);
|
||||
if (intraNode is null) continue;
|
||||
|
||||
if (!ts.IsAccessDirection(olderNode, intraNode) || !ts.IsAccessDirection(intraNode, currentNode)) continue;
|
||||
|
||||
var currentDirection = MathExtensions.GetOrientationStart(olderNode, currentNode, olderedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
var intraEdge = ts.GetEdge(olderNode, intraNode);
|
||||
if (intraEdge is null) continue;
|
||||
var branchDirection = MathExtensions.GetOrientationStart(olderNode, intraNode, intraEdge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
bool firstReverse = branchDirection != currentDirection;
|
||||
|
||||
bool endReverse = false;
|
||||
if (futureNode is not null && futureedge is not null)
|
||||
{
|
||||
startAngle = MathExtensions.GetEndAngle(olderNode, currentNode, olderedge, 1 - Ratio);
|
||||
currentDirection = MathExtensions.GetOrientationEnd(currentNode, futureNode, futureedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
intraEdge = ts.GetEdge(currentNode, intraNode);
|
||||
if (intraEdge is null) continue;
|
||||
startAngle = MathExtensions.GetEndAngle(intraNode, currentNode, intraEdge, 1 - Ratio);
|
||||
branchDirection = MathExtensions.GetOrientationEnd(currentNode, futureNode, futureedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
endReverse = branchDirection != currentDirection;
|
||||
}
|
||||
|
||||
if (!midleReverse)
|
||||
{
|
||||
if ((!firstReverse && !endReverse) || (firstReverse && endReverse)) continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((firstReverse && !endReverse) || (!firstReverse && endReverse)) continue;
|
||||
}
|
||||
return (true, intraNode, ts);
|
||||
}
|
||||
return (false, null, null);
|
||||
}
|
||||
|
||||
private List<GlobalNode> GetIntermediateNode(GlobalNode startNode, GlobalNode endNode)
|
||||
{
|
||||
var edge1s = Edges.Where(e => e.StartNodeId == startNode.Id).ToList();
|
||||
var edge2s = Edges.Where(e => e.StartNodeId == endNode.Id).ToList();
|
||||
if (edge1s is null || edge2s is null || edge1s.Count < 2 || edge2s.Count < 2) return [];
|
||||
List<GlobalNode> node1 = [];
|
||||
List<GlobalNode> IntermediateNode = [];
|
||||
foreach (var edge1 in edge1s)
|
||||
{
|
||||
if (edge1.Degree != 1) continue;
|
||||
if (edge1.EndNodeId == endNode.Id) continue;
|
||||
var interNode = Nodes.FirstOrDefault(n => n.Id == edge1.EndNodeId);
|
||||
if (interNode is null) continue;
|
||||
node1.Add(interNode);
|
||||
}
|
||||
if (node1.Count == 0) return [];
|
||||
foreach (var edge2 in edge2s)
|
||||
{
|
||||
if (edge2.Degree != 1) continue;
|
||||
if (edge2.EndNodeId == startNode.Id) continue;
|
||||
var interNode = Nodes.FirstOrDefault(n => n.Id == edge2.EndNodeId);
|
||||
if (interNode is null) continue;
|
||||
if (node1.Any(n => n.Id == interNode.Id) && !IntermediateNode.Any(n => n.Id == interNode.Id) && interNode.Id != startNode.Id)
|
||||
IntermediateNode.Add(interNode);
|
||||
}
|
||||
return IntermediateNode;
|
||||
}
|
||||
|
||||
private (GlobalNode[] NodesFilter, GlobalEdge[] EdgesFilter) FilterPathPlanning(GlobalNode[] nodes, GlobalEdge[] edges, GlobalEdge? closesEdge)
|
||||
{
|
||||
if (nodes.Length <= 1 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return ([], []);
|
||||
List<GlobalNode> nodeFilter = [nodes[0]];
|
||||
for (int i = 1; i < nodes.Length - 1; i++)
|
||||
{
|
||||
var IntermediateNode = GetIntermediateNode(nodes[i - 1], nodes[i]);
|
||||
if (IntermediateNode is null || IntermediateNode.Count == 0)
|
||||
{
|
||||
nodeFilter.Add(nodes[i]);
|
||||
continue;
|
||||
}
|
||||
if (IntermediateNode.Any(n => n.Id == nodes[i + 1].Id))
|
||||
{
|
||||
nodeFilter.Add(nodes[i + 1]);
|
||||
i++;
|
||||
}
|
||||
else nodeFilter.Add(nodes[i]);
|
||||
}
|
||||
if (nodeFilter[^1].Id != nodes[^1].Id)
|
||||
nodeFilter.Add(nodes[^1]);
|
||||
var edgeFilter = MathExtensions.GetEdgesPlanning([.. nodeFilter], [.. Edges], closesEdge);
|
||||
if (nodeFilter.Count - 1 != edgeFilter.Length) return ([], []);
|
||||
return ([.. nodeFilter], [.. edgeFilter]);
|
||||
}
|
||||
|
||||
private (GlobalNode[] Nodes, GlobalEdge[] Edges) CheckPathWithFinalDirection(GlobalNode[] nodes, GlobalEdge[] edges, double currentAngle, Orientation goalDirection = Orientation.NONE)
|
||||
{
|
||||
if ((nodes[^1].Orientation == goalDirection && MathExtensions.GetEdgesLength([.. edges], [.. Nodes]) < 10) || goalDirection == Orientation.NONE)
|
||||
return FilterPathPlanning([.. nodes], [.. edges], null);
|
||||
|
||||
var edgeplannings = edges.ToList();
|
||||
var nodeplannings = nodes.ToList();
|
||||
|
||||
var TStructures = GetTStructure();
|
||||
|
||||
Guid LastReverseDirectionId = Guid.Empty;
|
||||
GlobalNode LastNodeReverseDirection = new();
|
||||
for (int i = 1; i < nodeplannings.Count; i++)
|
||||
{
|
||||
if (nodeplannings[i].Orientation == Orientation.FORWARD) continue;
|
||||
GlobalNode? futureNode = null;
|
||||
GlobalEdge? futureEdge = null;
|
||||
if (i < nodeplannings.Count - 1)
|
||||
{
|
||||
futureNode = nodeplannings[i + 1];
|
||||
futureEdge = edgeplannings[i];
|
||||
}
|
||||
double startAngle = currentAngle;
|
||||
if (i >= 2) startAngle = MathExtensions.GetEndAngle(nodeplannings[i - 2], nodeplannings[i - 1], edgeplannings[i - 2], Ratio);
|
||||
(var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, [.. TStructures]);
|
||||
if (!IsSuccess || intraNode is null || tstructure is null) continue;
|
||||
var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
|
||||
var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
|
||||
if (edge1 is null || edge2 is null) continue;
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
edgeplannings.Insert(i - 1, new()
|
||||
{
|
||||
Id = edge1.Id,
|
||||
StartNodeId = nodeplannings[i - 1].Id,
|
||||
EndNodeId = intraNode.Id,
|
||||
Degree = edge1.Degree,
|
||||
ControlPoint1X = edge1.ControlPoint1X,
|
||||
ControlPoint1Y = edge1.ControlPoint1Y,
|
||||
ControlPoint2X = edge1.ControlPoint2X,
|
||||
ControlPoint2Y = edge1.ControlPoint2Y
|
||||
});
|
||||
edgeplannings.Insert(i, new()
|
||||
{
|
||||
Id = edge2.Id,
|
||||
StartNodeId = intraNode.Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
Degree = edge2.Degree,
|
||||
ControlPoint1X = edge2.ControlPoint1X,
|
||||
ControlPoint1Y = edge2.ControlPoint1Y,
|
||||
ControlPoint2X = edge2.ControlPoint2X,
|
||||
ControlPoint2Y = edge2.ControlPoint2Y
|
||||
});
|
||||
nodeplannings.Insert(i, intraNode);
|
||||
var directionInPath = MathExtensions.GetOrientations(nodeplannings[0].Orientation, [.. nodeplannings], [.. edgeplannings], Ratio, Options.ChangeOrientationAngle);
|
||||
for (int j = 0; j < nodeplannings.Count; j++)
|
||||
{
|
||||
nodeplannings[j].Orientation = directionInPath[j];
|
||||
}
|
||||
LastReverseDirectionId = tstructure.Id;
|
||||
LastNodeReverseDirection = nodeplannings[i + 1];
|
||||
i++;
|
||||
}
|
||||
|
||||
if (nodeplannings[^1].Orientation == goalDirection) return FilterPathPlanning([.. nodeplannings], [.. edgeplannings], null);
|
||||
|
||||
for (int i = nodeplannings.Count - 1; i > 0; i--)
|
||||
{
|
||||
GlobalNode? futureNode = null;
|
||||
GlobalEdge? futureEdge = null;
|
||||
if (i < nodeplannings.Count - 1)
|
||||
{
|
||||
futureNode = nodeplannings[i + 1];
|
||||
futureEdge = edgeplannings[i];
|
||||
}
|
||||
double startAngle = currentAngle;
|
||||
if (i >= 2) startAngle = MathExtensions.GetEndAngle(nodeplannings[i - 2], nodeplannings[i - 1], edgeplannings[i - 2], Ratio);
|
||||
(var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, [.. TStructures]);
|
||||
if (!IsSuccess || intraNode is null || tstructure is null) continue;
|
||||
|
||||
if (nodeplannings[i - 1].Id == LastNodeReverseDirection.Id)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
|
||||
(e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
|
||||
if (edge is null) continue;
|
||||
edgeplannings.Insert(i - 2, new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodeplannings[i - 2].Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
Degree = edge.Degree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y
|
||||
});
|
||||
edgeplannings.RemoveAt(i);
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
nodeplannings.RemoveAt(i - 1);
|
||||
}
|
||||
else if (tstructure.Id != LastReverseDirectionId || i < 2)
|
||||
{
|
||||
var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
|
||||
var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
|
||||
if (edge1 is null || edge2 is null) continue;
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
edgeplannings.Insert(i - 1, new()
|
||||
{
|
||||
Id = edge1.Id,
|
||||
StartNodeId = nodeplannings[i - 1].Id,
|
||||
EndNodeId = intraNode.Id,
|
||||
Degree = edge1.Degree,
|
||||
ControlPoint1X = edge1.ControlPoint1X,
|
||||
ControlPoint1Y = edge1.ControlPoint1Y,
|
||||
ControlPoint2X = edge1.ControlPoint2X,
|
||||
ControlPoint2Y = edge1.ControlPoint2Y,
|
||||
});
|
||||
edgeplannings.Insert(i, new()
|
||||
{
|
||||
Id = edge2.Id,
|
||||
StartNodeId = intraNode.Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
Degree = edge2.Degree,
|
||||
ControlPoint1X = edge2.ControlPoint1X,
|
||||
ControlPoint1Y = edge2.ControlPoint1Y,
|
||||
ControlPoint2X = edge2.ControlPoint2X,
|
||||
ControlPoint2Y = edge2.ControlPoint2Y,
|
||||
});
|
||||
nodeplannings.Insert(i, intraNode);
|
||||
}
|
||||
else
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
|
||||
(e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
|
||||
if (edge is null) continue;
|
||||
edgeplannings.Insert(i - 2, new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodeplannings[i - 2].Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
Degree = edge.Degree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
});
|
||||
edgeplannings.RemoveAt(i);
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
nodeplannings.RemoveAt(i - 1);
|
||||
}
|
||||
var directionInPath = MathExtensions.GetOrientations(nodeplannings[0].Orientation, [.. nodeplannings], [.. edgeplannings], Ratio, Options.ChangeOrientationAngle);
|
||||
if (directionInPath[^1] == goalDirection)
|
||||
{
|
||||
for (int j = 0; j < nodeplannings.Count; j++)
|
||||
{
|
||||
nodeplannings[j].Orientation = directionInPath[j];
|
||||
}
|
||||
return FilterPathPlanning([.. nodeplannings], [.. edgeplannings], null);
|
||||
}
|
||||
}
|
||||
throw new Exception("The path to the destination does not satisfy the conditions");
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
|
||||
try
|
||||
{
|
||||
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
||||
(var path, var closesEdge) = AStarPathPlanner.Planning(x,
|
||||
y,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}, {theta}]");
|
||||
if (path.Length == 1) return (path, []);
|
||||
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i].Orientation = FinalDirection[i];
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
||||
(var path, var closesEdge) = AStarPathPlanner.Planning(startNode, goal, cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
|
||||
if (path.Length == 1) return (path, []);
|
||||
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i].Orientation = FinalDirection[i];
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (basicPath.Nodes.Length < 1) return basicPath;
|
||||
|
||||
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.Forklift;
|
||||
|
||||
public enum TStructureDirection
|
||||
{
|
||||
NODE1_NODE2_NODE3,
|
||||
NODE1_NODE3_NODE2,
|
||||
NODE2_NODE1_NODE3,
|
||||
NODE2_NODE3_NODE1,
|
||||
NODE3_NODE2_NODE1,
|
||||
NODE3_NODE1_NODE2,
|
||||
}
|
||||
|
||||
public class TStructure
|
||||
{
|
||||
public Guid Id { get; set; } = Guid.NewGuid();
|
||||
public GlobalNode Node1 { get; set; } = new();
|
||||
public GlobalNode Node2 { get; set; } = new();
|
||||
public GlobalNode Node3 { get; set; } = new();
|
||||
public GlobalEdge? Edge12 { get; set; }
|
||||
public GlobalEdge? Edge13 { get; set; }
|
||||
public GlobalEdge? Edge23 { get; set; }
|
||||
public GlobalEdge? Edge21 { get; set; }
|
||||
public GlobalEdge? Edge31 { get; set; }
|
||||
public GlobalEdge? Edge32 { get; set; }
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
public bool IsDriectionReverse(TStructureDirection direction, double changeOrientationAngle)
|
||||
{
|
||||
GlobalNode OriginNode = new();
|
||||
GlobalNode ToWardNode1 = new();
|
||||
GlobalNode ToWardNode2 = new();
|
||||
GlobalEdge ToWardEdge1 = new();
|
||||
GlobalEdge ToWardEdge2 = new();
|
||||
|
||||
switch (direction)
|
||||
{
|
||||
case TStructureDirection.NODE3_NODE2_NODE1:
|
||||
if (Edge21 is null || Edge32 is null) return false;
|
||||
OriginNode = Node2;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge21;
|
||||
ToWardEdge2 = Edge32;
|
||||
break;
|
||||
case TStructureDirection.NODE1_NODE2_NODE3:
|
||||
if (Edge12 is null || Edge23 is null) return false;
|
||||
OriginNode = Node2;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge12;
|
||||
ToWardEdge2 = Edge23;
|
||||
break;
|
||||
case TStructureDirection.NODE2_NODE3_NODE1:
|
||||
if (Edge31 is null || Edge23 is null) return false;
|
||||
OriginNode = Node3;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node2;
|
||||
ToWardEdge1 = Edge23;
|
||||
ToWardEdge2 = Edge31;
|
||||
break;
|
||||
case TStructureDirection.NODE1_NODE3_NODE2:
|
||||
if (Edge13 is null || Edge32 is null) return false;
|
||||
OriginNode = Node3;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node2;
|
||||
ToWardEdge1 = Edge13;
|
||||
ToWardEdge2 = Edge32;
|
||||
break;
|
||||
case TStructureDirection.NODE3_NODE1_NODE2:
|
||||
if (Edge31 is null || Edge12 is null) return false;
|
||||
OriginNode = Node1;
|
||||
ToWardNode1 = Node2;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge31;
|
||||
ToWardEdge2 = Edge12;
|
||||
break;
|
||||
case TStructureDirection.NODE2_NODE1_NODE3:
|
||||
if (Edge21 is null || Edge13 is null) return false;
|
||||
OriginNode = Node1;
|
||||
ToWardNode1 = Node2;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge21;
|
||||
ToWardEdge2 = Edge13;
|
||||
break;
|
||||
}
|
||||
|
||||
var NearToWardNode1 = MathExtensions.BezierPoint(Ratio, OriginNode, ToWardNode1, ToWardEdge1);
|
||||
var NearToWardNode3 = MathExtensions.BezierPoint(Ratio, OriginNode, ToWardNode2, ToWardEdge2);
|
||||
var angle = MathExtensions.GetAngle(OriginNode, NearToWardNode1, NearToWardNode3);
|
||||
if (angle < changeOrientationAngle) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
public bool IsDriectionReverse(GlobalNode node1, GlobalNode node2, double changeOrientationAngle)
|
||||
{
|
||||
if (node1.Id == Node1.Id)
|
||||
{
|
||||
if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE3_NODE2, changeOrientationAngle);
|
||||
else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE2_NODE3, changeOrientationAngle);
|
||||
}
|
||||
else if (node1.Id == Node2.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE3_NODE1, changeOrientationAngle);
|
||||
else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE1_NODE3, changeOrientationAngle);
|
||||
}
|
||||
else if (node1.Id == Node3.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE2_NODE1, changeOrientationAngle);
|
||||
else if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE1_NODE2, changeOrientationAngle);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public GlobalNode? GetIntraNode(GlobalNode node1, GlobalNode node2)
|
||||
{
|
||||
if (node1.Id == Node1.Id)
|
||||
{
|
||||
if (node2.Id == Node2.Id) return Node3;
|
||||
else if (node2.Id == Node3.Id) return Node2;
|
||||
}
|
||||
else if (node1.Id == Node2.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return Node3;
|
||||
else if (node2.Id == Node3.Id) return Node1;
|
||||
}
|
||||
else if (node1.Id == Node3.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return Node2;
|
||||
else if (node2.Id == Node2.Id) return Node1;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public GlobalEdge? GetEdge(GlobalNode node1, GlobalNode node2)
|
||||
{
|
||||
if (Edge12 is not null && Edge12.StartNodeId == node1.Id && Edge12.EndNodeId == node2.Id) return Edge12;
|
||||
if (Edge21 is not null && Edge21.StartNodeId == node1.Id && Edge21.EndNodeId == node2.Id) return Edge21;
|
||||
if (Edge13 is not null && Edge13.StartNodeId == node1.Id && Edge13.EndNodeId == node2.Id) return Edge13;
|
||||
if (Edge31 is not null && Edge31.StartNodeId == node1.Id && Edge31.EndNodeId == node2.Id) return Edge31;
|
||||
if (Edge23 is not null && Edge23.StartNodeId == node1.Id && Edge23.EndNodeId == node2.Id) return Edge23;
|
||||
if (Edge32 is not null && Edge32.StartNodeId == node1.Id && Edge32.EndNodeId == node2.Id) return Edge32;
|
||||
return null;
|
||||
}
|
||||
|
||||
public bool IsAccessDirection(GlobalNode startNode, GlobalNode endNode)
|
||||
{
|
||||
return GetEdge(startNode, endNode) is not null;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,331 @@
|
||||
using RobotNet10.GlobalPathPlanner.AStar;
|
||||
using RobotNet10.GlobalPathPlanner.Forklift;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.ForkliftV2;
|
||||
|
||||
public class ForkLiftPathPlannerV2 : IPathPlanner
|
||||
{
|
||||
private List<GlobalNode> Nodes = [];
|
||||
private List<GlobalEdge> Edges = [];
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private PathPlannerOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
ChangeOrientationAngle = 89
|
||||
};
|
||||
public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlannerOptions options)
|
||||
{
|
||||
Options = options;
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y},
|
||||
Orientation.NONE,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanning(x,y,theta,goal.Id, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch(OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalDirection,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(x, y, theta, goal.Id, goalDirection, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalAngle,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(x, y, theta, goal.Id, goalAngle, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Orientation.NONE,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, Orientation.NONE, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalDirection,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, goalDirection, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
|
||||
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
||||
|
||||
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
||||
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
||||
try
|
||||
{
|
||||
var SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalAngle,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(startNodeId, theta, goal.Id, goalAngle, cancellation.Token);
|
||||
}
|
||||
if (path.Length == 1) return (path, []);
|
||||
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
||||
|
||||
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
||||
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
||||
for (int i = 0; i < path.Length; i++)
|
||||
{
|
||||
path[i] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,567 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
using RobotNet10.GlobalPathPlanner.Space;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.ForkliftV2;
|
||||
|
||||
public class SSEAStarPlanner(List<GlobalNode> Nodes, List<GlobalEdge> Edges, PathPlannerOptions Options)
|
||||
{
|
||||
private const double Ratio = 0.01;
|
||||
private GlobalNode? GetOnNode(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
if (cancellationToken?.IsCancellationRequested == true) return null;
|
||||
KDTree KDTree = new(Nodes);
|
||||
return KDTree.FindNearest(x, y, limitDistance);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
public List<GlobalNode> GetNegativeNodes(Guid nodeId)
|
||||
{
|
||||
var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
|
||||
if (node is null) return [];
|
||||
|
||||
var listNodesNegative = 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 != null) listNodesNegative.Add(negativeNode);
|
||||
}
|
||||
return listNodesNegative;
|
||||
}
|
||||
|
||||
private double GetNegativeCost(SSEAStarNode currenNode, SSEAStarNode 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<SSEAStarNode> GetNegativeAStarNodes(SSEAStarNode nodeCurrent, GlobalNode endNode)
|
||||
{
|
||||
var possiblePointNegative = new List<SSEAStarNode>();
|
||||
if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;
|
||||
|
||||
var listNodesNegative = GetNegativeNodes(nodeCurrent.Id);
|
||||
|
||||
foreach (var negativeNode in listNodesNegative)
|
||||
{
|
||||
if (nodeCurrent.Parent is null) continue;
|
||||
var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
|
||||
var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == negativeNode.Id);
|
||||
var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
|
||||
var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == negativeNode.Id);
|
||||
var parentEdge = Edges.FirstOrDefault(e => e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);
|
||||
|
||||
if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;
|
||||
|
||||
var nearNodeNevgative = MathExtensions.BezierPoint(Ratio, nodeDtoCurrent, nodeDtoNegative, negativeEdge);
|
||||
var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MathExtensions.BezierPoint(Ratio, nodeDtoParent, nodeDtoCurrent, parentEdge) :
|
||||
new()
|
||||
{
|
||||
Id = nodeCurrent.Parent.Id,
|
||||
X = nodeCurrent.Parent.X,
|
||||
Y = nodeCurrent.Parent.Y,
|
||||
Name = nodeCurrent.Parent.Name
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
|
||||
Orientation orientation = angle < Options.ChangeOrientationAngle ? nodeCurrent.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD : nodeCurrent.Orientation;
|
||||
|
||||
var nodeNegative = new SSEAStarNode
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Orientation = orientation,
|
||||
Parent = nodeCurrent
|
||||
};
|
||||
|
||||
var cost = GetNegativeCost(nodeCurrent, nodeNegative);
|
||||
cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2));
|
||||
nodeNegative.Cost = cost + nodeCurrent.Cost + (orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0);
|
||||
var distance = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
|
||||
nodeNegative.Heuristic = distance * (1 + (orientation == Orientation.BACKWARD ? Math.Sqrt(2) / 2 : 0.0));
|
||||
possiblePointNegative.Add(nodeNegative);
|
||||
}
|
||||
if (nodeCurrent.NegativeNodes is not null && nodeCurrent.NegativeNodes.Count > 0) possiblePointNegative.AddRange(nodeCurrent.NegativeNodes);
|
||||
return possiblePointNegative;
|
||||
}
|
||||
|
||||
public List<SSEAStarNode> Find(SSEAStarNode startNode, GlobalNode goal, Orientation goalDirection, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
|
||||
var visitedNodes = new HashSet<SSEAStarNode>();
|
||||
var path = new List<SSEAStarNode>();
|
||||
var shortestPath = new HashSet<SSEAStarNode>();
|
||||
|
||||
activeNodes.Enqueue(startNode);
|
||||
|
||||
while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
|
||||
{
|
||||
var checkNode = activeNodes.Dequeue();
|
||||
if (checkNode.Id == goal.Id)
|
||||
{
|
||||
if (checkNode.Orientation == goalDirection || goalDirection == Orientation.NONE)
|
||||
{
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
path.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
else
|
||||
{
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
shortestPath.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
visitedNodes.Add(checkNode);
|
||||
|
||||
var listNodeNegative = GetNegativeAStarNodes(checkNode, goal);
|
||||
foreach (var node in listNodeNegative)
|
||||
{
|
||||
if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
|
||||
{
|
||||
if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
|
||||
{
|
||||
visitedNodes.Remove(value);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Orientation == node.Orientation);
|
||||
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 List<SSEAStarNode> Find(SSEAStarNode startNode, GlobalNode goal, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
|
||||
var visitedNodes = new HashSet<SSEAStarNode>();
|
||||
var path = new List<SSEAStarNode>();
|
||||
var shortestPath = new HashSet<SSEAStarNode>();
|
||||
|
||||
activeNodes.Enqueue(startNode);
|
||||
|
||||
while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
|
||||
{
|
||||
var checkNode = activeNodes.Dequeue();
|
||||
if (checkNode.Id == goal.Id)
|
||||
{
|
||||
if (checkNode.Parent is not null)
|
||||
{
|
||||
var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
|
||||
var edge = Edges.FirstOrDefault(e => e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
|
||||
if (edge is not null && nodeParentDto is not null)
|
||||
{
|
||||
var nearParent = MathExtensions.BezierPoint(Ratio, nodeParentDto, goal, edge);
|
||||
var nearGoalNode = new GlobalNode()
|
||||
{
|
||||
X = goal.X + Math.Cos(goalAngle * Math.PI / 180),
|
||||
Y = goal.Y + Math.Sin(goalAngle * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(goal, nearParent, nearGoalNode);
|
||||
Orientation goalDirection = angle < Options.ChangeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
if (checkNode.Orientation == goalDirection)
|
||||
{
|
||||
var returnNode = checkNode;
|
||||
while (returnNode != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
path.Add(returnNode);
|
||||
returnNode = returnNode.Parent;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
shortestPath.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
}
|
||||
|
||||
visitedNodes.Add(checkNode);
|
||||
|
||||
var listNodeNegative = GetNegativeAStarNodes(checkNode, goal);
|
||||
foreach (var node in listNodeNegative)
|
||||
{
|
||||
if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
|
||||
{
|
||||
if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
|
||||
{
|
||||
visitedNodes.Remove(value);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Orientation == node.Orientation);
|
||||
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 [];
|
||||
}
|
||||
}
|
||||
|
||||
private SSEAStarNode GetClosesNode(GlobalNode closesNode, GlobalNode goal, double theta)
|
||||
{
|
||||
SSEAStarNode closesAStarNode = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
foreach (var negativeNode in GetNegativeNodes(closesAStarNode.Id))
|
||||
{
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(closesNode, negativeNode, RobotNearNode);
|
||||
Orientation orientation = angle < 91 ? Orientation.FORWARD : Orientation.BACKWARD;
|
||||
|
||||
var cost = GetNegativeCost(closesAStarNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
|
||||
cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(closesAStarNode.X - negativeNode.X, 2) + Math.Pow(closesAStarNode.Y - negativeNode.Y, 2));
|
||||
cost += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Orientation = orientation;
|
||||
closesAStarNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Orientation = orientation,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return closesAStarNode;
|
||||
}
|
||||
|
||||
private SSEAStarNode[] GetStartNegativeNodes(GlobalEdge[] closesEdges, GlobalNode goal, SSEAStarNode robotNode, double theta)
|
||||
{
|
||||
List<SSEAStarNode> negativeNodes = [];
|
||||
foreach(var edge in closesEdges)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (endNode == null) continue;
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = robotNode.Id,
|
||||
X = robotNode.X,
|
||||
Y = robotNode.Y,
|
||||
Name = robotNode.Name,
|
||||
};
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = robotNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
var angle = MathExtensions.GetAngle(new() { X = robotNode.X, Y = robotNode.Y}, endNode, RobotNearNode);
|
||||
Orientation orientation = angle < Options.ChangeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;
|
||||
|
||||
double cost = Math.Sqrt(Math.Pow(robotNode.X - endNode.X, 2) + Math.Pow(robotNode.Y - endNode.Y, 2));
|
||||
cost += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Orientation = orientation;
|
||||
negativeNodes.Add(new()
|
||||
{
|
||||
Id = endNode.Id,
|
||||
X = endNode.X,
|
||||
Y = endNode.Y,
|
||||
Name = endNode.Name,
|
||||
Orientation = orientation,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - endNode.X) + Math.Abs(goal.Y - endNode.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return [.. negativeNodes];
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(double x, double y, double theta, GlobalNode goal, Orientation goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
|
||||
SSEAStarNode RobotNode = new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = x,
|
||||
Y = y,
|
||||
Name = "RobotCurrentNode",
|
||||
};
|
||||
var closesNode = GetOnNode(x, y, maxDistanceToNode);
|
||||
if (closesNode is not null)
|
||||
{
|
||||
if (closesNode.Id == goal.Id) return ([goal], null);
|
||||
RobotNode = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var 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);
|
||||
}
|
||||
|
||||
RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
|
||||
}
|
||||
|
||||
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, goalDirection, 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.Id == path.First().Id)
|
||||
{
|
||||
Path.Add(new()
|
||||
{
|
||||
Id = node.Id,
|
||||
Name = node.Name,
|
||||
X = node.X,
|
||||
Y = node.Y,
|
||||
Orientation = node.Orientation,
|
||||
});
|
||||
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(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(double x, double y, double theta, GlobalNode goal, double goalAngle, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
|
||||
SSEAStarNode RobotNode = new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = x,
|
||||
Y = y,
|
||||
Name = "RobotCurrentNode",
|
||||
};
|
||||
var closesNode = GetOnNode(x, y, maxDistanceToNode);
|
||||
if (closesNode is not null)
|
||||
{
|
||||
if (closesNode.Id == goal.Id) return ([goal], null);
|
||||
RobotNode = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var 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);
|
||||
}
|
||||
|
||||
RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
|
||||
}
|
||||
|
||||
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, goalAngle, 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.Id == path.First().Id)
|
||||
{
|
||||
Path.Add(new()
|
||||
{
|
||||
Id = node.Id,
|
||||
Name = node.Name,
|
||||
X = node.X,
|
||||
Y = node.Y,
|
||||
Orientation = node.Orientation,
|
||||
});
|
||||
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(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(GlobalNode startNode, double theta, GlobalNode goal, Orientation goalOrientation, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
var path = Find(RobotNode, goal, goalOrientation, 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(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(GlobalNode startNode, double theta, GlobalNode goal, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
var path = Find(RobotNode, goal, goalAngle, 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(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// Interface for path planning algorithms that calculate optimal routes between nodes in a graph.
|
||||
/// Supports various robot types (differential drive, forklift, omni-drive) with different planning strategies.
|
||||
/// </summary>
|
||||
public interface IPathPlanner
|
||||
{
|
||||
/// <summary>
|
||||
/// Sets the graph data (nodes and edges) that will be used for path planning.
|
||||
/// This method must be called before any path planning operations.
|
||||
/// </summary>
|
||||
/// <param name="nodes">Array of nodes representing waypoints in the map.</param>
|
||||
/// <param name="edges">Array of edges representing connections between nodes.</param>
|
||||
void SetData(GlobalNode[] nodes, GlobalEdge[] edges);
|
||||
|
||||
/// <summary>
|
||||
/// Configures the path planner with custom options such as distance limits, resolution, and timeout.
|
||||
/// This method is optional; if not called, default options will be used.
|
||||
/// </summary>
|
||||
/// <param name="options">Configuration options for the path planner.</param>
|
||||
void SetOptions(PathPlannerOptions options);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path from the specified starting coordinates to the goal node.
|
||||
/// Uses A* algorithm to find the optimal route through the graph.
|
||||
/// </summary>
|
||||
/// <param name="x">Starting X coordinate.</param>
|
||||
/// <param name="y">Starting Y coordinate.</param>
|
||||
/// <param name="theta">Starting orientation angle in degrees.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path with a specified starting direction constraint.
|
||||
/// The planner will attempt to ensure the robot starts moving in the specified direction.
|
||||
/// </summary>
|
||||
/// <param name="x">Starting X coordinate.</param>
|
||||
/// <param name="y">Starting Y coordinate.</param>
|
||||
/// <param name="theta">Starting orientation angle in degrees.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="startDiretion">Desired starting direction (FORWARD, BACKWARD, or NONE to use default).</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path with a specified final direction constraint.
|
||||
/// The planner will attempt to ensure the robot arrives at the goal facing the specified direction.
|
||||
/// </summary>
|
||||
/// <param name="x">Starting X coordinate.</param>
|
||||
/// <param name="y">Starting Y coordinate.</param>
|
||||
/// <param name="theta">Starting orientation angle in degrees.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="goalDirection">Desired final direction at the goal (FORWARD, BACKWARD, or NONE to use default).</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path with a specified final angle constraint.
|
||||
/// The planner will attempt to ensure the robot arrives at the goal with the specified orientation angle.
|
||||
/// </summary>
|
||||
/// <param name="x">Starting X coordinate.</param>
|
||||
/// <param name="y">Starting Y coordinate.</param>
|
||||
/// <param name="theta">Starting orientation angle in degrees.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="goalAngle">Desired final orientation angle in degrees at the goal.</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path from the specified starting node to the goal node.
|
||||
/// This overload uses node IDs instead of coordinates, which is more efficient when the robot is already at a known node.
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">Unique identifier of the starting node.</param>
|
||||
/// <param name="theta">Current orientation angle in degrees at the start node.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the start or goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path from a starting node with a specified starting direction constraint.
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">Unique identifier of the starting node.</param>
|
||||
/// <param name="theta">Current orientation angle in degrees at the start node.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="startDiretion">Desired starting direction (FORWARD, BACKWARD, or NONE to use default).</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the start or goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path from a starting node with a specified final direction constraint.
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">Unique identifier of the starting node.</param>
|
||||
/// <param name="theta">Current orientation angle in degrees at the start node.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="goalDirection">Desired final direction at the goal (FORWARD, BACKWARD, or NONE to use default).</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the start or goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null);
|
||||
|
||||
/// <summary>
|
||||
/// Calculates a path from a starting node with a specified final angle constraint.
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">Unique identifier of the starting node.</param>
|
||||
/// <param name="theta">Current orientation angle in degrees at the start node.</param>
|
||||
/// <param name="goalId">Unique identifier of the goal node.</param>
|
||||
/// <param name="goalAngle">Desired final orientation angle in degrees at the goal.</param>
|
||||
/// <param name="cancellationToken">Optional cancellation token to cancel the operation.</param>
|
||||
/// <returns>A tuple containing the sequence of nodes and edges that form the path from start to goal.</returns>
|
||||
/// <exception cref="Exception">Thrown when the start or goal node does not exist or no path can be found.</exception>
|
||||
/// <exception cref="OperationCanceledException">Thrown when the operation is cancelled.</exception>
|
||||
/// <exception cref="TimeoutException">Thrown when the operation times out.</exception>
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// Factory interface for creating path planner instances optimized for different robot types.
|
||||
/// Each planner type uses algorithms and strategies tailored to the specific kinematics and constraints of the robot.
|
||||
/// </summary>
|
||||
public interface IPathPlannerFactory
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a path planner optimized for differential drive robots.
|
||||
/// Differential drive robots have two independently driven wheels on a common axis.
|
||||
/// </summary>
|
||||
/// <returns>An instance of <see cref="IPathPlanner"/> configured for differential drive robots.</returns>
|
||||
IPathPlanner CreateDifferentialPlanner();
|
||||
|
||||
/// <summary>
|
||||
/// Creates a path planner optimized for forklift robots (version 1).
|
||||
/// Forklift robots have specific constraints related to their lifting mechanism and turning radius.
|
||||
/// </summary>
|
||||
/// <returns>An instance of <see cref="IPathPlanner"/> configured for forklift robots.</returns>
|
||||
IPathPlanner CreateForkliftPlanner();
|
||||
|
||||
/// <summary>
|
||||
/// Creates an enhanced path planner optimized for forklift robots (version 2).
|
||||
/// This version uses improved algorithms (SSE A*) for better performance and path quality.
|
||||
/// </summary>
|
||||
/// <returns>An instance of <see cref="IPathPlanner"/> configured for forklift robots with enhanced algorithms.</returns>
|
||||
IPathPlanner CreateForkliftPlannerV2();
|
||||
|
||||
/// <summary>
|
||||
/// Creates a path planner optimized for omni-directional drive robots.
|
||||
/// Omni-drive robots can move in any direction without changing orientation.
|
||||
/// Note: Currently uses the same planner as differential drive.
|
||||
/// </summary>
|
||||
/// <returns>An instance of <see cref="IPathPlanner"/> configured for omni-directional drive robots.</returns>
|
||||
IPathPlanner CreateOmniDrivePlanner();
|
||||
}
|
||||
@@ -0,0 +1,417 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
using System.ComponentModel.DataAnnotations;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
public class MathExtensions
|
||||
{
|
||||
public static GlobalNode BezierPoint([Range(0, 1)] double t, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
||||
{
|
||||
t = Math.Clamp(t, 0.0, 1.0);
|
||||
if (edge.Degree == 1)
|
||||
{
|
||||
return new()
|
||||
{
|
||||
X = startNode.X + t * (endNode.X - startNode.X),
|
||||
Y = startNode.Y + t * (endNode.Y - startNode.Y)
|
||||
};
|
||||
}
|
||||
else if (edge.Degree == 2)
|
||||
{
|
||||
return new()
|
||||
{
|
||||
X = (1 - t) * (1 - t) * startNode.X + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * endNode.X,
|
||||
Y = (1 - t) * (1 - t) * startNode.Y + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * endNode.Y
|
||||
};
|
||||
}
|
||||
else if (edge.Degree == 3)
|
||||
{
|
||||
return new()
|
||||
{
|
||||
X = Math.Pow(1 - t, 3) * startNode.X + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * endNode.X,
|
||||
Y = Math.Pow(1 - t, 3) * startNode.Y + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * endNode.Y,
|
||||
};
|
||||
}
|
||||
return endNode;
|
||||
}
|
||||
|
||||
public static double GetEdgeLength(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
||||
{
|
||||
var lineLength = Math.Sqrt(Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2));
|
||||
if (edge.Degree == 1)
|
||||
{
|
||||
return lineLength;
|
||||
}
|
||||
else if (edge.Degree == 2)
|
||||
{
|
||||
if (lineLength <= 0) return 0;
|
||||
double step = 0.1 / lineLength;
|
||||
double distance = 0;
|
||||
|
||||
for (double t = step; t <= 1.001; t += step)
|
||||
{
|
||||
var timePoint = BezierPoint(t - step, startNode, endNode, edge);
|
||||
var lastTimePoint = BezierPoint(t, startNode, endNode, edge);
|
||||
distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
|
||||
}
|
||||
|
||||
return Math.Round(distance, 3);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lineLength <= 0) return 0;
|
||||
double step = 0.1 / lineLength;
|
||||
double distance = 0;
|
||||
for (double t = step; t <= 1.001; t += step)
|
||||
{
|
||||
var sTime = t - step;
|
||||
var timePoint = BezierPoint(1 - sTime, startNode, endNode, edge);
|
||||
sTime = t;
|
||||
var lastTimePoint = BezierPoint(1 - sTime, startNode, endNode, edge);
|
||||
|
||||
distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
|
||||
}
|
||||
return Math.Round(distance, 3);
|
||||
}
|
||||
}
|
||||
|
||||
public static double GetEdgesLength(GlobalEdge[] edges, GlobalNode[] Nodes)
|
||||
{
|
||||
if (edges.Length == 0) return -1;
|
||||
double distance = 0;
|
||||
for (int i = 0; i < edges.Length; i++)
|
||||
{
|
||||
var edge = edges[i];
|
||||
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 999;
|
||||
distance += GetEdgeLength(startNode, endNode, edge);
|
||||
}
|
||||
return distance;
|
||||
}
|
||||
|
||||
// Phương pháp chính xác hơn sử dụng giải phương trình bậc 3
|
||||
public static double DistanceToQuadraticBezier(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
||||
{
|
||||
// Đạo hàm của hàm khoảng cách bình phương theo t
|
||||
// Giải phương trình bậc 3: d/dt[|P(t) - G|²] = 0
|
||||
|
||||
double ax = startNode.X - 2 * edge.ControlPoint1X + endNode.X;
|
||||
double ay = startNode.Y - 2 * edge.ControlPoint1Y + endNode.Y;
|
||||
double bx = 2 * (edge.ControlPoint1X - startNode.X);
|
||||
double by = 2 * (edge.ControlPoint1Y - startNode.Y);
|
||||
double cx = startNode.X - nodeRef.X;
|
||||
double cy = startNode.Y - nodeRef.Y;
|
||||
|
||||
// Hệ số của phương trình bậc 3: At³ + Bt² + Ct + D = 0
|
||||
// Từ: (P(t) - G) · P'(t) = 0
|
||||
// Với P(t) = P₀ + bt + at², P'(t) = b + 2at, c = P₀ - G
|
||||
// Khai triển: c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
|
||||
// Vậy: A = 2a·a, B = 3a·b, C = 2c·a + b·b, D = c·b
|
||||
double A = 2 * (ax * ax + ay * ay);
|
||||
double B = 3 * (ax * bx + ay * by);
|
||||
double C = 2 * (ax * cx + ay * cy) + (bx * bx + by * by);
|
||||
double D = bx * cx + by * cy;
|
||||
|
||||
// Tìm các nghiệm của phương trình bậc 3
|
||||
var roots = SolveCubic(A, B, C, D);
|
||||
|
||||
double minDist = double.MaxValue;
|
||||
|
||||
// Kiểm tra khoảng cách tại các điểm tới hạn
|
||||
foreach (double t in roots)
|
||||
{
|
||||
if (t >= 0 && t <= 1)
|
||||
{
|
||||
GlobalNode p = BezierPoint(t, startNode, endNode, edge);
|
||||
double dist = nodeRef.DistanceTo(p);
|
||||
minDist = Math.Min(minDist, dist);
|
||||
}
|
||||
}
|
||||
|
||||
// Kiểm tra khoảng cách tại 2 đầu mút
|
||||
minDist = Math.Min(minDist, nodeRef.DistanceTo(startNode));
|
||||
minDist = Math.Min(minDist, nodeRef.DistanceTo(endNode));
|
||||
|
||||
return minDist;
|
||||
}
|
||||
|
||||
// Giải phương trình bậc 3: ax³ + bx² + cx + d = 0
|
||||
public static double[] SolveCubic(double a, double b, double c, double d)
|
||||
{
|
||||
if (Math.Abs(a) < 1e-10)
|
||||
{
|
||||
// Phương trình bậc 2
|
||||
return SolveQuadratic(b, c, d);
|
||||
}
|
||||
|
||||
// Chuẩn hóa về dạng x³ + px + q = 0
|
||||
b /= a;
|
||||
c /= a;
|
||||
d /= a;
|
||||
|
||||
double p = (3 * c - b * b) / 3;
|
||||
double q = (2 * b * b * b - 9 * b * c + 27 * d) / 27;
|
||||
|
||||
double discriminant = q * q / 4 + p * p * p / 27;
|
||||
|
||||
var roots = new List<double>();
|
||||
|
||||
if (discriminant >= 0)
|
||||
{
|
||||
// Một nghiệm thực
|
||||
double sqrtD = Math.Sqrt(discriminant);
|
||||
double term1 = -q / 2 + sqrtD;
|
||||
double term2 = -q / 2 - sqrtD;
|
||||
|
||||
// Tính căn bậc 3, xử lý số âm
|
||||
// Math.Pow(negative, 1.0/3) trả về NaN trong C#, cần xử lý riêng
|
||||
double u = term1 >= 0
|
||||
? Math.Pow(term1, 1.0 / 3)
|
||||
: -Math.Pow(-term1, 1.0 / 3);
|
||||
double v = term2 >= 0
|
||||
? Math.Pow(term2, 1.0 / 3)
|
||||
: -Math.Pow(-term2, 1.0 / 3);
|
||||
|
||||
double root = u + v - b / 3;
|
||||
if (!double.IsNaN(root) && !double.IsInfinity(root))
|
||||
{
|
||||
roots.Add(root);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Ba nghiệm thực (trường hợp lượng giác)
|
||||
double r = Math.Sqrt(-p * p * p / 27);
|
||||
|
||||
if (r > 1e-10)
|
||||
{
|
||||
double acosArg = -q / (2 * r);
|
||||
// Clamp acosArg vào [-1, 1] để tránh NaN
|
||||
acosArg = Math.Max(-1.0, Math.Min(1.0, acosArg));
|
||||
double phi = Math.Acos(acosArg);
|
||||
double temp = 2 * Math.Pow(r, 1.0 / 3);
|
||||
|
||||
roots.Add(temp * Math.Cos(phi / 3) - b / 3);
|
||||
roots.Add(temp * Math.Cos((phi + 2 * Math.PI) / 3) - b / 3);
|
||||
roots.Add(temp * Math.Cos((phi + 4 * Math.PI) / 3) - b / 3);
|
||||
}
|
||||
}
|
||||
|
||||
return [.. roots];
|
||||
}
|
||||
|
||||
// Giải phương trình bậc 2: ax² + bx + c = 0
|
||||
public static double[] SolveQuadratic(double a, double b, double c)
|
||||
{
|
||||
var roots = new List<double>();
|
||||
|
||||
if (Math.Abs(a) < 1e-10)
|
||||
{
|
||||
if (Math.Abs(b) > 1e-10)
|
||||
{
|
||||
roots.Add(-c / b);
|
||||
}
|
||||
return [.. roots];
|
||||
}
|
||||
|
||||
double discriminant = b * b - 4 * a * c;
|
||||
|
||||
if (discriminant >= 0)
|
||||
{
|
||||
double sqrtD = Math.Sqrt(discriminant);
|
||||
roots.Add((-b + sqrtD) / (2 * a));
|
||||
roots.Add((-b - sqrtD) / (2 * a));
|
||||
}
|
||||
|
||||
return [.. roots];
|
||||
}
|
||||
|
||||
// Phương pháp lấy mẫu - Đơn giản nhưng chậm hơn
|
||||
public static double DistanceToCubicBezier(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
||||
{
|
||||
double bestT = 0;
|
||||
double minDistance = Math.Sqrt(Math.Pow(nodeRef.X - startNode.X, 2) + Math.Pow(nodeRef.Y - startNode.Y, 2));
|
||||
var length = GetEdgeLength(startNode, endNode, edge);
|
||||
double step = 0.1 / (length == 0 ? 0.1 : length);
|
||||
|
||||
// Bước 1: Lấy mẫu thô
|
||||
for (double t = 0; t <= 1; t += step)
|
||||
{
|
||||
GlobalNode p = BezierPoint(t, startNode, endNode, edge);
|
||||
double dist = nodeRef.DistanceTo(p);
|
||||
|
||||
if (dist < minDistance)
|
||||
{
|
||||
minDistance = dist;
|
||||
bestT = t;
|
||||
}
|
||||
}
|
||||
|
||||
// Bước 2: Tối ưu hóa chính xác hơn
|
||||
double epsilon = 1e-6;
|
||||
step = 0.01;
|
||||
|
||||
for (int iter = 0; iter < 10; iter++)
|
||||
{
|
||||
double t1 = Math.Max(0, bestT - step);
|
||||
double t2 = Math.Min(1, bestT + step);
|
||||
|
||||
double d0 = nodeRef.DistanceTo(BezierPoint(t1, startNode, endNode, edge));
|
||||
double d1 = nodeRef.DistanceTo(BezierPoint(bestT, startNode, endNode, edge));
|
||||
double d2 = nodeRef.DistanceTo(BezierPoint(t2, startNode, endNode, edge));
|
||||
|
||||
if (d0 < d1)
|
||||
{
|
||||
bestT = t1;
|
||||
minDistance = d0;
|
||||
}
|
||||
else if (d2 < d1)
|
||||
{
|
||||
bestT = t2;
|
||||
minDistance = d2;
|
||||
}
|
||||
else
|
||||
{
|
||||
step *= 0.5;
|
||||
}
|
||||
|
||||
if (step < epsilon) break;
|
||||
}
|
||||
|
||||
return minDistance;
|
||||
}
|
||||
|
||||
public static double DistanceToEdge(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
||||
{
|
||||
if (edge.Degree == 2)
|
||||
{
|
||||
return DistanceToQuadraticBezier(nodeRef, startNode, endNode, edge);
|
||||
}
|
||||
else if (edge.Degree == 3)
|
||||
{
|
||||
return DistanceToCubicBezier(nodeRef, startNode, endNode, edge);
|
||||
}
|
||||
else
|
||||
{
|
||||
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, ((nodeRef.X - startNode.X) * (endNode.X - startNode.X) + (nodeRef.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 Math.Sqrt(Math.Pow(nodeRef.X - nearestX, 2) + Math.Pow(nodeRef.Y - nearestY, 2));
|
||||
}
|
||||
}
|
||||
|
||||
public static double GetAngle(GlobalNode originNode, GlobalNode Node1, GlobalNode Node2)
|
||||
{
|
||||
double BA_x = Node1.X - originNode.X;
|
||||
double BA_y = Node1.Y - originNode.Y;
|
||||
double BC_x = Node2.X - originNode.X;
|
||||
double BC_y = Node2.Y - originNode.Y;
|
||||
// Tính độ dài của các vector AB và BC
|
||||
double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y);
|
||||
double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y);
|
||||
// Tính tích vô hướng của AB và BC
|
||||
double dotProduct = BA_x * BC_x + BA_y * BC_y;
|
||||
if (lengthAB * lengthBC == 0) return 0;
|
||||
if (dotProduct / (lengthAB * lengthBC) > 1) return 0;
|
||||
if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
|
||||
return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
|
||||
}
|
||||
|
||||
public static double GetStartAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
|
||||
{
|
||||
GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
|
||||
return Math.Atan2(NearNode.Y - startNode.Y, NearNode.X - startNode.X) * 180 / Math.PI;
|
||||
}
|
||||
|
||||
public static double GetEndAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
|
||||
{
|
||||
GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
|
||||
return Math.Atan2(endNode.Y - NearNode.Y, endNode.X - NearNode.X) * 180 / Math.PI;
|
||||
}
|
||||
|
||||
public static Orientation[] GetOrientations(Orientation currentDirection, GlobalNode[] nodes, GlobalEdge[] edges, double ratio, double changeOrientationAngle)
|
||||
{
|
||||
Orientation[] Orientations = new Orientation[nodes.Length];
|
||||
if (nodes.Length > 0) Orientations[0] = currentDirection;
|
||||
if (nodes.Length > 2)
|
||||
{
|
||||
for (int i = 1; i < nodes.Length - 1; i++)
|
||||
{
|
||||
GlobalNode startNode = BezierPoint(1 - ratio, nodes[i - 1], nodes[i], edges[i - 1]);
|
||||
GlobalNode endNode = BezierPoint(ratio, nodes[i], nodes[i + 1], edges[i]);
|
||||
var angle = GetAngle(nodes[i], startNode, endNode);
|
||||
if (angle < changeOrientationAngle) Orientations[i] = Orientations[i - 1] == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
else Orientations[i] = Orientations[i - 1];
|
||||
}
|
||||
}
|
||||
if (nodes.Length > 1) Orientations[^1] = Orientations[^2];
|
||||
return Orientations;
|
||||
}
|
||||
|
||||
public static Orientation GetOrientationStart(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
|
||||
{
|
||||
GlobalNode NearNode = BezierPoint(ratio, nodeRef, nearNode, edge);
|
||||
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
|
||||
Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
|
||||
};
|
||||
var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
|
||||
return angle > changeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
}
|
||||
|
||||
public static Orientation GetOrientationEnd(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
|
||||
{
|
||||
GlobalNode NearNode = BezierPoint(1 - ratio, nearNode, nodeRef, edge);
|
||||
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
|
||||
Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
|
||||
};
|
||||
var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
|
||||
return angle > changeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;
|
||||
}
|
||||
|
||||
public static GlobalEdge[] GetEdgesPlanning(GlobalNode[] path, GlobalEdge[] edges, GlobalEdge? closesEdge)
|
||||
{
|
||||
var EdgesPlanning = new List<GlobalEdge>();
|
||||
for (int i = 0; i < path.Length - 1; i++)
|
||||
{
|
||||
var edge = edges.FirstOrDefault(e => e.StartNodeId == path[i].Id && e.EndNodeId == path[i + 1].Id);
|
||||
if (edge is null)
|
||||
{
|
||||
if (i != 0) return [];
|
||||
EdgesPlanning.Add(new GlobalEdge()
|
||||
{
|
||||
Id = closesEdge is null ? Guid.NewGuid() : closesEdge.Id,
|
||||
StartNodeId = path[i].Id,
|
||||
EndNodeId = path[i + 1].Id,
|
||||
Degree = 1,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
EdgesPlanning.Add(new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = path[i].Id,
|
||||
EndNodeId = path[i + 1].Id,
|
||||
Degree = edge.Degree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y
|
||||
});
|
||||
}
|
||||
return [.. EdgesPlanning];
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
public class AStarNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double Cost { get; set; }
|
||||
public double Heuristic { get; set; }
|
||||
public double TotalCost => Cost + Heuristic;
|
||||
public string? Name { get; set; }
|
||||
public AStarNode? Parent { get; set; }
|
||||
public List<AStarNode> NegativeNodes { get; set; } = [];
|
||||
|
||||
public override bool Equals(object? obj)
|
||||
{
|
||||
if (obj is AStarNode other)
|
||||
return Id == other.Id;
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Id);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
public record GlobalEdge
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public Guid MapId { get; set; }
|
||||
public Guid StartNodeId { get; set; }
|
||||
public Guid EndNodeId { get; set; }
|
||||
public int Degree { get; set; }
|
||||
public double ControlPoint1X { get; set; }
|
||||
public double ControlPoint1Y { get; set; }
|
||||
public double ControlPoint2X { get; set; }
|
||||
public double ControlPoint2Y { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
public class GlobalNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public Guid MapId { get; set; }
|
||||
public string? Name { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public Orientation Orientation { get; set; }
|
||||
public override string ToString() => Name ?? typeof(GlobalNode).ToString();
|
||||
// Tính khoảng cách giữa 2 điểm
|
||||
public double DistanceTo(GlobalNode other)
|
||||
{
|
||||
double dx = X - other.X;
|
||||
double dy = Y - other.Y;
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
/// <summary>
|
||||
/// Represents a node in the KD-Tree structure.
|
||||
/// Immutable to prevent structural corruption.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Initializes a new KD-Tree node.
|
||||
/// </remarks>
|
||||
public class KDTreeNode(GlobalNode node, int axis, KDTreeNode? left = null, KDTreeNode? right = null)
|
||||
{
|
||||
/// <summary>
|
||||
/// The spatial node stored at this tree node.
|
||||
/// </summary>
|
||||
public GlobalNode Node { get; } = node ?? throw new ArgumentNullException(nameof(node));
|
||||
|
||||
/// <summary>
|
||||
/// Left child (contains points with smaller values along the split axis).
|
||||
/// </summary>
|
||||
public KDTreeNode? Left { get; } = left;
|
||||
|
||||
/// <summary>
|
||||
/// Right child (contains points with larger values along the split axis).
|
||||
/// </summary>
|
||||
public KDTreeNode? Right { get; } = right;
|
||||
|
||||
/// <summary>
|
||||
/// The axis used for splitting: 0 for X, 1 for Y.
|
||||
/// </summary>
|
||||
public int Axis { get; } = axis;
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
public enum Orientation
|
||||
{
|
||||
FORWARD,
|
||||
BACKWARD,
|
||||
NONE
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
namespace RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
public class SSEAStarNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public Orientation Orientation { get; set; }
|
||||
public double Cost { get; set; }
|
||||
public double Heuristic { get; set; }
|
||||
public double TotalCost => Cost + Heuristic;
|
||||
public string? Name { get; set; }
|
||||
public SSEAStarNode? Parent { get; set; }
|
||||
public List<SSEAStarNode> NegativeNodes { get; set; } = [];
|
||||
|
||||
public override bool Equals(object? obj)
|
||||
{
|
||||
if (obj is SSEAStarNode other)
|
||||
return Id == other.Id && Orientation == other.Orientation;
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Id, Orientation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
using RobotNet10.GlobalPathPlanner.Differential;
|
||||
using RobotNet10.GlobalPathPlanner.Forklift;
|
||||
using RobotNet10.GlobalPathPlanner.ForkliftV2;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
public class PathPlannerFactory : IPathPlannerFactory
|
||||
{
|
||||
public IPathPlanner CreateDifferentialPlanner() => new DifferentialPlanner();
|
||||
|
||||
public IPathPlanner CreateForkliftPlanner() => new ForkliftPathPlanner();
|
||||
|
||||
public IPathPlanner CreateForkliftPlannerV2() => new ForkLiftPathPlannerV2();
|
||||
|
||||
public IPathPlanner CreateOmniDrivePlanner() => new DifferentialPlanner();
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
public record PathPlannerOptions
|
||||
{
|
||||
public double LimitDistanceToEdge { get; set; }
|
||||
public double LimitDistanceToNode { get; set; }
|
||||
public double ResolutionSplit { get; set; }
|
||||
public TimeSpan? TimeOut { get; set; }
|
||||
public double ChangeOrientationAngle { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
namespace RobotNet10.GlobalPathPlanner;
|
||||
|
||||
public class PriorityQueue<T>(Comparison<T> comparison)
|
||||
{
|
||||
public List<T> Items => items;
|
||||
private readonly List<T> items = [];
|
||||
private readonly IComparer<T> comparer = Comparer<T>.Create(comparison);
|
||||
|
||||
public void Enqueue(T item)
|
||||
{
|
||||
int index = items.BinarySearch(item, comparer);
|
||||
if (index < 0) index = ~index;
|
||||
items.Insert(index, item);
|
||||
}
|
||||
|
||||
public T Dequeue()
|
||||
{
|
||||
if (items.Count == 0) throw new InvalidOperationException("Queue is empty");
|
||||
var item = items[0];
|
||||
items.RemoveAt(0);
|
||||
return item;
|
||||
}
|
||||
|
||||
public int Count => items.Count;
|
||||
}
|
||||
818
srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/README.md
Normal file
818
srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/README.md
Normal file
@@ -0,0 +1,818 @@
|
||||
# RobotNet10.GlobalPathPlanner
|
||||
|
||||
Thư viện path planning cho hệ thống robot, cung cấp các thuật toán tìm đường tối ưu trên graph với hỗ trợ nhiều loại robot khác nhau.
|
||||
|
||||
## 📋 Mục lục
|
||||
|
||||
- [Tổng quan](#tổng-quan)
|
||||
- [Cài đặt](#cài-đặt)
|
||||
- [Bắt đầu nhanh](#bắt-đầu-nhanh)
|
||||
- [Hướng dẫn sử dụng](#hướng-dẫn-sử-dụng)
|
||||
- [Các loại Path Planner](#các-loại-path-planner)
|
||||
- [Configuration Options](#configuration-options)
|
||||
- [API Reference](#api-reference)
|
||||
- [Ví dụ nâng cao](#ví-dụ-nâng-cao)
|
||||
- [Best Practices](#best-practices)
|
||||
- [Troubleshooting](#troubleshooting)
|
||||
|
||||
## 🎯 Tổng quan
|
||||
|
||||
`RobotNet10.GlobalPathPlanner` là một thư viện .NET cung cấp các thuật toán path planning cho robot, bao gồm:
|
||||
|
||||
- **A* Algorithm**: Thuật toán tìm đường tối ưu trên graph
|
||||
- **SSE A* Algorithm**: State-Space Enhanced A* cho forklift robots
|
||||
- **Hỗ trợ nhiều loại robot**: Differential drive, Forklift, Omni-directional drive
|
||||
- **Bezier Curve Support**: Hỗ trợ edges với curves bậc 1, 2, hoặc 3
|
||||
- **KD-Tree Optimization**: Tối ưu hóa tìm kiếm nearest neighbor
|
||||
- **Cancellation & Timeout**: Hỗ trợ hủy bỏ và timeout cho operations
|
||||
|
||||
## 📦 Cài đặt
|
||||
|
||||
### Thêm Project Reference
|
||||
|
||||
Thêm reference đến project trong file `.csproj`:
|
||||
|
||||
```xml
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="path/to/RobotNet10.GlobalPathPlanner.csproj" />
|
||||
</ItemGroup>
|
||||
```
|
||||
|
||||
### Using Statements
|
||||
|
||||
```csharp
|
||||
using RobotNet10.GlobalPathPlanner;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
```
|
||||
|
||||
## 🚀 Bắt đầu nhanh
|
||||
|
||||
### Ví dụ cơ bản
|
||||
|
||||
```csharp
|
||||
using RobotNet10.GlobalPathPlanner;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
// 1. Tạo factory và planner
|
||||
var factory = new PathPlannerFactory();
|
||||
var planner = factory.CreateDifferentialPlanner();
|
||||
|
||||
// 2. Chuẩn bị dữ liệu map (nodes và edges)
|
||||
var nodes = new GlobalNode[]
|
||||
{
|
||||
new GlobalNode { Id = Guid.NewGuid(), X = 0, Y = 0, Name = "Start" },
|
||||
new GlobalNode { Id = Guid.NewGuid(), X = 10, Y = 10, Name = "Goal" }
|
||||
};
|
||||
|
||||
var edges = new GlobalEdge[]
|
||||
{
|
||||
new GlobalEdge
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
StartNodeId = nodes[0].Id,
|
||||
EndNodeId = nodes[1].Id,
|
||||
Degree = 1 // Linear edge
|
||||
}
|
||||
};
|
||||
|
||||
// 3. Set data cho planner
|
||||
planner.SetData(nodes, edges);
|
||||
|
||||
// 4. Tính toán đường đi
|
||||
var (pathNodes, pathEdges) = planner.PathPlanning(
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
theta: 0.0,
|
||||
goalId: nodes[1].Id
|
||||
);
|
||||
|
||||
// 5. Sử dụng kết quả
|
||||
foreach (var node in pathNodes)
|
||||
{
|
||||
Console.WriteLine($"Node: {node.Name} at ({node.X}, {node.Y})");
|
||||
}
|
||||
```
|
||||
|
||||
## 📖 Hướng dẫn sử dụng
|
||||
|
||||
### Workflow cơ bản
|
||||
|
||||
1. **Tạo Planner**: Sử dụng `PathPlannerFactory` để tạo planner phù hợp với loại robot
|
||||
2. **Set Data**: Gọi `SetData()` để load map data (nodes và edges)
|
||||
3. **Configure Options** (Optional): Gọi `SetOptions()` để cấu hình
|
||||
4. **Path Planning**: Gọi các method path planning để tính toán đường đi
|
||||
5. **Xử lý kết quả**: Sử dụng mảng nodes và edges trả về
|
||||
|
||||
### Tạo Path Planner
|
||||
|
||||
```csharp
|
||||
var factory = new PathPlannerFactory();
|
||||
|
||||
// Cho differential drive robot
|
||||
var differentialPlanner = factory.CreateDifferentialPlanner();
|
||||
|
||||
// Cho forklift robot (version 1)
|
||||
var forkliftPlanner = factory.CreateForkliftPlanner();
|
||||
|
||||
// Cho forklift robot (version 2 - enhanced)
|
||||
var forkliftPlannerV2 = factory.CreateForkliftPlannerV2();
|
||||
|
||||
// Cho omni-directional drive robot
|
||||
var omniPlanner = factory.CreateOmniDrivePlanner();
|
||||
```
|
||||
|
||||
### Load Map Data
|
||||
|
||||
```csharp
|
||||
// Chuẩn bị nodes (waypoints)
|
||||
var nodes = new GlobalNode[]
|
||||
{
|
||||
new GlobalNode
|
||||
{
|
||||
Id = Guid.Parse("..."),
|
||||
MapId = Guid.Parse("..."),
|
||||
X = 0.0,
|
||||
Y = 0.0,
|
||||
Name = "Node1",
|
||||
Orientation = Orientation.FORWARD
|
||||
},
|
||||
// ... thêm các nodes khác
|
||||
};
|
||||
|
||||
// Chuẩn bị edges (connections)
|
||||
var edges = new GlobalEdge[]
|
||||
{
|
||||
new GlobalEdge
|
||||
{
|
||||
Id = Guid.Parse("..."),
|
||||
MapId = Guid.Parse("..."),
|
||||
StartNodeId = nodes[0].Id,
|
||||
EndNodeId = nodes[1].Id,
|
||||
Degree = 2, // Bezier curve bậc 2
|
||||
ControlPoint1X = 5.0,
|
||||
ControlPoint1Y = 5.0,
|
||||
ControlPoint2X = 0.0,
|
||||
ControlPoint2Y = 0.0
|
||||
},
|
||||
// ... thêm các edges khác
|
||||
};
|
||||
|
||||
// Set data cho planner
|
||||
planner.SetData(nodes, edges);
|
||||
```
|
||||
|
||||
### Path Planning Methods
|
||||
|
||||
#### 1. Path Planning cơ bản (từ tọa độ)
|
||||
|
||||
```csharp
|
||||
var (nodes, edges) = planner.PathPlanning(
|
||||
x: 5.0, // Starting X coordinate
|
||||
y: 5.0, // Starting Y coordinate
|
||||
theta: 45.0, // Starting orientation (degrees)
|
||||
goalId: goalNodeId // Goal node ID
|
||||
);
|
||||
```
|
||||
|
||||
#### 2. Path Planning từ Node ID
|
||||
|
||||
```csharp
|
||||
var (nodes, edges) = planner.PathPlanning(
|
||||
startNodeId: startNodeId, // Starting node ID
|
||||
theta: 45.0, // Current orientation
|
||||
goalId: goalNodeId // Goal node ID
|
||||
);
|
||||
```
|
||||
|
||||
#### 3. Path Planning với Starting Direction
|
||||
|
||||
```csharp
|
||||
var (nodes, edges) = planner.PathPlanningWithStartDirection(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 45.0,
|
||||
goalId: goalNodeId,
|
||||
startDiretion: Orientation.FORWARD // FORWARD, BACKWARD, or NONE
|
||||
);
|
||||
```
|
||||
|
||||
#### 4. Path Planning với Final Direction
|
||||
|
||||
```csharp
|
||||
var (nodes, edges) = planner.PathPlanningWithFinalDirection(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 45.0,
|
||||
goalId: goalNodeId,
|
||||
goalDirection: Orientation.BACKWARD // FORWARD, BACKWARD, or NONE
|
||||
);
|
||||
```
|
||||
|
||||
#### 5. Path Planning với Final Angle
|
||||
|
||||
```csharp
|
||||
var (nodes, edges) = planner.PathPlanningWithAngle(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 45.0,
|
||||
goalId: goalNodeId,
|
||||
goalAngle: 90.0 // Desired final angle in degrees
|
||||
);
|
||||
```
|
||||
|
||||
### Sử dụng Cancellation Token
|
||||
|
||||
```csharp
|
||||
using var cts = new CancellationTokenSource();
|
||||
|
||||
// Set timeout
|
||||
cts.CancelAfter(TimeSpan.FromSeconds(5));
|
||||
|
||||
try
|
||||
{
|
||||
var (nodes, edges) = planner.PathPlanning(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 45.0,
|
||||
goalId: goalNodeId,
|
||||
cancellationToken: cts.Token
|
||||
);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
Console.WriteLine("Path planning was cancelled");
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
Console.WriteLine("Path planning timed out");
|
||||
}
|
||||
```
|
||||
|
||||
## 🤖 Các loại Path Planner
|
||||
|
||||
### 1. DifferentialPlanner
|
||||
|
||||
**Sử dụng cho:**
|
||||
- Differential drive robots (2 bánh độc lập)
|
||||
- Omni-directional drive robots
|
||||
|
||||
**Đặc điểm:**
|
||||
- Sử dụng A* algorithm cơ bản
|
||||
- Tính toán orientation (FORWARD/BACKWARD) tự động
|
||||
- Default options phù hợp cho hầu hết các trường hợp
|
||||
|
||||
**Ví dụ:**
|
||||
```csharp
|
||||
var planner = factory.CreateDifferentialPlanner();
|
||||
```
|
||||
|
||||
### 2. ForkliftPathPlanner
|
||||
|
||||
**Sử dụng cho:**
|
||||
- Forklift robots (phiên bản 1)
|
||||
|
||||
**Đặc điểm:**
|
||||
- Xử lý các ràng buộc đặc thù của forklift
|
||||
- Tính toán turning radius phù hợp
|
||||
|
||||
**Ví dụ:**
|
||||
```csharp
|
||||
var planner = factory.CreateForkliftPlanner();
|
||||
```
|
||||
|
||||
### 3. ForkLiftPathPlannerV2
|
||||
|
||||
**Sử dụng cho:**
|
||||
- Forklift robots (phiên bản 2 - enhanced)
|
||||
|
||||
**Đặc điểm:**
|
||||
- Sử dụng SSE A* algorithm (State-Space Enhanced A*)
|
||||
- Hiệu năng và chất lượng đường đi tốt hơn
|
||||
- Khuyến nghị sử dụng cho forklift
|
||||
|
||||
**Ví dụ:**
|
||||
```csharp
|
||||
var planner = factory.CreateForkliftPlannerV2();
|
||||
```
|
||||
|
||||
### 4. OmniDrivePlanner
|
||||
|
||||
**Sử dụng cho:**
|
||||
- Omni-directional drive robots
|
||||
|
||||
**Lưu ý:** Hiện tại sử dụng cùng planner với DifferentialPlanner
|
||||
|
||||
**Ví dụ:**
|
||||
```csharp
|
||||
var planner = factory.CreateOmniDrivePlanner();
|
||||
```
|
||||
|
||||
## ⚙️ Configuration Options
|
||||
|
||||
### PathPlannerOptions
|
||||
|
||||
```csharp
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
// Khoảng cách tối đa đến edge để được coi là "gần" edge
|
||||
LimitDistanceToEdge = 1.0,
|
||||
|
||||
// Khoảng cách tối đa đến node để được coi là "tại" node
|
||||
LimitDistanceToNode = 0.3,
|
||||
|
||||
// Độ phân giải khi split path (khoảng cách giữa các điểm)
|
||||
ResolutionSplit = 0.1,
|
||||
|
||||
// Timeout cho path planning operation
|
||||
TimeOut = TimeSpan.FromSeconds(10),
|
||||
|
||||
// Góc (degrees) để quyết định đổi hướng (FORWARD/BACKWARD)
|
||||
ChangeOrientationAngle = 89.0
|
||||
};
|
||||
|
||||
planner.SetOptions(options);
|
||||
```
|
||||
|
||||
### Giải thích các tham số
|
||||
|
||||
| Tham số | Mô tả | Giá trị mặc định | Đơn vị |
|
||||
|---------|-------|------------------|--------|
|
||||
| `LimitDistanceToEdge` | Khoảng cách tối đa để được coi là gần edge | 1.0 | meters |
|
||||
| `LimitDistanceToNode` | Khoảng cách tối đa để được coi là tại node | 0.3 | meters |
|
||||
| `ResolutionSplit` | Độ phân giải khi chia nhỏ path | 0.1 | meters |
|
||||
| `TimeOut` | Timeout cho operation | null (no timeout) | TimeSpan |
|
||||
| `ChangeOrientationAngle` | Góc để quyết định đổi hướng | 89.0 | degrees |
|
||||
|
||||
## 📚 API Reference
|
||||
|
||||
### IPathPlanner Interface
|
||||
|
||||
#### SetData
|
||||
|
||||
```csharp
|
||||
void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
|
||||
```
|
||||
|
||||
Thiết lập dữ liệu graph (nodes và edges) cho planner. **Phải gọi trước khi thực hiện path planning.**
|
||||
|
||||
#### SetOptions
|
||||
|
||||
```csharp
|
||||
void SetOptions(PathPlannerOptions options)
|
||||
```
|
||||
|
||||
Cấu hình options cho planner. **Optional**, nếu không gọi sẽ dùng default options.
|
||||
|
||||
#### PathPlanning (từ tọa độ)
|
||||
|
||||
```csharp
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(
|
||||
double x,
|
||||
double y,
|
||||
double theta,
|
||||
Guid goalId,
|
||||
CancellationToken? cancellationToken = null
|
||||
)
|
||||
```
|
||||
|
||||
Tính toán đường đi từ tọa độ (x, y) đến goal node.
|
||||
|
||||
#### PathPlanning (từ Node ID)
|
||||
|
||||
```csharp
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(
|
||||
Guid startNodeId,
|
||||
double theta,
|
||||
Guid goalId,
|
||||
CancellationToken? cancellationToken = null
|
||||
)
|
||||
```
|
||||
|
||||
Tính toán đường đi từ start node đến goal node. **Hiệu quả hơn** khi robot đã ở tại một node đã biết.
|
||||
|
||||
#### PathPlanningWithStartDirection
|
||||
|
||||
```csharp
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(
|
||||
double x,
|
||||
double y,
|
||||
double theta,
|
||||
Guid goalId,
|
||||
Orientation startDiretion = Orientation.NONE,
|
||||
CancellationToken? cancellationToken = null
|
||||
)
|
||||
```
|
||||
|
||||
Tính toán đường đi với ràng buộc hướng bắt đầu.
|
||||
|
||||
#### PathPlanningWithFinalDirection
|
||||
|
||||
```csharp
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(
|
||||
double x,
|
||||
double y,
|
||||
double theta,
|
||||
Guid goalId,
|
||||
Orientation goalDirection = Orientation.NONE,
|
||||
CancellationToken? cancellationToken = null
|
||||
)
|
||||
```
|
||||
|
||||
Tính toán đường đi với ràng buộc hướng kết thúc.
|
||||
|
||||
#### PathPlanningWithAngle
|
||||
|
||||
```csharp
|
||||
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(
|
||||
double x,
|
||||
double y,
|
||||
double theta,
|
||||
Guid goalId,
|
||||
double goalAngle,
|
||||
CancellationToken? cancellationToken = null
|
||||
)
|
||||
```
|
||||
|
||||
Tính toán đường đi với ràng buộc góc kết thúc.
|
||||
|
||||
### Data Models
|
||||
|
||||
#### GlobalNode
|
||||
|
||||
```csharp
|
||||
public class GlobalNode
|
||||
{
|
||||
public Guid Id { get; set; } // Unique identifier
|
||||
public Guid MapId { get; set; } // Map identifier
|
||||
public string? Name { get; set; } // Node name
|
||||
public double X { get; set; } // X coordinate
|
||||
public double Y { get; set; } // Y coordinate
|
||||
public Orientation Orientation { get; set; } // FORWARD, BACKWARD, or NONE
|
||||
|
||||
public double DistanceTo(GlobalNode other); // Calculate distance to another node
|
||||
}
|
||||
```
|
||||
|
||||
#### GlobalEdge
|
||||
|
||||
```csharp
|
||||
public record GlobalEdge
|
||||
{
|
||||
public Guid Id { get; set; } // Unique identifier
|
||||
public Guid MapId { get; set; } // Map identifier
|
||||
public Guid StartNodeId { get; set; } // Start node ID
|
||||
public Guid EndNodeId { get; set; } // End node ID
|
||||
public int Degree { get; set; } // Curve degree (1, 2, or 3)
|
||||
public double ControlPoint1X { get; set; } // Control point 1 X (for Bezier)
|
||||
public double ControlPoint1Y { get; set; } // Control point 1 Y (for Bezier)
|
||||
public double ControlPoint2X { get; set; } // Control point 2 X (for Bezier)
|
||||
public double ControlPoint2Y { get; set; } // Control point 2 Y (for Bezier)
|
||||
}
|
||||
```
|
||||
|
||||
#### Orientation Enum
|
||||
|
||||
```csharp
|
||||
public enum Orientation
|
||||
{
|
||||
FORWARD, // Di chuyển tiến
|
||||
BACKWARD, // Di chuyển lùi
|
||||
NONE // Không ràng buộc
|
||||
}
|
||||
```
|
||||
|
||||
## 💡 Ví dụ nâng cao
|
||||
|
||||
### Ví dụ 1: Path Planning với Error Handling
|
||||
|
||||
```csharp
|
||||
try
|
||||
{
|
||||
var factory = new PathPlannerFactory();
|
||||
var planner = factory.CreateDifferentialPlanner();
|
||||
|
||||
planner.SetData(nodes, edges);
|
||||
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
LimitDistanceToEdge = 1.0,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
TimeOut = TimeSpan.FromSeconds(5),
|
||||
ChangeOrientationAngle = 89.0
|
||||
};
|
||||
planner.SetOptions(options);
|
||||
|
||||
var (pathNodes, pathEdges) = planner.PathPlanning(
|
||||
x: currentX,
|
||||
y: currentY,
|
||||
theta: currentTheta,
|
||||
goalId: goalNodeId
|
||||
);
|
||||
|
||||
Console.WriteLine($"Path found with {pathNodes.Length} nodes");
|
||||
}
|
||||
catch (Exception ex) when (ex.Message.Contains("does not exist"))
|
||||
{
|
||||
Console.WriteLine($"Error: {ex.Message}");
|
||||
// Handle case when goal node doesn't exist or no path found
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
Console.WriteLine("Path planning timed out");
|
||||
// Handle timeout
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
Console.WriteLine("Path planning was cancelled");
|
||||
// Handle cancellation
|
||||
}
|
||||
```
|
||||
|
||||
### Ví dụ 2: Sử dụng với Async/Await
|
||||
|
||||
```csharp
|
||||
public async Task<(GlobalNode[] Nodes, GlobalEdge[] Edges)> PlanPathAsync(
|
||||
IPathPlanner planner,
|
||||
double x,
|
||||
double y,
|
||||
double theta,
|
||||
Guid goalId)
|
||||
{
|
||||
using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(10));
|
||||
|
||||
return await Task.Run(() =>
|
||||
{
|
||||
return planner.PathPlanning(x, y, theta, goalId, cts.Token);
|
||||
}, cts.Token);
|
||||
}
|
||||
|
||||
// Usage
|
||||
var planner = factory.CreateDifferentialPlanner();
|
||||
planner.SetData(nodes, edges);
|
||||
|
||||
var (pathNodes, pathEdges) = await PlanPathAsync(
|
||||
planner,
|
||||
currentX,
|
||||
currentY,
|
||||
currentTheta,
|
||||
goalNodeId
|
||||
);
|
||||
```
|
||||
|
||||
### Ví dụ 3: Path Planning với Direction Constraints
|
||||
|
||||
```csharp
|
||||
// Robot cần bắt đầu di chuyển lùi
|
||||
var (nodes, edges) = planner.PathPlanningWithStartDirection(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 180.0,
|
||||
goalId: goalNodeId,
|
||||
startDiretion: Orientation.BACKWARD
|
||||
);
|
||||
|
||||
// Robot cần đến đích và quay mặt về phía trước
|
||||
var (nodes2, edges2) = planner.PathPlanningWithFinalDirection(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 0.0,
|
||||
goalId: goalNodeId,
|
||||
goalDirection: Orientation.FORWARD
|
||||
);
|
||||
|
||||
// Robot cần đến đích với góc cụ thể (90 độ)
|
||||
var (nodes3, edges3) = planner.PathPlanningWithAngle(
|
||||
x: 5.0,
|
||||
y: 5.0,
|
||||
theta: 0.0,
|
||||
goalId: goalNodeId,
|
||||
goalAngle: 90.0
|
||||
);
|
||||
```
|
||||
|
||||
### Ví dụ 4: Xử lý kết quả Path
|
||||
|
||||
```csharp
|
||||
var (pathNodes, pathEdges) = planner.PathPlanning(
|
||||
startNodeId: startNodeId,
|
||||
theta: currentTheta,
|
||||
goalId: goalNodeId
|
||||
);
|
||||
|
||||
// Kiểm tra kết quả
|
||||
if (pathNodes.Length == 0)
|
||||
{
|
||||
Console.WriteLine("No path found");
|
||||
return;
|
||||
}
|
||||
|
||||
// In thông tin path
|
||||
Console.WriteLine($"Path contains {pathNodes.Length} nodes and {pathEdges.Length} edges");
|
||||
|
||||
for (int i = 0; i < pathNodes.Length; i++)
|
||||
{
|
||||
var node = pathNodes[i];
|
||||
Console.WriteLine($"Node {i}: {node.Name} at ({node.X:F2}, {node.Y:F2}) " +
|
||||
$"Orientation: {node.Orientation}");
|
||||
|
||||
if (i < pathEdges.Length)
|
||||
{
|
||||
var edge = pathEdges[i];
|
||||
Console.WriteLine($" Edge {i}: {edge.Id} (Degree: {edge.Degree})");
|
||||
}
|
||||
}
|
||||
|
||||
// Tính tổng khoảng cách
|
||||
double totalDistance = 0;
|
||||
for (int i = 0; i < pathNodes.Length - 1; i++)
|
||||
{
|
||||
totalDistance += pathNodes[i].DistanceTo(pathNodes[i + 1]);
|
||||
}
|
||||
Console.WriteLine($"Total path distance: {totalDistance:F2} meters");
|
||||
```
|
||||
|
||||
## ✅ Best Practices
|
||||
|
||||
### 1. Chọn đúng Planner cho Robot Type
|
||||
|
||||
```csharp
|
||||
// ✅ Đúng: Sử dụng ForkliftPlannerV2 cho forklift
|
||||
var forkliftPlanner = factory.CreateForkliftPlannerV2();
|
||||
|
||||
// ❌ Sai: Không dùng DifferentialPlanner cho forklift
|
||||
var wrongPlanner = factory.CreateDifferentialPlanner(); // Không phù hợp
|
||||
```
|
||||
|
||||
### 2. Luôn Set Data trước khi Planning
|
||||
|
||||
```csharp
|
||||
// ✅ Đúng
|
||||
planner.SetData(nodes, edges);
|
||||
var (nodes, edges) = planner.PathPlanning(...);
|
||||
|
||||
// ❌ Sai: Quên set data
|
||||
var (nodes, edges) = planner.PathPlanning(...); // Sẽ lỗi
|
||||
```
|
||||
|
||||
### 3. Sử dụng Node ID khi có thể
|
||||
|
||||
```csharp
|
||||
// ✅ Tốt hơn: Sử dụng Node ID khi robot đã ở tại node
|
||||
var (nodes, edges) = planner.PathPlanning(startNodeId, theta, goalId);
|
||||
|
||||
// ⚠️ Chấp nhận được: Sử dụng tọa độ khi robot không ở node
|
||||
var (nodes, edges) = planner.PathPlanning(x, y, theta, goalId);
|
||||
```
|
||||
|
||||
### 4. Cấu hình Timeout cho Operations dài
|
||||
|
||||
```csharp
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
TimeOut = TimeSpan.FromSeconds(10) // Tránh hang
|
||||
};
|
||||
planner.SetOptions(options);
|
||||
```
|
||||
|
||||
### 5. Xử lý Exceptions đúng cách
|
||||
|
||||
```csharp
|
||||
try
|
||||
{
|
||||
var (nodes, edges) = planner.PathPlanning(...);
|
||||
}
|
||||
catch (Exception ex) when (ex.Message.Contains("does not exist"))
|
||||
{
|
||||
// Handle: Goal không tồn tại hoặc không tìm thấy đường đi
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
// Handle: Timeout
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// Handle: Cancellation
|
||||
}
|
||||
```
|
||||
|
||||
### 6. Reuse Planner Instance
|
||||
|
||||
```csharp
|
||||
// ✅ Tốt: Tạo một lần, dùng nhiều lần
|
||||
var planner = factory.CreateDifferentialPlanner();
|
||||
planner.SetData(nodes, edges);
|
||||
|
||||
for (int i = 0; i < 100; i++)
|
||||
{
|
||||
var (pathNodes, pathEdges) = planner.PathPlanning(...);
|
||||
// Process path
|
||||
}
|
||||
|
||||
// ❌ Không hiệu quả: Tạo mới mỗi lần
|
||||
for (int i = 0; i < 100; i++)
|
||||
{
|
||||
var planner = factory.CreateDifferentialPlanner(); // Không cần thiết
|
||||
planner.SetData(nodes, edges);
|
||||
var (pathNodes, pathEdges) = planner.PathPlanning(...);
|
||||
}
|
||||
```
|
||||
|
||||
## 🔧 Troubleshooting
|
||||
|
||||
### Vấn đề: "Destination does not exist in the map"
|
||||
|
||||
**Nguyên nhân:** Goal node ID không có trong danh sách nodes đã set.
|
||||
|
||||
**Giải pháp:**
|
||||
```csharp
|
||||
// Kiểm tra goal node có tồn tại không
|
||||
var goalNode = nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goalNode == null)
|
||||
{
|
||||
throw new ArgumentException($"Goal node {goalId} not found in map");
|
||||
}
|
||||
```
|
||||
|
||||
### Vấn đề: "The path does not exist"
|
||||
|
||||
**Nguyên nhân:** Không có đường đi từ start đến goal (graph không liên thông).
|
||||
|
||||
**Giải pháp:**
|
||||
- Kiểm tra edges có kết nối start và goal không
|
||||
- Đảm bảo graph liên thông
|
||||
- Kiểm tra `LimitDistanceToEdge` và `LimitDistanceToNode` có quá nhỏ không
|
||||
|
||||
### Vấn đề: Timeout thường xuyên
|
||||
|
||||
**Nguyên nhân:** Graph quá lớn hoặc phức tạp.
|
||||
|
||||
**Giải pháp:**
|
||||
```csharp
|
||||
// Tăng timeout
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
TimeOut = TimeSpan.FromSeconds(30) // Tăng từ 10s lên 30s
|
||||
};
|
||||
planner.SetOptions(options);
|
||||
```
|
||||
|
||||
### Vấn đề: Path không mượt
|
||||
|
||||
**Nguyên nhân:** `ResolutionSplit` quá lớn.
|
||||
|
||||
**Giải pháp:**
|
||||
```csharp
|
||||
// Giảm ResolutionSplit để có nhiều điểm hơn
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
ResolutionSplit = 0.05 // Giảm từ 0.1 xuống 0.05
|
||||
};
|
||||
planner.SetOptions(options);
|
||||
```
|
||||
|
||||
### Vấn đề: Robot không đổi hướng đúng
|
||||
|
||||
**Nguyên nhân:** `ChangeOrientationAngle` không phù hợp.
|
||||
|
||||
**Giải pháp:**
|
||||
```csharp
|
||||
// Điều chỉnh góc đổi hướng
|
||||
var options = new PathPlannerOptions
|
||||
{
|
||||
ChangeOrientationAngle = 85.0 // Thử các giá trị khác nhau
|
||||
};
|
||||
planner.SetOptions(options);
|
||||
```
|
||||
|
||||
## 📝 Lưu ý
|
||||
|
||||
- **Thread Safety**: Planner instances không thread-safe. Mỗi thread nên có planner riêng.
|
||||
- **Memory**: Planner lưu toàn bộ nodes và edges trong memory. Với map lớn, cần xem xét memory usage.
|
||||
- **Performance**: Path planning với Node ID nhanh hơn so với tọa độ vì không cần tìm nearest node/edge.
|
||||
|
||||
## 🤝 Đóng góp
|
||||
|
||||
Nếu bạn phát hiện bug hoặc có đề xuất cải thiện, vui lòng tạo issue hoặc pull request.
|
||||
|
||||
## 📄 License
|
||||
|
||||
[Thêm thông tin license nếu có]
|
||||
|
||||
---
|
||||
|
||||
**Version**: 1.0
|
||||
**Last Updated**: 2024
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net10.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
<IsPackable>true</IsPackable>
|
||||
</PropertyGroup>
|
||||
|
||||
</Project>
|
||||
@@ -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