public void IncreasingDecreaseKeyCascadeCut()
{
FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; i++)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
count++;
}
int?lastValue = null;
lastValue = heap.Top.Priority;
heap.Dequeue();
count--;
heap.ChangeKey(cells[6], 3);
heap.ChangeKey(cells[7], 2);
foreach (var value in heap.GetDestructiveEnumerator())
{
if (lastValue == null)
{
lastValue = value.Key;
}
Assert.False(lastValue > value.Key);
lastValue = value.Key;
count--;
}
Assert.Equal(0, count);
}
public void IncreasingDecreaseKeyCascadeCut()
{
FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; i++)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
count++;
}
int?lastValue = null;
lastValue = heap.Top.Priority;
heap.Dequeue();
count--;
heap.ChangeKey(cells[6], 3);
heap.ChangeKey(cells[7], 2);
foreach (var value in heap.GetDestructiveEnumerator)
{
if (lastValue == null)
{
lastValue = value.Key;
}
if (lastValue > value.Key)
{
Assert.Fail("Heap condition has been violated");
}
lastValue = value.Key;
count--;
}
Assert.AreEqual(count, 0, "Not all elements enqueued were dequeued");
}
public void IncreasingDecreaseKeyCascadeCut()
{
var heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; ++i)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
++count;
}
int lastValue = heap.Top.Priority;
heap.Dequeue();
--count;
heap.ChangeKey(cells[6], 3);
heap.ChangeKey(cells[7], 2);
foreach (KeyValuePair <int, string> value in heap.GetDestructiveEnumerator())
{
if (lastValue > value.Key)
{
Assert.Fail("Heap condition has been violated.");
}
lastValue = value.Key;
--count;
}
Assert.AreEqual(0, count, "Not all elements enqueued were dequeued.");
}
public void ChangeKeyToSelf()
{
FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Decreasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; i++)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
count++;
}
int?lastValue = null;
heap.ChangeKey(cells[0], 0);
foreach (var value in heap.GetDestructiveEnumerator())
{
if (lastValue == null)
{
lastValue = value.Key;
}
Assert.False(lastValue < value.Key);
lastValue = value.Key;
count--;
}
Assert.Equal(0, count);
}
public void IncreaseKeyOnDecreasing()
{
FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Decreasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; i++)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
count++;
}
int?lastValue = null;
heap.ChangeKey(cells[0], 100);
foreach (var value in heap.GetDestructiveEnumerator())
{
if (lastValue == null)
{
lastValue = value.Key;
}
if (lastValue < value.Key)
{
// Assert.Fail("Heap condition has been violated");
//FIXME TODO
}
lastValue = value.Key;
count--;
}
Assert.Equal(0, count);
}
public void ChangeKeyToSelf()
{
var heap = new FibonacciHeap <int, string>(HeapDirection.Decreasing);
int count = 0;
var cells = new Dictionary <int, FibonacciHeapCell <int, string> >();
for (int i = 0; i < 10; ++i)
{
cells.Add(i, heap.Enqueue(i, i.ToString()));
++count;
}
int?lastValue = null;
heap.ChangeKey(cells[0], 0);
foreach (KeyValuePair <int, string> value in heap.GetDestructiveEnumerator())
{
if (lastValue is null)
{
lastValue = value.Key;
}
if (lastValue < value.Key)
{
Assert.Fail("Heap condition has been violated.");
}
lastValue = value.Key;
--count;
}
Assert.AreEqual(0, count, "Not all elements enqueued were dequeued.");
}
public void NextCutOnGreaterThan()
{
var heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
var heap2 = new FibonacciHeap <int, string>(HeapDirection.Increasing);
var toCutNodes = new List <FibonacciHeapCell <int, string> >();
heap.Enqueue(1, "1");
toCutNodes.Add(heap.Enqueue(5, "5"));
toCutNodes.Add(heap.Enqueue(6, "6"));
toCutNodes.Add(heap.Enqueue(7, "7"));
heap.Enqueue(-10, "-10");
heap.Dequeue();
heap.Enqueue(0, "0");
heap2.Enqueue(-1, "-1");
heap2.Enqueue(5, "5");
heap2.Enqueue(-10, "-10");
heap2.Dequeue();
heap.Merge(heap2);
heap.Enqueue(-10, "-10");
heap.Dequeue();
toCutNodes.ForEach(x => heap.ChangeKey(x, -5));
heap.Enqueue(-10, "-10");
heap.Dequeue();
int count = 7;
int?lastValue = null;
foreach (KeyValuePair <int, string> value in heap.GetDestructiveEnumerator())
{
if (lastValue is null)
{
lastValue = value.Key;
}
if (lastValue > value.Key)
{
Assert.Fail("Heap condition has been violated.");
}
lastValue = value.Key;
--count;
}
Assert.AreEqual(0, count, "Not all elements enqueued were dequeued.");
}
public void NextCutOnGreaterThan()
{
var heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
var heap2 = new FibonacciHeap <int, string>(HeapDirection.Increasing);
var toCutNodes = new List <FibonacciHeapCell <int, string> >();
int count = 0;
heap.Enqueue(1, "1");
toCutNodes.Add(heap.Enqueue(5, "5"));
toCutNodes.Add(heap.Enqueue(6, "6"));
toCutNodes.Add(heap.Enqueue(7, "7"));
heap.Enqueue(-10, "-10");
heap.Dequeue();
heap.Enqueue(0, "0");
heap2.Enqueue(-1, "-1");
heap2.Enqueue(5, "5");
heap2.Enqueue(-10, "-10");
heap2.Dequeue();
heap.Merge(heap2);
heap.Enqueue(-10, "-10");
heap.Dequeue();
toCutNodes.ForEach(x => heap.ChangeKey(x, -5));
heap.Enqueue(-10, "-10");
heap.Dequeue();
count = 7;
int?lastValue = null;
foreach (var value in heap.GetDestructiveEnumerator())
{
if (lastValue == null)
{
lastValue = value.Key;
}
Assert.False(lastValue > value.Key);
lastValue = value.Key;
count--;
}
Assert.Equal(0, count);
}
public void FibbonaciJavaPerformance()
{
var fib = new FibonacciHeap <double, string>();
Assert.IsTrue(fib.Count == 0);
var rnd = new Random(0);
var ent = new FibonacciHeapHeapCell <double, string> [runs];
for (int i = 0; i < runs; i++)
{
ent[i] = fib.Enqueue(rnd.NextDouble(), "x");
}
for (int i = 0; i < runs / 2; i++)
{
var nd = rnd.NextDouble();
fib.ChangeKey(ent[i], nd);
}
for (int i = 0; i < runs / 2; i++)
{
ent[i] = fib.Enqueue(rnd.NextDouble(), "x");
fib.Dequeue();
}
Assert.IsTrue(fib.Count == runs);
for (int i = 0; i < runs; i++)
{
Assert.IsTrue(fib.Dequeue().Value == "x");
}
Assert.IsTrue(fib.Count == 0);
}
public void RandomTest()
{
FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing);
Random rand = new Random(10);
int NumberOfRecords = 10000;
int RangeMultiplier = 10;
int count = 0;
var cells = new List <FibonacciHeapCell <int, string> >();
for (int i = 0; i < NumberOfRecords; i++)
{
cells.Add(heap.Enqueue(rand.Next(0, NumberOfRecords * RangeMultiplier), i.ToString()));
count++;
}
while (!heap.IsEmpty)
{
int action = rand.Next(1, 6);
int i = 0;
while (action == 1 && i < 2)
{
action = rand.Next(1, 6);
i++;
}
int lastValue = int.MinValue;
switch (action)
{
case 1:
cells.Add(heap.Enqueue(rand.Next(0, NumberOfRecords * RangeMultiplier), "SomeValue"));
count++;
break;
case 2:
Assert.IsFalse(lastValue > heap.Top.Priority, "Heap Condition Violated");
lastValue = heap.Top.Priority;
cells.Remove(heap.Top);
heap.Dequeue();
count--;
break;
case 3:
int deleteIndex = rand.Next(0, cells.Count);
heap.Delete(cells[deleteIndex]);
cells.RemoveAt(deleteIndex);
count--;
break;
case 4:
int decreaseIndex = rand.Next(0, cells.Count);
int newValue = rand.Next(0, cells[decreaseIndex].Priority);
if (newValue < lastValue)
{
lastValue = newValue;
}
heap.ChangeKey(cells[decreaseIndex], newValue);
break;
case 5:
int increaseIndex = rand.Next(0, cells.Count);
heap.ChangeKey(cells[increaseIndex], rand.Next(cells[increaseIndex].Priority, NumberOfRecords * RangeMultiplier));
break;
default:
break;
}
}
Assert.AreEqual(count, 0, "Not all elements enqueued were dequeued");
}
/// <summary>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime"></param>
/// <param name="agents">agents</param>
/// <param name="obstacleNodes">The way points of the obstacles.</param>
/// <param name="lockedNodes"></param>
/// <param name="nextReoptimization">The next re-optimization time.</param>
/// <exception cref="System.NotImplementedException"></exception>
public override void FindPaths(double currentTime, List <Elements.Agent> agents)
{
Stopwatch.Restart();
//Reservation Table
_reservationTable.Clear();
//get fixed Blockage
var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);
foreach (var agent in agents)
{
_reservationTable.Add(fixedBlockage[agent], agent.ID, MaxPriorities + 1);
}
//initial agent times
var agentTimesHeap = new FibonacciHeap <double, Agent>(HeapDirection.Increasing);
var agentTimesDict = agents.Where(a => !a.FixedPosition).ToDictionary(a => a, a => agentTimesHeap.Enqueue(a.ArrivalTimeAtNextNode, a));
//initial agent nodes
var agentPrios = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => 0);
var agentNodes = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => 0);
var agentReservations = agents.Where(a => !a.FixedPosition).ToDictionary(a => a.ID, a => new List <ReservationTable.Interval>());
//initiate action generator
var actionGenerator = new Dictionary <int, PASActionGenerator>();
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
//Create RRA* search if necessary.
//Necessary if the agent has none or the agents destination has changed
ReverseResumableAStar rraStar;
if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar.StartNode != agent.DestinationNode)
{
rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
}
actionGenerator.Add(agent.ID, new PASActionGenerator(Graph, LengthOfAWaitStep, agent, rraStars[agent.ID]));
}
//action sorter
var actionSorter = new FibonacciHeap <double, Tuple <int, List <ReservationTable.Interval>, List <Collision> > >(HeapDirection.Increasing);
//loop
double cancelAt = currentTime + LengthOfAWindow;
while (agentTimesHeap.Count > 0 && Stopwatch.ElapsedMilliseconds / 1000.0 < RuntimeLimitPerAgent * agents.Count * 0.9 && Stopwatch.ElapsedMilliseconds / 1000.0 < RunTimeLimitOverall)
{
//pick the agent that has the smallest time
if (cancelAt <= agentTimesHeap.Top.Priority)
{
break;
}
var currentAgent = agentTimesHeap.Top.Value;
var currentActionGenerator = actionGenerator[currentAgent.ID];
var currentAgentNode = agentNodes[currentAgent.ID];
var currentAgentReservations = agentReservations[currentAgent.ID];
//if (currentAgent.ID == 41)
// currentAgent.ID = currentAgent.ID;
var reservationSuccessfull = false;
//initiate sorter
while (actionSorter.Count > 0)
{
actionSorter.Dequeue();
}
//get and sort actions
var actions = currentActionGenerator.GetActions(currentAgentNode, agentPrios[currentAgent.ID], _reservationTable);
var allInfinity = actions.All(a => double.IsInfinity(currentActionGenerator.h(currentAgentNode, a.Item1))) && actions.Count > 1;
foreach (var action in actions)
{
actionSorter.Enqueue(currentActionGenerator.g(action.Item1) + currentActionGenerator.h(currentAgentNode, action.Item1, allInfinity), action);
}
//try to reserve
while (actionSorter.Count > 0)
{
var actionNode = actionSorter.Top.Value.Item1;
var actionReservatios = actionSorter.Top.Value.Item2;
var actionCollisions = actionSorter.Top.Value.Item3;
actionSorter.Dequeue();
//reservation possible?
if (actionCollisions == null || actionCollisions.All(c => c.priority < agentPrios[currentAgent.ID] || c.agentId == currentAgent.ID))
{
//delete other reservations
if (actionCollisions != null)
{
//delete own reservations till last turn node
while (currentAgentReservations.Count > 0 &&
currentAgentReservations.Last().Start >= currentActionGenerator.NodeTime[currentActionGenerator.NodeBackpointerLastStopId[actionNode]] - ReservationTable.TOLERANCE)
{
_reservationTable.Remove(currentAgentReservations.Last());
currentAgentReservations.RemoveAt(currentAgentReservations.Count - 1);
}
//delete other reservations
foreach (var collsion in actionCollisions.Where(c => c.agentId != currentAgent.ID))
{
var nodeToSetBackToPast = agentNodes[collsion.agentId];
var reservationToSetBackToPast = agentReservations[collsion.agentId];
setAgentBackToPast(collsion.agentId, actionGenerator[collsion.agentId], ref nodeToSetBackToPast, ref reservationToSetBackToPast, collsion.time);
agentNodes[collsion.agentId] = nodeToSetBackToPast;
agentReservations[collsion.agentId] = reservationToSetBackToPast; //note: I know - it is only a reference to the list => but for clearance
}
}
//reserve me
_reservationTable.Add(actionReservatios, currentAgent.ID, agentPrios[currentAgent.ID]);
currentAgentReservations.AddRange(actionReservatios);
//set my node
currentAgentNode = agentNodes[currentAgent.ID] = actionNode;
//reached destination?
if (currentActionGenerator.NodeTo2D(currentAgentNode) == currentAgent.DestinationNode)
{
//Here the reason of commenting: Only 2 or 3 Nodes will be checked by reservation table, the rest will be added blind. If there are 10 Nodes in the hop, 7 reservations will possibly rejected. So the whole transfer will be rejected.
//cancelAt = Math.Min(cancelAt,currentActionGenerator.NodeTime[currentAgentNode]);
agentTimesHeap.Dequeue();
if (_reservationTable.IntersectionFree(currentActionGenerator.NodeTo2D(currentAgentNode), currentActionGenerator.NodeTime[currentAgentNode], double.PositiveInfinity))
{
_reservationTable.Add(currentActionGenerator.NodeTo2D(currentAgentNode), currentActionGenerator.NodeTime[currentAgentNode], double.PositiveInfinity, currentAgent.ID, MaxPriorities + 1);
}
}
else
{
//set the time and node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], actionReservatios.Last().End);
}
//reservation successful
reservationSuccessfull = true;
break;
}
}
//could not find an action
if (reservationSuccessfull)
{
agentPrios[currentAgent.ID] = 0;
}
else
{
agentPrios[currentAgent.ID]++;
//wait step
var waitNode = currentActionGenerator.GenerateWaitNode(currentAgentNode, _reservationTable);
if (agentPrios[currentAgent.ID] < MaxPriorities)
{
if (waitNode.Item3 == null || waitNode.Item3.All(c => c.priority < agentPrios[currentAgent.ID]))
{
//delete other reservations
if (waitNode.Item3 != null)
{
foreach (var collsion in waitNode.Item3)
{
//reset agent moves
var nodeToSetBackToPast = agentNodes[collsion.agentId];
var reservationToSetBackToPast = agentReservations[collsion.agentId];
setAgentBackToPast(collsion.agentId, actionGenerator[collsion.agentId], ref nodeToSetBackToPast, ref reservationToSetBackToPast, collsion.time);
agentNodes[collsion.agentId] = nodeToSetBackToPast;
agentReservations[collsion.agentId] = reservationToSetBackToPast; //note: I know - it is only a reference to the list => but for clearance
}
}
//reserve me
currentAgentReservations.AddRange(waitNode.Item2);
_reservationTable.Add(waitNode.Item2, currentAgent.ID, agentPrios[currentAgent.ID]);
//set next node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], waitNode.Item2.Last().End);
currentAgentNode = agentNodes[currentAgent.ID] = waitNode.Item1;
}
}
else
{
//no reservation
agentPrios[currentAgent.ID] = 0;
//set next node
agentTimesHeap.ChangeKey(agentTimesDict[currentAgent], waitNode.Item2.Last().End);
currentAgentNode = agentNodes[currentAgent.ID] = waitNode.Item1;
}
}
}//agent pick loop
// Signal potential timeout
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
}
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
agent.Path = agent.Path ?? new Path();
//+ WHCA* Nodes
List <ReservationTable.Interval> reservations;
actionGenerator[agent.ID].GetPathAndReservations(ref agent.Path, out reservations, agentNodes[agent.ID], 0.0);
//+ RRA* Nodes
rraStars[agent.ID].addPath(agent.Path, actionGenerator[agent.ID].NodeTo2D(agentNodes[agent.ID]));
//add the next node again
if (fixedBlockage.Count > 0 && (agent.Path.Count == 0 || agent.Path.NextAction.Node != agent.NextNode || agent.Path.NextAction.StopAtNode == false))
{
agent.Path.AddFirst(agent.NextNode, true, 0);
}
//next time ready?
if (agent.Path.Count == 0)
{
rraStars[agent.ID] = null;
}
}
}
/// <summary>
/// Executes the search.
/// </summary>
/// <returns>found node</returns>
public virtual bool Search()
{
//local data
double gPrimeSuccessor;
double hSuccessor;
bool successorInOpen;
bool successorInClosed;
//open is not empty
while (Open.Count > 0)
{
//get n with lowest value
int n = Q.Dequeue().Value;
//set on closed
Open.Remove(n);
Closed.Add(n);
//get successor
foreach (var successor in Successors(n))
{
//f(successor) = g'(successor) + h(successor)
gPrimeSuccessor = gPrime(n, successor);
hSuccessor = h(successor);
//reject no solution nodes
if (hSuccessor == double.PositiveInfinity)
{
continue;
}
//precompute the contains
successorInOpen = Open.ContainsKey(successor);
successorInClosed = Closed.Contains(successor);
if (!successorInOpen && !successorInClosed)
{
//node is new
Open.Add(successor, Q.Enqueue(gPrimeSuccessor + hSuccessor, successor));
setBackPointer(n, successor, gPrimeSuccessor, hSuccessor);
}
else
{
if (successorInOpen && Open[successor].Priority > gPrimeSuccessor + hSuccessor) //smaller f-Value implies smaller g-Value
{
//node has better f-Value
//Open[successor].Priority = gPrimeSuccessor + hSuccessor;
Q.ChangeKey(Open[successor], gPrimeSuccessor + hSuccessor);
setBackPointer(n, successor, gPrimeSuccessor, hSuccessor);
}
}
}
//any stop condition
if (StopCondition(n))
{
return(true);
}
}
return(false);
}
