protected virtual List <BlockLoc> Path(IslandPathingProfile startProfile, ref BlockLoc startLoc,
HashSet <BlockLoc> goals, int heightOfEntity)
{
foreach (BlockLoc goal in goals)
{
if (startLoc.Equals(goal))
{
List <BlockLoc> noPathResult = new List <BlockLoc>();
noPathResult.Add(startLoc);
return(noPathResult);
}
}
PathNodePriorityQueue openNodes = new PathNodePriorityQueue();
HashSet <BlockLoc> visitedLocations = new HashSet <BlockLoc>();
openNodes.insertNode(new PathNode(null, startLoc, 0, goals.First()));
IslandPathingProfile profile = startProfile;
while (openNodes.size() > 0)
{
PathNode from = openNodes.pop();
List <BlockLoc> nextSteps = profile.getSpacesThatCanBeMovedToFrom(from.loc, heightOfEntity);
//adding new nodes to the openNodes unmippedArray
foreach (BlockLoc next in nextSteps)
{
if (!visitedLocations.Contains(next))
{
PathNode toAdd = new PathNode(from, next, from.costToGetHere + 1, goals.First());
if (goals.Contains(toAdd.loc))
{
List <BlockLoc> finalPath = getPathListFromEnd(toAdd);
finalPath.RemoveAt(0);
finalPath.Add(toAdd.loc);
Console.WriteLine(finalPath.Count);
return(finalPath);
}
openNodes.insertNode(toAdd);
visitedLocations.Add(next);
}
}
}
return(null);//no path found
}