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