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 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 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); } }
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); } }
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; }
internal void Set(VisibilityVertexRectilinear v, double w) { this.Vertex = v; this.Weight = w; }
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; }
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; }
/// <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 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); }
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. #if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with &= operator this.EntryDirectionsToTarget = this.EntryDirectionsToTarget & ~bestEntry.Direction; #else this.EntryDirectionsToTarget &= ~bestEntry.Direction; #endif 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); }
internal void Clear() { this.Vertex = null; this.Weight = double.NaN; }
/// <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); }