public AStarNodes(Node currentNode, Node endNode, float costSoFar = 0, AStarNodes cameFrom = null)
{
this.costSoFar = costSoFar;
heuristic = GetDistance(currentNode, endNode);
nodeVal = currentNode;
this.cameFrom = cameFrom;
}
// Check is another node of the same name exists in the given list
private static int AnotherExists(AStarNodes temp, List <AStarNodes> open)
{
for (int i = 0; i < open.Count; i++)
{
if (temp.nodeVal.name.Equals(open[i].nodeVal.name))
{
return(i);
}
}
return(-1);
}
// Get the lowest AStar Node with the lowest total estimetad cost
private static AStarNodes GetLowestTecAStarNode(List <AStarNodes> open)
{
float lowest = float.NaN;
AStarNodes smallestNode = null;
for (int i = 0; i < open.Count; i++)
{
if (float.IsNaN(lowest))
{
lowest = open[i].tec;
smallestNode = open[i];
}
else if (open[i].tec < lowest)
{
lowest = open[i].tec;
smallestNode = open[i];
}
}
return(smallestNode);
}
private static String EvaluateNode(Node startNode, Node endNode)
{
List <AStarNodes> open = new List <AStarNodes>();
List <AStarNodes> closed = new List <AStarNodes>();
Node currNode = startNode;
float costSoFar;
AStarNodes currANode = new AStarNodes(currNode, endNode);
open.Add(currANode);
for (int i = 0; i < open.Count; i++)
{
currNode = GetLowestTecNode(open);
currANode = GetLowestTecAStarNode(open);
open.Remove(currANode);
costSoFar = currANode.costSoFar;
if (currNode.name.Equals(endNode.name))
{
break;
}
for (int j = 0; j < currNode.links.Length; j++)
{
AStarNodes temp = new AStarNodes(currNode.links[j], endNode, costSoFar + GetDistance(currNode, currNode.links[j]),
currANode);
if (AnotherExists(temp, open) != -1)
{
if (temp.costSoFar < open[AnotherExists(temp, open)].costSoFar)
{
open.RemoveAt(AnotherExists(temp, open));
open.Add(temp);
}
}
if (AnotherExists(temp, closed) != -1)
{
if (temp.costSoFar < closed[AnotherExists(temp, closed)].costSoFar)
{
closed.RemoveAt(AnotherExists(temp, closed));
open.Add(temp);
}
}
else
{
open.Add(temp);
}
}
closed.Add(open[i]);
}
Stack <String> path = new Stack <string>();
while (currANode.cameFrom != null)
{
path.Push(currANode.nodeVal.name);
currANode = currANode.cameFrom;
}
path.Push(currANode.nodeVal.name);
String finalPath = "";
foreach (String letter in path)
{
finalPath += letter + ">";
}
return(finalPath.Substring(0, finalPath.Length - 1));
}
private List<AStarNodes> GetNextPositions(AStarNodes CurrentNode, IVec2 MapPosEnd)
{
List<AStarNodes> NextPositions = new List<AStarNodes>();
for (IVec2 offset = new IVec2(-1, -1); offset.x <= 1; ++offset.x)
{
for (offset.y = -1; offset.y <= 1; ++offset.y)
{
IVec2 newPos = CurrentNode.NodeInfo.MapPos + offset;
if(newPos.x <0 ||
newPos.x > Map.CurrentMap.sizeX)
break;
if (newPos.y < 0 || newPos == CurrentNode.NodeInfo.MapPos ||
newPos.y > Map.CurrentMap.sizeY ||
offset == new IVec2(0,0))
continue;
if(Map.Trees.Contains(Map.CurrentMap.getTile(newPos.x, newPos.y)) ||
Map.Terrain.Contains(Map.CurrentMap.getTile(newPos.x, newPos.y)))
{
AStarNodes newNode = new AStarNodes();
newNode.DistanceGone = CurrentNode.DistanceGone + offset.magnitude();
//newNode.NodeInfo.MapSymbol = PathFinder.CurrentMap.getTile(newPos.x, newPos.y)
newNode.NodeInfo = new Node();
newNode.NodeInfo.MapPos = newPos;
newNode.NodeInfo.PrevNode = CurrentNode.NodeInfo;
newNode.Distance2Go = GetDirectDistance2End(newPos, MapPosEnd);
NextPositions.Add(newNode);
}
}
}
return NextPositions;
}
private bool isValid(AStarNodes testNode, ref List<AStarNodes> CloseList)
{
bool found = false;
for(int idx = CloseList.Count-1; idx>=0; --idx)
{
if(CloseList[idx].NodeInfo.MapPos == testNode.NodeInfo.MapPos)
{
if(CloseList[idx].DistanceGone > testNode.DistanceGone)
{
CloseList.RemoveAt(idx);
break;
}
found = true;
}
}
return !found;
}
public path GetPath(IVec2 MapPosStart, IVec2 MapPosEnd)
{
List<Node> result = new List<Node>();
List<AStarNodes> openQueue = new List<AStarNodes>();
List<AStarNodes> CloseList = new List<AStarNodes>();
AStarNodes startNode = new AStarNodes();
Node EndNode = null;
startNode.NodeInfo = new Node();
startNode.NodeInfo.MapPos = MapPosStart;
startNode.NodeInfo.MapSymbol = Map.CurrentMap.getTile(MapPosStart.x, MapPosStart.y);
startNode.DistanceGone = 0;
startNode.Distance2Go = GetDirectDistance2End(MapPosStart, MapPosEnd);
openQueue.Add(startNode);
CloseList.Add(startNode);
while (openQueue.Count > 0)
{
var currentNode = openQueue[0];
openQueue.RemoveAt(0);
if(currentNode.NodeInfo.MapPos == MapPosEnd)
{
EndNode = currentNode.NodeInfo;
break;
}
var newNodes = GetNextPositions(currentNode, MapPosEnd);
foreach (var item in newNodes)
{
if (isValid(item, ref CloseList))
{
openQueue.Add(item);
CloseList.Add(item);
}
}
openQueue.Sort((AStarNodes a, AStarNodes b) => (a.Distance2Go + a.DistanceGone).CompareTo(b.Distance2Go + b.DistanceGone));
}
while(EndNode != null)
{
if(EndNode.PrevNode != null)
EndNode.PrevNode.NextNode = EndNode;
result.Insert(0, EndNode);
EndNode = EndNode.PrevNode;
}
// Debug.Log("AStarStopped path size: " + result.Count);
//output
{
path theWay = new path();
theWay.FoundPath = result;
theWay.isPathFound = result.Count > 0;
return theWay;
}
}