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);
                }
            }
        }
    }
    public void FloodFill2(Vector3 startPoint)
    {
        // clear our nodes and edges, we'll be finding new ones
        Nodes = new List<GraphNode>();
        Edges = new List<GraphEdge>();

        mNumOfNodes = 0; // this serves as the size of the array
        GraphNode seed = new GraphNode(mNumOfNodes, startPoint); // make startPoint into the first GraphNode
        ++mNumOfNodes;

        // initialize our queue and add the first node
        Queue<GraphNode> q = new Queue<GraphNode>();
        q.Enqueue(seed);

        // We will keep track of all the valid nodes in our Nodes array
        Nodes.Add(seed);

        // note: incrementDist is already a float
        // we add 1 to the cast distances because in our loops we start at 0
        NavigationGraph = new Vector3[(int)( (xCastDistance+2)*(zCastDistance+2) )];

        GraphNode currentNode = new GraphNode(0, startPoint);

        // TODO put the startPoint.y in a #if. Fix this so the first node gets added
        // Do one iteration to add the base point. Not totally necessary, this point will probably get found anyway
        //Vector3 neighbor = GetNeighborPoint(startPoint.x, rayCastHeight, startPoint.y);
        //AddNode(neighbor, ref currentNode, ref q);
        //NavigationGraph[0] = startPoint;

        /** Try a new searching method. BFS **/
        int count = 0;
        while ( count < Nodes.Count && Nodes[count] != null )
        {
            currentNode = Nodes[count];

            float xPoint = currentNode.GetPosition().x;
        #if USE_XZ
            float zPoint = currentNode.GetPosition().z;
        #else
            float zPoint = currentNode.GetPosition().y;
        #endif
            // determine the next point we want to check for
            Vector3 neighborPoint = GetNeighborPoint(xPoint + incrementDistX, rayCastHeight, zPoint);
            AddNode(neighborPoint, ref currentNode, ref q);

            neighborPoint = GetNeighborPoint(xPoint - incrementDistX, rayCastHeight, zPoint);
            AddNode(neighborPoint, ref currentNode, ref q);

            neighborPoint = GetNeighborPoint(xPoint, rayCastHeight, zPoint + incrementDistZ);
            AddNode(neighborPoint, ref currentNode, ref q);

            neighborPoint = GetNeighborPoint(xPoint, rayCastHeight, zPoint - incrementDistZ);
            AddNode(neighborPoint, ref currentNode, ref q);

            ++count;
        }
    }