using System;
using System.Collections.Generic;
namespace MonoTest
{
public class AStar
{
public static int resultLength=0;
class Node
{
public Node(int x, int y)
{
posX = x;
posY = y;
neighbors = new List<Node>();
}
public List<Node> neighbors;
public int posX;
public int posY;
public float g = float.PositiveInfinity;
public float f = float.PositiveInfinity;
public Node previous = null;
}
public static void Main()
{
for(int i=0; i<1000; ++i)
{
performAStar();
}
}
public static void performAStar()
{
string map = "S XXX X\n"
+"XX XXXXXXXXXXXXX X\n"
+" XX X\n"
+"X XXX XXXXXX\n"
+" XXXX XXXXXXXX\n"
+"XXXXXXX X X\n"
+"XX X XXXXX X\n"
+" XX XX X\n"
+"XXXXXXXXXXX X\n"
+"XX XXXXXXXXX X\n"
+" XXXXXX X\n"
+"X XXX XXXXXX\n"
+" XXXX XXXXXXXX\n"
+"XXXXXXX X X\n"
+"XX X XXXXX X\n"
+" XX XX X\n"
+"XXXXX XXXXX X\n"
+"XX XXXX XXXX X\n"
+" XXXXXX X\n"
+"X XXX XXXXXX\n"
+" XXXX XXXXXXXX\n"
+"XXXXX XX X X\n"
+"XX X XXXXX X\n"
+" XX XX X\n"
+"XXXXX XXXXXXXXXXXX X\n"
+" G XX X\n"
+"XXXXXXXXXXXXXXXXXXXXXXX";
string[] lines = map.Split ('\n');
Node start = null;
Node goal = null;
int width = lines [0].Length;
int height = lines.Length;
Node[,] nodes = new Node[width,height];
//initialize the node grid with zeros
for (int x = 0; x < width; x++)
for (int y = 0; y < height; y++)
nodes [x,y] = null;
//create the accessible nodes on the grid
for (int y = 0; y < height; y++)
{
string line = lines [y];
for (int x = 0; x < width; x++)
{
char c = line [x];
//accessible node?
if (c != 'X')
{
Node node = new Node (x, y);
if (c == 'S')
start = node;
if (c == 'G')
goal = node;
nodes [x,y] = node;
}
}
}
//connect the nodes
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
if(nodes [x,y] == null)
continue;
//up
if (y > 0 && nodes [x,y - 1] != null)
nodes [x,y].neighbors.Add (nodes [x,y - 1]);
//up right
if (y > 0 && x < width - 1 && nodes [x + 1,y - 1] != null)
nodes [x,y].neighbors.Add (nodes [x + 1,y - 1]);
//right
if (x < width - 1 && nodes [x + 1,y] != null)
nodes [x,y].neighbors.Add (nodes [x + 1,y]);
//right down
if (x < width - 1 && y < height && nodes [x + 1,y + 1] != null)
nodes [x,y].neighbors.Add (nodes [x + 1,y + 1]);
//down
if (y < height && nodes [x,y + 1] != null)
nodes [x,y].neighbors.Add (nodes [x,y + 1]);
//down left
if (y < height && x > 0 && nodes [x - 1,y + 1] != null)
nodes [x,y].neighbors.Add (nodes [x - 1,y + 1]);
//left
if (x > 0 && nodes [x - 1,y] != null)
nodes [x,y].neighbors.Add (nodes [x - 1,y]);
//left up
if (x > 0 && y > 0 && nodes [x - 1,y - 1] != null)
nodes [x,y].neighbors.Add (nodes [x - 1,y - 1]);
}
}
//perform the actal pathfinding
if (!aStar(start, goal))
{
Console.Error.WriteLine("Unable to find a path.");
}
//reconstruct the path
List<Node> path = new List<Node> ();
Node waypoint = goal;
path.Add (goal);
while (waypoint.previous != null)
{
path.Add (waypoint.previous);
waypoint = waypoint.previous;
}
string result = "";
for (int i = path.Count - 1; i >= 0; i--)
{
Node p = path[i];
result += "("+p.posX.ToString()+","+p.posY.ToString()+")";
}
//make sure the compiler can't optimize away any important computations by saving the result:
resultLength = result.Length;
//Console.WriteLine(result);
}
private static bool aStar(Node start, Node goal)
{
List<Node> closedSet = new List<Node> ();
List<Node> openSet = new List<Node> ();
openSet.Add (start);
start.g = 0.0f;
start.f = h (start, goal);
while (openSet.Count > 0)
{
Node current = findBest(openSet);
if(current == goal)
return true;
openSet.Remove(current);
closedSet.Add(current);
foreach(Node neighbor in current.neighbors)
{
if(closedSet.Contains(neighbor))
continue;
float newG = current.g + h(current, neighbor);
if(newG < neighbor.g)
{
neighbor.g = newG;
neighbor.f = newG + h(neighbor, goal);
neighbor.previous = current;
}
if(!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
return false;
}
private static float h(Node current, Node goal)
{
double dx = current.posX - goal.posX;
double dy = current.posY - goal.posY;
return (float)Math.Sqrt(dx*dx + dy*dy);
}
private static Node findBest(List<Node> list)
{
Node best = null;
float bestF = float.PositiveInfinity;
foreach (Node node in list)
{
if(node.f < bestF)
{
best = node;
bestF = node.f;
}
}
return best;
}
}
}
