public void Test_Integration_2()
{
PriorityQueue <double> q = null;
double[] items = { 82, 100, 9.3, 1.19, 10, 29, 12, 9.0006, 22, 20.9, 207, 13.56, 30, 2, 66 };
SortedList <double, double> monitor = new SortedList <double, double>();
q = new MinPriorityQueue <double>();
for (int i = 0; i < items.Length; i++)
{
q.Enqueue(items[i]);
monitor.Add(items[i], items[i]);
if (i == 3)
{
q.Dequeue();
monitor.Remove(1.19);
}
else if (i == 8)
{
q.Dequeue();
monitor.Remove(9.0006);
}
}
foreach (double monitorItem in monitor.Values)
{
Assert.AreEqual(monitorItem, q.Dequeue());
}
}
public void Test_Dequeue()
{
PriorityQueue <double> q = null;
q = new MinPriorityQueue <double>();
q.Enqueue(3);
q.Enqueue(2);
q.Enqueue(1);
Assert.AreEqual(1, q.Dequeue());
Assert.AreEqual(2, q.Dequeue());
Assert.AreEqual(3, q.Dequeue());
}
public void PriorityQueue_Test()
{
var queue = new MinPriorityQueue <int>();
queue.Enqueue(10);
queue.Enqueue(9);
queue.Enqueue(1);
queue.Enqueue(21);
Assert.AreEqual(queue.Dequeue(), 1);
Assert.AreEqual(queue.Dequeue(), 9);
Assert.AreEqual(queue.Dequeue(), 10);
Assert.AreEqual(queue.Dequeue(), 21);
}
public void MinPriorityQueueMovesNextLowestPriorityItemToHeadAfterRemoveItem()
{
var priorityQueue = new MinPriorityQueue <string, int>(50);
List <Tuple <string, int> > items = new List <Tuple <string, int> >
{
new Tuple <string, int>("A_50", 50),
new Tuple <string, int>("A_41", 41),
new Tuple <string, int>("A_38", 38),
new Tuple <string, int>("A_37", 37),
new Tuple <string, int>("A_23", 23),
new Tuple <string, int>("A_11", 11),
new Tuple <string, int>("A_5", 5),
new Tuple <string, int>("A_3", 3),
}.Randomize()
.ToList();
priorityQueue.EnqueueRange(items);
priorityQueue.Dequeue();
string item = priorityQueue.Peek();
Assert.AreEqual("A_5", item);
}
public void Test_Remove()
{
PriorityQueue <double> q = null;
double[] items = { 5, 85, 43, 2, 28, 99, 67, 1.98, 33, 19, 17, 44 };
SortedList <double, double> monitor = new SortedList <double, double>();
q = new MinPriorityQueue <double>(7);
for (int i = 0; i < items.Length; i++)
{
q.Enqueue(items[i]);
monitor.Add(items[i], items[i]);
}
q.Remove(43);
monitor.Remove(43);
q.Remove(17);
monitor.Remove(17);
foreach (double monitorItem in monitor.Values)
{
Assert.AreEqual(monitorItem, q.Dequeue());
}
}
public void MinPriorityQueue_FullTest_Successful()
{
var source = new List <int>();
var sourceCount = Faker.RandomNumber.Next(20, 100);
for (int i = 0; i < sourceCount; i++)
{
source.Add(Faker.RandomNumber.Next());
}
var additionalDataCount = Faker.RandomNumber.Next(10, 50);
// Act 1. Build priority queue with some initial data.
var queue = new MinPriorityQueue <int>(source);
// Act 2. Add some additional data using Enqueue
for (int i = 0; i < additionalDataCount; i++)
{
queue.Enqueue(Faker.RandomNumber.Next());
}
// Act 3 & Assert - Check results from the queue
var prev = -1; // Set a negative number as previous; all data in the queue is assumed to be positive.
while (!queue.IsEmpty())
{
var curr = queue.Dequeue();
Assert.IsTrue(curr >= prev);
prev = curr;
}
}
public void Test_Dequeue_EmptyQ()
{
PriorityQueue <double> q = null;
q = new MinPriorityQueue <double>();
Assert.AreEqual(default(double), q.Dequeue());
}
public void Dequeue_Should_Always_Return_The_Min_Value()
{
var pq = new MinPriorityQueue <int>();
pq.Enqueue(6);
pq.Enqueue(9);
pq.Enqueue(8);
pq.Enqueue(3);
pq.Enqueue(5);
pq.Enqueue(1);
pq.Enqueue(3);
pq.Count.Should().Be(7);
pq.Dequeue().Should().Be(1);
pq.Count.Should().Be(6);
pq.Dequeue().Should().Be(3);
pq.Count.Should().Be(5);
pq.Dequeue().Should().Be(3);
pq.Count.Should().Be(4);
pq.Dequeue().Should().Be(5);
pq.Count.Should().Be(3);
pq.Dequeue().Should().Be(6);
pq.Count.Should().Be(2);
pq.Dequeue().Should().Be(8);
pq.Count.Should().Be(1);
pq.Dequeue().Should().Be(9);
pq.Count.Should().Be(0);
}
public IEnumerable <Node> GetPath(Node start, Node goal)
{
var cost = new Dictionary <Node, int>();
var parents = new Dictionary <Node, Node>();
var visited = new HashSet <Node>();
var queue = new MinPriorityQueue <Node>();
cost[start] = 0;
parents[start] = null;
queue.Enqueue(start);
bool foundPath = false;
while (queue.Count > 0)
{
var current = queue.Dequeue();
visited.Add(current);
if (current.Equals(goal))
{
foundPath = true;
break;
}
var neighbours = this.GetNeighbours(current);
foreach (var neighbour in neighbours)
{
if (visited.Contains(neighbour) || this.IsWall(neighbour))
{
continue;
}
var newCost = cost[current] + 1;
if (!cost.ContainsKey(neighbour) || newCost < cost[neighbour])
{
cost[neighbour] = newCost;
neighbour.F = newCost + this.GetH(neighbour, goal);
queue.Enqueue(neighbour);
parents[neighbour] = current;
}
}
}
if (!foundPath)
{
throw new InvalidOperationException("No path found.");
}
return(this.GetPath(parents, goal));
}
public void Dequeue_ShouldRemoveTheSmallestItem()
{
var items = new List <char> {
'T', 'Z', 'B'
};
var target = new MinPriorityQueue <char>();
items.ForEach(anItem => target.Enqueue(anItem));
items.Sort();
Assert.That(target.Dequeue(), Is.EqualTo(items[0]));
}
public void ReplacingMinElementAndCheckingIfExtractedInSortedOrder()
{
var pq = new MinPriorityQueue <int, int>();
int maxHeapElement = 50000;
int minHeapElement = -50000;
int addedElements = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minHeapElement;
while (el <= maxHeapElement)
{
pq.Enqueue(el, -el);
addedElements++;
el += i;
}
}
if (pq.Count != addedElements)
{
Assert.Fail();
}
pq.ReplaceFirst(int.MaxValue, -int.MaxValue);
pq.ReplaceFirst(int.MaxValue, -int.MaxValue);
pq.ReplaceFirst(int.MaxValue, -int.MaxValue);
int removedElements = 0;
var min = pq.Peek();
while (!pq.IsEmpty)
{
var kvp = pq.Peek();
if (min.Key > kvp.Key)
{
Assert.Fail();
}
min = pq.Dequeue();
removedElements++;
}
Assert.IsTrue(pq.IsEmpty &&
pq.Count == 0 &&
addedElements == removedElements);
}
public void HeapifyUnsortedCollectionAndCheckIfExtractedInSortedOrder()
{
var pq = new MinPriorityQueue <int, int>();
var unsortedList = new List <KeyValuePair <int, int> >();
int maxHeapElement = 50000;
int minHeapElement = -50000;
int addedElements = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minHeapElement;
while (el <= maxHeapElement)
{
unsortedList.Add(new KeyValuePair <int, int>(el, -el));
addedElements++;
el += i;
}
}
pq.Heapify(unsortedList);
if (pq.Count != addedElements)
{
Assert.Fail("1");
}
int removedElements = 0;
var min = pq.Peek();
while (!pq.IsEmpty)
{
var kvp = pq.Peek();
if (min.Key > kvp.Key)
{
Assert.Fail("2");
}
min = pq.Dequeue();
removedElements++;
}
Assert.IsTrue(pq.IsEmpty &&
pq.Count == 0 &&
addedElements == removedElements);
}
public void AddingElementsWithCustomComparerAndCheckingIfExtractedInSortedOrder()
{
//Creating heap with reversed comparer
var pq = new MinPriorityQueue <int, int>(Comparer <int> .Create(
(x, y) => y.CompareTo(x)));
int maxHeapElement = 50000;
int minHeapElement = -50000;
int addedElements = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minHeapElement;
while (el <= maxHeapElement)
{
pq.Enqueue(el, -el);
addedElements++;
el += i;
}
}
if (pq.Count != addedElements)
{
Assert.Fail();
}
int removedElements = 0;
// because of the reversed comparer
var max = pq.Peek();
while (!pq.IsEmpty)
{
var kvp = pq.Peek();
if (max.Key < kvp.Key)
{
Assert.Fail();
}
max = pq.Dequeue();
removedElements++;
}
Assert.IsTrue(pq.IsEmpty &&
pq.Count == 0 &&
addedElements == removedElements);
}
private static int[] DequeueAll(MinPriorityQueue <int> queue)
{
var result = new List <int>();
while (!queue.IsEmpty())
{
Console.WriteLine("Queue: {0}", queue);
var dq = queue.Dequeue();
Console.WriteLine(" Dequeue: {0}", dq);
result.Add(dq);
}
return(result.ToArray());
}
public void DequeueMinValue()
{
var sut = new MinPriorityQueue <int, int>();
for (var i = 0; i < 50000; i++)
{
sut.Enqueue(i, i);
}
for (var i = 0; i < 50000; i++)
{
Assert.AreEqual(i, sut.Dequeue());
}
}
/// <summary>
/// Nearest neighbor search method to find the single nearest neighbor at a given point.
/// Translated from https://github.com/gvd/kdtree/blob/master/kdtree.h
/// </summary>
/// <param name="query">The nearest neighbor search query point.</param>
/// <returns>The closest node to the parameter query is returned.</returns>
//public KDNode NNSearch(Point query)
//{
// MinPriorityQueue<Tuple<double, KDNode>> pq = new MinPriorityQueue<Tuple<double, KDNode>>();
// //Tuple<double, KDNode> best = new Tuple<double, KDNode>(1.79769e+308, root);
// pq.Enqueue(new Tuple<double, KDNode>(0.0, root));
// do
// {
// var current = pq.Dequeue();
// //if (current.Item1 >= best.Item1)
// //{
// // if (EuclideanDistance(query, current.Item2.Data) > EuclideanDistance(query, best.Item2.Data))
// // return best.Item2;
// // else
// // return current.Item2;
// //}
// List<KDNode> a = new List<KDNode>
// {
// };
// var currentNode = current.Item2;
// //double d = EuclideanDistance(query, currentNode.Data);
// //double dx = Subtract(query, currentNode.Data, currentNode.Depth);
// if ()
// {
// best = new Tuple<double, KDNode>(d, currentNode);
// }
// KDNode near, far;
// near = currentNode.Left;
// else
// near = currentNode.Right;
// far = currentNode.Right;
// else
// far = currentNode.Left;
// if (near != null)
// pq.Enqueue(new Tuple<double, KDNode>(0, near));
// if (far != null)
// pq.Enqueue(new Tuple<double, KDNode>(0, far));
// } while (pq.Count() != 0);
// return best.Item2;
//}
public List <KDNode> LocSearch(Point query)
{
MinPriorityQueue <Tuple <double, KDNode> > pq = new MinPriorityQueue <Tuple <double, KDNode> >();
Point p = new Point(0, 0);
List <KDNode> a = new List <KDNode>
{
};
pq.Enqueue(new Tuple <double, KDNode>(0.0, root));
do
{
var current = pq.Dequeue();
// current.Item2.Data;
var currentNode = current.Item2;
if (query.x < currentNode.Data.x && currentNode.Data.x < query.x && query.y < currentNode.Data.y)
{
a.Add(currentNode);
}
double d = EuclideanDistance(query, currentNode.Data);
double dx = Subtract(query, currentNode.Data, currentNode.Depth);
KDNode near;
if (dx <= 0)
{
near = currentNode.Left;
}
else
{
near = currentNode.Right;
}
//if (dx <= 0)
// far = currentNode.Right;
//else
// far = currentNode.Left;
if (near != null)
{
pq.Enqueue(new Tuple <double, KDNode>(0, near));
}
} while (pq.Count() != 0);
return(a);
}
public void Enqueue_ShouldEnsureTheAscendingOrder()
{
var items = new List <char> {
'K', 'L', 'A', 'Z'
};
var target = new MinPriorityQueue <char>();
items.ForEach(anItem => target.Enqueue(anItem));
items.Sort();
foreach (var item in items)
{
Assert.That(target.Dequeue(), Is.EqualTo(item));
}
}
public void MinPriorityQueue_Enqueue()
{
int capacity = 5;
var q = new MinPriorityQueue<Element>(capacity);
for (int i = capacity; i > 0; i--)
{
q.Enqueue(new Element { Number = i, Data = new byte[i]});
}
Assert.IsTrue(q.Size == capacity);
for (int i = 1; i <= capacity; i++)
{
var e = q.Dequeue();
Assert.IsTrue(i == e.Number);
}
}
public void PrioritizeInALifoManner()
{
var sut = new MinPriorityQueue <int, int>(Stability.Lifo);
for (var i = 0; i < 10000; i++)
{
for (var j = 0; j < 5; j++)
{
sut.Enqueue(i, i * 5 + j);
}
}
for (var i = 0; i < 10000; i++)
{
for (var j = 4; j >= 0; j--)
{
Assert.AreEqual(i * 5 + j, sut.Dequeue());
}
}
}
public void MaintainMinHeap()
{
var sut = new MinPriorityQueue <int, int>();
var random = new Random();
var sortedResults = new List <int>();
for (var i = 0; i < 50000; i++)
{
sortedResults.Add(random.Next(1000));
}
sut.AddRange(sortedResults, x => x);
sortedResults = sortedResults.OrderBy(x => x).ToList();
for (var i = 0; i < 50000; i++)
{
Assert.AreEqual(sut.Dequeue(), sortedResults[i]);
}
}
public void HandleUnsortedInput()
{
var sut = new MinPriorityQueue <int, int>();
var random = new Random();
for (var i = 0; i < 50000; i++)
{
var randomValue = random.Next(1000);
sut.Enqueue(randomValue, randomValue);
}
var results = new List <int>();
for (var i = 0; i < 50000; i++)
{
results.Add(sut.Dequeue());
}
var sortedResults = new List <int>(results).OrderBy(x => x).ToList();
CollectionAssert.AreEqual(sortedResults, results);
}
public void MinPriorityQueueDequeueReturnsItemWithLowestPriority()
{
var priorityQueue = new MinPriorityQueue <string, int>(50);
List <Tuple <string, int> > items = new List <Tuple <string, int> >
{
new Tuple <string, int>("A_50", 50),
new Tuple <string, int>("A_41", 41),
new Tuple <string, int>("A_38", 38),
new Tuple <string, int>("A_37", 37),
new Tuple <string, int>("A_23", 23),
new Tuple <string, int>("A_11", 11),
new Tuple <string, int>("A_5", 5),
new Tuple <string, int>("A_3", 3),
}.Randomize()
.ToList();
priorityQueue.EnqueueRange(items);
string item = priorityQueue.Dequeue();
Assert.AreEqual("A_3", item);
}
public void MinPriorityQueueDequeueDoesRemoveItemFromQueue()
{
var priorityQueue = new MinPriorityQueue <string, int>(50);
List <Tuple <string, int> > items = new List <Tuple <string, int> >
{
new Tuple <string, int>("A_50", 50),
new Tuple <string, int>("A_41", 41),
new Tuple <string, int>("A_38", 38),
new Tuple <string, int>("A_37", 37),
new Tuple <string, int>("A_23", 23),
new Tuple <string, int>("A_11", 11),
new Tuple <string, int>("A_5", 5),
new Tuple <string, int>("A_3", 3),
}.Randomize()
.ToList();
priorityQueue.EnqueueRange(items);
priorityQueue.Dequeue();
Assert.AreEqual(7, priorityQueue.Count);
}
public Stack <State> Run(int[] nodes, HeuristicMethod heuristic)
{
List <State> nextStates = new List <State>();
HashSet <string> openStates = new HashSet <string>();
MinPriorityQueue <State> openedQueue = new MinPriorityQueue <State>(nodes.Length);
Dictionary <string, State> closedQueue = new Dictionary <string, State>();
State state = new State(parent: null, nodes: nodes, heuristic: heuristic);
openedQueue.Enqueue(state);
openStates.Add(state.GetStateCode());
while (!openedQueue.IsEmpty())
{
State currentState = openedQueue.Dequeue();
openStates.Remove(currentState.GetStateCode());
// Is this final state
if (currentState.IsFinalState())
{
return(GetFinalPath(currentState));
}
// Look into next state
currentState.GetNextStates(ref nextStates);
if (nextStates.Count > 0)
{
State closedState;
State openState;
State nextState;
for (int i = 0; i < nextStates.Count; i++)
{
closedState = null;
openState = null;
nextState = nextStates[i];
if (openStates.Contains(nextState.GetStateCode()))
{
// We already have same state in the open queue.
openState = openedQueue.Find(nextState, out int openStateIndex);
if (openState.IsCostlierThan(nextState))
{
// We have found a better way to reach at this state. Discard the costlier one
openedQueue.Remove(openStateIndex);
openedQueue.Enqueue(nextState);
}
}
else
{
// Check if state is in closed queue
string stateCode = nextState.GetStateCode();
if (closedQueue.TryGetValue(stateCode, out closedState))
{
// We have found a better way to reach at this state. Discard the costlier one
if (closedState.IsCostlierThan(nextState))
{
closedQueue.Remove(stateCode);
closedQueue[stateCode] = nextState;
}
}
}
// Either this is a new state, or better than previous one.
if (openState == null && closedState == null)
{
openedQueue.Enqueue(nextState);
openStates.Add(nextState.GetStateCode());
}
}
closedQueue[currentState.GetStateCode()] = currentState;
}
}
// no result
return(null);
}
/// <summary>
/// A* on the graph.
/// </summary>
/// <param name="pathStoreLoc">The Queue to store the path in.</param>
/// <param name="start">The starting node.</param>
/// <param name="end">The ending node.</param>
public double AStar(Queue<Node> pathStoreLoc, Node start, Node end, Node toIgnore = null)
{
MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>();
initializePathfinding(true);
if (toIgnore != null)
toIgnore.Visited = false;
System.Func<Node, Node, float> Heuristic;
if (allowedPaths == Paths.quadDir)
Heuristic = ManhattenHeuristic;
else if (allowedPaths == Paths.octDir)
Heuristic = DiagonalHeuristic;
else
Heuristic = DiagonalHeuristic;
start.CameFrom = null;
start.heuristicCost = Heuristic(start, end);
start.realCost = 0;
nodeList.Enqueue(start, start.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
StringBuilder encountered = new StringBuilder();
StringBuilder nodes = new StringBuilder();
nodes.Append("Start node ").Append(start.Number).AppendLine();
encountered.Append("Start node ").Append(start.Number).AppendLine();
nodes.Append("End node ").Append(end.Number).AppendLine();
encountered.Append("End node ").Append(end.Number).AppendLine();
#endif
while (nodeList.Count > 0)
{
//Pick the best looking node, by f-value.
Node best = nodeList.Dequeue();
double bestDist = best.realCost;
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine();
nodes.Append("Node ").Append(best.Number).AppendLine();
#endif
//If this is the end, stop, show the path, and return it.
if (best.Equals(end))
{
ReconstructPath(pathStoreLoc, end);
ShowPath(pathStoreLoc);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Finished!\n\nFinal dist: ")
.Append(best.realCost).AppendLine();
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return best.realCost;
}
best.Visited = true;
//string updateString = "updating: ";
foreach (Edge e in best.getEdges())
{
Node other = e.getNode();
//We already visited this node, move along,
if (other.Visited)
continue;
//Tentative distance.
double testDist = e.getWeight() + bestDist;
//If the other node isn't in the priority queue, add it.
if (!nodeList.Contains(other))
{
other.CameFrom = best;
other.realCost = testDist;
other.heuristicCost = Heuristic(other, end);
nodeList.Enqueue(other, other.realCost + other.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" added ").Append(other.Number)
.Append(", total estimated cost ")
.Append(other.realCost + other.heuristicCost)
.AppendLine();
#endif
continue;
}
//If the other node was a bad path, and this one's better, replace it.
else if (other.realCost > testDist)
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Update(other, other.realCost + other.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" updated ").Append(other.Number)
.Append(", total new estimated cost ")
.Append(other.realCost + other.heuristicCost)
.AppendLine();
#endif
}
}
}
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Failed!\n");
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return double.PositiveInfinity;
}
/// <summary>
/// Returns a sorted list of nodes within a given endurance value.
/// Performs a Dijkstra-like algorithm.
/// </summary>
/// <param name="satifies">The predicate each node must follow.</param>
/// <param name="endurance">The maximum endurance to follow out.</param>
/// <returns>A sorted list of nodes within a given endurance value.</returns>
public List<Node> nodesThatSatisfyPred(Node startNode, System.Predicate<Node> satifies, float endurance = 16.0f, bool stopOnFirst = false, bool isPathfinding = true)
{
List<Node> foundNodes = new List<Node>();
MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>();
initializePathfinding(isPathfinding);
startNode.realCost = 0;
nodeList.Enqueue(startNode, startNode.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
StringBuilder encountered = new StringBuilder();
StringBuilder nodes = new StringBuilder();
nodes.Append("Start node ").Append(startNode.Number).AppendLine();
encountered.Append("Start node ").Append(startNode.Number).AppendLine();
encountered.Append("endurance = ").Append(endurance).AppendLine();
#endif
while (nodeList.Count > 0)
{
//Pick the best looking node, by f-value.
Node best = nodeList.Dequeue();
double bestDist = best.realCost;
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine();
nodes.Append("Node ").Append(best.Number).AppendLine();
#endif
best.Visited = true;
if (satifies(best))
{
foundNodes.Add(best);
if (stopOnFirst)
return foundNodes;
}
//string updateString = "updating: ";
foreach (Edge e in best.getEdges())
{
Node other = e.getNode();
//We already visited this node, move along,
if (other.Visited)
continue;
//Tentative distance.
double testDist = e.getWeight() + bestDist;
if (testDist > endurance)
continue;
//If the other node isn't in the priority queue, add it.
if (!nodeList.Contains(other))
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Enqueue(other, other.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" added ").Append(other.Number)
.Append(", total estimated cost ")
.Append(other.realCost).AppendLine();
#endif
continue;
}
//If the other node was a bad path, and this one's better, replace it.
else if (other.realCost > testDist)
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Update(other, other.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" updated ").Append(other.Number)
.Append(", total new estimated cost ")
.Append(other.realCost).AppendLine();
#endif
}
}
}
#if DEBUG_PATHFINDER_LOGDEBUG
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return foundNodes;
}
/// <summary>
/// Uses Prim's algorithm to find the minimum spanning tree of the undirected weighted graph. Returns a <see cref="WeightedALGraph{TVertex, TWeight}"/> representing the MST.
/// </summary>
/// <typeparam name="TVertex">The data type of the vertices. TVertex implements <see cref="IComparable{T}"/>.</typeparam>
/// <typeparam name="TWeight">The data type of weight of the edges. TWeight implements <see cref="IComparable{T}"/>.</typeparam>
/// <param name="graph">The graph structure that implements <see cref="IWeightedGraph{TVertex, TWeight}"/>.</param>
/// <returns>Returns a <see cref="WeightedALGraph{TVertex, TWeight}"/> representing the MST.</returns>
public static WeightedALGraph <TVertex, TWeight> PrimMST <TVertex, TWeight>(this IWeightedGraph <TVertex, TWeight> graph)
where TVertex : IComparable <TVertex>
where TWeight : IComparable <TWeight>
{
if (graph.IsDirected)
{
throw new ArgumentException("Graph is directed!");
}
var mst = new WeightedALGraph <TVertex, TWeight>();
// A hash set of the added vertices to the MST
var addedVertices = new HashSet <TVertex>();
// Comparer for the kvp in the priority queue
var kvpComparer = Comparer <KeyValuePair <TWeight, KeyValuePair <TVertex, TVertex> > > .Create((x, y) =>
{
int cmp = 0;
// null checking because TWeight can be null
if (x.Key == null)
{
if (y.Key == null)
{
cmp = -1; // change cmp to skip next comparisons
}
else
{
return(int.MinValue);
}
}
if (cmp == 0)// if x.Key and y.Key are not null
{
if (y.Key == null)
{
return(int.MaxValue);
}
// normal check if both weights are not null
cmp = x.Key.CompareTo(y.Key);
}
else
{
cmp = 0; // if x.Key and y.Key were both null, compare the kvp value
}
if (cmp == 0)
{
cmp = x.Value.Key.CompareTo(y.Value.Key);
}
if (cmp == 0)
{
cmp = x.Value.Value.CompareTo(y.Value.Value);
}
return(cmp);
});
// Priority queue of the edges for computing. Having the weight as key and as value - another kvp with source vertex as key and destination vertex as value.
var priorityQueue = new MinPriorityQueue <TWeight, KeyValuePair <TVertex, TVertex> >(kvpComparer);
var vertices = graph.Vertices.ToList();
if (vertices.Count == 0)
{
return(mst);
}
int verticesIndex = 0;
mst.AddVertices(vertices);
// Add dummy edge to the priority queue
priorityQueue.Enqueue(default(TWeight), new KeyValuePair <TVertex, TVertex>(vertices[verticesIndex], vertices[verticesIndex]));
addedVertices.Add(vertices[verticesIndex]);
while (priorityQueue.Count > 0)
{
var curKVP = priorityQueue.Dequeue();
TVertex curSourceVertex = curKVP.Value.Key;
TVertex curDestinationVertex = curKVP.Value.Value;
// If the destination verex of the edge is not added we add the edge
if (!addedVertices.Contains(curDestinationVertex))
{
mst.AddEdge(curSourceVertex, curDestinationVertex, curKVP.Key);
addedVertices.Add(curDestinationVertex);
}
foreach (var edge in graph.OutgoingEdgesSorted(curDestinationVertex))
{
var edgeDestination = edge.Destination;
var edgeWeight = edge.Weight;
// If the destination vertex is added to the MST we skip it
if (addedVertices.Contains(edgeDestination))
{
continue;
}
// Add the edge for computing
priorityQueue.Enqueue(edgeWeight, new KeyValuePair <TVertex, TVertex>(curDestinationVertex, edgeDestination));
}
// If priority queue is empty
if (priorityQueue.Count == 0)
{
// Check if all vertices of the graph were added to the MST
if (addedVertices.Count != vertices.Count)
{
// If not we have a forest.
// Add the next non added vertex
while (verticesIndex < vertices.Count)
{
if (!addedVertices.Contains(vertices[verticesIndex]))
{
// Add dummy edge to the priority queue
priorityQueue.Enqueue(default(TWeight), new KeyValuePair <TVertex, TVertex>(vertices[verticesIndex], vertices[verticesIndex]));
addedVertices.Add(vertices[verticesIndex]);
break;
}
verticesIndex++;
}
}
}
}
// Return MST
return(mst);
}
public void MergingTwoPriorityQueuesAndCheckingIfExtractedInSortedOrder()
{
var pq1 = new MinPriorityQueue <int, int>();
var pq2 = new MinPriorityQueue <int, int>();
int maxElementInFirstHeap = 100000;
int minElementInFirstHeap = 0;
int addedElementsInFirstHeap = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minElementInFirstHeap;
while (el <= maxElementInFirstHeap)
{
pq1.Enqueue(el, -el);
addedElementsInFirstHeap++;
el += i;
}
}
int maxElementInSecondHeap = 50000;
int minElementInSecondHeap = -50000;
int addedElementsInSecondHeap = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minElementInSecondHeap;
while (el <= maxElementInSecondHeap)
{
pq2.Enqueue(el, -el);
addedElementsInSecondHeap++;
el += i;
}
}
if (pq1.Count != addedElementsInFirstHeap)
{
Assert.Fail("first priority queue incorrect count");
}
if (pq2.Count != addedElementsInSecondHeap)
{
Assert.Fail("second priority queue incorrect count");
}
var oldHeap = new MinPriorityQueue <int, int>();
oldHeap.Heapify(pq1.ToArray());
pq1.Merge(pq2);
pq2.Merge(oldHeap);
int mergedHeapElements = addedElementsInFirstHeap + addedElementsInSecondHeap;
if (pq1.Count != mergedHeapElements)
{
Assert.Fail("merged first with second incorect count");
}
if (pq2.Count != mergedHeapElements)
{
Assert.Fail("merged second with first incorrect count");
}
var min1 = pq1.Peek();
var min2 = pq2.Peek();
if (min1.Key != min2.Key)
{
Assert.Fail("merged priority queues min element is different");
}
int removedElements = 0;
while (!pq1.IsEmpty && !pq2.IsEmpty)
{
var kvp1 = pq1.Peek();
var kvp2 = pq2.Peek();
if (min1.Key > kvp1.Key)
{
Assert.Fail();
}
if (min2.Key > kvp2.Key)
{
Assert.Fail();
}
min1 = pq1.Dequeue();
min2 = pq2.Dequeue();
removedElements++;
if (min1.Key != min2.Key)
{
Assert.Fail("merged priority queues min element is different");
}
}
Assert.IsTrue(pq1.IsEmpty &&
pq2.IsEmpty &&
pq1.Count == 0 &&
pq2.Count == 0 &&
mergedHeapElements == removedElements);
}
public void AddingAfterClearingHeap()
{
var pq = new MinPriorityQueue <int, int>();
int maxHeapElement = 50000;
int minHeapElement = -50000;
int addedElements = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minHeapElement;
while (el <= maxHeapElement)
{
pq.Enqueue(el, -el);
addedElements++;
el += i;
}
}
if (pq.Count != addedElements)
{
Assert.Fail();
}
pq.Clear();
if (pq.Count != 0)
{
Assert.Fail();
}
addedElements = 0;
//Adding every seventh number, then every fifth number,
//every third and at last all numbers
//NOTE: some items are added more than once
for (int i = 7; i > 0; i -= 2)
{
int el = minHeapElement;
while (el < maxHeapElement)
{
pq.Enqueue(el, -el);
addedElements++;
el += i;
}
}
if (pq.Count != addedElements)
{
Assert.Fail();
}
int removedElements = 0;
var min = pq.Peek();
while (!pq.IsEmpty)
{
var kvp = pq.Peek();
if (min.Key > kvp.Key)
{
Assert.Fail();
}
min = pq.Dequeue();
removedElements++;
}
Assert.IsTrue(pq.IsEmpty &&
pq.Count == 0 &&
addedElements == removedElements);
}
public List <String> LocSearch(Point query1, Point query2)
{
MinPriorityQueue <Tuple <double, KDNode> > pq = new MinPriorityQueue <Tuple <double, KDNode> >();
List <String> a = new List <String>();
pq.Enqueue(new Tuple <double, KDNode>(0.0, root));
do
{
var current = pq.Dequeue();
var currentNode = current.Item2;
if (query1.x > query2.x && query1.y > query2.y)
{
if (query1.x >= currentNode.Data.x && query2.x <= currentNode.Data.x && query1.y >= currentNode.Data.y && query2.y <= currentNode.Data.y)
{
a.Add(currentNode.Data.x.ToString());
a.Add(currentNode.Data.y.ToString());
}
}
else if (query1.x < query2.x && query1.y < query2.y)
{
if (query1.x <= currentNode.Data.x && query2.x >= currentNode.Data.x && query1.y <= currentNode.Data.y && query2.y >= currentNode.Data.y)
{
a.Add(currentNode.Data.x.ToString());
a.Add(currentNode.Data.y.ToString());
}
}
else if (query1.x < query2.x && query1.y > query2.y)
{
if (query1.x <= currentNode.Data.x && query2.x >= currentNode.Data.x && query1.y >= currentNode.Data.y && query2.y <= currentNode.Data.y)
{
a.Add(currentNode.Data.x.ToString());
a.Add(currentNode.Data.y.ToString());
}
}
else if (query1.x > query2.x && query1.y < query2.y)
{
if (query1.x >= currentNode.Data.x && query2.x <= currentNode.Data.x && query1.y <= currentNode.Data.y && query2.y >= currentNode.Data.y)
{
a.Add(currentNode.Data.x.ToString());
a.Add(currentNode.Data.y.ToString());
}
}
KDNode near, far;
near = currentNode.Right;
far = currentNode.Left;
if (near != null)
{
pq.Enqueue(new Tuple <double, KDNode>(0, near));
}
if (far != null)
{
pq.Enqueue(new Tuple <double, KDNode>(0, far));
}
} while (pq.Count() != 0);
return(a);
}
protected virtual Stack<PathNode> AStar(Tile source, Tile destination)
{
List<Tile> closed = new List<Tile>();
List<Tile> toDetermine;
List<Tile> toAdd = new List<Tile>();
Stack<PathNode> finalPath = null;
MinPriorityQueue<PathNode> open = new MinPriorityQueue<PathNode>();
List<PathNode> openList = new List<PathNode>();
open.Enqueue(new PathNode(source, destination));
openList.Add(open.Peek());
PathNode currentNode = null;
bool endReached = false;
do
{
currentNode = open.Dequeue();
openList.Remove(currentNode);
if (currentNode.HCost == 0)
{
endReached = true;
finalPath = currentNode.GetPath();
}
else
{
closed.Add(tiles[currentNode.Current.X, currentNode.Current.Y]);
toDetermine = getAdjacent(currentNode);
foreach (Tile t in toDetermine)
{
bool remove = false;
if (t.Collision > source.Unit.Collision)
remove = true;
if (t.HasUnit())
if (t.Unit.Army != source.Unit.Army) //added so that AI works
remove = true;
if (closed.Contains(t))
remove = true;
//Add this if I want to have no duplicate pathnodes (currently,
//multiple exist with different source nodes
/*
PathNode temp = new PathNode(t.GridwiseLocation, currentNode, destination.GridwiseLocation);
foreach (PathNode p in openList)
{
if (p.Current == temp.Current)
{
if (p.GCost > temp.GCost)
{
p.Source = temp.Source;
remove = true;
}
}
}'*/
if (!remove)
toAdd.Add(t);
}
foreach (Tile t in toAdd)
{
PathNode temp = new PathNode(t.GridwiseLocation, currentNode, destination.GridwiseLocation);
open.Enqueue(temp);
}
toAdd.Clear();
}
} while (!endReached);
return finalPath;
}
/// <summary>
/// Uses Dijkstra's algorithm to compute the shortest paths in an unweighted graph. Shortest paths are computed with the distance between the vertices. Returns an <see cref="UnweightedGraphShortestPaths{TVertex}"/> object containg the shortest paths.
/// </summary>
/// <typeparam name="TVertex">The data type of the vertices. TVertex implements <see cref="IComparable{T}"/>.</typeparam>
/// <param name="graph">The graph structure that implements <see cref="IGraph{TVertex}"/>.</param>
/// <param name="source">The source vertex for which the shortest paths are computed.</param>
/// <returns>Returns a <see cref="UnweightedGraphShortestPaths{TVertex}"/> containing the shortest paths for the given source vertex.</returns>
public static UnweightedGraphShortestPaths <TVertex> DijkstraShortestPaths <TVertex>(this IGraph <TVertex> graph, TVertex source)
where TVertex : IComparable <TVertex>
{
if (!graph.ContainsVertex(source))
{
throw new ArgumentException("Vertex does not belong to the graph!");
}
// A dictionary holding a vertex as key and its previous vertex in the path as value.
var previousVertices = new Dictionary <TVertex, TVertex>();
// A dictionary holding a vertex as key and the distance from the source vertex as value.
var pathDistance = new Dictionary <TVertex, int>();
// Comparer for the key-value pair in the sorted set
var kvpComparer = Comparer <KeyValuePair <int, TVertex> > .Create((x, y) =>
{
var cmp = x.Key.CompareTo(y.Key);
if (cmp == 0)
{
cmp = x.Value.CompareTo(y.Value);
}
return(cmp);
});
// Sorted set containing the vertices for computing. Having a kvp with the distance from the source vertex as a key and the vertex as a value
var priorityQueue = new MinPriorityQueue <int, TVertex>(kvpComparer);
// Add the source vertex
priorityQueue.Enqueue(0, source);
while (priorityQueue.Count > 0)
{
var curKVP = priorityQueue.Dequeue();
TVertex curVertex = curKVP.Value;
int curDistance = curKVP.Key;
foreach (var edge in graph.OutgoingEdgesSorted(curVertex))
{
var edgeDestination = edge.Destination;
// If the edge destination is the source we continue with the next edge
if (object.Equals(source, edgeDestination))
{
continue;
}
int newDistance = curDistance + 1;
// If this distance is bigger or equal than an already computed distance we continue with the next edge
if (pathDistance.ContainsKey(edgeDestination))
{
if (newDistance.CompareTo(pathDistance[edgeDestination]) >= 0)
{
continue;
}
}
// Else we save the path
previousVertices[edgeDestination] = curVertex;
pathDistance[edgeDestination] = newDistance;
// Add the destination vertex for computing
priorityQueue.Enqueue(newDistance, edgeDestination);
}
}
var shortestPaths = new UnweightedGraphShortestPaths <TVertex>(source, previousVertices, pathDistance);
return(shortestPaths);
}
public static IEnumerable <Point> FindShortestPath(
Map map, Point source, Point dest, Action <Point, Color> UpdateColor = null)
{
// For each point we keep their distance to the destination.
int[,] dist = new int[map.Height, map.Width];
// For each point we keep the predecessor from source.
Point[,] prev = new Point[map.Height, map.Width];
// Initialize all distance to infinity.
for (int i = 0; i < map.Height; i++)
{
for (int j = 0; j < map.Width; j++)
{
dist[i, j] = int.MaxValue;
}
}
// Set distance of source to 0.
dist[source.Y, source.X] = 0;
// .Net does not provide a priority queue, we emulate
// with a sorted set.
var q = new MinPriorityQueue <Point>();
q.Enqueue(source, 0);
while (prev[dest.Y, dest.X] == null && q.Count != 0)
{
// Get the next element in the queue.
var current = q.Dequeue();
UpdateColor(current, VisitedColor);
// Get all the neighbors of the element.
var neighbors = GetNeighbors(
map.Points, current, map.Height, map.Width);
var currentDist = dist[current.Y, current.X];
var cost = GetCost(current);
foreach (var point in neighbors)
{
// If the distance to the current element + distance to the
// neighbor is less than the distance recorded for neighbor,
// the shortest pass to the neighbor is through the current
// element. We add the neighbor to the queue.
int newDist = currentDist + cost;
if (dist[point.Y, point.X] > newDist)
{
dist[point.Y, point.X] = newDist;
prev[point.Y, point.X] = current;
q.Enqueue(point, newDist);
UpdateColor(point, AddedColor);
}
}
}
// Get the path from dest to source.
if (prev[dest.Y, dest.X] != null)
{
var prevList = new List <Point>();
var current = dest;
do
{
prevList.Add(current);
current = prev[current.Y, current.X];
} while (current != null);
return(prevList);
}
return(Array.Empty <Point>());
}
// run away to the node that's farthest away from all enemies.
private KeyValuePair<Result, UnitAction> RunAway()
{
MinPriorityQueue<UnitAction> farthestActions = new MinPriorityQueue<UnitAction>();
foreach (Node n in pathManager.getAccessibleNodes(subjectRef))
{
int curCount = 0;
foreach (Unit en in subjectsEnemiesRef)
curCount += Node.range(n, en.getNode());
// min priority queue: negate results.
UnitAction runAwayTo = new UnitAction(subjectRef, null, n);
farthestActions.Enqueue(runAwayTo, -curCount);
}
if (farthestActions.Count == 0)
return new KeyValuePair<Result, UnitAction>(Result.NothingFound, UnitAction.DoNothing(subjectRef));
else
return new KeyValuePair<Result, UnitAction>(Result.Success, farthestActions.Dequeue());
}
public int GetLengthOfShortestPath(char source, char destination)
{
CheckIfPathCanExist(source, destination);
var allNodes = GetAllNodes();
var pathIsALoop = source == destination;
var minPathLengthFromSource = new Dictionary <char, int>(MAX_NUMBER_OF_NODES);
var previousNodeOnShortestPathFromSource = new Dictionary <char, char>(MAX_NUMBER_OF_NODES);
var remainingPaths = new MinPriorityQueue <PathFromSource>(MAX_NUMBER_OF_NODES);
var unknownNode = '-';
if (pathIsALoop)
{
destination = DUMMY_NODE_NAME;
allNodes.Add(destination);
}
foreach (var node in allNodes)
{
var initialPathLength = INFINITY;
if (node == source)
{
initialPathLength = 0;
remainingPaths.Enqueue(new PathFromSource(node, initialPathLength));
}
minPathLengthFromSource.Add(node, initialPathLength);
previousNodeOnShortestPathFromSource.Add(node, unknownNode);
}
while (remainingPaths.Count != 0)
{
var currentPath = remainingPaths.Dequeue();
var currentNode = currentPath.Stop;
if (currentPath.Length > minPathLengthFromSource[currentNode] ||
(pathIsALoop && currentNode == destination))
{
continue;
}
var neighbors = GetNeighborsOf(currentNode);
foreach (var neighbor in neighbors)
{
var currentNeighbor = neighbor;
var additionalPathLength = GetLengthOf(currentNode, currentNeighbor);
if (pathIsALoop && currentNeighbor == source)
{
currentNeighbor = destination;
}
if (minPathLengthFromSource[currentNode] + additionalPathLength >= minPathLengthFromSource[currentNeighbor])
{
continue;
}
minPathLengthFromSource[currentNeighbor] = minPathLengthFromSource[currentNode] + additionalPathLength;
previousNodeOnShortestPathFromSource[currentNeighbor] = currentNode;
remainingPaths.Enqueue(new PathFromSource(currentNeighbor, minPathLengthFromSource[currentNeighbor]));
}
}
var lengthOfShortestPath = minPathLengthFromSource[destination];
CheckIfPathLengthIsValid(lengthOfShortestPath);
return(lengthOfShortestPath);
}