//public static IEnumerator AStar(Graph2d _graph, IVector2 _start, IVector2 _end, double _turnCostAdder, double _defVaul, Func<IVector2, IVector2, double, double> _heuristic) public static void AStar(Graph2d _graph, IVector2 _start, IVector2 _end, double _turnCostAdder, double _defVaul, Func <IVector2, IVector2, double, double> _heuristic) { var openList = new ioMinPriQ <AStarOpenNode>(); openList.Enqueue(new AStarOpenNode(_start, null, 0, _heuristic(_start, _end, _defVaul))); var closedList = new Dictionary <IVector2, AStarClosedNode>(); /* //UNITY * bool debugMe = true; * var texture = (Texture2D)TileMap2D.meshRenderer.material.mainTexture; * int frameUpdateRate = 30; * if (debugMe) * { * * texture.SetPixel(_start.x, _start.y, new Color(0.6f, 1f, 0.6f, 1)); * texture.SetPixel(_end.x, _end.y, new Color(0.6f, 1f, 0.6f, 1)); * texture.Apply(); * } * */ AStarOpenNode curNode = null; var prevDir = -1; while (!openList.IsEmpty) { //Find smallest element in open list (est total cost) curNode = openList.Dequeue(); //Get direction of step from current node if (curNode.Parent != null) { prevDir = (int)curNode.StepDirection; } //Exit if goal found if (curNode.Coord.Equals(_end)) { closedList[_end] = new AStarClosedNode(curNode); break; } for (int i = 1; i < 8; i = i + 2) //foreach (var neighbor in curNode.Coord.Neighbors()) { IVector2 neighbor = curNode.Coord + DirMap.Get(i); if (!_graph.InBounds(neighbor) || (_graph.IsImpassable(neighbor))) { continue; } //Set end node and add cost //var endNode = neighbor; //var endNodeCost = curNode.CostSoFar + _graph.GetEdgeWeight(curNode.Coord, (G2Dir) i);//_graph[neighbor].GetEdge((G2Dir)i); var endNodeCost = curNode.CostSoFar + _graph.GetNodeWeight(neighbor); if (i != prevDir && prevDir != -1) { endNodeCost += _turnCostAdder; } //Skip if not walkable if (endNodeCost == Graph2d.IMPASSABLE) { continue; } //Skip if better path already exists var skipNode = false; for (int j = 0; j < openList.DataList.Count; ++j) { if (openList.DataList[j].Coord.Equals(neighbor)) { if (openList.DataList[j].CostSoFar <= endNodeCost) { skipNode = true; break; } openList.Remove(openList.DataList[j]); } } if (skipNode) { continue; } //Skip or update if current path is worse/better if (closedList.ContainsKey(neighbor)) { if (closedList[neighbor].CostSoFar <= endNodeCost) { continue; } closedList[neighbor] = new AStarClosedNode(curNode.Coord, endNodeCost); /* //UNITY * if (debugMe) * texture.SetPixel(neighbor.x, neighbor.y, new Color(0.0f, 1f, 1f, 1f)); */ } var endNode = new AStarOpenNode(neighbor, curNode.Coord, endNodeCost, endNodeCost + _heuristic(neighbor, _end, _defVaul)); openList.Enqueue(endNode); } closedList[curNode.Coord] = new AStarClosedNode(curNode); /* //UNITY * if (debugMe) * { * * var pixel = texture.GetPixel(curNode.Coord.x, curNode.Coord.y); * pixel = new Color(pixel.r + 0.3f, pixel.g, pixel.b, pixel.a); * texture.SetPixel(curNode.Coord.x, curNode.Coord.y, pixel); * texture.Apply(); * if (++debugFrameCount == frameUpdateRate) * { * debugFrameCount = 0; * yield return null; * } * * } */ } if (curNode.Coord != _end) { String dbgMsg = "End Node Null -- No path found?"; Msg.LogDebug(TAG_DEBUG, dbgMsg, MsgPriLvl.HIGH); AStarPath = null; //return null; new List<IVector2>(); } else { var path = new List <IVector2>() { curNode.Coord }; var closedNode = new AStarClosedNode(curNode); ; while (closedNode.Parent != null) { path.Insert(0, closedNode.Parent.Value); Msg.LogDebug(TAG_DEBUG, "Added to path: " + closedNode.Parent, MsgPriLvl.LOW); closedNode = closedList[closedNode.Parent.Value]; //TODO Seed 705 crashes here with invalid key } AStarPath = path; closedList[curNode.Coord] = new AStarClosedNode(curNode); /* //UNITY * if (debugMe) * { * foreach (var coord in path) * texture.SetPixel(coord.x, coord.y, new Color(0.8f, 0.8f, 1f, 1)); * texture.Apply(); * yield return null; * } */ } //yield return null; //UNITY }
public AStarClosedNode(AStarOpenNode _node) { Parent = _node.Parent; CostSoFar = _node.CostSoFar; }