void Reset()
{
foreach (Node node in touchedNodes)
{
node.Reset();
}
touchedNodes.Clear();
priorityQueue.Clear();
targetNode = null;
distCalc = null;
}
Node FindPathInNodes(Vector3 source,
INode target,
INodeDistCalculator nodeDistCalculator)
{
Node startNode = GetTileNode(source).GetClosestNode(source);
distCalc = nodeDistCalculator as NodeDistCalculator;
if (distCalc == null)
{
throw new ArgumentException("Cannot calculate node distances with calculator for a different algorithm", nameof(nodeDistCalculator));
}
targetNode = target as Node;
if (targetNode == null)
{
throw new ArgumentException("Target node was not created by this pathfinding algorithm", nameof(target));
}
// Enque the starting node
priorityQueue.Enqueue(startNode, 0);
// Add the starting node to touched nodes, so it does not get enqued again
touchedNodes.Add(startNode);
double minDistToTarget = Vector3.Distance(startNode.Position, targetNode.Position);
//Main loop
while (priorityQueue.Count != 0)
{
Node currentNode = priorityQueue.Dequeue();
//If we hit the target, finish and return the sourceNode
if (currentNode == targetNode)
{
if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.TouchedNodes)
{
VisualizeTouchedNodes(targetNode.Time);
}
return(currentNode);
}
//If not finished, add untouched neighbours to the queue and touched nodes
currentNode.ProcessNeighbours(currentNode, priorityQueue, touchedNodes, targetNode, distCalc, ref minDistToTarget);
}
if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.TouchedNodes)
{
VisualizeTouchedNodes(targetNode.Time);
}
//Did not find path
return(null);
}
public override void ProcessNeighbours(Node source,
FastPriorityQueue <Node> priorityQueue,
List <Node> touchedNodes,
Node targetNode,
NodeDistCalculator distCalc,
ref double minDistToTarget)
{
State = NodeState.Closed;
foreach (var neighbour in outgoingEdges)
{
ProcessNeighbour(neighbour.Key,
priorityQueue,
touchedNodes,
targetNode,
distCalc,
neighbour.Value,
ref minDistToTarget);
}
}
public override IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator nodeDist)
{
if (PreviousNode.NodeType == NodeType.Tile && PreviousNode.GetMovementTypeToNeighbour(this) == MovementType.Linear)
{
var borderNode = new TempNode(GetEdgePosition((ITileNode)PreviousNode), Map);
float totalTime = Time - PreviousNode.Time;
if (!nodeDist.GetTime(PreviousNode, borderNode, MovementType.Linear, out float firstTime))
{
throw new ArgumentException($"Wrong {nameof(nodeDist)} implementation, does not give the same result on path building.");
}
return(new[]
{
new Waypoint(borderNode, firstTime, MovementType.Linear),
new Waypoint(this, totalTime - firstTime, MovementType.Linear)
});
}
else
{
return(new[] { new Waypoint(this, Time - PreviousNode.Time, PreviousNode.GetMovementTypeToNeighbour(this)) });
}
}
/// <summary> /// Returns waypoints to get from <see cref="previousNode"/> to this node /// </summary> /// <returns>Returns waypoints to get from <see cref="previousNode"/> to this node</returns> public abstract IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator nodeDist);
public abstract void ProcessNeighbours(Node source,
FastPriorityQueue <Node> priorityQueue,
List <Node> touchedNodes,
Node targetNode,
NodeDistCalculator distCalc,
ref double minDistToTarget);
protected void ProcessNeighbour(Node neighbour,
FastPriorityQueue <Node> priorityQueue,
List <Node> touchedNodes,
Node targetNode,
NodeDistCalculator distCalc,
MovementType movementType,
ref double minDistToTarget)
{
if (neighbour.State == NodeState.Closed)
{
//Already closed, either not passable or the best path there can be found
return;
}
double distance = Vector3.Distance(neighbour.Position, targetNode.Position);
//If the neighbor is too far out of the way from the closest path we found yet
if (distance > minDistToTarget + AStar.Cutoff)
{
if (neighbour.State == NodeState.Untouched)
{
touchedNodes.Add(neighbour);
}
neighbour.State = NodeState.Closed;
return;
}
else if (distance < minDistToTarget)
{
minDistToTarget = distance;
}
//If Unit can pass to target node from source node
if (distCalc.GetTime(this, neighbour, movementType, out float timeToTarget))
{
if (neighbour.State == NodeState.Untouched)
{
// Compute the heuristic for the new node
float heuristic = distCalc.GetMinimalAproxTime(neighbour.Position.XZ(), targetNode.Position.XZ());
neighbour.State = NodeState.Opened;
neighbour.Heuristic = heuristic;
neighbour.PreviousNode = this;
neighbour.Time = Time + timeToTarget;
//Unit can pass through this node, enqueue it
priorityQueue.Enqueue(neighbour, neighbour.Value);
touchedNodes.Add(neighbour);
}
else if (neighbour.State == NodeState.Opened)
{
float newTime = Time + timeToTarget;
//if it is closer through the current sourceNode
if (newTime < neighbour.Time)
{
neighbour.Time = newTime;
neighbour.PreviousNode = this;
priorityQueue.UpdatePriority(neighbour, neighbour.Value);
}
}
}
}
public override IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator distCalc)
{
return(new[] { new Waypoint(this,
Time - PreviousNode.Time,
PreviousNode.GetMovementTypeToNeighbour(this)) });
}
