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 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 DecreaseKeyOnIncreasing() { 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 = null; heap.ChangeKey(cells[9], -1); 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 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() { 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 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 InsertAndMinimum <TPriority, TValue>( [PexAssumeUnderTest] FibonacciHeap <TPriority, TValue> target, [PexAssumeNotNull] KeyValuePair <TPriority, TValue>[] kvs) { PexAssume.IsTrue(kvs.Length > 0); var count = target.Count; TPriority minimum = default(TPriority); for (int i = 0; i < kvs.Length; ++i) { KeyValuePair <TPriority, TValue> kv = kvs[i]; if (i == 0) { minimum = kv.Key; } else { minimum = target.PriorityComparison(kv.Key, minimum) < 0 ? kv.Key : minimum; } target.Enqueue(kv.Key, kv.Value); // check minimum var kvMin = target.Top.Priority; Assert.AreEqual(minimum, kvMin); } AssertInvariant(target); }
public void EnumeratorIncreasing() { FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing); int count = 0; for (int i = 10; i >= 0; i--) { heap.Enqueue(i, i.ToString()); count++; } int?lastValue = null; foreach (var value in heap) { 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 SimpleEnqueueDequeDecreasing() { var heap = new FibonacciHeap <int, string>(HeapDirection.Decreasing); int count = 0; for (int i = 0; i < 10; ++i) { heap.Enqueue(i, i.ToString()); ++count; } 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 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 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() { 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; } 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 MergeTest() { FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing); FibonacciHeap <int, string> heap2 = new FibonacciHeap <int, string>(HeapDirection.Increasing); int count = 0; for (int i = 11; i > 0; i--) { heap.Enqueue(i, i.ToString()); heap2.Enqueue(i * 11, i.ToString()); count += 2; } heap2.Merge(heap); int?lastValue = null; foreach (var value in heap2.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 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 MergeTest() { FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing); FibonacciHeap <int, string> heap2 = new FibonacciHeap <int, string>(HeapDirection.Increasing); int count = 0; for (int i = 11; i > 0; i--) { heap.Enqueue(i, i.ToString()); heap2.Enqueue(i * 11, i.ToString()); count += 2; } heap2.Merge(heap); int?lastValue = null; foreach (var value in heap2.GetDestructiveEnumerator()) { if (lastValue == null) { lastValue = value.Key; } Assert.False(lastValue > value.Key); lastValue = value.Key; count--; } Assert.Equal(0, count); }
public void Merge() { var heap = new FibonacciHeap <int, string>(HeapDirection.Increasing); var heap2 = new FibonacciHeap <int, string>(HeapDirection.Increasing); int count = 0; for (int i = 11; i > 0; --i) { heap.Enqueue(i, i.ToString()); heap2.Enqueue(i * 11, i.ToString()); count += 2; } heap2.Merge(heap); int?lastValue = null; foreach (KeyValuePair <int, string> value in heap2.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 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); }
/// <summary> /// Clears this instance. /// </summary> public virtual void Clear(int startNode, int goalNode) { this.Open.Clear(); this.Closed.Clear(); this.StartNode = startNode; this.GoalNode = goalNode; //#if FIB_HEAP_J // this.Q = new FibonacciHeapJ<int>(); //#else this.Q = new FibonacciHeap <double, int>(); //#endif //initiate open node this.Open.Add(startNode, Q.Enqueue(h(startNode), startNode)); }
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 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); } }
public void SimpleEnqueDequeDecreasing() { FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Decreasing); int count = 0; for (int i = 0; i < 10; i++) { heap.Enqueue(i, i.ToString()); count++; } 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 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 EnumeratorIncreasing() { FibonacciHeap <int, string> heap = new FibonacciHeap <int, string>(HeapDirection.Increasing); int count = 0; for (int i = 10; i >= 0; i--) { heap.Enqueue(i, i.ToString()); count++; } int?lastValue = null; foreach (var value in heap) { if (lastValue == null) { lastValue = value.Key; } Assert.False(lastValue > value.Key); lastValue = value.Key; count--; } Assert.Equal(0, count); }
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> /// 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); } } }