/// <summary>
/// Initializes the pathfinding list and assigns all necessary variables.
/// </summary>
/// <param name="graph"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="heuristic"></param>
public virtual void Init(IGraph <T> graph, T start, T end, Heuristic <T> heuristic)
{
this.graph = graph;
this.start = start;
this.end = end;
this.heuristic = heuristic;
//Initialize the open and closed lists
pathFindingList = new PathfindingList <T>();
pathFindingList.Build(graph, start, end, heuristic);
this.current = pathFindingList.SmallestElement();
//pathFindingList[start] = startRecord;
}
public List <Connection> pathfindDijkstra(Graph graph, Node start, Node to)
{
int count = 0;
NodeRecord startRecord = new NodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0f;
Node endNode = null;
NodeRecord endNodeRecord = new NodeRecord();
float endNodeCost;
List <Connection> output = new List <Connection>();
PathfindingList open = new PathfindingList();
open.nodeRecords.Add(startRecord);
PathfindingList closed = new PathfindingList();
NodeRecord current = new NodeRecord();
while (open.length() > 0)
{
current = open.smallestElement();
Debug.Log("open.length");
Debug.Log(open.length());
Debug.Log("count = ");
Debug.Log(count);
Debug.Log("current = " + current);
Debug.Log("current.node = " + open.smallestElement().node);
count++;
if (current.node == to)
{
Debug.Log("success");
break;
}
connections = graph.getConnections(current.node);
foreach (Connection connection in connections)
{
endNode = connection.to;
endNodeCost = current.costSoFar + connection.getCost();
if (closed.contains(endNode))
{
Debug.Log("continue");
Debug.Log("continue");
Debug.Log("continue");
continue;
}
else if (open.contains(endNode))
{
endNodeRecord = open.find(endNode);
if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost)
{
continue;
}
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
}
endNodeRecord.costSoFar = endNodeCost;
endNodeRecord.connection = connection;
if (!open.contains(endNode))
{
open.nodeRecords.Add(endNodeRecord);
}
}
open.nodeRecords.Remove(current);
closed.nodeRecords.Add(current);
Debug.Log("open.length");
Debug.Log("open.length");
Debug.Log("open.length");
Debug.Log("open.length");
Debug.Log(open.length());
}
//Debug.Log(to);
//Debug.Log(current.node);
if (current.node != to)
{
return(null);
}
else
{
while (current.node != start)
{
output.Add(current.connection);
Node fromNode = current.connection.from;
current = closed.find(current.connection.from);
}
}
output.Reverse();
return(output);
}
public List <Connection> PathfindAStar(Graph aGraph, GameObject start, GameObject end, Heuristic myHeuristic)
{
// Set up the start record.
NodeRecord StartRecord = new NodeRecord();
StartRecord.Node = start;
StartRecord.Connection = null;
StartRecord.CostSoFar = 0;
StartRecord.EstimatedTotalCost = myHeuristic.Estimate(start, end);
// Create the lists.
PathfindingList OpenList = new PathfindingList();
PathfindingList ClosedList = new PathfindingList();
// Add the start record to the open list.
OpenList.AddNodeRecord(StartRecord);
// Iterate through and process each node.
NodeRecord CurrentRecord = null;
List <Connection> Connections;
while (OpenList.GetSize() > 0)
{
// Find the smallest element in the open list (using the estimatedTotalCost).
CurrentRecord = OpenList.GetSmallestElement();
// If it is the goal node, then terminate.
if (CurrentRecord.Node.Equals(end))
{
break;
}
// Otherwise get its outgoing connections.
Connections = aGraph.GetConnections(CurrentRecord.Node);
// Loop through each connection in turn.
GameObject EndNode;
float EndNodeCost;
NodeRecord EndNodeRecord;
float EndNodeHeuristic;
foreach (Connection aConnection in Connections)
{
// Get the cost estimate for the end node.
EndNode = aConnection.ToNode;
EndNodeCost = CurrentRecord.CostSoFar + aConnection.Cost;
// If the node is closed we may have to skip, or remove it from the closed list.
if (ClosedList.Contains(EndNode))
{
// Here we find the record in the closed list corresponding to the endNode.
EndNodeRecord = ClosedList.Find(EndNode);
// If we didn’t find a shorter route, skip.
if (EndNodeRecord.CostSoFar <= EndNodeCost)
{
continue;
}
// Otherwise remove it from the closed list.
ClosedList.RemoveNodeRecord(EndNodeRecord);
// We can use the node’s old cost values to calculate its heuristic without calling
// the possibly expensive heuristic function.
EndNodeHeuristic = EndNodeRecord.EstimatedTotalCost - EndNodeRecord.CostSoFar;
}
// Skip if the node is open and we’ve not found a better route.
else if (OpenList.Contains(EndNode))
{
// Here we find the record in the open list corresponding to the endNode.
EndNodeRecord = OpenList.Find(EndNode);
// If our route is no better, then skip.
if (EndNodeRecord.CostSoFar <= EndNodeCost)
{
continue;
}
// We can use the node’s old cost values to calculate its heuristic without calling
// the possibly expensive heuristic function.
EndNodeHeuristic = EndNodeRecord.EstimatedTotalCost - EndNodeRecord.CostSoFar;
}
// Otherwise we know we’ve got an unvisited node, so make a record for it.
else
{
EndNodeRecord = new NodeRecord();
EndNodeRecord.Node = EndNode;
// We’ll need to calculate the heuristic value using the function, since we don’t have an existing record to use.
EndNodeHeuristic = myHeuristic.Estimate(EndNode, end);
}
// We’re here if we need to update the node Update the cost, estimate and connection.
EndNodeRecord.CostSoFar = EndNodeCost;
EndNodeRecord.Connection = aConnection;
EndNodeRecord.EstimatedTotalCost = EndNodeCost + EndNodeHeuristic;
// And add it to the open list.
if (!(OpenList.Contains(EndNode)))
{
OpenList.AddNodeRecord(EndNodeRecord);
}
} //#END: Looping through Connections.
// We’ve finished looking at the connections for the current node, so add it to the closed list
// and remove it from the open list
OpenList.RemoveNodeRecord(CurrentRecord);
ClosedList.AddNodeRecord(CurrentRecord);
}
// We’re here if we’ve either found the goal, or if we’ve no more nodes to search, find which.
List <Connection> tempList = new List <Connection>();
if (!CurrentRecord.Node.Equals(end))
{
// We’ve run out of nodes without finding the goal, so there’s no solution
return(tempList);
}
else
{
while (!CurrentRecord.Node.Equals(start))
{
tempList.Add(CurrentRecord.Connection);
CurrentRecord = ClosedList.Find(CurrentRecord.Connection.FromNode);
}
// The path is in the wrong order. Reverse the path, and return it.
List <Connection> tempList2 = new List <Connection>();
for (int i = (tempList.Count - 1); i >= 0; i--)
{
tempList2.Add(tempList[i]);
}
return(tempList2);
}
}
public List <Connection> PathfindAStar(Graph graph, GameObject start, GameObject end, Heuristic heuristic)
{
NodeRecord startRecord = new NodeRecord();
startRecord.Node = start;
startRecord.Connection = null;
startRecord.CostSoFar = 0;
startRecord.EstimatedTotalCost = heuristic.Estimate(start, end);
PathfindingList openList = new PathfindingList();
PathfindingList closedList = new PathfindingList();
openList.AddRecord(startRecord);
NodeRecord currentRecord = null;
List <Connection> connections;
while (openList.GetSize() > 0)
{
currentRecord = openList.GetSmallestElement();
if (currentRecord.Node.Equals(end))
{
break;
}
connections = graph.GetConnections(currentRecord.Node);
GameObject endNode;
float endNodeCost;
NodeRecord endNodeRecord;
float endNodeHeuristic;
foreach (Connection aConnection in connections)
{
endNode = aConnection.ToNode;
endNodeCost = currentRecord.CostSoFar + aConnection.Cost;
if (closedList.Contains(endNode))
{
endNodeRecord = closedList.Find(endNode);
if (endNodeRecord.CostSoFar <= endNodeCost)
{
continue;
}
closedList.RemoveRecord(endNodeRecord);
endNodeHeuristic = endNodeRecord.EstimatedTotalCost - endNodeRecord.CostSoFar;
}
else if (openList.Contains(endNode))
{
endNodeRecord = openList.Find(endNode);
if (endNodeRecord.CostSoFar <= endNodeCost)
{
continue;
}
endNodeHeuristic = endNodeRecord.EstimatedTotalCost - endNodeRecord.CostSoFar;
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.Node = endNode;
endNodeHeuristic = heuristic.Estimate(endNode, end);
}
endNodeRecord.CostSoFar = endNodeCost;
endNodeRecord.Connection = aConnection;
endNodeRecord.EstimatedTotalCost = endNodeCost + endNodeHeuristic;
if (!openList.Contains(endNode))
{
openList.AddRecord(endNodeRecord);
}
}
openList.RemoveRecord(currentRecord);
closedList.AddRecord(currentRecord);
}
List <Connection> tempList = new List <Connection>();
if (!currentRecord.Node.Equals(end))
{
return(tempList);
}
else
{
while (!currentRecord.Node.Equals(start))
{
tempList.Add(currentRecord.Connection);
if (currentRecord.Connection == null)
{
Debug.Log("Current Record " + currentRecord.ToString() + " is null");
}
if (currentRecord.Connection.FromNode == null)
{
Debug.Log("Current FromNode " + currentRecord.Connection.FromNode.gameObject.name + " is null");
}
currentRecord = closedList.Find(currentRecord.Connection.FromNode);
}
// Reverse path and return
List <Connection> tempList2 = new List <Connection>();
for (int i = tempList.Count - 1; i >= 0; i--)
{
tempList2.Add(tempList[i]);
}
return(tempList2);
}
}
public static List <Connection> pathfind(Graph graph, Node start, Node goal)
{
// Initialize the record for the start node.
NodeRecord startRecord = new NodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0;
// Initialize the open and closed lists
PathfindingList open = new PathfindingList();
open.add(startRecord);
PathfindingList closed = new PathfindingList();
// Iterate through processing each node
NodeRecord current = new NodeRecord();
while (open.length() > 0)
{
// Find the smallest element in the open list
current = open.smallestElement();
// If it is the goal node, then terminate
if (current.node == goal)
{
break;
}
// Otherwise get its outgoing connections.
List <Connection> connections = graph.getConnections(current.node);
// Loop through each connection in turn.
foreach (Connection connection in connections)
{
// Get the cost estimate for the end node
Node endNode = connection.getToNode();
float endNodeCost = current.costSoFar + connection.getCost();
NodeRecord endNodeRecord = new NodeRecord();
// Skip if the node is closed
if (closed.contains(endNode))
{
continue;
}
// ... or if it is open and we've found a worse route
else if (open.contains(endNode))
{
// Here we find the record in the open list
// corresponding to the endNode
endNodeRecord = open.find(endNode);
if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost)
{
continue;
}
}
// Otherwise we know we've got an unvisited node, so make a
// record for it
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
}
// We're here if we need to update the node. Update the
// cost and connection.
endNodeRecord.costSoFar = endNodeCost;
endNodeRecord.connection = connection;
// And add it to the open list
if (!open.contains(endNode))
{
open.add(endNodeRecord);
}
}
// We've finished looking at the connections for the current
// node, so add it to the closed list and remove it from the
// open list.
open.remove(current);
closed.add(current);
}
// We're here if we've either found the goal, or if we've no more
// nodes to search. Find which.
if (current.node != goal)
{
// We've run out of nodes without finding the goal, so there's
// no solution
return(null);
}
else
{
// Compile the list of connections in the path.
List <Connection> path = new List <Connection>();
// Work back along the path, accumulating connections
while (current.node != start)
{
path.Add(current.connection);
//current = current.connection.getFromNode(); // << This is Millington. This doesn't work because current needs to be a full NodeRecord.
Node fromNode = current.connection.getFromNode();
current = closed.find(fromNode);
}
// Reverse the path and return it.
path.Reverse();
return(path);
}
}
public static List <Connection> pathfind(IGraph graph, Node start, Node goal)
{
NodeRecord startRecord = new NodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0;
PathfindingList open = new PathfindingList();
open.add(startRecord);
PathfindingList closed = new PathfindingList();
NodeRecord current = new NodeRecord();
while (open.length() > 0)
{
current = open.smallestElement();
if (current.node == goal)
{
break;
}
List <Connection> connections = graph.getConnections(current.node);
foreach (Connection connection in connections)
{
Node endNode = connection.getToNode();
float endNodeCost = current.costSoFar + connection.getCost();
NodeRecord endNodeRecord = new NodeRecord();
if (closed.contains(endNode))
{
continue;
}
else if (open.contains(endNode))
{
endNodeRecord = open.find(endNode);
if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost)
{
continue;
}
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
}
endNodeRecord.costSoFar = endNodeCost;
endNodeRecord.connection = connection;
if (!open.contains(endNode))
{
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 fromNode = current.connection.getFromNode();
current = closed.find(fromNode);
}
path.Reverse();
return(path);
}
}
public static List <Connection> pathfind(Graph graph, Node start, Node goal)
{
// initialize record for start node.
NodeRecord startRecord = new NodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0;
// initialize open and closed lists
PathfindingList open = new PathfindingList();
open.add(startRecord);
PathfindingList closed = new PathfindingList();
// iterate through processing each node
NodeRecord current = new NodeRecord();
while (open.length() > 0)
{
// find smallest element in open list
current = open.smallestElement();
// if its goal node, terminate
if (current.node == goal)
{
break;
}
// otherwise
List <Connection> connections = graph.getConnections(current.node);
foreach (Connection connection in connections)
{
// get cost estimate for end node
Node endNode = connection.getToNode();
float endNodeCost = current.costSoFar + connection.getCost();
NodeRecord endNodeRecord = new NodeRecord();
// skip if node is closed
if (closed.contains(endNode))
{
continue;
}
// or
else if (open.contains(endNode))
{
endNodeRecord = open.find(endNode);
if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost)
{
continue;
}
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
}
endNodeRecord.costSoFar = endNodeCost;
endNodeRecord.connection = connection;
if (!open.contains(endNode))
{
open.add(endNodeRecord);
}
}
open.remove(current);
closed.add(current);
}
if (current.node != goal)
{
return(null);
}
else
{
// compile list of connections in path
List <Connection> path = new List <Connection>();
// accumulate connections
while (current.node != start)
{
path.Add(current.connection);
Node fromNode = current.connection.getFromNode();
current = closed.find(fromNode);
}
// reverse path and return it
path.Reverse();
return(path);
}
}
