private void ExtendPathAlongInEdges(VertexEntry bestEntry, IEnumerable <VisibilityEdge> edges, Direction preferredBendDir) { foreach (var edge in edges) { ExtendPathAlongEdge(bestEntry, edge, true, preferredBendDir); } }
private void EnqueueEntry(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double length, int numberOfBends, double cost) { var entry = new VertexEntry(neigVer, bestEntry, length, numberOfBends, cost); neigVer.SetVertexEntry(entry); this.queue.Enqueue(entry, entry.Cost); }
private void ExtendPathAlongEdge(VertexEntry bestEntry, VisibilityEdge edge, bool isInEdges, Directions preferredBendDir) { if (!IsPassable(edge)) { return; } // This is after the initial source vertex so PreviousEntry won't be null. var neigVer = (VisibilityVertexRectilinear)(isInEdges ? edge.Source : edge.Target); if (neigVer == bestEntry.PreviousVertex) { // For multistage paths, the source may be a waypoint outside the graph boundaries that is collinear // with both the previous and next points in the path; in that case it may have only one degree. // For other cases, we just ignore it and the path will be abandoned. if ((bestEntry.Vertex.Degree > 1) || (bestEntry.Vertex != this.Source)) { return; } this.ExtendPathToNeighborVertex(bestEntry, neigVer, edge.Weight); return; } // Enqueue in reverse order of preference per comments on NextNeighbor class. var neigDir = CompassVector.PureDirectionFromPointToPoint(bestEntry.Vertex.Point, neigVer.Point); var nextNeighbor = this.nextNeighbors[2]; if (neigDir != bestEntry.Direction) { nextNeighbor = this.nextNeighbors[(neigDir == preferredBendDir) ? 1 : 0]; } Debug.Assert(nextNeighbor.Vertex == null, "bend neighbor already exists"); nextNeighbor.Set(neigVer, edge.Weight); }
private void TestShowAllPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) { // ReSharper restore UnusedMember.Local var edges = GetAllEdgesTest(source).Select(e => (ICurve)(new LineSegment(e.SourcePoint, e.TargetPoint))). Select(c => new DebugCurve(c)); var q = queue.Select(ent => CurveFactory.CreateDiamond(2, 2, ent.Vertex.Point)).Select(c => new DebugCurve(c)); var so = new[] { new DebugCurve(1, "brown", new Ellipse(3, 3, source.Point)), new DebugCurve(1, "purple", CurveFactory.CreateDiamond(4, 4, Target.Point)), new DebugCurve(1, "red", CurveFactory.CreateDiamond(6, 6, mostRecentlyExtendedPath.Vertex.Point)) }; var pathEdges = new List <DebugCurve>(); var newEntries = new List <VertexEntry>(); var count = this.visitedVertices.Count; for (int ii = 0; ii < count; ++ii) { var vertex = this.visitedVertices[ii]; if (vertex.VertexEntries == null) { continue; // this is the source vertex } foreach (var entry in vertex.VertexEntries) { if (entry == null) { continue; } var color = "green"; if (entry.PreviousEntry == mostRecentlyExtendedPath) { newEntries.Add(entry); color = "red"; } else if (!entry.IsClosed) { color = "yellow"; } pathEdges.Add(new DebugCurve(2, color, new LineSegment(entry.PreviousVertex.Point, vertex.Point))); } } Console.WriteLine("entry {0} seq = {1} len = {2} nbend = {3} ccost = {4} hcost = {5} iters = {6}/{7}", mostRecentlyExtendedPath, this.lastDequeueTimestamp, mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends, this.CombinedCost(mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends), this.HeuristicDistanceFromVertexToTarget(mostRecentlyExtendedPath.Vertex.Point, mostRecentlyExtendedPath.Direction), this.currentIterations, totalIterations); foreach (var newEntry in newEntries) { Console.WriteLine(" newEntry {0} len = {1} nbend = {2} ccost = {3} hcost = {4}", newEntry, newEntry.Length, newEntry.NumberOfBends, this.CombinedCost(newEntry.Length, newEntry.NumberOfBends), this.HeuristicDistanceFromVertexToTarget(newEntry.Vertex.Point, newEntry.Direction)); } DevTraceDisplay(edges.Concat(q).Concat(so).Concat(pathEdges)); }
internal void SetVertexEntry(VertexEntry entry) { if (this.VertexEntries == null) { this.VertexEntries = new VertexEntry[4]; } this.VertexEntries[CompassVector.ToIndex(entry.Direction)] = entry; }
void DevTraceShowAllPartialPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) { #if DEVTRACE if (ssstTrace.IsLevel(3)) { this.TestShowAllPaths(source, mostRecentlyExtendedPath); } #endif // DEVTRACE }
private void ExtendPathAlongOutEdges(VertexEntry bestEntry, RbTree <VisibilityEdge> edges, Directions preferredBendDir) { // Avoid GetEnumerator overhead. var outEdgeNode = edges.IsEmpty() ? null : edges.TreeMinimum(); for (; outEdgeNode != null; outEdgeNode = edges.Next(outEdgeNode)) { ExtendPathAlongEdge(bestEntry, outEdgeNode.Item, false, preferredBendDir); } }
private void ExtendPathAlongInEdges(VertexEntry bestEntry, List <VisibilityEdge> edges, Directions preferredBendDir) { // Iteration is faster than foreach and much faster than .Where. int count = edges.Count; for (int ii = 0; ii < count; ++ii) { var edge = edges[ii]; ExtendPathAlongEdge(bestEntry, edge, true, preferredBendDir); } }
private void UpdateEntryToNeighborVertexIfNeeded(VertexEntry bestEntry, VertexEntry neigEntry, double weight) { int numberOfBends; double length; var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigEntry.Vertex, weight, out numberOfBends, out length); if (CombinedCost(length, numberOfBends) < CombinedCost(neigEntry.Length, neigEntry.NumberOfBends)) { var newCost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigEntry.Vertex.Point, dirToNeighbor); neigEntry.ResetEntry(bestEntry, length, numberOfBends, newCost); queue.DecreasePriority(neigEntry, newCost); } }
private static Directions GetLengthAndNumberOfBendsToNeighborVertex(VertexEntry prevEntry, VisibilityVertex vertex, double weight, out int numberOfBends, out double length) { length = prevEntry.Length + ManhattanDistance(prevEntry.Vertex.Point, vertex.Point) * weight; Directions directionToVertex = CompassVector.PureDirectionFromPointToPoint(prevEntry.Vertex.Point, vertex.Point); numberOfBends = prevEntry.NumberOfBends; if (prevEntry.Direction != Directions.None && directionToVertex != prevEntry.Direction) { numberOfBends++; } return(directionToVertex); }
private void DevTraceShowPath(VisibilityVertex source, VertexEntry lastEntry) { #if DEVTRACE if (ssstTrace.IsLevel(1)) { var pathPoints = RestorePath(lastEntry); if (pathPoints != null) { this.TestShowPath(source, pathPoints, lastEntry.Cost, lastEntry.Length, lastEntry.NumberOfBends); return; } System.Diagnostics.Debug.WriteLine("path abandoned; iters = {0}/{1}", this.currentIterations, totalIterations); } #endif // DEVTRACE }
private void CreateAndEnqueueEntryToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) { int numberOfBends; double length; var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigVer, weight, out numberOfBends, out length); var cost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigVer.Point, dirToNeighbor); if (cost < this.upperBoundOnCost) { if (neigVer.VertexEntries == null) { this.visitedVertices.Add(neigVer); } EnqueueEntry(bestEntry, neigVer, length, numberOfBends, cost); } }
internal void ResetEntry(VertexEntry prevEntry, double length, int numberOfBends, double cost) { // A new prevEntry using the same previous vertex but a different entry to that vertex is valid here; // e.g. we could have prevEntry from S, which in turn had a prevEntry from E, replaced by prevEntry from // S which has a prevEntry from S. #if DEBUG if (this.PreviousEntry != null) { Debug.Assert(this.PreviousEntry.Vertex == prevEntry.Vertex, "Inconsistent prevEntry vertex"); Debug.Assert(this.PreviousEntry.Direction != prevEntry.Direction, "Duplicate prevEntry direction"); Debug.Assert(this.Direction == CompassVector.PureDirectionFromPointToPoint(this.PreviousEntry.Vertex.Point, this.Vertex.Point), "Inconsistent entryDir"); } #endif // DEBUG this.PreviousEntry = prevEntry; this.Length = length; this.NumberOfBends = numberOfBends; this.Cost = cost; }
private void QueueReversedEntryToNeighborVertexIfNeeded(VertexEntry bestEntry, VertexEntry entryFromNeighbor, double weight) { // If we have a lower-cost path from bestEntry to entryFromNeighbor.PreviousVertex than the cost of entryFromNeighbor, // or bestEntry has degree 1 (it is a dead-end), enqueue a path in the opposite direction (entryFromNeighbor will probably // never be extended from this point). int numberOfBends; double length; var neigVer = entryFromNeighbor.PreviousVertex; var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigVer, weight, out numberOfBends, out length); if ((CombinedCost(length, numberOfBends) < CombinedCost(entryFromNeighbor.Length, entryFromNeighbor.NumberOfBends)) || (bestEntry.Vertex.Degree == 1)) { var cost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigVer.Point, dirToNeighbor); EnqueueEntry(bestEntry, neigVer, length, numberOfBends, cost); } }
private void EnqueueInitialVerticesFromSource(double cost) { var bestEntry = new VertexEntry(this.Source, null, 0, 0, cost) { IsClosed = true }; // This routine is only called once so don't worry about optimizing foreach.where foreach (var edge in this.Source.OutEdges.Where(IsPassable)) { this.ExtendPathToNeighborVertex(bestEntry, (VisibilityVertexRectilinear)edge.Target, edge.Weight); } foreach (var edge in this.Source.InEdges.Where(IsPassable)) { this.ExtendPathToNeighborVertex(bestEntry, (VisibilityVertexRectilinear)edge.Source, edge.Weight); } }
private void ExtendPathToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) { var dirToNeighbor = CompassVector.PureDirectionFromPointToPoint(bestEntry.Vertex.Point, neigVer.Point); var neigEntry = (neigVer.VertexEntries != null) ? neigVer.VertexEntries[CompassVector.ToIndex(dirToNeighbor)] : null; if (neigEntry == null) { if (!this.CreateAndEnqueueReversedEntryToNeighborVertex(bestEntry, neigVer, weight)) { this.CreateAndEnqueueEntryToNeighborVertex(bestEntry, neigVer, weight); } } else if (!neigEntry.IsClosed) { this.UpdateEntryToNeighborVertexIfNeeded(bestEntry, neigEntry, weight); } }
internal void ResetEntry(VertexEntry prevEntry, double length, int numberOfBends, double cost) { // A new prevEntry using the same previous vertex but a different entry to that vertex is valid here; // e.g. we could have prevEntry from S, which in turn had a prevEntry from E, replaced by prevEntry from // S which has a prevEntry from S. #if TEST_MSAGL if (this.PreviousEntry != null) { Debug.Assert(this.PreviousEntry.Vertex == prevEntry.Vertex, "Inconsistent prevEntry vertex"); Debug.Assert(this.PreviousEntry.Direction != prevEntry.Direction, "Duplicate prevEntry direction"); Debug.Assert(this.Direction == CompassVector.PureDirectionFromPointToPoint(this.PreviousEntry.Vertex.Point, this.Vertex.Point), "Inconsistent entryDir"); } #endif // TEST_MSAGL this.PreviousEntry = prevEntry; this.Length = length; this.NumberOfBends = numberOfBends; this.Cost = cost; }
internal static IEnumerable <Point> RestorePath(ref VertexEntry entry, VisibilityVertex firstVertexInStage) { if (entry == null) { return(null); } var list = new List <Point>(); bool skippedCollinearEntry = false; Directions lastEntryDir = Directions.None; while (true) { // Reduce unnecessary AxisEdge creations in Nudger by including only bend points, not points in the middle of a segment. if (lastEntryDir == entry.Direction) { skippedCollinearEntry = true; } else { skippedCollinearEntry = false; list.Add(entry.Vertex.Point); lastEntryDir = entry.Direction; } var previousEntry = entry.PreviousEntry; if ((previousEntry == null) || (entry.Vertex == firstVertexInStage)) { break; } entry = previousEntry; } if (skippedCollinearEntry) { list.Add(entry.Vertex.Point); } list.Reverse(); return(list); }
private bool InitPath(VertexEntry[] sourceVertexEntries, VisibilityVertexRectilinear source, VisibilityVertexRectilinear target) { if ((source == target) || !InitEntryDirectionsAtTarget(target)) { return false; } this.Target = target; this.Source = source; double cost = this.TotalCostFromSourceToVertex(0, 0) + HeuristicDistanceFromVertexToTarget(source.Point, Directions. None); if (cost >= this.upperBoundOnCost) { return false; } // This path starts lower than upperBoundOnCost, so create our structures and process it. this.queue = new GenericBinaryHeapPriorityQueueWithTimestamp<VertexEntry>(); this.visitedVertices = new List<VisibilityVertexRectilinear> { source }; if (sourceVertexEntries == null) { EnqueueInitialVerticesFromSource(cost); } else { EnqueueInitialVerticesFromSourceEntries(sourceVertexEntries); } return this.queue.Count > 0; }
private static void UpdateTargetEntriesForEachDirection(VertexEntry[] targetVertexEntries, VertexEntry[] tempTargetEntries, ref double bestCost, ref VertexEntry bestEntry) { for (int ii = 0; ii < tempTargetEntries.Length; ++ii) { var tempEntry = tempTargetEntries[ii]; if (tempEntry == null) { continue; } if ((targetVertexEntries[ii] == null) || (tempEntry.Cost < targetVertexEntries[ii].Cost)) { targetVertexEntries[ii] = tempEntry; if (tempEntry.Cost < bestCost) { // This does not have the ratio tiebreaker because the individual stage path is only used as a success indicator. bestCost = tempEntry.Cost; bestEntry = tempEntry; } } } return; }
private bool CreateAndEnqueueReversedEntryToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) { // VertexEntries is null for the initial source. Otherwise, if there is already a path into bestEntry's vertex // from neigVer, we're turning back on the path; therefore we have already enqueued the neighbors of neigVer. // However, the path cost includes both path length to the current point and the lookahead; this means that we // may now be coming into the neigVer from the opposite side with an equal score to the previous entry, but // the new path may be going toward the target while the old one (from neigVer to bestEntry) went away from // the target. So, if we score better going in the opposite direction, enqueue bestEntry->neigVer; ignore // neigVer->bestEntry as it probably won't be extended again. if (bestEntry.Vertex.VertexEntries != null) { var dirFromNeighbor = CompassVector.PureDirectionFromPointToPoint(neigVer.Point, bestEntry.Vertex.Point); var entryFromNeighbor = bestEntry.Vertex.VertexEntries[CompassVector.ToIndex(dirFromNeighbor)]; if (entryFromNeighbor != null) { Debug.Assert(entryFromNeighbor.PreviousVertex == neigVer, "mismatch in turnback PreviousEntry"); Debug.Assert(entryFromNeighbor.PreviousEntry.IsClosed, "turnback PreviousEntry should be closed"); this.QueueReversedEntryToNeighborVertexIfNeeded(bestEntry, entryFromNeighbor, weight); return(true); } } return(false); }
private void DevTraceShowPath(VisibilityVertex source, VertexEntry lastEntry) { #if DEVTRACE if (ssstTrace.IsLevel(1)) { var pathPoints = RestorePath(lastEntry); if (pathPoints != null) { this.TestShowPath(source, pathPoints, lastEntry.Cost, lastEntry.Length, lastEntry.NumberOfBends); return; } Console.WriteLine("path abandoned; iters = {0}/{1}", this.currentIterations, totalIterations); } #endif // DEVTRACE }
private bool CreateAndEnqueueReversedEntryToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) { // VertexEntries is null for the initial source. Otherwise, if there is already a path into bestEntry's vertex // from neigVer, we're turning back on the path; therefore we have already enqueued the neighbors of neigVer. // However, the path cost includes both path length to the current point and the lookahead; this means that we // may now be coming into the neigVer from the opposite side with an equal score to the previous entry, but // the new path may be going toward the target while the old one (from neigVer to bestEntry) went away from // the target. So, if we score better going in the opposite direction, enqueue bestEntry->neigVer; ignore // neigVer->bestEntry as it probably won't be extended again. if (bestEntry.Vertex.VertexEntries != null) { var dirFromNeighbor = CompassVector.PureDirectionFromPointToPoint(neigVer.Point, bestEntry.Vertex.Point); var entryFromNeighbor = bestEntry.Vertex.VertexEntries[CompassVector.ToIndex(dirFromNeighbor)]; if (entryFromNeighbor != null) { Debug.Assert(entryFromNeighbor.PreviousVertex == neigVer, "mismatch in turnback PreviousEntry"); Debug.Assert(entryFromNeighbor.PreviousEntry.IsClosed, "turnback PreviousEntry should be closed"); this.QueueReversedEntryToNeighborVertexIfNeeded(bestEntry, entryFromNeighbor, weight); return true; } } return false; }
private void EnqueueInitialVerticesFromSourceEntries(VertexEntry[] sourceEntries) { foreach (var entry in sourceEntries) { if (entry != null) { this.queue.Enqueue(entry, entry.Cost); } } }
private void ExtendPathAlongInEdges(VertexEntry bestEntry, List<VisibilityEdge> edges, Directions preferredBendDir) { // Iteration is faster than foreach and much faster than .Where. int count = edges.Count; for (int ii = 0; ii < count; ++ii) { var edge = edges[ii]; ExtendPathAlongEdge(bestEntry, edge, true, preferredBendDir); } }
private static Directions GetLengthAndNumberOfBendsToNeighborVertex(VertexEntry prevEntry, VisibilityVertex vertex, double weight, out int numberOfBends, out double length) { length = prevEntry.Length + ManhattanDistance(prevEntry.Vertex.Point, vertex.Point)*weight; Directions directionToVertex = CompassVector.PureDirectionFromPointToPoint(prevEntry.Vertex.Point, vertex.Point); numberOfBends = prevEntry.NumberOfBends; if (prevEntry.Direction != Directions. None && directionToVertex != prevEntry.Direction) { numberOfBends++; } return directionToVertex; }
private static IEnumerable <IEnumerable <Point> > RestorePathStages(List <WaypointEntry> waypointEntries, VertexEntry lastEntry) { // Back up to restore the path stages back to each waypoint. var paths = new List <IEnumerable <Point> >(); waypointEntries.Reverse(); foreach (var waypointEntry in waypointEntries) { paths.Add(SsstRectilinearPath.RestorePath(ref lastEntry, waypointEntry.waypointVector[0])); } // Add the first stage. paths.Add(SsstRectilinearPath.RestorePath(lastEntry)); paths.Reverse(); return(paths); }
private bool GetLastPathStage(WaypointEntry sourceWaypointEntry, IEnumerable <VisibilityVertex> targets, out VertexEntry lastEntry) { // Final stage: no need to get separate entries to the targets. And because we send null for the // target entry vector, this will return the final vertexEntry of the path. lastEntry = this.GetPathStage(sourceWaypointEntry.entryVector, sourceWaypointEntry.waypointVector, null, targets); return(lastEntry != null); }
/// <summary> /// Route a single stage of a possibly multi-stage (due to waypoints) path. /// </summary> /// <param name="sourceVertexEntries">The VertexEntry array that was in the source vertex if it was the target of a prior stage.</param> /// <param name="sources">The enumeration of source vertices; must be only one if sourceVertexEntries is non-null.</param> /// <param name="targets">The enumeration of target vertex entries; must be only one if targetVertexEntries is non-null.</param> /// <param name="targetVertexEntries">The VertexEntry array that is in the target at the end of the stage.</param> private VertexEntry GetPathStage(VertexEntry[] sourceVertexEntries, IEnumerable <VisibilityVertex> sources, VertexEntry[] targetVertexEntries, IEnumerable <VisibilityVertex> targets) { var ssstCalculator = new SsstRectilinearPath(); VertexEntry bestEntry = null; // This contains the best (lowest) path cost after normalizing origins to the center of the sources // and targets. This is used to avoid selecting a vertex pair whose path has more bends than another pair of // vertices, but the bend penalty didn't total enough to offset the additional length between the "better" pair. // This also plays the role of an upper bound on the path length; if a path cost is greater than adjustedMinCost // then we stop exploring it, which saves considerable time after low-cost paths have been found. double bestCost = double.MaxValue / ScanSegment.OverlappedWeight; double bestPathCostRatio = double.PositiveInfinity; // Calculate the bend penalty multiplier. This is a percentage of the distance between the source and target, // so that we have the same relative importance if we have objects of about size 20 that are about 100 apart // as for objects of about size 200 that are about 1000 apart. Point sourceCenter = Barycenter(sources); Point targetCenter = Barycenter(targets); var distance = SsstRectilinearPath.ManhattanDistance(sourceCenter, targetCenter); ssstCalculator.BendsImportance = Math.Max(0.001, distance * (this.bendPenaltyAsAPercentageOfDistance * 0.01)); // We'll normalize by adding (a proportion of) the distance (only; not bends) from the current endpoints to // their centers. This is similar to routeToCenter, but routing multiple paths like this means we'll always // get at least a tie for the best vertex pair, whereas routeToCenter can introduce extraneous bends // if the sources/targets are not collinear with the center (such as an E-R diagram). // interiorLengthAdjustment is a way to decrease the cost adjustment slightly to allow a bend if it saves moving // a certain proportion of the distance parallel to the object before turning to it. var interiorLengthAdjustment = ssstCalculator.LengthImportance; // VertexEntries for the current pass of the current stage, if multistage. var tempTargetEntries = (targetVertexEntries != null) ? this.currentPassTargetEntries : null; // Process closest pairs first, so we can skip longer ones (jump out of SsstRectilinear sooner, often immediately). // This means that we'll be consistent on tiebreaking for equal scores with differing bend counts (the shorter // path will win). In overlapped graphs the shortest path may have more higher-weight edges. foreach (var pair in from VisibilityVertexRectilinear source in sources from VisibilityVertexRectilinear target in targets orderby SsstRectilinearPath.ManhattanDistance(source.Point, target.Point) select new { sourceV = source, targetV = target }) { var source = pair.sourceV; var target = pair.targetV; if (PointComparer.Equal(source.Point, target.Point)) { continue; } var sourceCostAdjustment = SsstRectilinearPath.ManhattanDistance(source.Point, sourceCenter) * interiorLengthAdjustment; var targetCostAdjustment = SsstRectilinearPath.ManhattanDistance(target.Point, targetCenter) * interiorLengthAdjustment; var adjustedBestCost = bestCost; if (targetVertexEntries != null) { Array.Clear(tempTargetEntries, 0, tempTargetEntries.Length); adjustedBestCost = ssstCalculator.MultistageAdjustedCostBound(bestCost); } VertexEntry lastEntry = ssstCalculator.GetPathWithCost(sourceVertexEntries, source, sourceCostAdjustment, tempTargetEntries, target, targetCostAdjustment, adjustedBestCost); if (tempTargetEntries != null) { UpdateTargetEntriesForEachDirection(targetVertexEntries, tempTargetEntries, ref bestCost, ref bestEntry); continue; } // This is the final (or only) stage. Break ties by picking the lowest ratio of cost to ManhattanDistance between the endpoints. if (lastEntry == null) { continue; } var costRatio = lastEntry.Cost / SsstRectilinearPath.ManhattanDistance(source.Point, target.Point); if ((lastEntry.Cost < bestCost) || ApproximateComparer.Close(lastEntry.Cost, bestCost) && (costRatio < bestPathCostRatio)) { bestCost = lastEntry.Cost; bestEntry = lastEntry; bestPathCostRatio = lastEntry.Cost / SsstRectilinearPath.ManhattanDistance(source.Point, target.Point); } } return(bestEntry); }
/// <summary> /// A class that records an entry from a specific direction for a vertex. /// </summary> /// <param name="vertex">Vertex that this VertexEntry enters</param> /// <param name="prevEntry">The previous VertexEntry along this path; null for a path source</param> /// <param name="length">Length of the path up to this vertex</param> /// <param name="numberOfBends">Number of bends in the path up to this vertex</param> /// <param name="cost">Cost of the path up to this vertex</param> internal VertexEntry(VisibilityVertexRectilinear vertex, VertexEntry prevEntry, double length, int numberOfBends, double cost) { this.Vertex = vertex; this.Direction = (prevEntry != null) ? CompassVector.PureDirectionFromPointToPoint(prevEntry.Vertex.Point, vertex.Point) : Directions. None; this.ResetEntry(prevEntry, length, numberOfBends, cost); }
/// <summary> /// A class that records an entry from a specific direction for a vertex. /// </summary> /// <param name="vertex">Vertex that this VertexEntry enters</param> /// <param name="prevEntry">The previous VertexEntry along this path; null for a path source</param> /// <param name="length">Length of the path up to this vertex</param> /// <param name="numberOfBends">Number of bends in the path up to this vertex</param> /// <param name="cost">Cost of the path up to this vertex</param> internal VertexEntry(VisibilityVertexRectilinear vertex, VertexEntry prevEntry, double length, int numberOfBends, double cost) { this.Vertex = vertex; this.Direction = (prevEntry != null) ? CompassVector.PureDirectionFromPointToPoint(prevEntry.Vertex.Point, vertex.Point) : Directions.None; this.ResetEntry(prevEntry, length, numberOfBends, cost); }
private static IEnumerable<IEnumerable<Point>> RestorePathStages(List<WaypointEntry> waypointEntries, VertexEntry lastEntry) { // Back up to restore the path stages back to each waypoint. var paths = new List<IEnumerable<Point>>(); waypointEntries.Reverse(); foreach (var waypointEntry in waypointEntries) { paths.Add(SsstRectilinearPath.RestorePath(ref lastEntry, waypointEntry.waypointVector[0])); } // Add the first stage. paths.Add(SsstRectilinearPath.RestorePath(lastEntry)); paths.Reverse(); return paths; }
internal VertexEntry GetPathWithCost(VertexEntry[] sourceVertexEntries, VisibilityVertexRectilinear source, double adjustmentToSourceCost, VertexEntry[] targetVertexEntries, VisibilityVertexRectilinear target, double adjustmentToTargetCost, double priorBestCost) { this.upperBoundOnCost = priorBestCost; this.sourceCostAdjustment = adjustmentToSourceCost; this.targetCostAdjustment = adjustmentToTargetCost; DevTracePrintSourceAndTarget(source, target); if (!InitPath(sourceVertexEntries, source, target)) { this.DevTraceShowPath(source, null); return null; } #if TEST_MSAGL this.DevTraceShowAllPartialPaths(source, queue.Peek()); #endif // TEST_MSAGL while (queue.Count > 0) { this.TestPreDequeue(); var bestEntry = queue.Dequeue(); var bestVertex = bestEntry.Vertex; if (bestVertex == Target) { this.DevTraceShowPath(source, bestEntry); if (targetVertexEntries == null) { Cleanup(); return bestEntry; } // We'll never get a duplicate entry direction here; we either relaxed the cost via UpdateEntryToNeighborIfNeeded // before we dequeued it, or it was closed. So, we simply remove the direction from the valid target entry directions // and if we get to none, we're done. We return a null path until the final stage. this.EntryDirectionsToTarget &= ~bestEntry.Direction; if (this.EntryDirectionsToTarget == Directions. None) { this.Target.VertexEntries.CopyTo(targetVertexEntries, 0); Cleanup(); return null; } this.upperBoundOnCost = Math.Min(this.MultistageAdjustedCostBound(bestEntry.Cost), this.upperBoundOnCost); continue; } // It's safe to close this after removing it from the queue. Any updateEntryIfNeeded that changes it must come // while it is still on the queue; it is removed from the queue only if it has the lowest cost path, and we have // no negative path weights, so any other path that might try to extend to it after this cannot have a lower cost. bestEntry.IsClosed = true; // PerfNote: Array.ForEach is optimized, but don't use .Where. foreach (var bendNeighbor in this.nextNeighbors) { bendNeighbor.Clear(); } var preferredBendDir = Right(bestEntry.Direction); this.ExtendPathAlongInEdges(bestEntry, bestVertex.InEdges, preferredBendDir); this.ExtendPathAlongOutEdges(bestEntry, bestVertex.OutEdges, preferredBendDir); foreach (var bendNeighbor in this.nextNeighbors) { if (bendNeighbor.Vertex != null) { this.ExtendPathToNeighborVertex(bestEntry, bendNeighbor.Vertex, bendNeighbor.Weight); } } this.DevTraceShowAllPartialPaths(source, bestEntry); } // Either there is no path to the target, or we have abandoned the path due to exceeding priorBestCost. if ((targetVertexEntries != null) && (this.Target.VertexEntries != null)) { this.Target.VertexEntries.CopyTo(targetVertexEntries, 0); } this.DevTraceShowPath(source, null); Cleanup(); return null; }
private bool GetLastPathStage(WaypointEntry sourceWaypointEntry, IEnumerable<VisibilityVertex> targets, out VertexEntry lastEntry) { // Final stage: no need to get separate entries to the targets. And because we send null for the // target entry vector, this will return the final vertexEntry of the path. lastEntry = this.GetPathStage(sourceWaypointEntry.entryVector, sourceWaypointEntry.waypointVector, null, targets); return lastEntry != null; }
private void ExtendPathAlongOutEdges(VertexEntry bestEntry, RbTree<VisibilityEdge> edges, Directions preferredBendDir) { // Avoid GetEnumerator overhead. var outEdgeNode = edges.IsEmpty() ? null : edges.TreeMinimum(); for (; outEdgeNode != null; outEdgeNode = edges.Next(outEdgeNode)) { ExtendPathAlongEdge(bestEntry, outEdgeNode.Item, false, preferredBendDir); } }
internal static IEnumerable<Point> RestorePath(VertexEntry entry) { return RestorePath(ref entry, null); }
internal static IEnumerable<Point> RestorePath(ref VertexEntry entry, VisibilityVertex firstVertexInStage) { if (entry == null) { return null; } var list = new List<Point>(); bool skippedCollinearEntry = false; Directions lastEntryDir = Directions. None; while (true) { // Reduce unnecessary AxisEdge creations in Nudger by including only bend points, not points in the middle of a segment. if (lastEntryDir == entry.Direction) { skippedCollinearEntry = true; } else { skippedCollinearEntry = false; list.Add(entry.Vertex.Point); lastEntryDir = entry.Direction; } var previousEntry = entry.PreviousEntry; if ((previousEntry == null) || (entry.Vertex == firstVertexInStage)) { break; } entry = previousEntry; } if (skippedCollinearEntry) { list.Add(entry.Vertex.Point); } list.Reverse(); return list; }
private void TestShowAllPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) { // ReSharper restore UnusedMember.Local var edges = GetAllEdgesTest(source).Select(e => (ICurve) (new LineSegment(e.SourcePoint, e.TargetPoint))). Select(c => new DebugCurve(c)); var q = queue.Select(ent => CurveFactory.CreateDiamond(2, 2, ent.Vertex.Point)).Select(c => new DebugCurve(c)); var so = new[] { new DebugCurve(1, "brown", new Ellipse(3, 3, source.Point)), new DebugCurve(1, "purple", CurveFactory.CreateDiamond(4, 4, Target.Point)), new DebugCurve(1, "red", CurveFactory.CreateDiamond(6, 6, mostRecentlyExtendedPath.Vertex.Point)) }; var pathEdges = new List<DebugCurve>(); var newEntries = new List<VertexEntry>(); var count = this.visitedVertices.Count; for (int ii = 0; ii < count; ++ii) { var vertex = this.visitedVertices[ii]; if (vertex.VertexEntries == null) { continue; // this is the source vertex } foreach (var entry in vertex.VertexEntries) { if (entry == null) { continue; } var color = "green"; if (entry.PreviousEntry == mostRecentlyExtendedPath) { newEntries.Add(entry); color = "red"; } else if (!entry.IsClosed) { color = "yellow"; } pathEdges.Add(new DebugCurve(2, color, new LineSegment(entry.PreviousVertex.Point, vertex.Point))); } } Console.WriteLine("entry {0} seq = {1} len = {2} nbend = {3} ccost = {4} hcost = {5} iters = {6}/{7}", mostRecentlyExtendedPath, this.lastDequeueTimestamp, mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends, this.CombinedCost(mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends), this.HeuristicDistanceFromVertexToTarget(mostRecentlyExtendedPath.Vertex.Point, mostRecentlyExtendedPath.Direction), this.currentIterations, totalIterations); foreach (var newEntry in newEntries) { Console.WriteLine(" newEntry {0} len = {1} nbend = {2} ccost = {3} hcost = {4}", newEntry, newEntry.Length, newEntry.NumberOfBends, this.CombinedCost(newEntry.Length, newEntry.NumberOfBends), this.HeuristicDistanceFromVertexToTarget(newEntry.Vertex.Point, newEntry.Direction)); } DevTraceDisplay(edges.Concat(q).Concat(so).Concat(pathEdges)); }
/// <summary> /// Route a single stage of a possibly multi-stage (due to waypoints) path. /// </summary> /// <param name="sourceVertexEntries">The VertexEntry array that was in the source vertex if it was the target of a prior stage.</param> /// <param name="sources">The enumeration of source vertices; must be only one if sourceVertexEntries is non-null.</param> /// <param name="targets">The enumeration of target vertex entries; must be only one if targetVertexEntries is non-null.</param> /// <param name="targetVertexEntries">The VertexEntry array that is in the target at the end of the stage.</param> private VertexEntry GetPathStage(VertexEntry[] sourceVertexEntries, IEnumerable<VisibilityVertex> sources, VertexEntry[] targetVertexEntries, IEnumerable<VisibilityVertex> targets) { var ssstCalculator = new SsstRectilinearPath(); VertexEntry bestEntry = null; // This contains the best (lowest) path cost after normalizing origins to the center of the sources // and targets. This is used to avoid selecting a vertex pair whose path has more bends than another pair of // vertices, but the bend penalty didn't total enough to offset the additional length between the "better" pair. // This also plays the role of an upper bound on the path length; if a path cost is greater than adjustedMinCost // then we stop exploring it, which saves considerable time after low-cost paths have been found. double bestCost = double.MaxValue / ScanSegment.OverlappedWeight; double bestPathCostRatio = double.PositiveInfinity; // Calculate the bend penalty multiplier. This is a percentage of the distance between the source and target, // so that we have the same relative importance if we have objects of about size 20 that are about 100 apart // as for objects of about size 200 that are about 1000 apart. Point sourceCenter = GetBarycenterOfUniquePortLocations(sources); Point targetCenter = GetBarycenterOfUniquePortLocations(targets); var distance = SsstRectilinearPath.ManhattanDistance(sourceCenter, targetCenter); ssstCalculator.BendsImportance = Math.Max(0.001, distance * (this.bendPenaltyAsAPercentageOfDistance * 0.01)); // We'll normalize by adding (a proportion of) the distance (only; not bends) from the current endpoints to // their centers. This is similar to routeToCenter, but routing multiple paths like this means we'll always // get at least a tie for the best vertex pair, whereas routeToCenter can introduce extraneous bends // if the sources/targets are not collinear with the center (such as an E-R diagram). // interiorLengthAdjustment is a way to decrease the cost adjustment slightly to allow a bend if it saves moving // a certain proportion of the distance parallel to the object before turning to it. var interiorLengthAdjustment = ssstCalculator.LengthImportance; // VertexEntries for the current pass of the current stage, if multistage. var tempTargetEntries = (targetVertexEntries != null) ? this.currentPassTargetEntries : null; // Process closest pairs first, so we can skip longer ones (jump out of SsstRectilinear sooner, often immediately). // This means that we'll be consistent on tiebreaking for equal scores with differing bend counts (the shorter // path will win). In overlapped graphs the shortest path may have more higher-weight edges. foreach (var pair in from VisibilityVertexRectilinear source in sources from VisibilityVertexRectilinear target in targets orderby SsstRectilinearPath.ManhattanDistance(source.Point, target.Point) select new { sourceV = source, targetV = target }) { var source = pair.sourceV; var target = pair.targetV; if (PointComparer.Equal(source.Point, target.Point)) { continue; } var sourceCostAdjustment = SsstRectilinearPath.ManhattanDistance(source.Point, sourceCenter) * interiorLengthAdjustment; var targetCostAdjustment = SsstRectilinearPath.ManhattanDistance(target.Point, targetCenter) * interiorLengthAdjustment; var adjustedBestCost = bestCost; if (targetVertexEntries != null) { Array.Clear(tempTargetEntries, 0, tempTargetEntries.Length); adjustedBestCost = ssstCalculator.MultistageAdjustedCostBound(bestCost); } VertexEntry lastEntry = ssstCalculator.GetPathWithCost(sourceVertexEntries, source, sourceCostAdjustment, tempTargetEntries, target, targetCostAdjustment, adjustedBestCost); if (tempTargetEntries != null) { UpdateTargetEntriesForEachDirection(targetVertexEntries, tempTargetEntries, ref bestCost, ref bestEntry); continue; } // This is the final (or only) stage. Break ties by picking the lowest ratio of cost to ManhattanDistance between the endpoints. if (lastEntry == null) { continue; } var costRatio = lastEntry.Cost / SsstRectilinearPath.ManhattanDistance(source.Point, target.Point); if ((lastEntry.Cost < bestCost) || ApproximateComparer.Close(lastEntry.Cost, bestCost) && (costRatio < bestPathCostRatio)) { bestCost = lastEntry.Cost; bestEntry = lastEntry; bestPathCostRatio = lastEntry.Cost / SsstRectilinearPath.ManhattanDistance(source.Point, target.Point); } } return bestEntry; }
internal static IEnumerable <Point> RestorePath(VertexEntry entry) { return(RestorePath(ref entry, null)); }