/// <summary>
/// Remove the lowest cost Path from the queue
/// </summary>
/// <returns>The lowest cost Vector2Path</returns>
public Vector2Path dequeue()
{
--count;
Vector2Path temp = queue.First.Value;
queue.RemoveFirst();
return(temp);
}
/// <summary>
/// Takes in a queue and it's most recently popped Vector2Path, then if applicable, enqueues a new Vector2Path, which is the old Vector2Path and the goal vector added on.
/// </summary>
/// <param name="nextStep"></param>
/// <param name="queue"></param>
/// <param name="soFar"></param>
/// <param name="useWeights"></param>
private void dijkstraEnqueue(Vector2 nextStep, DijkstraQueue queue, Vector2Path soFar, bool useWeights, Vector2 goalNode, bool moveThroughActors)
{
LinkedListNode <Vector2> currentNode = soFar.pathSoFar.Last;
Vector2 currentPos = currentNode.Value;
if (withinGrid(nextStep.X, nextStep.Y) && (!visited[(int)nextStep.X, (int)nextStep.Y] &&
connections[(int)currentPos.X, (int)currentPos.Y, (int)nextStep.X, (int)nextStep.Y] &&
moveThroughActors ? true : !spaceArray[(int)nextStep.X, (int)nextStep.Y].IsOccupied))
{
Vector2Path newPath = new Vector2Path(soFar.pathSoFar, nextStep,
soFar.totalCost + (useWeights ? movementCosts[(int)currentPos.X, (int)currentPos.Y, (int)nextStep.X, (int)nextStep.Y] : 1));
//If we are using weights, add the weight, otherwise add 1 for constant weighting.
queue.enqueue(newPath, goalNode);
}
}
/// <summary>
/// Runs Dijkstra's Two Points Shortest Path algorithm
/// </summary>
/// <param name="sourceNodePos">Position at which the Vector2Path must start.</param>
/// <param name="targetNodePos">Position at which the Vector2Path must end.</param>
/// <param name="useWeights">Determines whether to use simple connection, or weighted paths.</param>
/// <returns>The shortest Vector2Path between the two points</returns>
public LinkedList <Vector2> Dijkstra(Vector2 sourceNodePos, Vector2 targetNodePos, bool useWeights, bool moveThroughActors)
{
LinkedList <Vector2> result = new LinkedList <Vector2>(), startingPath = new LinkedList <Vector2>();
if (sourceNodePos == targetNodePos || (moveThroughActors && spaceArray[(int)targetNodePos.X, (int)targetNodePos.Y].IsOccupied))
{
return(result);
}
for (int i = 0; i < sizeX; ++i)
{
for (int j = 0; j < sizeY; ++j)
{
visited[i, j] = false;
}
}
int targetX = (int)targetNodePos.X;
int targetY = (int)targetNodePos.Y;
visited[(int)sourceNodePos.X, (int)sourceNodePos.Y] = true;
Vector2Path currentPath = new Vector2Path(startingPath, sourceNodePos, 0);
DijkstraQueue dkQ = new DijkstraQueue(visited);
dkQ.enqueue(currentPath, targetNodePos);
while (!visited[targetX, targetY] && dkQ.count > 0)
{
currentPath = dkQ.dequeue();
currentPath.pathSoFar.AddLast(currentPath.nextStep);
visited[(int)currentPath.nextStep.X, (int)currentPath.nextStep.Y] = true;
//Enqueue the 4 cardinal directional neighbors
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitY, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitY, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitX, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitX, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
//Enqueue the two off-kilter neighbors, which will either be above or below the horizontal neighbors.
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitX + Vector2.UnitY * (currentPath.nextStep.X % 2 == 0 ? -1 : 1),
dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitX + Vector2.UnitY * (currentPath.nextStep.X % 2 == 0 ? -1 : 1),
dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
}
result = currentPath.pathSoFar;
return(result);
}
/// <summary>
/// Put the Vector2Path into the correct position of the list
/// </summary>
/// <param name="newPath">Vector2Path to insert</param>
public void enqueue(Vector2Path newPath, Vector2 finalGoal)
{
if (queue.Count == 0)
{
queue.AddFirst(newPath);
}
else
{
LinkedListNode <Vector2Path> current = queue.First;
LinkedListNode <Vector2Path> last = queue.First;
while (current != null && (newPath.totalCost > current.Value.totalCost || (newPath.totalCost == current.Value.totalCost &&
Vector2.DistanceSquared(newPath.nextStep, finalGoal) > Vector2.DistanceSquared(current.Value.nextStep, finalGoal))))
{
if (visited[(int)current.Value.nextStep.X, (int)current.Value.nextStep.Y])
{
if (current == queue.First)
{
queue.RemoveFirst();
current = queue.First;
last = queue.First;
}
else
{
current = last;
queue.Remove(current.Next);
current = current.Next;
}
--count;
}
else
{
last = current;
current = current.Next;
}
}
LinkedListNode <Vector2Path> newNode = new LinkedListNode <Vector2Path>(newPath);
queue.AddAfter(last, newNode);
}
++count;
}
/// <summary>
/// Put the Vector2Path into the correct position of the list
/// </summary>
/// <param name="newPath">Vector2Path to insert</param>
public void enqueue(Vector2Path newPath, Vector2 finalGoal)
{
if (queue.Count == 0)
{
queue.AddFirst(newPath);
}
else
{
LinkedListNode<Vector2Path> current = queue.First;
LinkedListNode<Vector2Path> last = queue.First;
while (current != null && (newPath.totalCost > current.Value.totalCost || (newPath.totalCost == current.Value.totalCost
&& Vector2.DistanceSquared(newPath.nextStep, finalGoal) > Vector2.DistanceSquared(current.Value.nextStep, finalGoal))))
{
if (visited[(int)current.Value.nextStep.X, (int)current.Value.nextStep.Y])
{
if (current == queue.First)
{
queue.RemoveFirst();
current = queue.First;
last = queue.First;
}
else
{
current = last;
queue.Remove(current.Next);
current = current.Next;
}
--count;
}
else
{
last = current;
current = current.Next;
}
}
LinkedListNode<Vector2Path> newNode = new LinkedListNode<Vector2Path>(newPath);
queue.AddAfter(last, newNode);
}
++count;
}
/// <summary>
/// Takes in a queue and it's most recently popped Vector2Path, then if applicable, enqueues a new Vector2Path, which is the old Vector2Path and the goal vector added on.
/// </summary>
/// <param name="nextStep"></param>
/// <param name="queue"></param>
/// <param name="soFar"></param>
/// <param name="useWeights"></param>
private void dijkstraEnqueue(Vector2 nextStep, DijkstraQueue queue, Vector2Path soFar, bool useWeights, Vector2 goalNode, bool moveThroughActors)
{
LinkedListNode<Vector2> currentNode = soFar.pathSoFar.Last;
Vector2 currentPos = currentNode.Value;
if (withinGrid(nextStep.X, nextStep.Y) && (!visited[(int)nextStep.X, (int)nextStep.Y] &&
connections[(int)currentPos.X, (int)currentPos.Y, (int)nextStep.X, (int)nextStep.Y]
&& moveThroughActors ? true : !spaceArray[(int)nextStep.X, (int)nextStep.Y].IsOccupied))
{
Vector2Path newPath = new Vector2Path(soFar.pathSoFar, nextStep,
soFar.totalCost + (useWeights ? movementCosts[(int)currentPos.X, (int)currentPos.Y, (int)nextStep.X, (int)nextStep.Y] : 1));
//If we are using weights, add the weight, otherwise add 1 for constant weighting.
queue.enqueue(newPath, goalNode);
}
}
/// <summary>
/// Runs Dijkstra's Two Points Shortest Path algorithm
/// </summary>
/// <param name="sourceNodePos">Position at which the Vector2Path must start.</param>
/// <param name="targetNodePos">Position at which the Vector2Path must end.</param>
/// <param name="useWeights">Determines whether to use simple connection, or weighted paths.</param>
/// <returns>The shortest Vector2Path between the two points</returns>
public LinkedList<Vector2> Dijkstra(Vector2 sourceNodePos, Vector2 targetNodePos, bool useWeights, bool moveThroughActors)
{
LinkedList<Vector2> result = new LinkedList<Vector2>(), startingPath = new LinkedList<Vector2>();
if(sourceNodePos == targetNodePos || (moveThroughActors && spaceArray[(int)targetNodePos.X, (int)targetNodePos.Y].IsOccupied)) return result;
for(int i = 0; i < sizeX; ++i)
{
for(int j = 0; j < sizeY; ++j)
{
visited[i,j] = false;
}
}
int targetX = (int) targetNodePos.X;
int targetY = (int) targetNodePos.Y;
visited[(int) sourceNodePos.X, (int) sourceNodePos.Y] = true;
Vector2Path currentPath = new Vector2Path(startingPath, sourceNodePos, 0);
DijkstraQueue dkQ = new DijkstraQueue(visited);
dkQ.enqueue(currentPath, targetNodePos);
while(!visited[targetX, targetY] && dkQ.count > 0)
{
currentPath = dkQ.dequeue();
currentPath.pathSoFar.AddLast(currentPath.nextStep);
visited[(int)currentPath.nextStep.X, (int)currentPath.nextStep.Y] = true;
//Enqueue the 4 cardinal directional neighbors
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitY, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitY, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitX, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitX, dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
//Enqueue the two off-kilter neighbors, which will either be above or below the horizontal neighbors.
dijkstraEnqueue(currentPath.nextStep + Vector2.UnitX + Vector2.UnitY * (currentPath.nextStep.X % 2 == 0 ? -1 : 1),
dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
dijkstraEnqueue(currentPath.nextStep - Vector2.UnitX + Vector2.UnitY * (currentPath.nextStep.X % 2 == 0 ? -1 : 1),
dkQ, currentPath, useWeights, targetNodePos, moveThroughActors);
}
result = currentPath.pathSoFar;
return result;
}
