private void Draw(IDrawDevice device, IReadOnlyList <ICollisionLayerNode> network, IDefinitionNodeNetwork definitionNodeNetwork) { var canvas = new Canvas(device, new CanvasBuffer()); canvas.State.ZOffset = -8; for (var i = 0; i < network.Count; i++) { var node = network[i]; canvas.State.ColorTint = ColorRgba.LightGrey; var nodeWorldPosition = definitionNodeNetwork.Transformer.ToWorld(node.DefinitionNode.Position); canvas.FillCircle(nodeWorldPosition.X, nodeWorldPosition.Y, NodeSize); canvas.State.ColorTint = ColorRgba.VeryLightGrey; if (node.DefinitionNode.Connections != null) { canvas.State.ColorTint = new ColorRgba(199, 21, 133); foreach (var connection in node.DefinitionNode.Connections) { var toNode = NodePointer.Dereference(connection.To, definitionNodeNetwork); var vector = (definitionNodeNetwork.Transformer.ToWorld(toNode.Position) - nodeWorldPosition) * 0.5f; //Times 0.5f so we can see the connections in both directions. canvas.DrawDashLine(nodeWorldPosition.X, nodeWorldPosition.Y, nodeWorldPosition.X + vector.X, nodeWorldPosition.Y + vector.Y); } if (!float.IsNaN(node.Clearance)) { canvas.State.ColorTint = ColorRgba.Black; canvas.DrawText(node.Clearance.ToString(), nodeWorldPosition.X, nodeWorldPosition.Y, -1f, Alignment.Center); } } } }
public NodePath FindPath(IPathfindNodeNetwork <AstarNode> nodeNetwork, IPathRequest pathRequest, out bool succes) { var pathfindingNetwork = nodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory); var startNode = NodePointer.Dereference(pathRequest.PathStart.Index, pathfindingNetwork); var endNode = NodePointer.Dereference(pathRequest.PathEnd.Index, pathfindingNetwork); var path = FindPath(pathfindingNetwork, startNode, endNode, pathRequest.AgentSize, pathRequest.CollisionCategory); if (path == null) { succes = false; return(new NodePath(new[] { startNode.DefinitionNode }, nodeNetwork.DefinitionNodeNetwork.Transformer)); } succes = true; switch (nodeNetwork.DefinitionNodeNetwork) { case IDefinitionNodeGrid definitionNodeGrid: var offset = GridClearanceHelper.GridNodeOffset(pathRequest.AgentSize, definitionNodeGrid.Transformer.Scale); return(new NodePath(path.ToArray(), definitionNodeGrid.Transformer)); case IDefinitionNodeNetwork definitionNodeNetwork: return(new NodePath(path.ToArray(), definitionNodeNetwork.Transformer)); default: throw new NotSupportedException($"{nodeNetwork.DefinitionNodeNetwork.GetType()} is not supported"); } }
private static List <DefinitionNode> RetracePath(AstarNode[] pathfindingNetwork, AstarNode startGridNode, AstarNode endGridNode) { var path = new List <DefinitionNode>(); var currentNode = endGridNode; while (true) { path.Add(currentNode.DefinitionNode); if (currentNode == startGridNode) { break; } currentNode = NodePointer.Dereference(currentNode.Parent, pathfindingNetwork); } path.Reverse(); return(path); }
public bool FindPath(DijkstraNode[] pathfindingNetwork, DijkstraNode targetNode, DijkstraNode startNode, IPathRequest pathRequest) { if (targetNode.Clearance < pathRequest.AgentSize) { return(false); } ResetNetwork(pathfindingNetwork); var openSet = new MaxHeap <DijkstraNode>(pathfindingNetwork.Length); var closedSet = new HashSet <DijkstraNode>(); openSet.Add(targetNode); targetNode.GCost = 0f; while (openSet.Count > 0) { var currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); foreach (var connection in currentNode.DefinitionNode.Connections) { var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork); if ((connection.CollisionCategory & pathRequest.CollisionCategory) != 0 || closedSet.Contains(toNode)) { continue; } if (toNode.Clearance < pathRequest.AgentSize) { toNode.GCost = float.NaN; } else { var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier; if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode)) { toNode.GCost = newMovementCostToNeighbour; if (!openSet.Contains(toNode)) { openSet.Add(toNode); } } } } } return(true); }
public PotentialField FindPath(DijkstraNodeGrid dijkstraNodeNetwork, IPathRequest pathRequest, out bool succes) { try { if (pathRequest.AgentSize % 2 == 0) { throw new InvalidAgentSizeException("Potential fields only support uneven agent sizes such as 1,3,5 etc."); } if (_potentialFieldCache == null || !_potentialFieldCache.TryGetValue(pathRequest, out var potentialField)) { var sw = Stopwatch.StartNew(); var pathfindingNetwork = dijkstraNodeNetwork.GetCollisionLayerNetwork(pathRequest.CollisionCategory); var startNode = NodePointer.Dereference(pathRequest.PathStart.Index, pathfindingNetwork); var targetNode = NodePointer.Dereference(pathRequest.PathEnd.Index, pathfindingNetwork); if (_dijkstraAlgorithm.FindPath(pathfindingNetwork, targetNode, startNode, pathRequest)) { potentialField = FindPath(dijkstraNodeNetwork, pathfindingNetwork, targetNode, pathRequest); } else { potentialField = new PotentialField(dijkstraNodeNetwork.DefinitionNodeGrid.Transformer, (Point2)targetNode.DefinitionNode.Position); } _potentialFieldCache?.Add(pathRequest, potentialField); Debug.WriteLine($"Potentialfield created in {sw.ElapsedMilliseconds} ms."); } var nodeWorldPosition = potentialField.GridTransformer.ToWorld(pathRequest.PathStart.Position); var offset = GridClearanceHelper.GridNodeOffset(pathRequest.AgentSize, dijkstraNodeNetwork.DefinitionNodeGrid.Transformer.Scale); succes = potentialField.GetHeading(nodeWorldPosition + offset).Length > 0; return(potentialField); } catch (Exception ex) { Debug.WriteLine(ex); Debugger.Break(); succes = false; return(null); } }
private static List <DefinitionNode> FindPath(AstarNode[] pathfindingNetwork, AstarNode startNode, AstarNode targetNode, float neededClearance, PathfindaxCollisionCategory collisionCategory) { try { var sw = new Stopwatch(); sw.Start(); var pathSucces = false; if (startNode == targetNode) { return(new List <DefinitionNode> { targetNode.DefinitionNode }); } if (startNode.Clearance >= neededClearance && targetNode.Clearance >= neededClearance) { var openSet = new MaxHeap <AstarNode>(pathfindingNetwork.Length); var closedSet = new HashSet <AstarNode>(); var itterations = 0; var neighbourUpdates = 0; openSet.Add(startNode); while (openSet.Count > 0) { itterations++; var currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); Debug.WriteLine($"NodePath found in {sw.ElapsedMilliseconds} ms. Itterations: {itterations} Neighbourupdates: {neighbourUpdates}"); pathSucces = true; break; } foreach (var connection in currentNode.DefinitionNode.Connections) { var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork); if ((connection.CollisionCategory & collisionCategory) != 0 || closedSet.Contains(toNode)) { continue; } if (toNode.Clearance >= neededClearance) { var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier; if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode)) { toNode.GCost = newMovementCostToNeighbour; toNode.HCost = GetDistance(toNode.DefinitionNode, targetNode.DefinitionNode); toNode.Parent = currentNode.DefinitionNode.Index; neighbourUpdates++; if (!openSet.Contains(toNode)) { openSet.Add(toNode); } } } } } } if (pathSucces) { return(RetracePath(pathfindingNetwork, startNode, targetNode)); } Debug.WriteLine("Did not find a path :("); return(null); } catch (Exception ex) { Debug.WriteLine(ex); Debugger.Break(); return(null); } }