//Pathfinding tattico: costruisce il percorso a costo minimo con A* con costi delle connessioni che tengono conto della mappa di influenza
public List<PathFindingVertex> GetPath(Graph graph, int start, int goal)
{
PathFindingNode startNode = new PathFindingNode (start, 0, EstimateFrom(start,goal));
PathFindingNode current = new PathFindingNode(-1,0,0);
//Inizializza lista nodi chiusi (già visitati) e aperti (da visitare)
PathFindingList open = new PathFindingList();
PathFindingList closed = new PathFindingList();
open.AddNode(startNode);
//Itera tutti i nodi da visitare
while (open.Length() > 0) {
current = open.SmallestElement();
//Se il nodo corrente da visitare è l'obiettivo, termina iterazione
if (current.ID == goal){
break;
}
List<Vertex> connections = graph.GetVertices(current.ID);
//Itera tutte le connessioni che partono dal nodo corrente
foreach (Vertex connection in connections){
Node end = connection.END;
float endCost = current.costSoFar + connection.Cost;
float endHeuristic;
PathFindingNode endRecord;
//Se il nodo al termine della connessione è tra i chiusi....
if (closed.Contains(end)){
//Record corrispondente al nodo terminale nella lista dei visitati
endRecord = closed.Find(end);
/*Se è stato trovato un percorso migliore verso il nodo terminale
rispetto a quello già registrato tra gli aperti...*/
if (endRecord!=null && endRecord.costSoFar > endCost) {
//Rimuove il nodo dai chiusi
closed.RemoveNode(endRecord);
//Usa i vecchi valori del nodo per ricalcolare l'euristica
endHeuristic = endRecord.estimatedTotalCost - endRecord.costSoFar;
}
else continue;
}
//Se invece il nodo terminale è tra gli aperti....
else if (open.Contains(end)) {
//Record corrispondente al nodo terminale nella lista degli aperti
endRecord = open.Find(end);
/*Se è stato trovato un percorso migliore verso il nodo terminale
rispetto a quello già registrato tra gli aperti....*/
if (endRecord.costSoFar > endCost) {
//Uso i vecchi valori del nodo per ricalcolare l'euristica
endHeuristic = endRecord.estimatedTotalCost - endRecord.costSoFar;
}
else continue;
}
/*Se invece il nodo terminale non è nè tra i chiusi nè tra gli aperti, crea un nuovo record
* ed effettua la stima della distanza dall'obiettivo*/
else {
endRecord = new PathFindingNode(end.ID);
endHeuristic = EstimateFrom(end.ID,goal);
}
//Aggiorno record con nuovi valori
endRecord.costSoFar = endCost;
endRecord.connection = new PathFindingVertex(current,endRecord);
endRecord.estimatedTotalCost = endCost + endHeuristic;
if (!open.Contains(end))
open.AddNode(endRecord);
}
open.RemoveNode(current);
closed.AddNode(current);
}
//Se il nodo corrente non è l'obiettivo, il nodo non è stato trovato e restituisce null
if (current.ID!=goal)
return null;
//Altrimenti ricostruisce il percorso all'indietro partendo dal nodo corrente.
else{
List<PathFindingVertex> path = new List<PathFindingVertex>();
while (current.ID!=start){
path.Add(current.connection);
current = current.connection.START;
}
path.Reverse();
return path;
}
}
public static List <Connection> pathFind(Graph graph, Node start, Node goal)
{
NodeRecord startR = new NodeRecord();
startR.node = start;
startR.connection = null;
startR.currentCost = 0;
PathFindingList open = new PathFindingList();
open.add(startR);
PathFindingList closed = new PathFindingList();
NodeRecord current = new NodeRecord();
while (open.length() > 0)
{
current = open.smallElement();
if (current.node == goal)
{
break;
}
List <Connection> connections = graph.getConnections(current.node);
foreach (Connection connection in connections)
{
Node endN = connection.getToNode();
float endNodeCost = current.currentCost + connection.getCost();
NodeRecord endNodeRecord = new NodeRecord();
if (closed.contains(endN))
{
continue;
}
else if (open.contains(endN))
{
endNodeRecord = open.find(endN);
if (endNodeRecord != null && endNodeRecord.currentCost < endNodeCost)
{
continue;
}
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endN;
}
endNodeRecord.currentCost = endNodeCost;
endNodeRecord.connection = connection;
if (!open.contains(endN))
{
open.add(endNodeRecord);
}
}
open.remove(current);
closed.add(current);
}
if (current.node != goal)
{
return(null);
}
else
{
List <Connection> path = new List <Connection>();
while (current.node != start)
{
path.Add(current.connection);
Node fromN = current.connection.getFromNode();
current = closed.find(fromN);
}
path.Reverse();
return(path);
}
}
