/// <summary>
/// Reconstructs the path when given the last Node
/// </summary>
/// <param name="target">Last Node of the path</param>
/// <returns>Path in correct order, from first point to the last point</returns>
Path MakePath(Vector3 source, Node target)
{
List <Node> nodes = new List <Node>();
while (target != null)
{
nodes.Add(target);
target = target.PreviousNode;
}
//Reverse so that source is first and target is last
nodes.Reverse();
if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.FinalPath)
{
VisualizePath(nodes);
}
if (nodes.Count == 1)
{
return(StartIsFinish(nodes[0], source));
}
List <Waypoint> waypoints = new List <Waypoint>();
waypoints.Add(new Waypoint(new TempNode(source, Map), 0, MovementType.None));
for (int i = 1; i < nodes.Count; i++)
{
waypoints.AddRange(nodes[i].GetWaypoints(distCalc));
}
switch (waypoints[1].MovementType)
{
case MovementType.None:
throw new InvalidOperationException("Waypoint on a path cannot have a MovementType of None");
case MovementType.Teleport:
//NOTHING
break;
default:
//Default to linear movement
Waypoint sourceWaypoint = waypoints[0];
Waypoint firstWaypoint = waypoints[1];
if (distCalc.GetTime(sourceWaypoint.Node, firstWaypoint.Node, MovementType.Linear, out float time))
{
waypoints[1] = firstWaypoint.WithTimeToWaypointSet(time);
}
else
{
waypoints[1] = firstWaypoint.WithTimeToWaypointSet(distCalc.GetMinimalAproxTime(source, firstWaypoint.Position));
}
break;
}
return(Path.CreateFrom(waypoints, Map));
}
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);
}
}
}
}