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); } } } } } }