public bool addNode(NavNode para_nwNode)
{
bool successFlag = false;
if( ! vertices.ContainsKey(para_nwNode.getNodeID()))
{
vertices.Add(para_nwNode.getNodeID(),para_nwNode);
successFlag = true;
}
return successFlag;
}
protected override float g_func(ref NavGraph para_graph, ref NavNode para_node1, ref NavNode para_node2)
{
NavEdge reqEdge = para_graph.getEdge(para_node1.getNodeID(),para_node2.getNodeID());
float retGVal = 0;
if(reqEdge == null)
{
retGVal = float.PositiveInfinity;
}
else
{
retGVal = reqEdge.getCost();
}
return retGVal;
}
public List<NavNode> getChildNodesAtHopDistance(NavNode para_sourceNode,
int para_distanceFromSrc,
HashSet<int> para_untraversibleTypes,
HashSet<int> para_untraversibleNodes)
{
// Uses modified Breadth First Search.
List<NavNode> retList = new List<NavNode>();
int depthLevel = 0;
List<int> candidateNodeIDs = new List<int>();
List<int> nwCandidateNodeIDs = new List<int>();
HashSet<int> seenNodes = new HashSet<int>();
candidateNodeIDs.Add(para_sourceNode.getNodeID());
if(para_distanceFromSrc > 0)
{
do
{
for(int i=0; i<candidateNodeIDs.Count; i++)
{
int cNodeID = candidateNodeIDs[i];
if( ! seenNodes.Contains(cNodeID))
{
seenNodes.Add(cNodeID);
NavNode cNode = getNode(cNodeID);
HashSet<int> cNodeNeighbours = cNode.getAllNeighbourIDs();
foreach(int neighbourID in cNodeNeighbours)
{
bool isValid = true;
if(depthLevel == (para_distanceFromSrc-1))
{
if(seenNodes.Contains(neighbourID))
{
isValid = false;
}
}
if(para_untraversibleNodes != null)
{
if(para_untraversibleNodes.Contains(neighbourID))
{
isValid = false;
}
}
if(para_untraversibleTypes != null)
{
if(para_untraversibleTypes.Contains(getNode(neighbourID).getNodeType()))
{
isValid = false;
}
}
if(isValid)
{
nwCandidateNodeIDs.Add(neighbourID);
}
}
}
}
candidateNodeIDs.Clear();
candidateNodeIDs = nwCandidateNodeIDs;
nwCandidateNodeIDs = new List<int>();
depthLevel++;
}
while((depthLevel < para_distanceFromSrc)&&(candidateNodeIDs.Count > 0));
}
for(int i=0; i<candidateNodeIDs.Count; i++)
{
retList.Add(getNode(candidateNodeIDs[i]));
}
return retList;
}
public List<int> searchForPath(ref NavGraph para_graph, NavNode para_sourceNode, NavNode para_destNode, HashSet<int> para_untraversibleTypes)
{
int sourceNodeID = para_sourceNode.getNodeID();
int destNodeID = para_destNode.getNodeID();
HashSet<int> closedSet = new HashSet<int>();
HashSet<int> openSet = new HashSet<int>();
Dictionary<int,int> cameFromMap = new Dictionary<int,int>();
Dictionary<int,float> gScoreMap = new Dictionary<int, float>();
Dictionary<int,float> fScoreMap = new Dictionary<int, float>();
openSet.Add(sourceNodeID);
gScoreMap.Add(sourceNodeID,0);
fScoreMap.Add(sourceNodeID, (gScoreMap[sourceNodeID] + h_func(ref para_graph,ref para_sourceNode,ref para_destNode)));
bool foundPath = false;
while((openSet.Count > 0)&&( ! foundPath))
{
// Search for node in openSet which has the lowest fScore.
int smallestFScoreNodeID = -1;
float smallestFScoreInSet = -1;
foreach(int openItemID in openSet)
{
float tmpFScoreForNode = fScoreMap[openItemID];
if((smallestFScoreInSet == -1)||(tmpFScoreForNode < smallestFScoreInSet))
{
smallestFScoreNodeID = openItemID;
smallestFScoreInSet = tmpFScoreForNode;
}
}
int currentNodeID = smallestFScoreNodeID;
NavNode currentNode = para_graph.getNode(currentNodeID);
if(currentNodeID == destNodeID)
{
foundPath = true;
}
else
{
openSet.Remove(currentNodeID);
closedSet.Add(currentNodeID);
HashSet<int> neighboursToCurrent = currentNode.getAllNeighbourIDs();
foreach(int neighbourID in neighboursToCurrent)
{
if(closedSet.Contains(neighbourID))
{
continue;
}
else
{
NavNode neighbourNode = para_graph.getNode(neighbourID);
if((para_untraversibleTypes == null)||( ! para_untraversibleTypes.Contains(neighbourNode.getNodeType())))
{
float tentativeGScore = gScoreMap[currentNodeID] + g_func(ref para_graph,ref currentNode,ref neighbourNode);
if(( ! openSet.Contains(neighbourID))
||(tentativeGScore < gScoreMap[neighbourID]))
{
cameFromMap[neighbourID] = currentNodeID;
gScoreMap[neighbourID] = tentativeGScore;
fScoreMap[neighbourID] = gScoreMap[neighbourID] + h_func(ref para_graph,ref neighbourNode,ref para_destNode);
if( ! openSet.Contains(neighbourID))
{
openSet.Add(neighbourID);
}
}
}
}
}
}
}
if(foundPath)
{
// Success.
List<int> reqPath = reconstructPath(ref cameFromMap, destNodeID);
return reqPath;
}
else
{
// Failure.
return null;
}
}
public bool isNeighbour(NavNode para_testNode)
{
return (neighbours.Contains(para_testNode.getNodeID()));
}