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