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;
}
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 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 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 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);
}
}
/// <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 void Set(VisibilityVertexRectilinear v, double w)
{
this.Vertex = v;
this.Weight = w;
}
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;
}
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);
}