public StreetCorner getNodeFromClosedList(StreetCorner target, List <StreetCorner> list)
{
StreetCorner node = null;
foreach (var n in list)
{
if (n.name == target.name)
{
node = n;
break;
}
}
return(node);
}
public StreetCorner getNodeFromOpenList(StreetCorner target, SimplePriorityQueue <StreetCorner> queue)
{
StreetCorner node = null;
foreach (var n in queue)
{
if (n.name == target.name)
{
node = n;
break;
}
}
return(node);
}
public void reconstruct_path(StreetCorner root, StreetCorner goal)
{
// Print values from goal node
Console.WriteLine(" ");
Console.Write("The shortest path are calculated to: \n");
List <StreetCorner> path = new List <StreetCorner>();
for (StreetCorner node = goal; node != null; node = (StreetCorner)node.parent)
{
path.Add(node);
}
path.Reverse();
foreach (StreetCorner node in path)
{
if (node == root)
{
Console.WriteLine("Node " + node.name + " - : F: " + node.f_score + " G: " + node.g_score + " H: " + node.h_score);
}
else
{
Console.WriteLine("Node " + node.name + " - : F: " + node.f_score + " G: " + node.g_score + " H: " + node.h_score);
}
}
foreach (StreetCorner node in path)
{
if (node == goal)
{
Console.Write(node.name);
}
else
{
Console.Write(node.name + "---");
}
}
Console.WriteLine(" ");
Console.WriteLine(" ");
Console.WriteLine(" ");
}
public List <StreetCorner> MapStreetGraph()
{
StreetCorner A = new StreetCorner("A", 10, 70);
StreetCorner B = new StreetCorner("B", 20, 50);
StreetCorner C = new StreetCorner("C", 25, 100);
StreetCorner D = new StreetCorner("D", 35, 80);
StreetCorner E = new StreetCorner("E", 35, 35);
StreetCorner F = new StreetCorner("F", 45, 70);
StreetCorner G = new StreetCorner("G", 55, 55);
StreetCorner H = new StreetCorner("H", 50, 90);
StreetCorner I = new StreetCorner("I", 35, 120);
StreetCorner J = new StreetCorner("J", 60, 150);
StreetCorner K = new StreetCorner("K", 65, 110);
StreetCorner L = new StreetCorner("L", 65, 100);
StreetCorner M = new StreetCorner("M", 70, 85);
StreetCorner N = new StreetCorner("N", 80, 70);
// Set edges to all the nodes
A.neighbors = new List <AbstractEdge>
{
new Road(B, "Vestervoldgade"),
new Road(D, "SktPedersStraede"),
new Road(C, "Noerrevoldgade")
};
B.neighbors = new List <AbstractEdge>
{
new Road(A, "Vestervoldgade"),
new Road(F, "Studiestraede"),
new Road(E, "Vestervoldgade"),
};
C.neighbors = new List <AbstractEdge>
{
new Road(A, "Vestervoldgade"),
new Road(D, "TeglgaardsStraede"),
new Road(I, "Noerrevoldgade")
};
D.neighbors = new List <AbstractEdge>
{
new Road(H, "SktPedersStraede")
};
E.neighbors = new List <AbstractEdge>
{
new Road(B, "Vestervoldgade"),
new Road(G, "Vestergade")
};
F.neighbors = new List <AbstractEdge>
{
new Road(D, "Larsbjoernsstraede"),
new Road(G, "Larsbjoernsstraede"),
};
G.neighbors = new List <AbstractEdge>
{
new Road(N, "Vestergade")
};
H.neighbors = new List <AbstractEdge>
{
new Road(I, "LarslejStraede")
};
I.neighbors = new List <AbstractEdge>
{
new Road(C, "Noerrevoldgade"),
new Road(J, "Noerrevoldgade")
};
J.neighbors = new List <AbstractEdge>
{
new Road(I, "Noerrevoldgade"),
new Road(K, "Noerregade")
};
K.neighbors = new List <AbstractEdge>
{
new Road(J, "Noerregade"),
new Road(L, "Noerregade")
};
L.neighbors = new List <AbstractEdge>
{
new Road(K, "Noerregade"),
new Road(H, "SktPedersStraede"),
new Road(M, "Noerregade")
};
M.neighbors = new List <AbstractEdge>
{
new Road(L, "Noerregade"),
new Road(N, "Noerregade"),
new Road(F, "Studiestraede")
};
N.neighbors = new List <AbstractEdge>
{
};
return(new List <StreetCorner> {
A, // 0
B, // 1
C, // 2
D, // 3
E, // 4
F, // 5
G, // 6
H, // 7
I, // 8
J, // 9
K, // 10
L, // 11
M, // 12
N, // 13
});
}
public SearchProblem(StreetCorner root, StreetCorner goal)
{
this.root = root;
this.goal = goal;
}
// Searching using the Astar algorithm
public void search(StreetCorner root, StreetCorner goal)
{
Console.WriteLine("Searchproblem: " + "Start node: " + root.name + " Goal node: " + goal.name);
Console.WriteLine("");
SearchHelper sh = new SearchHelper();
SimplePriorityQueue <StreetCorner> openList = new SimplePriorityQueue <StreetCorner>();
List <StreetCorner> closedList = new List <StreetCorner>();
// Cost to be calculted from start to current node
double tentative_g_score = 0;
// Put starting node into openList
openList.Enqueue(root, Convert.ToSingle(root.f_score));
// Inizialize currentNode object
StreetCorner currentNode = root;
while (openList.Count > 0)
{
currentNode = openList.Dequeue();
currentNode.visited = true;
closedList.Add(currentNode);
// If goal has been found
if (currentNode.Equals(goal))
{
// Print goal
reconstruct_path(root, goal);
break;
}
// Calculate g cost from current and set parent
foreach (var neighbor in currentNode.neighbors)
{
if (closedList.Contains(neighbor.node))
{
// Skip node if it has been visited before
continue;
}
// The distance from start to a neighbor
tentative_g_score = currentNode.getG_score() + neighbor.node.calculate_G_Cost(currentNode);
if (!openList.Contains(neighbor.node))
{
openList.Enqueue((StreetCorner)neighbor.node, Convert.ToSingle(root.f_score));
}
else if (tentative_g_score >= neighbor.node.g_score)
{
continue;
}
// This path is the best so far
neighbor.node.parent = currentNode;
// Calculate and set g, h, and f values for neighbor
neighbor.node.g_score = tentative_g_score;
neighbor.node.h_score = calculateHeuristic((StreetCorner)neighbor.node, goal);
neighbor.node.f_score = neighbor.node.g_score + neighbor.node.h_score;
}
}
}
// Calculating heuristic distance from node n to goal with pythagoras.
public double calculateHeuristic(StreetCorner target, StreetCorner goal)
{
return(Math.Sqrt((goal.x - target.x) * (goal.x - target.x) + (goal.y - target.y) * (goal.y - target.y)));
}
public Road(StreetCorner s, string name) : base(s, name)
{
}