void AStar(int startPos, int goalPos)
{
Node startNode = new Node {
NodeCell = startPos
};
Node goalNode = new Node {
NodeCell = goalPos
};
startNode.h_Cost = map.GetDistance(startNode, goalNode);
open.Add(startNode);
openList.Add(startNode.NodeCell);
while (open.Count > 0)
{
Node current = open[0];
for (int i = 1; i < open.Count; i++)
{
if ((open[i].F_Cost < current.F_Cost || open[i].F_Cost == current.F_Cost) && open[i].h_Cost < current.h_Cost)
{
current = open[i];
}
}
print(current.NodeCell);
open.Remove(current);
openList.Remove(current.NodeCell);
closed.Add(current.NodeCell);
if (current.NodeCell == goalNode.NodeCell || current.h_Cost == 0)
{
hashStack.Push(current.NodeCell);
before4AStar = current.parent;
hashStack.Push(before4AStar.NodeCell);
//Debug.DrawLine(groundGraph[current.NodeCell / mapSize, current.NodeCell % mapSize].position, groundGraph[before4AStar.NodeCell / mapSize, before4AStar.NodeCell % mapSize].position, Color.red, 3);
if (before4AStar.NodeCell == startPos)
{
break;
}
while (true)
{
//Debug.DrawLine(groundGraph[before4AStar.NodeCell / mapSize, before4AStar.NodeCell % mapSize].position, groundGraph[before4AStar.parent.NodeCell / mapSize, before4AStar.parent.NodeCell % mapSize].position, Color.red, 3);
before4AStar = before4AStar.parent;
hashStack.Push(before4AStar.NodeCell);
if (before4AStar.NodeCell == startPos)
{
hashStack.Pop();
break;
}
}
break;
}
for (int k = 0; k < 8; k++)
{
neighbour = new Node {
NodeCell = current.NodeCell + map.direction[k]
};
if (map.Area(current.NodeCell, k))
{
continue;
}
if (map.map[neighbour.NodeCell / map.mapSize, neighbour.NodeCell % map.mapSize] == 1 && !closed.Contains(neighbour.NodeCell))
{
if (k == 1 || k == 3 || k == 5 || k == 7)
{
int numCheckPoint1 = current.NodeCell + map.direction[(k - 1) % 8];
int numCheckPoint2 = current.NodeCell + map.direction[(k + 1) % 8];
if (map.Area(current.NodeCell, k))
{
continue;
}
if (map.map[numCheckPoint1 / map.mapSize, numCheckPoint1 % map.mapSize] == 0 && map.map[numCheckPoint2 / map.mapSize, numCheckPoint2 % map.mapSize] == 0)
{
continue;
}
}
if (neighbourArray[neighbour.NodeCell] == null)
{
neighbourArray[neighbour.NodeCell] = neighbour;
}
int newMoveCost = current.g_Cost + map.GetDistance(current, neighbour);
if (newMoveCost < neighbourArray[neighbour.NodeCell].g_Cost || !openList.Contains(neighbour.NodeCell))
{
neighbourArray[neighbour.NodeCell].g_Cost = newMoveCost;
neighbourArray[neighbour.NodeCell].h_Cost = map.GetDistance(neighbour, goalNode);
neighbourArray[neighbour.NodeCell].parent = current;
if (!openList.Contains(neighbour.NodeCell))
{
open.Add(neighbourArray[neighbour.NodeCell]);
openList.Add(neighbour.NodeCell);
}
}
}
}
}
}
