public List <Connection> FindPathRealTimeAStar(EnvironmentGraph graph, Node start, Node end, Heuristic heuristic)
{
// If the end node isn't in the graph
// and therefore isn't able to be found
// change the end position to the lowest
// estimated cost node
Node finalEndNode = new Node();
// Find the end node in the graph
finalEndNode = graph.FindClosestEstimatedNode(end, heuristic);
// If in the graph run normal A* pathfinding
return(FindPathAStar(graph, start, finalEndNode, heuristic));
//else
//TODO: change the finalEndNode to the closest estimated Cost node
//TODO: real-time A* pathfinding
}
void Start()
{
m_graph = new EnvironmentGraph(mesh, nodeMat2);
m_graph.Reset();
m_CurrentNode = new GameObject();
m_CurrentNode.transform.localScale *= 0.5f;
m_CurrentNode.name = "CurrentNode";
m_CurrentNode.AddComponent <MeshRenderer>();
m_CurrentNode.AddComponent <MeshFilter>();
m_CurrentNode.GetComponent <MeshFilter>().mesh = mesh;
m_CurrentNode.GetComponent <MeshFilter>().mesh.name = "Sphere";
m_CurrentNode.GetComponent <MeshRenderer>().material = CurrNodeMat;
StartCoroutine(RegenerateGrid());
m_searchRange = 2;
}
public List <Connection> FindPathAStar(EnvironmentGraph graph, Node start, Node end, Heuristic heuristic)
{
m_OpenList.Clear();
m_ClosedList.Clear();
// Initialise the start node
NodeRecord startRecord = new NodeRecord();
startRecord.Node = start;
startRecord.Connection = null;
startRecord.CostSoFar = 0;
startRecord.EstimatedTotalCost = heuristic.Estimate(start);
// Add to start node to the open list
m_OpenList.Add(startRecord);
// Iterate through all open list nodes
while (m_OpenList.Count > 0)
{
// Find the closest node and make it the current node
if (m_OpenList.Count == 1)
{
m_Current = m_OpenList[0];
}
else
{
NodeRecord previousRecord = m_OpenList[0];
foreach (NodeRecord record in m_OpenList)
{
if (record.EstimatedTotalCost <= previousRecord.EstimatedTotalCost)
{
m_Current = record;
}
}
}
// If it is the goal node - terminate
if (m_Current.Node.NodeId == end.NodeId)
{
break;
}
// Otherwise get the nodes around the current node
List <Connection> connections = graph.GetSurroundingConnections(m_Current.Node);
// Value to be used later on...
float endNodeHeuristic = 0f;
// Iterate through all the connections
foreach (Connection connection in connections)
{
Node endNode = connection.GetToNode();
float endNodeCost = m_Current.CostSoFar + connection.GetCost();
NodeRecord endNodeRecord = new NodeRecord();
// If the node is in the closed list we may
// have to skip or remove it
if (m_ClosedList.Exists(nr => nr.Node.NodeId == endNode.NodeId))
{
//If the node is in the closed list
//get the node
endNodeRecord = m_ClosedList.Find(nr => nr.Node.NodeId == endNode.NodeId);
//If the route isn't shorter, skip the node
if (endNodeRecord.CostSoFar <= endNodeCost)
{
continue;
}
//Otherwise remove it from the closed list
m_ClosedList.Remove(endNodeRecord);
//Calculate end node heuristic
//[Cost to get from the current node to this node]
endNodeHeuristic = endNodeRecord.Cost - endNodeRecord.CostSoFar;
}
// Skip if the node is open and we've
//found a better route
else if (m_OpenList.Exists(nr => nr.Node.NodeId == endNode.NodeId))
{
//If the node is in the open list
//get the node
endNodeRecord = m_OpenList.Find(nr => nr.Node.NodeId == endNode.NodeId);
//If the route isn't better, then skip the node
if (endNodeRecord.CostSoFar <= endNodeCost)
{
continue;
}
//Calculate end node heuristic
//[Cost to get from the current node to this node]
endNodeHeuristic = endNodeRecord.Cost - endNodeRecord.CostSoFar;
}
// Otherwise we know we've got an unvisited node
else
{
//So make a new node
endNodeRecord.Node = endNode;
//Calculate end node heuristic using
//the heuristic function
endNodeHeuristic = heuristic.Estimate(endNode);
}
// Update the node
// Update the cost, estimate and connection
endNodeRecord.Cost = endNodeCost;
endNodeRecord.CostSoFar += endNodeCost;
endNodeRecord.Connection = connection;
endNodeRecord.EstimatedTotalCost = endNodeCost + endNodeHeuristic;
if (!m_OpenList.Contains(endNodeRecord))
{
m_OpenList.Add(endNodeRecord);
}
}
// We are finished looking at connections
// from the current node therefore we
// remove it from the open list and add
// it to the closed list
m_OpenList.Remove(m_Current);
m_ClosedList.Add(m_Current);
}
// Either the goal has been found or
// there are no more nodes to search
if (m_Current.Node.NodeId != end.NodeId)
{
// Ran out of nodes without finding
// a solution, thus no solution
Debug.Log("Ran out of nodes before finding a solution");
return(null);
}
else
{
// Compile a list of nodes to create the path
List <Connection> path = new List <Connection>();
List <Connection> tempPath = new List <Connection> {
m_Current.Connection
};
//NodeRecord previousRecord = m_Current;
// Work back along the path, accumilating the nodes
while (m_Current.Node.NodeId != start.NodeId)
{
if (m_Current.Node.NodeId == start.NodeId)
{
NodeRecord startRecordListItem = m_ClosedList.Find(r => r.Node.NodeId == start.NodeId);
tempPath.Add(startRecordListItem.Connection);
break;
}
Node fromNode = m_Current.Connection.GetFromNode();
NodeRecord record = m_ClosedList.Find(r => r.Node.NodeId == fromNode.NodeId);
if (record.Connection != null)
{
tempPath.Add(record.Connection);
}
m_Current = record;
}
// Reverse the path and return it
for (int i = tempPath.Count - 1; i >= 0; i--)
{
path.Add(tempPath[i]);
}
// Debug.Log("Found a solution: " + path.Count + " Node Connections");
return(path);
}
}
