/// <summary> /// Removes a node from the queue. Note that the node does not need to be the head of the queue. O(log n) /// </summary> public void Remove(RoutePoint node) { if (!Contains(node)) { return; } if (Count <= 1) { nodes[1] = null; Count = 0; return; } //Make sure the node is the last node in the queue var wasSwapped = false; var formerLastNode = nodes[Count]; if (node.QueueIndex != Count) { //Swap the node with the last node Swap(node, formerLastNode); wasSwapped = true; } Count--; nodes[node.QueueIndex] = null; if (wasSwapped) { //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate UpdatePriority(formerLastNode); } }
/// <summary> /// Route a single connection /// </summary> /// <param name="conn"></param> /// <returns></returns> public bool RouteConnection(Connector conn) { Debug.WriteLine("Connecting {0}:{1} to {2}:{3}", conn.TaskFrom.TaskId, conn.Flow.Name, conn.Flow.Target, conn.Flow.TargetPin); var start = SnapToMap(conn.TaskFrom.Symbol.Shape.Bounds.X + conn.PinFrom.Centre.X, conn.TaskFrom.Symbol.Shape.Bounds.Y + conn.PinFrom.Centre.Y); target = SnapToMap(conn.TaskTo.Symbol.Shape.Bounds.X + conn.PinTo.Centre.X, conn.TaskTo.Symbol.Shape.Bounds.Y + conn.PinTo.Centre.Y); Debug.WriteLine("Coordinates ({0}, {1}) to ({2}, {3})", start.X, start.Y, target.X, target.Y); map.ClearVisitedFlags(); queue = new PriorityQueue(200); routePoints = new Dictionary <MapRef, RoutePoint>(); var initial = new RoutePoint(start.X, start.Y) { Direction = conn.PinFrom.Direction, Heuristic = Manhatten(start, target) }; queue.Enqueue(initial); RoutePoint route = null; while (route == null) { if (queue.Count == 0) { Debug.WriteLine("FAILED TO CONNECT {0}:{1} to {2}:{3}", conn.TaskFrom.TaskId, conn.Flow.Name, conn.Flow.Target, conn.Flow.TargetPin); return(false); } route = Astar(); } var connectorPoints = new List <Point>(); var trace = route; connectorPoints.Add(MapToDiagram(trace.X, trace.Y)); while (trace != null) { AddRoutedPoint(trace.Location, target, trace.Direction); if (trace.Previous == null) { connectorPoints.Add(MapToDiagram(trace.X, trace.Y)); } else if (trace.Previous.Direction != trace.Direction) { connectorPoints.Add(MapToDiagram(trace.Previous.X, trace.Previous.Y)); } trace = trace.Previous; } connectorPoints.Reverse(); conn.Points = connectorPoints; return(true); }
public void Enqueue(RoutePoint node) { if (++Count > Capacity) { Capacity = Capacity * 2; } nodes[Count] = node; node.QueueIndex = Count; node.InsertionIndex = nodeCounter++; CascadeUp(nodes[Count]); }
private void Swap(RoutePoint node1, RoutePoint node2) { //Swap the nodes nodes[node1.QueueIndex] = node2; nodes[node2.QueueIndex] = node1; //Swap their indicies var temp = node1.QueueIndex; node1.QueueIndex = node2.QueueIndex; node2.QueueIndex = temp; }
private void CascadeDown(RoutePoint node) { var finalQueueIndex = node.QueueIndex; while (true) { var newParent = node; var childLeftIndex = 2 * finalQueueIndex; //Check if the left-child is x-priority than the current node if (childLeftIndex > Count) { //This could be placed outside the loop, but then we'd have to check newParent != node twice node.QueueIndex = finalQueueIndex; nodes[finalQueueIndex] = node; break; } var childLeft = nodes[childLeftIndex]; if (HasPriority(childLeft, newParent)) { newParent = childLeft; } //Check if the right-child is x-priority than either the current node or the left child var childRightIndex = childLeftIndex + 1; if (childRightIndex <= Count) { var childRight = nodes[childRightIndex]; if (HasPriority(childRight, newParent)) { newParent = childRight; } } //If either of the children has x (smaller) priority, swap and continue cascading if (newParent != node) { nodes[finalQueueIndex] = newParent; var temp = newParent.QueueIndex; newParent.QueueIndex = finalQueueIndex; finalQueueIndex = temp; } else { node.QueueIndex = finalQueueIndex; nodes[finalQueueIndex] = node; break; } } }
public void UpdatePriority(RoutePoint node) { //Bubble the updated node up or down as appropriate var parentIndex = node.QueueIndex / 2; var parentNode = nodes[parentIndex]; if (parentIndex > 0 && HasPriority(node, parentNode)) { CascadeUp(node); } else { //Note that CascadeDown will be called if parentNode == node (that is, node is the root) CascadeDown(node); } }
/// <summary> /// Estimate the minimum number of bends in the remaining route /// This is the actual number of bends assuming no obstacles /// </summary> /// <param name="start"></param> /// <param name="dir"></param> /// <returns></returns> private int MinBendEstimate(RoutePoint start, Direction dir) { var inline = dir.Vertical && start.Y == target.Y || dir.Horizontal && start.X == target.X; var dx = target.X - start.X; var dy = target.Y - start.Y; var ahead = dx > 0 && dir == Direction.East || dy > 0 && dir == Direction.South || dx < 0 && dir == Direction.West || dy < 0 && dir == Direction.North; return(inline ? (ahead ? 0 : 3) : (ahead ? 1 : 2)); }
private void CascadeUp(RoutePoint node) { //aka Heapify-up var parent = node.QueueIndex / 2; while (parent >= 1) { var parentNode = nodes[parent]; if (HasPriority(parentNode, node)) { break; } //Node has y priority value, so move it up the heap Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown() parent = node.QueueIndex / 2; } }
public bool Contains(RoutePoint node) { return(nodes[node.QueueIndex] == node); }
private static bool HasPriority(RoutePoint x, RoutePoint y) { return(x.Priority < y.Priority || (x.Priority == y.Priority && x.InsertionIndex < y.InsertionIndex)); }
/// <summary> /// A* algorithm to route connections /// Returns the target RoutePoint upon success or null upon failure. /// The route can be determined by following the Previous pointers from /// RoutePoint to RoutePoint. Changes in direction provide a simple way /// to detect the bends in the route so as to reduce it to straight line segments /// </summary> /// <returns>The target route point</returns> private RoutePoint Astar() { var current = queue.Dequeue(); routePoints.Remove(current.Location); // Debug.WriteLine("Dequeued {0}, {1}", current.X, current.Y); if (current.Location == target) { return(current); } foreach (var dir in Direction.All()) { var mr = current.Location.Step(dir); var mapState = map[mr]; if (mapState != RouteState.Empty) { continue; } RoutedPoint routedPoint; if (routedPoints.TryGetValue(mr, out routedPoint)) { if (routedPoint.Target == MapRef.Empty) // This indicates an existing crossing point { continue; } } var crossing = routedPoint != null && routedPoint.Target != target; RoutedPoint currentRoutedPoint; if (routedPoints.TryGetValue(current.Location, out currentRoutedPoint)) { // No changes of direction when crossing an existing route if (currentRoutedPoint.Target != target && dir != current.Direction) { continue; } // Don't try a direction that doesn't match the one for the existing trace if (currentRoutedPoint.Target == target && dir != currentRoutedPoint.Direction && currentRoutedPoint.Direction != Direction.Empty) { continue; } // Don't follow existing routed point for the wrong target if (currentRoutedPoint.Target != target && routedPoint?.Target == currentRoutedPoint.Target) { continue; } } // Don't join at an existing join point if TwoWayJoins is set if (TwoWayJoins && routedPoint != null && routedPoint.Direction == Direction.Empty && currentRoutedPoint?.Target != target) { continue; } var existing = true; RoutePoint n; if (!routePoints.TryGetValue(mr, out n)) { n = new RoutePoint(mr.X, mr.Y); existing = false; } var bends = current.Bends; if (current.Previous != null && current.Previous.X != mr.X && current.Previous.Y != mr.Y) { bends++; } var crossings = current.Crossings + (crossing ? 1 : 0); var length = current.Length + 1; var cost = length + crossings * CrossingPenalty; var bendEstimate = MinBendEstimate(n, dir); if (!existing || cost + bendEstimate * BendPenalty < n.Cost + n.BendEstimate * BendPenalty) { n.Cost = cost; n.Length = length; n.Bends = bends; n.Crossings = crossings; n.Previous = current; n.BendEstimate = bendEstimate; n.Heuristic = Manhatten(n.Location, target) + bendEstimate * BendPenalty; n.Direction = dir; n.Priority = n.Cost + n.Heuristic; } if (existing) { queue.UpdatePriority(n); } else { queue.Enqueue(n); routePoints.Add(n.Location, n); } // Debug.WriteLine("Enqueued {0}, {1} total cost = {2}", n.X, n.Y, n.Cost + n.Heuristic); } map[current.Location] = RouteState.Visited; return(null); }