/// <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);
}
