//A* implementation
public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){
#if DEBUG
Stopwatch sw = new Stopwatch();
sw.Start();
#endif
var openList = new Heap<PathNode>(allNodes.Length);
var closedList = new HashSet<PathNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0){
var currentNode = openList.RemoveFirst();
closedList.Add(currentNode);
if (currentNode == endNode){
#if DEBUG
sw.Stop();
//UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms.");
#endif
success = true;
break;
}
foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){
if (closedList.Contains(neighbour))
continue;
var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour );
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour)){
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
}
yield return null;
if (success){
callback( RetracePath(startNode, endNode), true );
} else {
callback( new Vector2[0], false );
}
}
private static Vector2[] RetracePath(PathNode startNode, PathNode endNode){
var path = new List<Vector2>();
var currentNode = endNode;
while(currentNode != startNode){
path.Add(currentNode.pos);
currentNode = currentNode.parent;
}
path.Add(startNode.pos);
path.Reverse();
return path.ToArray() ;
}
private static Vector2[] RetracePath(PathNode startNode, PathNode endNode)
{
var path = new List <Vector2>();
var currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode.pos);
currentNode = currentNode.parent;
}
path.Add(startNode.pos);
path.Reverse();
return(path.ToArray());
}
public int Compare(System.Object a, System.Object b)
{
PathNode nodeA = a as PathNode;
PathNode nodeB = b as PathNode;
if (nodeA.estimatedCost < nodeB.estimatedCost)
{
return(-1);
}
else if (nodeA.estimatedCost > nodeB.estimatedCost)
{
return(1);
}
else
{
return(0);
}
}
private static float GetDistance(PathNode a, PathNode b){
return (a.pos - b.pos).magnitude;
}
//A* implementation
public static IEnumerator CalculatePath(PathNode start, PathNode end, PathNode[] allNodes, System.Action <List <Vector2> > callback)
{
int n = 0;
PriorityQueue openList = new PriorityQueue();
PriorityQueue closedList = new PriorityQueue();
openList.Push(start);
start.cost = 0;
start.estimatedCost = HeuristicEstimate(start, end, heuristicWeight);
PathNode currentNode = null;
while (openList.Count != 0)
{
currentNode = openList.Front();
if (currentNode == end)
{
break;
}
List <int> links = currentNode.links;
for (int i = 0; i != links.Count; i++)
{
var endNode = allNodes[links[i]];
float incrementalCost = GetCost(currentNode, endNode);
float endNodeCost = currentNode.cost + incrementalCost;
if (closedList.Contains(endNode))
{
if (endNode.cost <= endNodeCost)
{
continue;
}
closedList.Remove(endNode);
}
else if (openList.Contains(endNode))
{
if (endNode.cost <= endNodeCost)
{
continue;
}
}
float endNodeHeuristic = HeuristicEstimate(endNode, end, heuristicWeight);
endNode.cost = endNodeCost;
endNode.parent = currentNode;
endNode.estimatedCost = endNodeCost + endNodeHeuristic;
if (!openList.Contains(endNode))
{
openList.Push(endNode);
}
}
closedList.Push(currentNode);
openList.Remove(currentNode);
n++;
if (n > 300)
{
yield return(null);
}
}
if (!currentNode.Equals(end))
{
// Debug.LogWarning("No path found :(");
callback(new List <Vector2>());
yield break;
}
else
{
var path = new List <Vector2>();
while (currentNode != null)
{
path.Add(currentNode.pos);
currentNode = currentNode.parent;
}
path.Reverse();
callback(path);
yield break;
}
}
public void Remove(PathNode node)
{
nodes.Remove(node);
nodes.Sort();
}
public bool Contains(PathNode node)
{
return(nodes.Contains(node));
}
public int Push(PathNode node)
{
nodes.Add(node);
nodes.Sort();
return(nodes.Count);
}
private static float GetCost(PathNode nodeA, PathNode nodeB)
{
return((nodeA.pos - nodeB.pos).magnitude);
}
private static float HeuristicEstimate(PathNode currentNode, PathNode endNode, float heuristicWeight)
{
return((currentNode.pos - endNode.pos).magnitude * heuristicWeight);
}
private static float GetDistance(PathNode a, PathNode b)
{
return((a.pos - b.pos).magnitude);
}
//A* implementation
public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action <Vector2[], bool> callback)
{
#if DEBUG
Stopwatch sw = new Stopwatch();
sw.Start();
#endif
var openList = new Heap <PathNode>(allNodes.Length);
var closedList = new HashSet <PathNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0)
{
var currentNode = openList.RemoveFirst();
closedList.Add(currentNode);
if (currentNode == endNode)
{
#if DEBUG
sw.Stop();
UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms.");
#endif
success = true;
break;
}
foreach (var neighbour in currentNode.links.Select(index => allNodes[index]))
{
if (closedList.Contains(neighbour))
{
continue;
}
var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour))
{
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
}
yield return(null);
if (success)
{
callback(RetracePath(startNode, endNode), true);
}
else
{
callback(new Vector2[0], false);
}
}
//A* implementation
public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes, Action <Vector2[]> callback)
{
var sw = new Stopwatch();
sw.Start();
var openList = new Heap <PathNode>(allNodes.Count);
var closedList = new HashSet <PathNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0)
{
var currentNode = openList.RemoveFirst();
closedList.Add(currentNode);
if (currentNode == endNode)
{
sw.Stop();
success = true;
break;
}
var linkIndeces = currentNode.links;
for (var i = 0; i < linkIndeces.Count; i++)
{
var neighbour = allNodes[linkIndeces[i]];
if (closedList.Contains(neighbour))
{
continue;
}
var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour))
{
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
if (sw.ElapsedMilliseconds > 30)
{
yield return(null);
}
}
yield return(null);
if (success)
{
callback(RetracePath(startNode, endNode));
}
else
{
callback(null);
}
}
private static Vector2[] Internal_CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes)
{
var openList = new Heap <PathNode>(allNodes.Count);
var closedList = new HashSet <PathNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0)
{
var currentNode = openList.RemoveFirst();
if (currentNode == endNode)
{
success = true;
break;
}
closedList.Add(currentNode);
var linkIndeces = currentNode.links;
for (var i = 0; i < linkIndeces.Count; i++)
{
var neighbour = allNodes[linkIndeces[i]];
if (closedList.Contains(neighbour))
{
continue;
}
var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour))
{
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
}
if (success) //Retrace Path if one exists
{
var path = new List <Vector2>();
var currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode.pos);
currentNode = currentNode.parent;
}
path.Add(startNode.pos);
path.Reverse();
return(path.ToArray());
}
return(null);
}
//A* implementation
public static void CalculatePath(PathNode startNode, PathNode endNode, List <PathNode> allNodes, Action <Vector2[]> callback)
{
var path = Internal_CalculatePath(startNode, endNode, allNodes);
callback(path);
}
