static void GraphTest()
{
Graphs.DirectedGraph <string, string> graph = new Graphs.DirectedGraph <string, string>();
//Nodes
graph.AddNode(new GraphNode("A"));
graph.AddNode(new GraphNode("B"));
graph.AddNode(new GraphNode("C"));
graph.AddNode(new GraphNode("D"));
graph.AddNode(new GraphNode("E"));
graph.AddNode(new GraphNode("F"));
//Edges
graph.AddEdge(graph.GetNode("A"), graph.GetNode("B"), new GraphEdge("A -> B", 10));
graph.AddEdge(graph.GetNode("A"), graph.GetNode("C"), new GraphEdge("A -> C", 2));
graph.AddEdge(graph.GetNode("B"), graph.GetNode("E"), new GraphEdge("B -> E", 4));
graph.AddEdge(graph.GetNode("B"), graph.GetNode("F"), new GraphEdge("B -> F", 1));
graph.AddEdge(graph.GetNode("C"), graph.GetNode("D"), new GraphEdge("C -> D", 5));
graph.AddEdge(graph.GetNode("D"), graph.GetNode("E"), new GraphEdge("D -> E", 6));
graph.AddEdge(graph.GetNode("F"), graph.GetNode("A"), new GraphEdge("F -> A", 0));
graph.AddEdge(graph.GetNode("F"), graph.GetNode("E"), new GraphEdge("F -> E", 4));
GraphNode[] path = AStar <string, string> .FindPath((GraphNode)graph.GetNode("A"), (GraphNode)graph.GetNode("E"), graph, hGraphTest);
foreach (GraphNode n in path)
{
Console.WriteLine(n);
}
Console.ReadKey();
}
public static Node[] FindPath(Node start, Node goal, Graphs.DirectedGraph <NodeData, EData> graph, Func <Node, Node, double> heuristic)
{
List <Node> visited = new List <Node>();
dataStructureHeap frontier = new dataStructureHeap(new List <HeapNode>(), dataStructureHeap.HeapProperty.MinHeap);
start.IntermediateCost = 0;
start.EstimatedCost = 0;
start.Parent = null;
frontier.HeapInsert(start);
//Continue as long as there are nodes to explore in the frontier
while (frontier.Count > 0)
{
Node current = (Node)frontier.HeapExtractRoot();
if (current == goal)
{
return(ReconstructPath(current));
}
visited.Add(current);
foreach (var edgeTo in graph.GetEdges(current)) //Expand frontier
{
Node to = (Node)edgeTo.Key;
if (visited.Contains(to))
{
continue;
}
double tentativeScore = current.IntermediateCost + ((EdgeData)edgeTo.Value).cost;
if (tentativeScore < to.IntermediateCost) //Best path to this node so far
{
to.IntermediateCost = tentativeScore;
to.EstimatedCost = tentativeScore + heuristic(to, goal);
to.Parent = current;
}
if (!frontier.Contains(to))
{
frontier.HeapInsert(to);
}
}
}
return(null);
}
static void MapTest()
{
Graphs.DirectedGraph <Vector, string> graph = new Graphs.DirectedGraph <Vector, string>();
GraphicsUnit gu = GraphicsUnit.Pixel;
Vector start = new Vector(0, 0);
Vector goal = new Vector(0, 0);
string file = Directory.GetCurrentDirectory() + "\\test";
Bitmap map = new Bitmap(file + ".png");
int width = (int)map.GetBounds(ref gu).Width;
int height = (int)map.GetBounds(ref gu).Height;
//Build nodes
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
Color c = map.GetPixel(x, y);
if (c == Color.FromArgb(0, 255, 0)) // start
{
start = new Vector(x, y);
graph.AddNode(new MapNode(start));
}
else if (c == Color.FromArgb(255, 0, 0)) // goal
{
goal = new Vector(x, y);
graph.AddNode(new MapNode(goal));
}
else if (c == Color.FromArgb(255, 255, 255)) //blank
{
graph.AddNode(new MapNode(new Vector(x, y)));
}
}
Console.WriteLine($"N - x: {x}");
}
//Build edges
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
Vector c = new Vector(x, y);
MapNode n = (MapNode)graph.GetNode(c);
if (n == null)
{
continue;
}
// x
if (y + 1 < height)
{
var m = graph.GetNode(new Vector(x, y + 1));
if (m != null)
{
graph.AddEdge(n, m, new MapEdge(n + "->" + m, 1));
}
}
if (y - 1 >= 0)
{
var m = graph.GetNode(new Vector(x, y - 1));
if (m != null)
{
graph.AddEdge(n, m, new MapEdge(n + "->" + m, 1));
}
}
// x + 1
if (x + 1 < width)
{
var m = graph.GetNode(new Vector(x + 1, y));
if (m != null)
{
graph.AddEdge(n, m, new MapEdge(n + "->" + m, 1));
}
if (y + 1 < height)
{
var o = graph.GetNode(new Vector(x + 1, y + 1));
if (o != null)
{
graph.AddEdge(n, o, new MapEdge(n + "->" + o, n.data.DistanceTo(o.data)));
}
}
if (y - 1 >= 0)
{
var o = graph.GetNode(new Vector(x + 1, y - 1));
if (o != null)
{
graph.AddEdge(n, o, new MapEdge(n + "->" + o, n.data.DistanceTo(o.data)));
}
}
}
// x - 1
if (x - 1 >= 0)
{
var m = graph.GetNode(new Vector(x - 1, y));
if (m != null)
{
graph.AddEdge(n, m, new MapEdge(n + "->" + m, 1));
}
if (y + 1 < height)
{
var o = graph.GetNode(new Vector(x - 1, y + 1));
if (o != null)
{
graph.AddEdge(n, o, new MapEdge(n + "->" + o, n.data.DistanceTo(o.data)));
}
}
if (y - 1 >= 0)
{
var o = graph.GetNode(new Vector(x - 1, y - 1));
if (o != null)
{
graph.AddEdge(n, o, new MapEdge(n + "->" + o, n.data.DistanceTo(o.data)));
}
}
}
}
Console.WriteLine($"E - x: {x}");
}
MapNode[] path = AStar <Vector, string> .FindPath((MapNode)graph.GetNode(start), (MapNode)graph.GetNode(goal), graph, hMapTest);
//Color path
foreach (MapNode n in path)
{
if (!(n.data.x == start.x && n.data.y == start.y) &&
!(n.data.x == goal.x && n.data.y == goal.y))
{
map.SetPixel((int)n.data.x, (int)n.data.y, Color.FromArgb(0, 0, 255));
}
}
map.Save(file + "2.png");
Console.WriteLine("DonnoDK");
Console.ReadKey();
}
