/// <summary> /// Gets the valid neighbors. /// </summary> /// <returns>The valid neighbors.</returns> /// <param name="currentPosition">Current position.</param> private Vector3 GetShortestNeighbor(Vector3 currentPosition, Vector3 targetPosition) { PriorityQueue result = new PriorityQueue(); int x, z; ConvertToIndex(out z, out x, currentPosition); for (int i = -2; i < 3; i++) { for (int j = -2; j < 3; j++) { if ((x + j) < 98 && (x + j) > 0 && (z + i) < 49 && (z + i) > 0) { if (!map [(z + i), (x + j)].Equals(Vector3.zero)) { PriorityQueue.LocationNode tempNode = new PriorityQueue.LocationNode(); tempNode.location = map [z + i, x + j]; tempNode.priority = HeuristicFunction(map [z + i, x + j], targetPosition); result.Enqueue(tempNode); } } } } return(result.Dequeue()); }
private Queue FillResultingQueue(PriorityQueue res) { Queue temp = new Queue(); List <Vector3> result = new List <Vector3> (); PriorityQueue.LocationNode tempNode = new PriorityQueue.LocationNode(); int index = res.priorityQueue.Count - 1; if (index >= 0) { tempNode = res.priorityQueue [index]; result.Add(tempNode.location); index--; Vector3 cFrom = tempNode.cameFrom; while (index >= 0) { if (res.priorityQueue[index].location.Equals(cFrom)) { result.Add(res.priorityQueue[index].location); cFrom = res.priorityQueue [index].cameFrom; } index--; } while (result.Count >= 1) { temp.Enqueue(result[result.Count - 1]); result.RemoveAt(result.Count - 1); } } return(temp); }
/// <summary> /// Gets A star search path. /// </summary> /// <returns>The A star search path.</returns> /// <param name="currentPosition">Current position.</param> /// <param name="targetPosition">Target position.</param> public Queue GetAStarSearchPath(Vector3 currentPosition, Vector3 targetPosition) { visitedList = new bool[200, 200]; //Round down the current position currentPosition = new Vector3((float)((int)currentPosition.x), 1f, (float)((int)currentPosition.z)); //Round down the target position targetPosition = new Vector3((float)((int)targetPosition.x), 1f, (float)((int)targetPosition.z)); //Result List Queue ResultingPath = new Queue(); PriorityQueue res = new PriorityQueue(); //Cost Vector int[] costSoFar = new int[100000]; //Path Vector Vector3[] pathSoFar = new Vector3[100000]; //Current position coordinates int currentX, currentZ; ConvertToIndex(out currentZ, out currentX, currentPosition); //Initial cost is 0 costSoFar [0] = 0; //Frontier queue PriorityQueue frontier = new PriorityQueue(); //Create node for start point PriorityQueue.LocationNode currentNode = new PriorityQueue.LocationNode(); //Give its credentials currentNode.location = currentPosition; currentNode.priority = 0f; res.Enqueue(currentNode); //Add it to queue frontier.Enqueue(currentNode); //To keep track of the last place to be modified int LastIndex = 1; while (!frontier.IsEmpty()) { //Get the current location PriorityQueue.LocationNode currentN = frontier.DequeueNode(); Vector3 currentLocation = currentN.location; if (currentLocation.Equals(targetPosition)) //Goal Achieved { res.Enqueue(currentN); ResultingPath = FillResultingQueue(res); ResultingPath.Enqueue(currentLocation); return(ResultingPath); } else { //For each valid neighbor GetValidNeighbors(currentLocation).queue.ForEach(delegate(Vector3 obj) { int neighborCost = costSoFar[LastIndex - 1] + 1; //As all the steps cost 1 if (costSoFar[LastIndex] == 0 || costSoFar[LastIndex] > neighborCost) { PriorityQueue.LocationNode newNeighbor = new PriorityQueue.LocationNode(); newNeighbor.cameFrom = currentLocation; newNeighbor.location = obj; newNeighbor.priority = costSoFar[LastIndex - 1] + 1 + (int)HeuristicFunction(obj, targetPosition); frontier.Enqueue(newNeighbor); res.Enqueue(newNeighbor); } }); } LastIndex++; } ResultingPath = FillResultingQueue(res); return(ResultingPath); }