/// <summary>
/// This runs A* assuming that both start and nextDestination
/// clusters are clusters belonging to the lowest level of the HPA* abstraction.
/// </summary>
/// <param name="start"></param>
/// <param name="nextDestination"></param>
/// <returns></returns>
private List <int> AstarHPALowestLevelSearch(ClusterNode start, ClusterNode nextDestination, int startPosition)
{
Cluster startC = clusterOfClusterNode(start);
Cluster nextC = clusterOfClusterNode(nextDestination);
HashSet <int> nodesToSearch = new HashSet <int>();
foreach (int id in startC.InnerNodes)
{
if (gMap.Nodes[id].IsTraversable())
{
nodesToSearch.Add(id);
}
}
foreach (int id in nextC.InnerNodes)
{
if (gMap.Nodes[id].IsTraversable())
{
nodesToSearch.Add(id);
}
}
List <int> sol = new List <int>();
Dictionary <int, bool> closedSet = new Dictionary <int, bool>();
foreach (int id in nodesToSearch)
{
closedSet.Add(id, false);
}
Dictionary <int, Node> openSet = new Dictionary <int, Node>();
//starting node is in the open set
openSet.Add(startPosition, gMap.Nodes[startPosition]);
// For each clusternode, which clusternode it can most efficiently be reached from.
// If a cnode can be reached from many cnodes, cameFrom will eventually contain the
// most efficient previous step.
Dictionary <int, int> cameFrom = new Dictionary <int, int>();
// For each node, the cost of getting from the start node to that node.
Dictionary <int, float> gScore = new Dictionary <int, float>();
//default values are infinity
foreach (int nodeID in nodesToSearch)
{
gScore.Add(nodeID, float.MaxValue);
}
// The cost of going from start to start is zero.
gScore[start.GNodeID] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
Dictionary <int, float> fScore = new Dictionary <int, float>();
//default values are infinity
foreach (int nodeID in nodesToSearch)
{
fScore.Add(nodeID, float.MaxValue);
}
// For the first node, that value is completely heuristic.
fScore[startPosition] = H_lowHPA(startPosition, nextDestination);
int currNode = 0; //default value
while (openSet.Count != 0)
{
//the node in openSet having the lowest fScore value
currNode = openSet.OrderBy(i => fScore[i.Key]).FirstOrDefault().Key;
if (currNode == nextDestination.GNodeID)
{
//break the loop and reconstruct path below
break;
}
openSet.Remove(currNode);
closedSet[currNode] = true; //"added" to closedList
foreach (var neighbor in gMap.Nodes[currNode].Neighbors)
{
// Ignore the neighbor which is already evaluated or a neighbor
//that doesn't belong to neither start nor nextDestination cluster.
if ((!startC.InnerNodes.Contains(neighbor.Key) && !nextC.InnerNodes.Contains(neighbor.Key)) ||
(!closedSet.ContainsKey(neighbor.Key) || closedSet[neighbor.Key] == true))
{
continue;
}
// The distance from start to a neighbor
float tentativeG = gScore[currNode] + neighbor.Value;
if (!openSet.ContainsKey(neighbor.Key)) // Discover a new node
{
searchedNodes.Add(neighbor.Key);
expandedNodesCount++;
setSearchedBgColor(neighbor.Key);
openSet.Add(neighbor.Key, gMap.Nodes[neighbor.Key]);
}
else if (tentativeG >= gScore[neighbor.Key])
{
continue; //not a better path
}
// This path is the best until now. Record it!
cameFrom[neighbor.Key] = currNode;
gScore[neighbor.Key] = tentativeG;
fScore[neighbor.Key] = gScore[neighbor.Key] + H_lowHPA(neighbor.Key, nextDestination);
}
}
sol.Add(currNode);
while (cameFrom.ContainsKey(currNode))
{
int child = currNode;
currNode = cameFrom[currNode];
pathCost += gMap.Nodes[currNode].Neighbors[child];
sol.Add(currNode);
}
// pathfinder.CancelAsync();
return(sol);
}
private void StartHPAstarSearch(object sender, DoWorkEventArgs e)
{
List <int> path = new List <int>();
bool startNew = true;
bool endNew = true;
//get the start node cluster
int startCID = gMap.Nodes[gMap.StartNodeID].HPAClusterParent;
int endCID = gMap.Nodes[gMap.EndNodeID].HPAClusterParent;
Cluster startingCluster = HierarchicalGraph[0].Clusters[startCID];
Cluster endingCluster = HierarchicalGraph[0].Clusters[endCID];
//we create the temporary start clusterNode and calculate the distance between
//it and other cluster nodes
ClusterNode tmpStart = new ClusterNode(gMap.StartNodeID);
if (startingCluster.ClusterNodes.ContainsKey(tmpStart.GNodeID))
{
startNew = false;
tmpStart = startingCluster.ClusterNodes[tmpStart.GNodeID];
}
else
{
startingCluster.ClusterNodes.Add(tmpStart.GNodeID, tmpStart);
HierarchicalGraph[0].AbstractNodes.Add(tmpStart.GNodeID, tmpStart);
tmpStart.ClusterParent = startCID;
calculateDistInnerClusterNodes_Tmp(tmpStart, startingCluster);
}
ClusterNode tmpEnd = new ClusterNode(gMap.EndNodeID);
if (endingCluster.ClusterNodes.ContainsKey(tmpEnd.GNodeID))
{
endNew = false;
tmpEnd = endingCluster.ClusterNodes[tmpEnd.GNodeID];
}
else
{
endingCluster.ClusterNodes.Add(tmpEnd.GNodeID, tmpEnd);
HierarchicalGraph[0].AbstractNodes.Add(tmpEnd.GNodeID, tmpEnd);
tmpEnd.ClusterParent = endCID;
calculateDistInnerClusterNodes_Tmp(tmpEnd, endingCluster);
}
var abstractPath = AstarAbstractHPASearch(tmpStart, endingCluster);
//goes through the low-level clusters and partially builds a path on the grid base.
//abstractPath contains the indices of low-level clusters. Therefore,
//by doing HierarchicalAbstraction[0].ClusterNodes[i] we get the cluster we need
int startID = gMap.StartNodeID;
for (int i = abstractPath.Count - 1; i > 0; --i)
{
List <int> partialPath = new List <int>();
partialPath = AstarHPALowestLevelSearch(HierarchicalGraph[0].AbstractNodes[abstractPath[i]],
HierarchicalGraph[0].AbstractNodes[abstractPath[i - 1]],
startID);
AddToPathfSol(partialPath);
if (i == abstractPath.Count - 1)
{
StartAgent();
}
//the next start node is going to be the end node of this search.
//since A* path returned is reversed, the first element was the last (end) node.
startID = partialPath[0];
}
if (endNew)
{
endingCluster.ClusterNodes.Remove(tmpEnd.GNodeID);
HierarchicalGraph[0].AbstractNodes.Remove(tmpEnd.GNodeID);
}
if (startNew)
{
startingCluster.ClusterNodes.Remove(tmpStart.GNodeID);
HierarchicalGraph[0].AbstractNodes.Remove(tmpStart.GNodeID);
}
pfStopWatch.Stop();
}
private List <int> AstarAbstractHPASearch(ClusterNode start, Cluster end)
{
bool lastCluster = false;
List <int> sol = new List <int>();
Dictionary <int, bool> closedSet = new Dictionary <int, bool>();
foreach (var c in HierarchicalGraph[0].Clusters)
{
foreach (var cn in c.Value.ClusterNodes.Values)
{
closedSet.Add(cn.GNodeID, false);
}
}
Dictionary <int, ClusterNode> openSet = new Dictionary <int, ClusterNode>();
//starting node is in the open set
openSet.Add(gMap.StartNodeID, start);
// For each clusternode, which clusternode it can most efficiently be reached from.
// If a cnode can be reached from many cnodes, cameFrom will eventually contain the
// most efficient previous step.
Dictionary <int, int> cameFrom = new Dictionary <int, int>();
// For each node, the cost of getting from the start node to that node.
Dictionary <int, float> gScore = new Dictionary <int, float>();
//default values are infinity
foreach (var c in HierarchicalGraph[0].Clusters)
{
foreach (var cn in c.Value.ClusterNodes.Values)
{
gScore.Add(cn.GNodeID, float.MaxValue);
}
}
// The cost of going from start to start is zero.
gScore[start.GNodeID] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
Dictionary <int, float> fScore = new Dictionary <int, float>();
//default values are infinity
foreach (var c in HierarchicalGraph[0].Clusters)
{
foreach (var cn in c.Value.ClusterNodes.Values)
{
fScore.Add(cn.GNodeID, float.MaxValue);
}
}
// For the first node, that value is completely heuristic.
fScore[start.GNodeID] = H_startEnd(start.GNodeID, gMap.EndNodeID);
int currNode = 0; //default value
while (openSet.Count != 0)
{
//the node in openSet having the lowest fScore value
currNode = openSet.OrderBy(i => fScore[i.Key]).FirstOrDefault().Key;
if (end.InnerNodes.Contains(currNode) && end.ClusterNodes[gMap.EndNodeID].Neighbors.ContainsKey(currNode))
{
//break the loop and reconstruct path below
//we reached the end cluster. Break and finish the path to the end node below
lastCluster = true;
break;
}
openSet.Remove(currNode);
closedSet[currNode] = true; //"added" to closedList
int cID = gMap.Nodes[currNode].HPAClusterParent;
Cluster c = HierarchicalGraph[0].Clusters[cID];
ClusterNode cn = HierarchicalGraph[0].AbstractNodes[currNode];
foreach (var neighbor in cn.Neighbors)
{
// Ignore the neighbor which is already evaluated.
if (!closedSet.ContainsKey(neighbor.Key) || closedSet[neighbor.Key] == true)
{
continue;
}
// The distance from start to a neighbor
float tentativeG = gScore[currNode] + neighbor.Value;
if (!openSet.ContainsKey(neighbor.Key)) // Discover a new node
{
Cluster neighC = HierarchicalGraph[0].Clusters[gMap.Nodes[neighbor.Key].HPAClusterParent];
openSet.Add(neighbor.Key, HierarchicalGraph[0].AbstractNodes[neighbor.Key]);
}
else if (tentativeG >= gScore[neighbor.Key])
{
continue; //not a better path
}
// This path is the best until now. Record it!
cameFrom[neighbor.Key] = currNode;
gScore[neighbor.Key] = tentativeG;
fScore[neighbor.Key] = gScore[neighbor.Key] + H_startEnd(neighbor.Key, gMap.EndNodeID);
}
}
if (lastCluster)
{
sol.Add(gMap.EndNodeID);
}
sol.Add(currNode);
while (cameFrom.ContainsKey(currNode))
{
currNode = cameFrom[currNode];
sol.Add(currNode);
}
//NOT setting pathcost since this is still an abstraction layer
//pathCost = gScore[gMap.EndNodeID];
// pathfinder.CancelAsync();
//invalidater.CancelAsync();
return(sol);
}
private Cluster clusterOfClusterNode(ClusterNode c)
{
return(HierarchicalGraph[0].Clusters[gMap.Nodes[c.GNodeID].HPAClusterParent]);
}