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); } } } }