public void AddNode(Vector3 neighborPoint, ref GraphNode currentNode, ref Queue<GraphNode> q )
{
RaycastHit hitInfo;
Vector3 rayDirection = Vector3.zero;
#if USE_XZ
rayDirection = new Vector3(0, -1, 0);
#else
rayDirection = new Vector3(0, 0, 1);
#endif //USE_XZ
int layerMask = 1 << 8;
layerMask = ~layerMask;
if ( Physics.Raycast(neighborPoint, rayDirection, out hitInfo, Mathf.Infinity, layerMask) )
{
if (hitInfo.transform.tag == "Ground")
{
GraphNode newNode = new GraphNode(mNumOfNodes, hitInfo.point); // make a new node for this point
GraphEdge newEdge = new GraphEdge(currentNode.GetIndex(), newNode.GetIndex()); // creat a new edge
int index = 0;
bool nodeFound = false;
while ( !nodeFound && index <= mNumOfNodes )
{
//Debug.Log (index + " out of " + NavigationGraph.Length + " thinks there's only" + mNumOfNodes);
nodeFound = ( NavigationGraph[index] == hitInfo.point );
++index;
}
if ( !nodeFound ) // if we have not found this node before, add it
{
Nodes.Add(newNode);
NavigationGraph[mNumOfNodes] = hitInfo.point;
++mNumOfNodes;
q.Enqueue(newNode);
}
else
{
newEdge.SetToIndex(index-1);
}
// If the raycast hit then we will always want to add the edge, since we want edges
// in both directions and there won't ever be duplicates.
// check if there is a clear path to add an edge
Vector3 heightOffset = Vector3.zero;
#if USE_XZ
heightOffset = new Vector3(0, 0.5f, 0);
#else
heightOffset = new Vector3(0, 0, -0.5f);
#endif // USE_XZ
if ( !Physics.Linecast(currentNode.GetPosition() + heightOffset, newNode.GetPosition() + heightOffset, out hitInfo, layerMask) )
{
Edges.Add(newEdge);
currentNode.AddEdge(newEdge);
}
}
}
}