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