Optimize multiroute

ICD.Common.Utils/RecursionUtils.cs

@@ -3,6 +3,7 @@ using System.Collections.Generic;
 using System.Linq;
 using ICD.Common.Properties;
 using ICD.Common.Utils.Collections;
+using ICD.Common.Utils.Extensions;
 
 namespace ICD.Common.Utils
 {
@@ -50,6 +51,15 @@ namespace ICD.Common.Utils
             return BreadthFirstSearchPath(root, destination, getChildren, EqualityComparer<T>.Default);
         }
 
+        [NotNull]
+        public static Dictionary<T2, IEnumerable<T>> BreadthFirstSearchManyDestinations<T, T2>(T root, Dictionary<T2, T> destinations, Func<T, IEnumerable<T>> getChildren)
+        {
+            if (getChildren == null)
+                throw new ArgumentNullException("getChildren");
+
+            return BreadthFirstSearchPathManyDestinations(root, destinations, getChildren, EqualityComparer<T>.Default);
+        }
+
         /// <summary>
         /// Returns the shortest path from root to destination via breadth-first search.
         /// </summary>
@@ -99,6 +109,72 @@ namespace ICD.Common.Utils
             return null;
         }
 
+        [NotNull]
+        public static Dictionary<T2, IEnumerable<T>> BreadthFirstSearchPathManyDestinations<T, T2>(T root, Dictionary<T2, T> destinations,
+                                                                                            Func<T, IEnumerable<T>> getChildren,
+                                                                                            IEqualityComparer<T> comparer)
+        {
+            if (getChildren == null)
+                throw new ArgumentNullException("getChildren");
+
+            if (comparer == null)
+                throw new ArgumentNullException("comparer");
+
+            Dictionary<T2, T> destinationsToBeProcessed = new Dictionary<T2, T>(destinations);
+            List<T> destinationsProcessed = new List<T>();
+            Dictionary<T2, IEnumerable<T>> pathsToReturn = new Dictionary<T2, IEnumerable<T>>();
+
+            //Edge case, root is the destination
+            foreach (var destination in destinationsToBeProcessed.Where(destination => comparer.Equals(root, destination.Value)))
+            {
+                destinationsProcessed.Add(destination.Value);
+                pathsToReturn.Add(destination.Key, new[] { root });
+            }
+
+            foreach (var destination in destinationsProcessed)
+            {
+                destinationsToBeProcessed.RemoveValue(destination);
+            }
+            destinationsProcessed.Clear();
+            if (destinationsToBeProcessed.Count == 0)
+            {
+                return pathsToReturn;
+            }
+
+            Queue<T> queue = new Queue<T>();
+            queue.Enqueue(root);
+
+            Dictionary<T, T> nodeParents = new Dictionary<T, T>();
+
+            while (queue.Count > 0)
+            {
+                T current = queue.Dequeue();
+
+                foreach (T node in getChildren(current).Where(node => !nodeParents.ContainsKey(node)))
+                {
+                    queue.Enqueue(node);
+                    nodeParents.Add(node, current);
+
+                    foreach (var destination in destinationsToBeProcessed.Where(destination => comparer.Equals(node, destination.Value)))
+                    {
+                        destinationsProcessed.Add(destination.Value);
+                        pathsToReturn.Add(destination.Key, GetPath(destination.Value, nodeParents).Reverse());
+                    }
+                    foreach (var destination in destinationsProcessed)
+                    {
+                        destinationsToBeProcessed.RemoveValue(destination);
+                    }
+                    destinationsProcessed.Clear();
+                    if (destinationsToBeProcessed.Count == 0)
+                    {
+                        return pathsToReturn;
+                    }
+                }
+            }
+
+            return pathsToReturn;
+        }
+
         /// <summary>
         /// Walks through a map of nodes from the starting point.
         /// </summary>