public static int Compare(int a, PFNode b)
{
return(a - b.GetFcost());
}
public static int Compare(PFNode a, int b)
{
return(a.GetFcost() - b);
}
public List <Vector2> FindPath(Vector2Int from, Vector2Int to)
{
Debug.Log(string.Format("Finding path from ({0},{1}) to ({2},{3})", from.x, from.y, to.x, to.y));
PFNode start = CreateInstance <PFNode>();
start.pos = from;
start.gcost = 0;
start.hcost = Dist(start.pos, to);
start.backpath = null;
Queue.Enqueue(start);
int loops = 0;
while (true)
{
if (loops >= 500)
{
Debug.LogError("TOO MANY LOOPS! BREAKING.");
return(null);
}
PFNode cur = Queue.Dequeue();
if (cur == null)
{
Debug.LogError("CAN'T FIND PATH");
return(null);
}
Closed.Add(cur);
if (cur.pos == to)
{
Debug.Log("FOUND DESTINATION");
break;
}
int newGcost = cur.gcost + 1;
for (int i = 0; i < 4; i++)
{
Vector2Int neighborPos = DirToValue(i) + cur.pos;
if (tm.HasTile(new Vector3Int(neighborPos.x, neighborPos.y, 0)) || CloseContains(neighborPos))
{
continue;
}
PFNode neighbor = Queue.Take(neighborPos);
if (neighbor == null)
{
neighbor = ScriptableObject.CreateInstance <PFNode>();
neighbor.pos = neighborPos;
neighbor.hcost = Dist(neighbor.pos, to);
}
else if (neighbor.gcost <= newGcost)
{
continue;
}
neighbor.gcost = newGcost;
neighbor.backpath = cur.pos;
if (neighbor.GetFcost() <= MaxFCost)
{
Queue.Enqueue(neighbor);
}
}
loops++;
}
PFNode endNode = Closed[Closed.Count - 1];
List <Vector2Int> BackPath = new List <Vector2Int>();
while (endNode != null)
{
BackPath.Insert(0, endNode.pos);
endNode = FindInClosed(endNode.backpath);
}
return(FinalizePositions(BackPath));
}
public static int Compare(PFNode a, PFNode b)
{
return(a.GetFcost() - b.GetFcost());
}
public void Enqueue(PFNode node)
{
int ind = InsertSearch(0, List.Count, node.GetFcost());
List.Insert(ind, node);
}