// Remove a node at a particular position in the queue.
private PriorityQueueItem <TValue, TPriority> RemoveAt(Int32 index)
{
// remove an item from the heap
PriorityQueueItem <TValue, TPriority> o = items[index];
PriorityQueueItem <TValue, TPriority> tmp = items[numItems - 1];
items[--numItems] = default(PriorityQueueItem <TValue, TPriority>);
if (numItems > 0)
{
int i = index;
int j = i + 1;
while (i < Count / 2)
{
if ((j < Count - 1) && (compareFunc(items[j].Priority, items[j + 1].Priority) > 0))
{
j++;
}
if (compareFunc(items[j].Priority, tmp.Priority) >= 0)
{
break;
}
items[i] = items[j];
i = j;
j *= 2;
}
items[i] = tmp;
}
return(o);
}
/// <summary>
/// Copies the queue elements to a new array.
/// </summary>
/// <returns>A new array containing elements copied from the Queue.</returns>
public PriorityQueueItem <TValue, TPriority>[] ToArray()
{
PriorityQueueItem <TValue, TPriority>[] newItems =
new PriorityQueueItem <TValue, TPriority> [numItems];
Array.Copy(items, newItems, numItems);
return(newItems);
}
/// <summary>
/// Adds an object to the queue, in order by priority.
/// </summary>
/// <param name="value">The object to be added.</param>
/// <param name="priority">Priority of the object to be added.</param>
public void Enqueue(TValue value, TPriority priority)
{
if (numItems == capacity)
{
// need to increase capacity
// grow by 50 percent
//SetCapacity((3 * Capacity) / 2);
SetCapacity((int)(Capacity * 1.5));
}
// Create the new item
PriorityQueueItem <TValue, TPriority> newItem = new PriorityQueueItem <TValue, TPriority>(value, priority);
int i = numItems;
++numItems;
// and insert it into the heap.
while ((i > 0) && (compareFunc(items[i / 2].Priority, newItem.Priority) > 0))
{
items[i] = items[i / 2];
i /= 2;
}
items[i] = newItem;
}
public List <PathReturnNode> FindPath(Point start, Point end)
{
nodeFound = false;
isStop = false;
isStopped = false;
NodeOpenVal += 2;
NodeCloseVal += 2;
OpenList.Clear();
Position = (uint)(start.X + start.Y * GridX);
startPosition = (uint)(start.X + start.Y * GridX);
endPosition = (uint)(end.X + end.Y * GridX);
//Set first node in info grid:
PathInfoGrid[Position].Gone = 0;
PathInfoGrid[Position].F = HeuristicEstimateVal;
PathInfoGrid[Position].Parrent = Position;
PathInfoGrid[Position].OpenOrClosed = NodeOpenVal;
//Enqueue first node
OpenList.Enqueue(Position, 1);
while (OpenList.Count > 0 && !isStop)
{
//Dequeue node with lowest cost
PriorityQueueItem <uint, int> item = OpenList.Dequeue();
Position = item.Value;
//Node Closed?
if (PathInfoGrid[Position].OpenOrClosed == NodeCloseVal)
{
continue;
}
PathInfoGrid[Position].OpenOrClosed = NodeCloseVal;
PosX = (ushort)PathInfoGrid[Position].PosX;
PosY = (ushort)PathInfoGrid[Position].PosY;
//Found end node - vaild path is found
if (Position == endPosition)
{
PathInfoGrid[Position].OpenOrClosed = NodeCloseVal;
nodeFound = true;
break;
}
//Foreach direction
for (int i = 0; i < 8; i += 1)
{
newPosX = (ushort)(PosX + Directions[i, 0]);
newPosY = (ushort)(PosY + Directions[i, 1]);
newPosition = (uint)(newPosY * GridX + newPosX);
//Outside of grid
if (newPosX >= GridX || newPosY >= GridY)
{
continue;
}
if (PathInfoGrid[newPosition].OpenOrClosed == NodeCloseVal)
{
continue;
}
//Solid block
if (PathGrid[newPosX, newPosY] == 0)
{
continue;
}
if (i > 3)
{
//Smooth path and do-not allow passing throw nodes with connected corners
int iminus4 = i - 4;
if (PathGrid[newPosX + SmoothDir[iminus4, 0], newPosY] == 0 || PathGrid[newPosX, newPosY + SmoothDir[iminus4, 1]] == 0)
{
continue;
}
//Passing thro diognal cost more:
newGone = PathInfoGrid[Position].Gone + (int)(PathGrid[newPosX, newPosY] * 2.41);
}
else
{
newGone = PathInfoGrid[Position].Gone + PathGrid[newPosX, newPosY];
}
if (PathInfoGrid[newPosition].OpenOrClosed == NodeOpenVal)
{
if (PathInfoGrid[newPosition].Gone < newGone)
{
continue;
}
}
PathInfoGrid[newPosition].Parrent = Position;
PathInfoGrid[newPosition].Gone = newGone;
//Calculate Euclidean heuristic
//Heuristic = (int)(HeuristicEstimateVal * (Math.Sqrt(Math.Pow((newPosX - end.X), 2) + Math.Pow((newPosY - end.Y), 2))));
//Manhattan
Heuristic = HeuristicEstimateVal * (Math.Abs(end.X - newPosX) + Math.Abs(end.Y - newPosY));
//Diognal distance
//Heuristic = HeuristicEstimateVal * Math.Max(Math.Abs(newPosX - end.X), Math.Abs(newPosX - end.Y));
PathInfoGrid[newPosition].F = newGone + Heuristic;
OpenList.Enqueue(newPosition, PathInfoGrid[newPosition].F);
PathInfoGrid[newPosition].OpenOrClosed = NodeOpenVal;
}
}
//Vaild path is found
if (nodeFound)
{
SolovedList.Clear();
//Add end node to found list
PathGridNode foundNodeEnd = PathInfoGrid[endPosition];
PathReturnNode foundNode;
foundNode.Parrent = foundNodeEnd.Parrent;
foundNode.Pos = foundNodeEnd.Pos;
foundNode.PosX = foundNodeEnd.PosX;
foundNode.PosY = foundNodeEnd.PosY;
SolovedList.Add(foundNode);
//Add rest of the nodes by parrents
while (foundNode.Pos != startPosition)
{
foundNodeEnd = PathInfoGrid[foundNode.Parrent];
foundNode.Parrent = foundNodeEnd.Parrent;
foundNode.Pos = foundNodeEnd.Pos;
foundNode.PosX = foundNodeEnd.PosX;
foundNode.PosY = foundNodeEnd.PosY;
SolovedList.Add(foundNode);
}
isStopped = true;
return(SolovedList);
}
isStopped = true;
//If no path found -> return null
return(null);
}