public void InsertAndRemoveMinimum <TPriority, TValue>(
[PexAssumeUnderTest] FibonacciHeap <TPriority, TValue> target,
[PexAssumeNotNull] KeyValuePair <TPriority, TValue>[] kvs)
{
var count = target.Count;
foreach (var kv in kvs)
{
target.Enqueue(kv.Key, kv.Value);
}
TPriority minimum = default(TPriority);
for (int i = 0; i < kvs.Length; ++i)
{
if (i == 0)
{
minimum = target.Dequeue().Key;
}
else
{
var m = target.Dequeue().Key;
Assert.IsTrue(target.PriorityComparison(minimum, m) <= 0);
minimum = m;
}
AssertInvariant(target);
}
Assert.AreEqual(0, target.Count);
}
public void IncreasingIncreaseKeyCascadeCut()
{
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[5], 10);
string s = (heap as FibonacciHeap <int, string>).DrawHeap();
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 IncreasingIncreaseKeyCascadeCut()
{
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[5], 10);
string s = (heap as FibonacciHeap <int, string>).DrawHeap();
foreach (var value in heap.GetDestructiveEnumerator)
{
if (lastValue == null)
{
lastValue = value.Key;
}
if (lastValue > value.Key)
{
Assert.Fail("Heap condition has been violated: {0} > {1}", lastValue, value.Key);
}
lastValue = value.Key;
count--;
}
Assert.AreEqual(count, 0, "Not all elements enqueued were dequeued");
}
public void DeleteKey()
{
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;
heap.Dequeue();
var DeletedCell = cells[8];
heap.Delete(DeletedCell);
count -= 2;
foreach (var value in heap.GetDestructiveEnumerator)
{
if (lastValue == null)
{
lastValue = value.Key;
}
Assert.IsFalse(lastValue > value.Key, "Heap condition has been violated");
Assert.AreNotEqual(DeletedCell, value, "Found item that was deleted");
lastValue = value.Key;
count--;
}
Assert.AreEqual(count, 0, "Not all elements enqueued were dequeued");
}
public void DeleteKey()
{
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;
heap.Dequeue();
var DeletedCell = cells[8];
heap.Delete(DeletedCell);
count -= 2;
foreach (var value in heap.GetDestructiveEnumerator())
{
if (lastValue == null)
{
lastValue = value.Key;
}
Assert.False(lastValue > value.Key);
//TODO FIXME Assert.NotEqual(DeletedCell, value);
lastValue = value.Key;
count--;
}
Assert.Equal(0, count);
}
public void IncreasingIncreaseKeyCascadeCut()
{
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[5], 10);
foreach (KeyValuePair <int, string> value in heap.GetDestructiveEnumerator())
{
if (lastValue > value.Key)
{
Assert.Fail($"Heap condition has been violated: {lastValue} > {value.Key}");
}
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 FibbonaciJavaCorrectness()
{
var fib = new FibonacciHeap <double, string>();
Assert.IsTrue(fib.Count == 0);
fib.Enqueue(2, "find");
fib.Enqueue(3, "das");
fib.Enqueue(1, "ich");
fib.Enqueue(4, "sehr");
fib.Enqueue(6, "gut");
Assert.IsTrue(fib.Count == 5);
Assert.IsTrue(fib.Dequeue().Value == "ich");
Assert.IsTrue(fib.Dequeue().Value == "find");
Assert.IsTrue(fib.Dequeue().Value == "das");
Assert.IsTrue(fib.Dequeue().Value == "sehr");
Assert.IsTrue(fib.Dequeue().Value == "gut");
Assert.IsTrue(fib.Count == 0);
}
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);
}
private bool ValidatePath(ConflictTree.Node node, List <Agent> agents, out int agentId1, out int agentId2, out ReservationTable.Interval interval)
{
//clear
_agentReservationTable.Clear();
//add next hop reservations
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
_agentReservationTable.Add(agent.ReservationsToNextNode, agent.ID);
}
//get all reservations sorted
var reservations = new FibonacciHeap <double, Tuple <Agent, ReservationTable.Interval> >();
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
foreach (var reservation in node.getReservation(agent.ID))
{
reservations.Enqueue(reservation.Start, Tuple.Create(agent, reservation));
}
}
//check all reservations
while (reservations.Count > 0)
{
var reservation = reservations.Dequeue().Value;
int collideWithAgentId;
var intersectionFree = _agentReservationTable.IntersectionFree(reservation.Item2, out collideWithAgentId);
if (!intersectionFree)
{
agentId1 = collideWithAgentId;
agentId2 = reservation.Item1.ID;
interval = _agentReservationTable.GetOverlappingInterval(reservation.Item2);
if (interval.End - interval.Start > ReservationTable.TOLERANCE)
{
return(false);
}
}
else
{
_agentReservationTable.Add(reservation.Item2, reservation.Item1.ID);
}
}
agentId1 = -1;
agentId2 = -1;
interval = null;
return(true);
}
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 Operations <TPriority, TValue>(
[PexAssumeUnderTest] FibonacciHeap <TPriority, TValue> target,
[PexAssumeNotNull] KeyValuePair <bool, TPriority>[] values)
{
foreach (var value in values)
{
if (value.Key)
{
target.Enqueue(value.Value, default(TValue));
}
else
{
var min = target.Dequeue();
}
AssertInvariant <TPriority, TValue>(target);
}
}
private void Operations <TPriority, TValue>(
FibonacciHeap <TPriority, TValue> target,
KeyValuePair <bool, TPriority>[] values)
{
foreach (var value in values)
{
if (value.Key)
{
target.Enqueue(value.Value, default(TValue));
}
else
{
var min = target.Dequeue();
}
AssertInvariant <TPriority, TValue>(target);
}
}
public void CompareBinary <TPriority, TValue>(
[PexAssumeNotNull] KeyValuePair <bool, TPriority>[] values)
{
var fib = new FibonacciHeap <TPriority, TValue>();
var bin = new BinaryHeap <TPriority, TValue>();
foreach (var value in values)
{
if (value.Key)
{
PexAssert.AreBehaviorsEqual(
() => fib.Enqueue(value.Value, default(TValue)),
() => bin.Add(value.Value, default(TValue))
);
}
else
{
PexAssert.AreBehaviorsEqual(
() => fib.Dequeue().Key,
() => bin.RemoveMinimum().Key
);
}
}
}
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>
/// 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);
}
/// <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>
/// Find the path for all the agents.
/// </summary>
/// <param name="currentTime">The current Time.</param>
/// <param name="agents">agents</param>
public override void FindPaths(double currentTime, List <Agent> agents)
{
Stopwatch.Restart();
//initialization data structures
var conflictTree = new ConflictTree();
var Open = new FibonacciHeap <double, ConflictTree.Node>();
var solvable = true;
var generatedNodes = 0;
ConflictTree.Node bestNode = null;
double bestTime = 0.0;
//deadlock handling
_deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
_deadlockHandler.MaximumWaitTime = 30;
_deadlockHandler.Update(agents, currentTime);
//simply blocked
foreach (var agent in agents.Where(a => a.FixedPosition))
{
Graph.NodeInfo[agent.NextNode].IsLocked = true;
}
// TODO this only works as long as a possible solution is guaranteed - maybe instead ignore paths to plan for agents with no possible solution and hope that it clears by others moving on?
//first node initialization
List <Agent> unsolvableAgents = null;
foreach (var agent in agents.Where(a => !a.FixedPosition))
{
bool agentSolved = Solve(conflictTree.Root, currentTime, agent);
if (!agentSolved)
{
if (unsolvableAgents == null)
{
unsolvableAgents = new List <Agent>()
{
agent
}
}
;
else
{
unsolvableAgents.Add(agent);
}
}
solvable = solvable && agentSolved;
}
//node selection strategy (Queue will pick the node with minimum value
NodeSelectionExpression nodeObjectiveSelector = node =>
{
switch (SearchMethod)
{
case CBSSearchMethod.BestFirst:
return(node.SolutionCost);
case CBSSearchMethod.BreathFirst:
return(node.Depth);
case CBSSearchMethod.DepthFirst:
return((-1) * node.Depth);
default:
return(0);
}
};
//Enqueue first node
if (solvable)
{
Open.Enqueue(conflictTree.Root.SolutionCost, conflictTree.Root);
}
else
{
Communicator.LogDefault("WARNING! Aborting CBS - could not obtain an initial solution for the following agents: " +
string.Join(",", unsolvableAgents.Select(a => "Agent" + a.ID.ToString() + "(" + a.NextNode.ToString() + "->" + a.DestinationNode.ToString() + ")")));
}
bestNode = conflictTree.Root;
//search loop
ConflictTree.Node p = conflictTree.Root;
while (Open.Count > 0)
{
//local variables
int agentId1;
int agentId2;
ReservationTable.Interval interval;
//pop out best node
p = Open.Dequeue().Value;
//check the path
var hasNoConflicts = ValidatePath(p, agents, out agentId1, out agentId2, out interval);
//has no conflicts?
if (hasNoConflicts)
{
bestNode = p;
break;
}
// time up? => return the best solution
if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
{
Communicator.SignalTimeout();
break;
}
//save best node
if (bestNode == null || interval.Start > bestTime)
{
bestTime = interval.Start;
bestNode = p;
}
//append child 1
var node1 = new ConflictTree.Node(agentId1, interval, p);
solvable = Solve(node1, currentTime, agents.First(a => a.ID == agentId1));
if (solvable)
{
Open.Enqueue(node1.SolutionCost, node1);
}
//append child 2
var node2 = new ConflictTree.Node(agentId2, interval, p);
solvable = Solve(node2, currentTime, agents.First(a => a.ID == agentId2));
if (solvable)
{
Open.Enqueue(node2.SolutionCost, node2);
}
generatedNodes += 2;
}
//return the solution => suboptimal
foreach (var agent in agents)
{
agent.Path = bestNode.getSolution(agent.ID);
if (_deadlockHandler.IsInDeadlock(agent, currentTime))
{
_deadlockHandler.RandomHop(agent);
}
}
}