public static List <Tile> GetPath(int fromX, int fromY, int toX, int toY)
{
finalPath.Clear();
int fromIndex = grid.TilePosToIndex(fromX, fromY);
int toIndex = grid.TilePosToIndex(toX, toY);
Tile initialTile = grid.Tiles[fromIndex];
Tile destinationTile = grid.Tiles[toIndex];
openListPriorityQueue.Enqueue(initialTile, 0);
Tile currentTile = null;
while (openListPriorityQueue.Count > 0)
{
currentTile = openListPriorityQueue.Dequeue();
closeDictionary.Add(currentTile.Index, currentTile);
if (Equals(currentTile, destinationTile))
{
break;
}
UpdateNeighbors(currentTile);
for (int i = neighbors.Length - 1; i >= 0; --i)
{
Tile neighbourPathTile = neighbors[i];
if (neighbourPathTile == null)
{
continue;
}
if (closeDictionary.ContainsKey(neighbourPathTile.Index))
{
continue;
}
bool isAtOpenList = openListPriorityQueue.Contains(neighbourPathTile);
float movementCostToNeighbour = currentTile.GCost + GetDistance(currentTile, neighbourPathTile);
if (movementCostToNeighbour < neighbourPathTile.GCost || !isAtOpenList)
{
neighbourPathTile.GCost = movementCostToNeighbour;
neighbourPathTile.HCost = GetDistance(neighbourPathTile, destinationTile);
neighbourPathTile.SetParent(currentTile);
if (!isAtOpenList)
{
openListPriorityQueue.Enqueue(neighbourPathTile, neighbourPathTile.FCost);
}
else
{
openListPriorityQueue.UpdatePriority(neighbourPathTile, neighbourPathTile.FCost);
}
}
}
}
while (currentTile.Parent != null && !Equals(currentTile, initialTile))
{
finalPath.Add(currentTile);
currentTile = currentTile.Parent;
}
finalPath.Add(initialTile);
openListPriorityQueue.Clear();
closeDictionary.Clear();
return(finalPath);
}
public void IterationCleanup()
{
Queue.Clear();
}
public void ResetCore()
{
_timeLine.Clear();
CurrentTime = 0.0;
}
public void getEmpty()
{
abiertos.Clear();
}
public override void Clear()
{
OpenSet.Clear();
ClosedSet.Clear();
KeyedDict.Clear();
}
public void CompareThreeQueuesPerformance()
{
var rng = new RandomNumberGenerator(456);
int size = 100000;
int attempts = 10;
int[] input = new int[size];
for (int i = 0; i < size; i++)
{
input[i] = rng.Next(size);
}
var oldQueueComparer =
FunctionalComparer <PQIndexedObject> .Create((first, second) => first.Priority.CompareTo(second.Priority));
var spatialAStarQueue = new PriorityQueue <PQIndexedObject>(oldQueueComparer, size);
var fastQueue = new FastPriorityQueue <FPQNode>(size);
var intervalHeapQueue = new IntervalHeap <int>();
var stopwatchOverall = Stopwatch.StartNew();
for (int i = 0; i < attempts; i++)
{
spatialAStarQueue.Clear();
var stopwatch = Stopwatch.StartNew();
for (int itemIndex = 0; itemIndex < size; itemIndex++)
{
var oldItem = new PQIndexedObject {
Priority = itemIndex
};
spatialAStarQueue.Push(oldItem);
}
Console.WriteLine(" Spatial A* queue " + stopwatch.ElapsedMilliseconds + " ms for " + attempts + " times " + size + " items.");
}
Console.WriteLine(" Spatial A* queue total: " + stopwatchOverall.ElapsedMilliseconds + " ms for " + size + " items.");
stopwatchOverall.Restart();
for (int i = 0; i < attempts; i++)
{
intervalHeapQueue = new IntervalHeap <int>();
var stopwatch = Stopwatch.StartNew();
for (int itemIndex = 0; itemIndex < size; itemIndex++)
{
intervalHeapQueue.Add(itemIndex);
}
Console.WriteLine(" Interval Heap " + stopwatch.ElapsedMilliseconds + " ms for " + attempts + " times " + size + " items.");
}
Console.WriteLine(" Interval Heap total: " + stopwatchOverall.ElapsedMilliseconds + " ms for " + size + " items.");
stopwatchOverall.Restart();
for (int i = 0; i < attempts; i++)
{
fastQueue.Clear();
var stopwatch = Stopwatch.StartNew();
for (int itemIndex = 0; itemIndex < size; itemIndex++)
{
var newItem = new FPQNode()
{
Priority = itemIndex
};
fastQueue.Enqueue(newItem, itemIndex);
}
Console.WriteLine(" Fast queue " + stopwatch.ElapsedMilliseconds + " ms for " + size + " items.");
}
Console.WriteLine(" Fast queue total: " + stopwatchOverall.ElapsedMilliseconds + " ms for " + attempts + " times " + size + " items.");
}
public INode <T> Run(INode <T> start, T goal, int maxIterations = 100, bool earlyExit = true)
{
frontier.Clear();
stateToNode.Clear();
explored.Clear();
frontier.Enqueue(start, start.GetCost());
var iterations = 0;
while ((frontier.Count > 0) && (iterations < maxIterations) && (frontier.Count + 1 < frontier.MaxSize))
{
iterations++;
var node = frontier.Dequeue();
if (node.IsGoal(goal))
{
ReGoapLogger.Log("[Astar] Success iterations: " + iterations);
#if DEBUG
AStarDebugRecorder.AddRecording(new AStarDebugRecording(explored.Values.Concat(frontier).Concat(new INode <T>[] { node }).Cast <BGoapNode>(), goal as BGoapState));
#endif
return(node);
}
explored[node.GetState()] = node;
foreach (var child in node.Expand())
{
if (earlyExit && child.IsGoal(goal))
{
ReGoapLogger.Log("[Astar] (early exit) Success iterations: " + iterations);
#if DEBUG
AStarDebugRecorder.AddRecording(new AStarDebugRecording(explored.Values.Concat(frontier).Concat(new INode <T>[] { child }).Cast <BGoapNode>(), goal as BGoapState));
#endif
return(child);
}
var childCost = child.GetCost();
var state = child.GetState();
if (explored.ContainsKey(state))
{
continue;
}
INode <T> similiarNode;
stateToNode.TryGetValue(state, out similiarNode);
if (similiarNode != null)
{
if (similiarNode.GetCost() > childCost)
{
frontier.Remove(similiarNode);
}
else
{
break;
}
}
frontier.Enqueue(child, childCost);
stateToNode[state] = child;
}
}
ReGoapLogger.LogWarning("[Astar] failed.");
#if DEBUG
AStarDebugRecorder.AddRecording(new AStarDebugRecording(explored.Values.Concat(frontier).Cast <BGoapNode>(), goal as BGoapState));
#endif
return(null);
}
//used to check if user input would create a final board that is consistent. Uses a priority queue to implement minimal remaining value algorithm
//on the list of squares that make up the board. Guesses when it comes to a square that cant be reduced to only one possible value. If a square has no possible
//values and the guessStack is empty then the state is consistent and an error that corresponds to the type of consistency error is returned. Otherwise if the board
//can be completed in a valid state then null is returned.
public Error checkConsistency(Square temp)
{
FastPriorityQueue <Square> testQueue = new FastPriorityQueue <Square>(81); //used for mrv algorithm
Stack <unCert> guessStack = new Stack <unCert>(); //stack that keeps track of guessed states
foreach (Square sq in this.Board) //populate priority queue with squares from the board
{
sq.genPossibles(this.Board);
testQueue.Enqueue(sq, sq.Heur);
}
while (testQueue.Any()) //loop until the queue is empty
{
if (testQueue.First.Given || temp == testQueue.First || testQueue.First.Solved) //if the square has been solved, is a given, or is user entered remove the sqaure from the queue
{
testQueue.Dequeue();
}
else if (testQueue.First.Heur == 1)//if the square only has one possible value set it to that value and declare it as solved, remove it from the queue and update the board, and the domain of the squares that that square constrains
{
testQueue.First.Number = testQueue.First.PossibleVals[0];
testQueue.First.Solved = true;
updateBoard(testQueue.First);
testQueue.Dequeue();
for (int i = 0; i < testQueue.Count; i++)//update the priority of the square in the queue based on the new number in the board
{
testQueue.UpdatePriority(testQueue.ElementAt(i), testQueue.ElementAt(i).Heur);
}
}
else if (testQueue.First.Heur == 0) //a square has no possible values so the current state is inconsistent( as proposed by ac3 algorithm)
{
if (guessStack.Count == 0) //no guesses have been made so the board is incosistent due to the user input so display an error
{
Error err = new Error(Error.errType.single, temp);
return(err);
}
else//otherwise a guess caused an inconsistency so a correction needs to be made
{
foreach (int x in guessStack.First().Guessed)//restrict the domain based on the guesses that have already been made, since those are wrong as they caused an inconsistency
{
guessStack.First().Gsquare.PossibleVals.Remove(x);
}
if (guessStack.First().Gsquare.PossibleVals.Count == 0)//if there are no more values left in the guessed squares domain that means the bad guess occured on a previous square, pop this state and check the lower guess in the stack
{
guessStack.Pop();
guessStack.First().Prev.board[guessStack.First().Gsquare.Y * 9 + guessStack.First().Gsquare.X].Solved = false;
guessStack.First().Prev.board[guessStack.First().Gsquare.Y * 9 + guessStack.First().Gsquare.X].Heur = -1;
continue;
}
if (guessStack.Count != 0)//as long as the stack isnt empty update the guessed square to be false again and set its heur value at -1 so it will be at the front of the queue, then reset the board to the backed up version
{
guessStack.First().Prev.board[guessStack.First().Gsquare.Y * 9 + guessStack.First().Gsquare.X].Solved = false;
guessStack.First().Prev.board[guessStack.First().Gsquare.Y * 9 + guessStack.First().Gsquare.X].Heur = -1;
foreach (Square sq in guessStack.First().Prev.board)
{
this.modBoard(sq);
}
this.Placed = guessStack.First().Prev.Placed;
testQueue.Clear();
foreach (Square sq in this.board)
{
testQueue.Enqueue(sq, sq.Heur);
}
}
}
}
else if (testQueue.First.Heur > 1 || testQueue.First.Heur == -1) //a guess needs to be made as it has multiple possible values
{
if (guessStack.Count == 0 || guessStack.First().Gsquare != testQueue.First()) //if the guess stack is empty or the guess at the top of the stack doesnt correspond to the current square make a new guess object
{
unCert tempCert = new unCert();
int guessing = 0;
tempCert.Gsquare = testQueue.First.deepCopy();
tempCert.Prev = this.deepCopy();
guessing = testQueue.First.PossibleVals[0];
testQueue.First.PossibleVals.Remove(0);
testQueue.First.Solved = true;
testQueue.First.Number = guessing;
tempCert.Guessed.Add(guessing);
guessStack.Push(tempCert);
updateBoard(testQueue.First);
testQueue.Dequeue();
for (int i = 0; i < testQueue.Count; i++)
{
testQueue.UpdatePriority(testQueue.ElementAt(i), testQueue.ElementAt(i).Heur);
}
}
else//otherwise modify the guess object at the top of the guess stack and make a new guess, and update the board based on the new guess.
{
guessStack.First().Gsquare = testQueue.First().deepCopy();
guessStack.First().Prev = this.deepCopy();
foreach (int x in guessStack.First().Guessed)
{
testQueue.First.PossibleVals.Remove(x);
}
if (testQueue.First.PossibleVals.Count == 0)
{
continue;
}
testQueue.First.Solved = true;
int guessing = testQueue.First.PossibleVals[0];
testQueue.First.Number = guessing;
guessStack.First().Guessed.Add(guessing);
updateBoard(testQueue.First);
testQueue.Dequeue();
for (int i = 0; i < testQueue.Count; i++)
{
testQueue.UpdatePriority(testQueue.ElementAt(i), testQueue.ElementAt(i).Heur);
}
}
}
}
return(null);//no errors encountered, state is consistent
}