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 static GameObject renderNavGraph(string para_renderName, NavGraph para_graph, Transform para_graphNodePrefab)
{
GameObject navGraphRenderObj = new GameObject(para_renderName);
// Extract color info if needed.
Color defaultColor = Color.white;
Dictionary<int,Color> nodeTypeIDToColorMap = null;
if(para_graph is ColoredNavGraph)
{
ColoredNavGraph castGraph = (ColoredNavGraph) para_graph;
nodeTypeIDToColorMap = castGraph.createTypeIDToColorMap();
}
// Render Nodes:
GameObject navNodesObj = new GameObject("Nodes");
List<int> nodeKeys = para_graph.getAllNodeKeys();
for(int i=0; i<nodeKeys.Count; i++)
{
WorldNode reqNode = (WorldNode) para_graph.getNode(nodeKeys[i]);
int reqNodeType = reqNode.getNodeType();
/*int[] imgLocPt = reqNode.get2DLocationPt();
Vector3 nodeWorldPt = new Vector3((gPropMapWorldBounds.x + (gPropMapWorldBounds.cellWidth/2f)) + (gPropMapWorldBounds.cellWidth * (imgLocPt[0] * 1.0f)),
(gPropMapWorldBounds.y - (gPropMapWorldBounds.cellHeight/2f)) - (gPropMapWorldBounds.cellHeight * (imgLocPt[1] * 1.0f)),
gPropMapWorldBounds.z);*/
Vector3 nodeWorldPt = reqNode.getWorldPt();
Transform nwNodePt = (Transform) Transform.Instantiate(para_graphNodePrefab,nodeWorldPt,Quaternion.identity);
nwNodePt.name = "Node-"+reqNode.getNodeID();
nwNodePt.parent = navNodesObj.transform;
Color paintColor = defaultColor;
if((nodeTypeIDToColorMap != null)
&&(nodeTypeIDToColorMap.ContainsKey(reqNodeType)))
{
paintColor = nodeTypeIDToColorMap[reqNodeType];
}
nwNodePt.renderer.material.color = paintColor;
}
// Render Edges:
GameObject navEdgesObj = new GameObject("Edges");
List<string> edgeKeys = para_graph.getAllEdgeKeys();
for(int i=0; i<edgeKeys.Count; i++)
{
NavEdge reqEdge = para_graph.getEdge(edgeKeys[i]);
int[] nodesOnEdge = reqEdge.getNodeIDs();
Vector3 node1Pt = (GameObject.Find("Node-"+nodesOnEdge[0])).transform.position;
Vector3 node2Pt = (GameObject.Find("Node-"+nodesOnEdge[1])).transform.position;
GameObject nwEdge = new GameObject("Edge:"+nodesOnEdge[0]+"-"+nodesOnEdge[1]);
LineRenderer lRend = nwEdge.AddComponent<LineRenderer>();
lRend.castShadows = false;
lRend.receiveShadows = false;
lRend.SetVertexCount(2);
lRend.SetWidth(0.1f,0.1f);
lRend.SetPosition(0,node1Pt);
lRend.SetPosition(1,node2Pt);
lRend.SetColors(Color.yellow,Color.yellow);
lRend.material = new Material(Shader.Find("Diffuse"));//"Particles/Additive"));
nwEdge.transform.parent = navEdgesObj.transform;
}
// Finish Object.
navNodesObj.transform.parent = navGraphRenderObj.transform;
navEdgesObj.transform.parent = navGraphRenderObj.transform;
return navGraphRenderObj;
}