public void DecreasePriorityTest()
{
// Heap
FibonacciHeap <float, char> heap = new FibonacciHeap <float, char>();
// Add collection of nodes
HeapNode <float, char> node2 = heap.Insert(7, 'b');
HeapNode <float, char> node1 = heap.Insert(3, 'a');
HeapNode <float, char> node3 = heap.Insert(14, 'c');
HeapNode <float, char> node4 = heap.Insert(35, 'd');
// Decrease priority of node
heap.DecreasePriority(node4, 1);
// Check minimum is correct, and contains correct values
Assert.AreEqual(heap.Minimum.Priority, 1);
Assert.AreEqual(heap.Minimum.Value, 'd');
Assert.AreEqual(heap.Minimum, node4);
Assert.AreEqual(heap.Count, 4);
// Delete min so trees are consolidated
heap.DeleteMin();
// Decrease priority of node
heap.DecreasePriority(node2, 0);
// Check minimum is correct, and contains correct values
Assert.AreEqual(heap.Minimum, node2);
Assert.AreEqual(heap.Minimum.Priority, 0);
Assert.AreEqual(heap.Minimum.Value, 'b');
Assert.AreEqual(heap.Count, 3);
}
public void DeleteMinTest()
{
// Heap
FibonacciHeap <float, char> heap = new FibonacciHeap <float, char>();
// Add collection of nodes
HeapNode <float, char> node2 = heap.Insert(7, 'b');
HeapNode <float, char> node1 = heap.Insert(3, 'a');
HeapNode <float, char> node3 = heap.Insert(14, 'c');
HeapNode <float, char> node4 = heap.Insert(35, 'd');
// Check minimum is correct, and contains correct values
Assert.AreEqual(heap.Minimum, node1);
Assert.AreEqual(heap.Minimum.Priority, 3);
Assert.AreEqual(heap.Minimum.Value, 'a');
Assert.AreEqual(heap.Count, 4);
// Remove minimum element
heap.DeleteMin();
// Check minimum is correct, and contains correct values
Assert.AreEqual(heap.Minimum, node2);
Assert.AreEqual(heap.Minimum.Priority, 7);
Assert.AreEqual(heap.Minimum.Value, 'b');
Assert.AreEqual(heap.Count, 3);
}
static void Main(string[] args)
{
var heap = new FibonacciHeap<int, int>();
heap.Insert(3, 3);
heap.Insert(8, 8);
heap.Insert(9, 9);
heap.Insert(11, 11);
heap.Insert(14, 14);
heap.Insert(38, 38);
heap.Insert(38, 49);
heap.Insert(3, 56);
heap.Insert(38, 18);
heap.Insert(38, 17);
heap.Insert(3, 4);
//heap.Insert(3, 3);
//heap.Insert(8, 8);
//heap.Insert(9, 9);
//heap.Insert(11, 11);
//heap.Insert(14, 14);
//heap.Insert(38, 38);
//heap.Insert(49, 49);
//heap.Insert(56, 56);
//heap.Insert(18, 18);
//heap.Insert(17, 17);
//heap.Insert(4, 4);
int len = heap.Count;
for (int i = 0; i < len; i++)
{
var min = heap.DeleteMin();
Console.WriteLine(min);
}
}
protected virtual Frame GetNearestNeighbourAssignment(Frame lastFrame, Frame currentFrame,
KdTree kdTreeCurrentTouches)
{
Frame trackedFrame = new Frame(currentFrame.TimeStamp);
int fibHeapCount = 0;
fibHeap.Initialize(MAXBLOBS);
foreach (var lastTouch in lastFrame)
{
IVertex lastVertex = new Vertex(lastTouch.X, lastTouch.Y);
int nn = kdTreeCurrentTouches.TopDownNearestNeighbour(lastVertex);
Touch currentTouch = currentFrame.GetTouch(nn);
IVertex currentVertex = new Vertex(currentTouch.X, currentTouch.Y);
lastToCurrentAssignment[lastTouch.ID] = nn;
fibHeap.InsertKey(lastTouch.ID, MetricDistances.EuclideanDistance(
lastVertex, currentVertex));
fibHeapCount++;
}
int min;
int tempCounter = 0;
while ((min = fibHeap.Minimum) > -1 && (min = fibHeap.DeleteMin()) != -1)
{
tempCounter++;
if (lastFrame.GetTouch(min) == null)
{
}
if (!kdTreeCurrentTouches.Included(lastToCurrentAssignment[min]))
{
Touch lastTouch = lastFrame.GetTouch(min);
IVertex lastVertex = new Vertex(lastTouch.X, lastTouch.Y);
int nn = kdTreeCurrentTouches.TopDownNearestNeighbour(lastVertex);
Touch currentTouch = currentFrame.GetTouch(nn);
if (currentTouch == null) // trajectory ended in last frame
{
return(trackedFrame); // if no radii search there is no point left to connect
}
else
{
IVertex currentVertex = new Vertex(currentTouch.X, currentTouch.Y);
lastToCurrentAssignment[lastTouch.ID] = currentTouch.ID;
fibHeap.InsertKey(lastTouch.ID, MetricDistances.EuclideanDistance(
lastVertex, currentVertex));
}
continue;
}
Touch t = currentFrame.GetTouch(lastToCurrentAssignment[min]);
kdTreeCurrentTouches.Delete(lastToCurrentAssignment[min]);
Touch newTouch = new Touch(min, t.X, t.Y, t.DimX, t.DimY, t.Intense);
trackedFrame.AddTouch(newTouch);
}
return(trackedFrame);
}
private void ProcessData(Tuple <Stack <byte>, Stack <Stack <int> > > data)
{
var sw = Stopwatch.StartNew();
var commands = data.Item1;
var arguments = data.Item2;
int argOffset = 0;
int listOffset = 0;
// Prepare the heap
var heap = new FibonacciHeap <int, int>();
int insertCount = GetNextArg(ref argOffset, arguments, ref listOffset);
NodeItem <int, int>[] insertedNodes = new NodeItem <int, int> [insertCount];
int deleteDepthCount = 0;
int deleteCount = 0;
// Do insert commands
int id = 0, key = 0;
NodeItem <int, int> node;
foreach (byte command in commands)
{
if (command != DelKey)
{
id = GetNextArg(ref argOffset, arguments, ref listOffset);
key = GetNextArg(ref argOffset, arguments, ref listOffset);
}
switch (command)
{
case InsKey:
node = heap.Add(key, id);
Debug.Assert(insertedNodes[id] == null);
insertedNodes[id] = node;
break;
case DelKey:
node = heap.PeekMin();
Debug.Assert(insertedNodes[node.Value] != null);
insertedNodes[node.Value] = null;
heap.DeleteMin();
deleteDepthCount += heap.LastConsolidateDepth;
deleteCount++;
break;
case DecKey:
node = insertedNodes[id];
if (node == null || key > node.Key)
{
break;
}
heap.DecreaseKey(node, key);
break;
}
}
// Cleanup and store the measurements
//heap.Clear(); // Disassembles tree node pointers .... not a good idea with a GC...
sw.Stop();
float avgDeleteDepthCount = 0;
if (deleteCount > 0)
{
avgDeleteDepthCount = deleteDepthCount / (float)deleteCount;
}
lock (_results)
_results.Add(deleteCount, avgDeleteDepthCount);
Interlocked.Increment(ref _currentJobsDone);
Log("{0}/{1} done/waiting :: {2:F} sec :: {3} inserts :: {4}/{5:F} deletes/delete depth average",
_currentJobsDone,
Buffer.WaitingItemCount,
sw.ElapsedMilliseconds * 0.001,
insertCount,
deleteCount,
avgDeleteDepthCount);
}
/// <summary>
/// Computes the most efficient path in the specified graph between the
/// specified pathfinding nodes using the A* algorithm.
/// </summary>
/// <param name="graph">Pathfinding graph to look at.</param>
/// <param name="start">Starting node.</param>
/// <param name="finish">Finish node.</param>
/// <returns>
/// List of pathfinding nodes representing the shortest path, if there is one,
/// and null otherwise.
/// </returns>
/// <typeparam name="T">Type of the pathfinding nodes.</typeparam>
/// <exception cref="ArgumentNullException">
/// Passed graph or start or finish node is <c>null</c>.
/// </exception>
public static List <T> FindPath <T>(IWeightedGraph <T> graph, T start, T finish) where T : IAStarNode
{
if (graph == null)
{
throw new ArgumentNullException("graph", "The passed pathfinding graph mustn't be null.");
}
if (start == null)
{
throw new ArgumentNullException("start", "The passed start node mustn't be null.");
}
if (finish == null)
{
throw new ArgumentNullException("finish", "The passed finish mustn't be null.");
}
// --- Initialization ---
// Initialize variables to check for the algorithm to terminate.
bool algorithmComplete = false;
bool algorithmAborted = false;
// Initialize list to choose the next node of the path from.
IPriorityQueue <T> openList = new FibonacciHeap <T>();
IPriorityQueueItem <T>[] fibHeapItems = new FibonacciHeapItem <T> [graph.VertexCount];
HashSet <T> dirtyNodes = new HashSet <T>();
// Initialize queue to hold the nodes along the path to the finish.
Queue <T> closedList = new Queue <T>();
// Declare current node to work on in order to calculate the path.
// Declare list to hold the neighbors of the current node.
// Add starting node to open list.
fibHeapItems[start.Index] = openList.Insert(start, 0);
dirtyNodes.Add(start);
start.Discovered = true;
// --- A* Pathfinding Algorithm ---
while ((!algorithmComplete) && (!algorithmAborted))
{
// Get the node with the lowest F score in the open list.
T currentNode = openList.DeleteMin().Item;
// Drop that node from the open list and add it to the closed list.
closedList.Enqueue(currentNode);
currentNode.Visited = true;
// We're done if the target node is added to the closed list.
if (currentNode.Equals(finish))
{
algorithmComplete = true;
break;
}
// Otherwise, get all adjacent nodes.
List <T> neighbors = graph.ListOfAdjacentVertices(currentNode);
// Add all nodes that aren't already on the open or closed list to the open list.
foreach (T node in neighbors)
{
// Ignore nodes in the closed list.
if (!node.Visited)
{
if (!node.Discovered)
{
// The parent node is the previous node on the path to the finish.
node.ParentNode = currentNode;
// The G score of the node is calculated by adding the G score
// of the parent node and the movement cost of the path between
// the node and the current node.
// In other words: The G score of the node is the total cost of the
// path between the starting node and this one.
node.G = node.ParentNode.G + graph.GetEdgeWeight(node, (T)node.ParentNode);
// The H score of the node is calculated by heuristically
// estimating the movement cost from the node to the finish.
// In other words: The H score of the node is the total remaining
// cost of the path between this node and the finish.
node.H = node.EstimateHeuristicMovementCost(finish);
// The F score of the node is calculated by adding the G and H scores.
// In other words: The F score is an indicator that tells whether this
// node should be crossed on the path to the finish, or not.
node.F = node.G + node.H;
// Add to open list.
fibHeapItems[node.Index] = openList.Insert(node, node.F);
dirtyNodes.Add(node);
node.Discovered = true;
}
else
{
// Node is already in open list!
// Check if the new path to this node is a better one.
if (currentNode.G + graph.GetEdgeWeight(node, currentNode) < node.G)
{
// G cost of new path is lower!
// Change parent node to current node.
node.ParentNode = currentNode;
// Recalculate F and G costs.
node.G = node.ParentNode.G + graph.GetEdgeWeight(node, (T)node.ParentNode);
node.F = node.G + node.H;
openList.DecreaseKeyTo(fibHeapItems[node.Index], node.F);
}
}
}
}
// We've failed to find a path if the open list is empty.
if (openList.IsEmpty())
{
algorithmAborted = true;
}
}
// Return the path to the finish, if there is one.
if (algorithmComplete)
{
// Generate path through parent pointers using a stack.
Stack <T> s = new Stack <T>();
T node = finish;
while (!node.Equals(start))
{
s.Push(node);
node = (T)node.ParentNode;
}
// Generate path in right order.
List <T> path = new List <T>();
while (s.Count > 0)
{
path.Add(s.Pop());
}
// Cleanup pathing.
foreach (T dirtyNode in dirtyNodes)
{
dirtyNode.Reset();
}
return(path);
}
else
{
// Cleanup pathing.
foreach (T dirtyNode in dirtyNodes)
{
dirtyNode.Reset();
}
return(null);
}
}