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);
}
}
