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