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 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 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 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 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 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 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 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);
}
}
internal void SetVertexEntry(VertexEntry entry) {
if (this.VertexEntries == null) {
this.VertexEntries = new VertexEntry[4];
}
this.VertexEntries[CompassVector.ToIndex(entry.Direction)] = entry;
}
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 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);
}
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);
}
}
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 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 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));
}
void DevTraceShowAllPartialPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) {
#if DEVTRACE
if (ssstTrace.IsLevel(3))
{
this.TestShowAllPaths(source, mostRecentlyExtendedPath);
}
#endif // DEVTRACE
}
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);
}
}
/// <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));
}