internal AxisEdge(VisibilityVertex source, VisibilityVertex target)
: base(source, target){
RightBound = double.PositiveInfinity;
LeftBound = double.NegativeInfinity;
Direction = CompassVector.DirectionsFromPointToPoint(source.Point, target.Point);
Debug.Assert(Direction == Directions.East || Direction == Directions.North);
}
internal void RemoveFromGraph() {
// Currently all transient removals and edge restorations are done by TransientGraphUtility itself.
foreach (var entrance in PortEntrances) {
entrance.RemoveFromGraph();
}
CenterVertex = null;
}
internal void AddToAdjacentVertex(TransientGraphUtility transUtil
, VisibilityVertex targetVertex, Directions dirToExtend, Rectangle limitRect) {
if (!PointComparer.Equal(this.Point, targetVertex.Point)) {
transUtil.FindOrAddEdge(this.Vertex, targetVertex, InitialWeight);
}
ExtendEdgeChain(transUtil, targetVertex, dirToExtend, limitRect);
}
void ProcessNeighbor(GenericBinaryHeapPriorityQueue<VisibilityVertex> pq, VisibilityEdge l,
VisibilityVertex v) {
var len = l.Length;
var c = current.Distance + len;
if (c >= upperBound)
return;
if (targets.Contains(v)) {
upperBound = c;
closestTarget = v;
}
if (v != source && _visGraph.PreviosVertex(v) == null) {
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
pq.Enqueue(v, c);
} else if (c < v.Distance) {
//This condition should never hold for the dequeued nodes.
//However because of a very rare case of an epsilon error it might!
//In this case DecreasePriority will fail to find "v" and the algorithm will continue working.
//Since v is not in the queue changing its .Distance will not mess up the queue.
//Changing v.Prev is fine since we come up with a path with an insignificantly
//smaller distance.
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
pq.DecreasePriority(v, c);
}
}
/// <summary>
/// needed for shortest path calculations
/// </summary>
internal VisibilityVertex PreviosVertex(VisibilityVertex v)
{
VisibilityEdge prev;
if (!_prevEdgesDictionary.TryGetValue(v, out prev))
return null;
if (prev.Source == v)
return prev.Target;
return prev.Source;
}
internal SingleSourceMultipleTargetsShortestPathOnVisibilityGraph(VisibilityVertex sourceVisVertex,
IEnumerable<VisibilityVertex> targetVisVertices, VisibilityGraph visibilityGraph) {
_visGraph = visibilityGraph;
_visGraph.ClearPrevEdgesTable();
foreach (var v in visibilityGraph.Vertices())
v.Distance = Double.PositiveInfinity;
source = sourceVisVertex;
targets = new Set<VisibilityVertex>(targetVisVertices);
source.Distance = 0;
}
internal static void CalculatePointVisibilityGraph(IEnumerable<Polyline> listOfHoles,
VisibilityGraph visibilityGraph, Point point,
VisibilityKind visibilityKind, out VisibilityVertex qVertex) {
//maybe there is nothing to do
var qv = visibilityGraph.FindVertex(point);
if (qv != null){
qVertex = qv;
return;
}
var calculator = new PointVisibilityCalculator(listOfHoles, visibilityGraph, point, visibilityKind);
calculator.FillGraph();
qVertex = calculator.QVertex;
Debug.Assert(qVertex != null);
}
/// <summary>
///
/// </summary>
/// <param name="pathStart"></param>
/// <param name="pathEnd"></param>
/// <param name="obstacles"></param>
/// <param name="sourceVertex">graph vertex corresponding to the source</param>
/// <param name="targetVertex">graph vertex corresponding to the target</param>
/// <returns></returns>
internal static VisibilityGraph GetVisibilityGraphForShortestPath(Point pathStart, Point pathEnd,
IEnumerable<Polyline> obstacles,
out VisibilityVertex sourceVertex,
out VisibilityVertex targetVertex) {
var holes = new List<Polyline>(OrientHolesClockwise(obstacles));
var visibilityGraph = CalculateGraphOfBoundaries(holes);
var polygons = holes.Select(hole => new Polygon(hole)).ToList();
TangentVisibilityGraphCalculator.AddTangentVisibilityEdgesToGraph(polygons, visibilityGraph);
PointVisibilityCalculator.CalculatePointVisibilityGraph(holes, visibilityGraph, pathStart,
VisibilityKind.Tangent, out sourceVertex);
PointVisibilityCalculator.CalculatePointVisibilityGraph(holes, visibilityGraph, pathEnd,
VisibilityKind.Tangent, out targetVertex);
return visibilityGraph;
}
internal void ExtendEdgeChain(TransientGraphUtility transUtil, VisibilityVertex paddedBorderVertex
, VisibilityVertex targetVertex, Rectangle limitRect, bool routeToCenter)
{
// Extend the edge chain to the opposite side of the limit rectangle.
transUtil.ExtendEdgeChain(targetVertex, limitRect, this.MaxVisibilitySegment, this.pointAndCrossingsList, this.IsOverlapped);
// In order for Nudger to be able to map from the (near-) endpoint vertex to a PortEntry, we must
// always connect a vertex at UnpaddedBorderIntersect to the paddedBorderVertex, even if routeToCenter.
var unpaddedBorderVertex = transUtil.FindOrAddVertex(UnpaddedBorderIntersect);
transUtil.FindOrAddEdge(unpaddedBorderVertex, paddedBorderVertex, this.unpaddedToPaddedBorderWeight);
if (routeToCenter)
{
// Link the CenterVertex to the vertex at UnpaddedBorderIntersect.
transUtil.ConnectVertexToTargetVertex(ObstaclePort.CenterVertex, unpaddedBorderVertex, OutwardDirection, InitialWeight);
}
}
// Adds an edge from this.Vertex to a (possibly new) vertex at an intersection with an
// existing Edge that adjoins the point. We take 'dir' as an input parameter for edge
// extension because we may be on the edge so can't calculate the direction.
internal VisibilityVertex AddEdgeToAdjacentEdge(TransientGraphUtility transUtil
, VisibilityEdge targetEdge, Directions dirToExtend, Rectangle limitRect)
{
Point targetIntersect = StaticGraphUtility.SegmentIntersection(targetEdge, this.Point);
VisibilityVertex targetVertex = transUtil.VisGraph.FindVertex(targetIntersect);
if (null != targetVertex)
{
AddToAdjacentVertex(transUtil, targetVertex, dirToExtend, limitRect);
}
else
{
targetVertex = transUtil.AddEdgeToTargetEdge(this.Vertex, targetEdge, targetIntersect);
}
ExtendEdgeChain(transUtil, targetVertex, dirToExtend, limitRect);
return(targetVertex);
}
internal VisibilityEdge FindNearestPerpendicularOrContainingEdge(VisibilityVertex startVertex
, Directions dir, Point pointLocation)
{
// Similar to FindPerpendicularEdge, but first try to move closer to pointLocation,
// as long as there are edges going in 'dir' that extend to pointLocation.
Directions dirTowardLocation = ~dir& PointComparer.GetDirections(startVertex.Point, pointLocation);
// If Directions. None then pointLocation is collinear.
VisibilityVertex currentVertex = startVertex;
Directions currentDirTowardLocation = dirTowardLocation;
// First move toward pointLocation far as we can.
while (Directions.None != currentDirTowardLocation)
{
VisibilityVertex nextVertex = StaticGraphUtility.FindNextVertex(currentVertex, dirTowardLocation);
if (null == nextVertex)
{
break;
}
if (0 != (CompassVector.OppositeDir(dirTowardLocation)
& PointComparer.GetDirections(nextVertex.Point, pointLocation)))
{
break;
}
currentVertex = nextVertex;
currentDirTowardLocation = ~dir& PointComparer.GetDirections(currentVertex.Point, pointLocation);
}
// Now find the first vertex that has a chain that intersects pointLocation, if any, moving away
// from pointLocation until we find it or arrive back at startVertex.
VisibilityEdge perpEdge;
while (true)
{
perpEdge = FindPerpendicularOrContainingEdge(currentVertex, dir, pointLocation);
if ((null != perpEdge) || (currentVertex == startVertex))
{
break;
}
currentVertex = StaticGraphUtility.FindNextVertex(currentVertex, CompassVector.OppositeDir(dirTowardLocation));
}
return(perpEdge);
}
private void AddCrossingEdge(VisibilityGraph vg, VisibilityVertex lowVertex, VisibilityVertex highVertex, GroupBoundaryCrossing[] crossings)
{
VisibilityEdge edge = null;
if (null != HighestVisibilityVertex)
{
// We may have a case where point xx.xxxxx8 has added an ascending-direction crossing, and now we're on
// xx.xxxxx9 adding a descending-direction crossing. In that case there should already be a VisibilityEdge
// in the direction we want.
if (PointComparer.Equal(this.HighestVisibilityVertex.Point, highVertex.Point))
{
edge = vg.FindEdge(lowVertex.Point, highVertex.Point);
Debug.Assert(edge != null, "Inconsistent forward-backward sequencing in HighVisibilityVertex");
}
else
{
AppendHighestVisibilityVertex(lowVertex);
}
}
if (edge == null)
{
edge = AddVisibilityEdge(lowVertex, highVertex);
}
var crossingsArray = crossings.Select(c => c.Group.InputShape).ToArray();
var prevIsPassable = edge.IsPassable;
if (prevIsPassable == null)
{
edge.IsPassable = delegate { return(crossingsArray.Any(s => s.IsTransparent)); }
}
;
else
{
// Because we don't have access to the previous delegate's internals, we have to chain. Fortunately this
// will never be more than two deep. File Test: Groups_Forward_Backward_Between_Same_Vertices.
edge.IsPassable = delegate { return(crossingsArray.Any(s => s.IsTransparent) || prevIsPassable()); };
}
if (null == LowestVisibilityVertex)
{
SetInitialVisibilityVertex(lowVertex);
}
HighestVisibilityVertex = highVertex;
}
/// <summary>
/// Returns a path
/// </summary>
/// <returns>a path or null if the target is not reachable from the source</returns>
internal IEnumerable<VisibilityVertex> GetPath() {
var pq = new GenericBinaryHeapPriorityQueue<VisibilityVertex>();
source.Distance = 0;
pq.Enqueue(source, 0);
while (!pq.IsEmpty()) {
current = pq.Dequeue();
if (targets.Contains(current))
break;
foreach (var e in current.OutEdges.Where(PassableOutEdge))
ProcessNeighbor(pq, e, e.Target);
foreach (var e in current.InEdges.Where(PassableInEdge))
ProcessNeighbor(pq, e, e.Source);
}
return _visGraph.PreviosVertex(current) == null ? null : CalculatePath();
}
// ReSharper restore InconsistentNaming
private VisibilityEdge CreateEdge(VisibilityVertex first, VisibilityVertex second, double weight)
{
// All edges in the graph are ascending.
VisibilityVertex source = first;
VisibilityVertex target = second;
if (!PointComparer.IsPureLower(source.Point, target.Point))
{
source = second;
target = first;
}
var edge = new TollFreeVisibilityEdge(source, target, weight);
VisibilityGraph.AddEdge(edge);
this.AddedEdges.Add(edge);
return(edge);
}
private static Point GetBarycenterOfUniquePortLocations(IEnumerable <VisibilityVertex> vertices)
{
var center = new Point();
VisibilityVertex prevVertex = null;
int count = 0;
foreach (var vertex in vertices.OrderBy(s => s.Point))
{
if ((prevVertex != null) && ApproximateComparer.CloseIntersections(vertex.Point, prevVertex.Point))
{
continue;
}
prevVertex = vertex;
++count;
center += vertex.Point;
}
return(center / count);
}
private static VisibilityVertex TraverseToFirstVertexAtOrAbove(VisibilityVertex startVertex, Point start, Directions dir)
{
var returnVertex = startVertex;
var oppositeDir = CompassVector.OppositeDir(dir);
for ( ; ;)
{
var nextVertex = StaticGraphUtility.FindNextVertex(returnVertex, dir);
// This returns Directions. None on a match.
if ((null == nextVertex) || (PointComparer.GetDirections(nextVertex.Point, start) == oppositeDir))
{
break;
}
returnVertex = nextVertex;
}
return(returnVertex);
}
internal VisibilityEdge FindOrAddEdge(VisibilityVertex sourceVertex, VisibilityVertex targetVertex, double weight)
{
// Since we're adding transient edges into the graph, we're not doing full intersection
// evaluation; thus there may already be an edge from the source vertex in the direction
// of the target vertex, but ending before or after the target vertex.
Directions dirToTarget = PointComparer.GetPureDirection(sourceVertex, targetVertex);
VisibilityVertex bracketSource, bracketTarget;
// Is there an edge in the chain from sourceVertex in the direction of targetVertex
// that brackets targetvertex?
// <sourceVertex> -> ..1.. -> ..2.. <end> 3
// Yes if targetVertex is at the x above 1 or 2, No if it is at 3. If false, bracketSource
// will be set to the vertex at <end> (if there are any edges in that direction at all).
VisibilityVertex splitVertex = targetVertex;
if (!FindBracketingVertices(sourceVertex, targetVertex.Point, dirToTarget
, out bracketSource, out bracketTarget))
{
// No bracketing of targetVertex from sourceVertex but bracketSource has been updated.
// Is there a bracket of bracketSource from the targetVertex direction?
// 3 <end> ..2.. <- ..1.. <targetVertex>
// Yes if bracketSource is at the x above 1 or 2, No if it is at 3. If false, bracketTarget
// will be set to the vertex at <end> (if there are any edges in that direction at all).
// If true, then bracketSource and splitVertex must be updated.
VisibilityVertex tempSource;
if (FindBracketingVertices(targetVertex, sourceVertex.Point, CompassVector.OppositeDir(dirToTarget)
, out bracketTarget, out tempSource))
{
Debug.Assert(bracketSource == sourceVertex, "Mismatched bracketing detection");
bracketSource = tempSource;
splitVertex = sourceVertex;
}
}
// If null != edge then targetVertex is between bracketSource and bracketTarget and SplitEdge returns the
// first half-edge (and weight is ignored as the split uses the edge weight).
var edge = VisGraph.FindEdge(bracketSource.Point, bracketTarget.Point);
edge = (null != edge)
? this.SplitEdge(edge, splitVertex)
: CreateEdge(bracketSource, bracketTarget, weight);
DevTrace_VerifyEdge(edge);
return(edge);
}
internal void ExtendEdgeChain(VisibilityVertex startVertex, Rectangle limitRect, LineSegment maxVisibilitySegment,
PointAndCrossingsList pacList, bool isOverlapped)
{
var dir = PointComparer.GetDirections(maxVisibilitySegment.Start, maxVisibilitySegment.End);
if (dir == Directions.None)
{
return;
}
Debug.Assert(CompassVector.IsPureDirection(dir), "impure max visibility segment");
// Shoot the edge chain out to the shorter of max visibility or intersection with the limitrect.
StaticGraphUtility.Assert(PointComparer.Equal(maxVisibilitySegment.Start, startVertex.Point) ||
(PointComparer.GetPureDirection(maxVisibilitySegment.Start, startVertex.Point) == dir)
, "Inconsistent direction found", ObstacleTree, VisGraph);
double oppositeFarBound = StaticGraphUtility.GetRectangleBound(limitRect, dir);
Point maxDesiredSplicePoint = StaticGraphUtility.IsVertical(dir)
? ApproximateComparer.Round(new Point(startVertex.Point.X, oppositeFarBound))
: ApproximateComparer.Round(new Point(oppositeFarBound, startVertex.Point.Y));
if (PointComparer.Equal(maxDesiredSplicePoint, startVertex.Point))
{
// Nothing to do.
return;
}
if (PointComparer.GetPureDirection(startVertex.Point, maxDesiredSplicePoint) != dir)
{
// It's in the opposite direction, so no need to do anything.
return;
}
// If maxDesiredSplicePoint is shorter, create a new shorter segment. We have to pass both segments
// through to the worker function so it knows whether it can go past maxDesiredSegment (which may be limited
// by limitRect).
var maxDesiredSegment = maxVisibilitySegment;
if (PointComparer.GetDirections(maxDesiredSplicePoint, maxDesiredSegment.End) == dir)
{
maxDesiredSegment = new LineSegment(maxDesiredSegment.Start, maxDesiredSplicePoint);
}
ExtendEdgeChain(startVertex, dir, maxDesiredSegment, maxVisibilitySegment, pacList, isOverlapped);
}
internal void AddToAdjacentVertex(TransientGraphUtility transUtil, VisibilityVertex targetVertex
, Rectangle limitRect, bool routeToCenter)
{
VisibilityVertex borderVertex = transUtil.VisGraph.FindVertex(this.VisibilityBorderIntersect);
if (null != borderVertex)
{
ExtendFromBorderVertex(transUtil, borderVertex, limitRect, routeToCenter);
return;
}
// There is no vertex at VisibilityBorderIntersect, so create it and link it to targetVertex.
// Note: VisibilityBorderIntersect may == targetIntersect if that is on our border, *and*
// targetIntersect may be on the border of a touching obstacle, in which case this will splice
// into or across the adjacent obstacle, which is consistent with "touching is overlapped".
// So we don't use UnpaddedBorderIntersect as prevPoint when calling ExtendEdgeChain.
// VisibilityBorderIntersect may be rounded just one Curve.DistanceEpsilon beyond the ScanSegment's
// perpendicular coordinate; e.g. our X may be targetIntersect.X + Curve.DistanceEpsilon, thereby
// causing the direction from VisibilityBorderIntersect to targetIntersect to be W instead of E.
// So use the targetIntersect if they are close enough; they will be equal for flat borders, and
// otherwise the exact value we use only needs be "close enough" to the border. (We can't use
// CenterVertex as the prevPoint because that could be an impure direction).
// Update: With the change to carry MaxVisibilitySegment within the PortEntrance, PortManager finds
// targetVertex between VisibilityBorderIntersect and MaxVisibilitySegment.End, so this should no longer
// be able to happen.
// See RectilinearTests.PaddedBorderIntersectMeetsIncomingScanSegment for an example of what happens
// when VisibilityBorderIntersect is on the incoming ScanSegment (it jumps out above with borderVertex found).
if (OutwardDirection == PointComparer.GetPureDirection(targetVertex.Point, this.VisibilityBorderIntersect))
{
Debug.Assert(false, "Unexpected reversed direction between VisibilityBorderIntersect and targetVertex");
// ReSharper disable HeuristicUnreachableCode
this.VisibilityBorderIntersect = targetVertex.Point;
borderVertex = targetVertex;
// ReSharper restore HeuristicUnreachableCode
}
else
{
borderVertex = transUtil.FindOrAddVertex(this.VisibilityBorderIntersect);
transUtil.FindOrAddEdge(borderVertex, targetVertex, InitialWeight);
}
ExtendEdgeChain(transUtil, borderVertex, targetVertex, limitRect, routeToCenter);
}
static internal VisibilityEdge FindAdjacentEdge(VisibilityVertex a, Direction dir)
{
foreach (var edge in a.InEdges)
{
if (PointComparer.GetPureDirection(edge.SourcePoint, a.Point) == dir)
{
return(edge);
}
}
foreach (var edge in a.OutEdges)
{
if (PointComparer.GetPureDirection(a.Point, edge.TargetPoint) == dir)
{
return(edge);
}
}
return(null);
}
internal void ExtendEdgeChain(TransientGraphUtility transUtil, VisibilityVertex targetVertex, Directions dirToExtend, Rectangle limitRect) {
// Extend the edge chain to the opposite side of the limit rectangle.
StaticGraphUtility.Assert(PointComparer.Equal(this.Point, targetVertex.Point)
|| (PointComparer.GetPureDirection(this.Point, targetVertex.Point) == dirToExtend)
, "input dir does not match with to-targetVertex direction", transUtil.ObstacleTree, transUtil.VisGraph);
var extendOverlapped = IsOverlapped;
if (extendOverlapped)
{
// The initial vertex we connected to may be on the border of the enclosing obstacle,
// or of another also-overlapped obstacle. If the former, we turn off overlap now.
extendOverlapped = transUtil.ObstacleTree.PointIsInsideAnObstacle(targetVertex.Point, dirToExtend);
}
// If we're inside an obstacle's boundaries we'll never extend past the end of the obstacle
// due to encountering the boundary from the inside. So start the extension at targetVertex.
SegmentAndCrossings segmentAndCrossings = GetSegmentAndCrossings(this.IsOverlapped ? targetVertex : this.Vertex, dirToExtend, transUtil);
transUtil.ExtendEdgeChain(targetVertex, limitRect, segmentAndCrossings.Item1, segmentAndCrossings.Item2, extendOverlapped);
}
internal VisibilityEdge FindOrAddEdge(VisibilityVertex sourceVertex, VisibilityVertex targetVertex, double weight)
{
// Since we're adding transient edges into the graph, we're not doing full intersection
// evaluation; thus there may already be an edge from the source vertex in the direction
// of the target vertex, but ending before or after the target vertex.
Direction dirToTarget = PointComparer.GetPureDirection(sourceVertex, targetVertex);
VisibilityVertex bracketSource, bracketTarget, splitVertex;
GetBrackets(sourceVertex, targetVertex, dirToTarget, out bracketSource, out bracketTarget, out splitVertex);
// If null != edge then targetVertex is between bracketSource and bracketTarget and SplitEdge returns the
// first half-edge (and weight is ignored as the split uses the edge weight).
var edge = VisGraph.FindEdge(bracketSource.Point, bracketTarget.Point);
edge = (edge != null)
? this.SplitEdge(edge, splitVertex)
: CreateEdge(bracketSource, bracketTarget, weight);
DevTrace_VerifyEdge(edge);
return(edge);
}
private void SpliceGroupBoundaryCrossing(VisibilityVertex currentVertex, PointAndCrossings pac, Directions dirToInside)
{
GroupBoundaryCrossing[] crossings = PointAndCrossingsList.ToCrossingArray(pac.Crossings, dirToInside);
if (null != crossings)
{
var outerVertex = VisGraph.FindVertex(pac.Location) ?? AddVertex(pac.Location);
if (currentVertex.Point != outerVertex.Point)
{
FindOrAddEdge(currentVertex, outerVertex);
}
var interiorPoint = crossings[0].GetInteriorVertexPoint(pac.Location);
var interiorVertex = VisGraph.FindVertex(interiorPoint) ?? AddVertex(interiorPoint);
// FindOrAddEdge splits an existing edge so may not return the portion bracketed by outerVertex and interiorVertex.
FindOrAddEdge(outerVertex, interiorVertex);
var edge = VisGraph.FindEdge(outerVertex.Point, interiorVertex.Point);
var crossingsArray = crossings.Select(c => c.Group.InputShape).ToArray();
edge.IsPassable = delegate { return(crossingsArray.Any(s => s.IsTransparent)); };
}
}
internal VisibilityVertex AddEdgeToTargetEdge(VisibilityVertex sourceVertex, VisibilityEdge targetEdge
, Point targetIntersect)
{
StaticGraphUtility.Assert(PointComparer.Equal(sourceVertex.Point, targetIntersect) ||
PointComparer.IsPureDirection(sourceVertex.Point, targetIntersect)
, "non-orthogonal edge request", ObstacleTree, VisGraph);
StaticGraphUtility.Assert(StaticGraphUtility.PointIsOnSegment(targetEdge.SourcePoint, targetEdge.TargetPoint, targetIntersect)
, "targetIntersect is not on targetEdge", ObstacleTree, VisGraph);
// If the target vertex does not exist, we must split targetEdge to add it.
VisibilityVertex targetVertex = VisGraph.FindVertex(targetIntersect);
if (null == targetVertex)
{
targetVertex = AddVertex(targetIntersect);
SplitEdge(targetEdge, targetVertex);
}
FindOrAddEdge(sourceVertex, targetVertex);
return(targetVertex);
}
void ProcessNeighbor(GenericBinaryHeapPriorityQueue <VisibilityVertex> pq, VisibilityVertex u, VisibilityEdge l, VisibilityVertex v)
{
var len = l.Length;
var c = u.Distance + len;
/*
* if (_visGraph.visVertexToId[l.Source] < _g.N || _visGraph.visVertexToId[l.Target] < _g.N)
* {
* if (!(l.Source == _source || l.Target == _source || l.Source == _target || l.Target == _target))
* {
* c = 500;
* }
* }
*/
// (v != _source && _visGraph.PreviosVertex(v) == null)
if (v != _source && _visGraph.PreviosVertex(v) == null)
{
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
if (v != _target)
{
pq.Enqueue(v, H(v));
}
}
else if (v != _source && c < v.Distance)
{ //This condition should never hold for the dequeued nodes.
//However because of a very rare case of an epsilon error it might!
//In this case DecreasePriority will fail to find "v" and the algorithm will continue working.
//Since v is not in the queue changing its .Distance will not influence other nodes.
//Changing v.Prev is fine since we come up with the path with an insignificantly
//smaller distance.
var prevV = _visGraph.PreviosVertex(v);
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
if (v != _target)
{
pq.DecreasePriority(v, H(v));
}
}
}
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);
}
internal void ExtendEdgeChain(TransientGraphUtility transUtil, VisibilityVertex targetVertex, Directions dirToExtend, Rectangle limitRect)
{
// Extend the edge chain to the opposite side of the limit rectangle.
StaticGraphUtility.Assert(PointComparer.Equal(this.Point, targetVertex.Point) ||
(PointComparer.GetPureDirection(this.Point, targetVertex.Point) == dirToExtend)
, "input dir does not match with to-targetVertex direction", transUtil.ObstacleTree, transUtil.VisGraph);
var extendOverlapped = IsOverlapped;
if (extendOverlapped)
{
// The initial vertex we connected to may be on the border of the enclosing obstacle,
// or of another also-overlapped obstacle. If the former, we turn off overlap now.
extendOverlapped = transUtil.ObstacleTree.PointIsInsideAnObstacle(targetVertex.Point, dirToExtend);
}
// If we're inside an obstacle's boundaries we'll never extend past the end of the obstacle
// due to encountering the boundary from the inside. So start the extension at targetVertex.
SegmentAndCrossings segmentAndCrossings = GetSegmentAndCrossings(this.IsOverlapped ? targetVertex : this.Vertex, dirToExtend, transUtil);
transUtil.ExtendEdgeChain(targetVertex, limitRect, segmentAndCrossings.Item1, segmentAndCrossings.Item2, extendOverlapped);
}
internal VisibilityEdge FindOrAddEdge(VisibilityVertex sourceVertex, VisibilityVertex targetVertex, double weight) {
// Since we're adding transient edges into the graph, we're not doing full intersection
// evaluation; thus there may already be an edge from the source vertex in the direction
// of the target vertex, but ending before or after the target vertex.
Directions dirToTarget = PointComparer.GetPureDirection(sourceVertex, targetVertex);
VisibilityVertex bracketSource, bracketTarget;
// Is there an edge in the chain from sourceVertex in the direction of targetVertex
// that brackets targetvertex?
// <sourceVertex> -> ..1.. -> ..2.. <end> 3
// Yes if targetVertex is at the x above 1 or 2, No if it is at 3. If false, bracketSource
// will be set to the vertex at <end> (if there are any edges in that direction at all).
VisibilityVertex splitVertex = targetVertex;
if (!FindBracketingVertices(sourceVertex, targetVertex.Point, dirToTarget
, out bracketSource, out bracketTarget)) {
// No bracketing of targetVertex from sourceVertex but bracketSource has been updated.
// Is there a bracket of bracketSource from the targetVertex direction?
// 3 <end> ..2.. <- ..1.. <targetVertex>
// Yes if bracketSource is at the x above 1 or 2, No if it is at 3. If false, bracketTarget
// will be set to the vertex at <end> (if there are any edges in that direction at all).
// If true, then bracketSource and splitVertex must be updated.
VisibilityVertex tempSource;
if (FindBracketingVertices(targetVertex, sourceVertex.Point, CompassVector.OppositeDir(dirToTarget)
, out bracketTarget, out tempSource)) {
Debug.Assert(bracketSource == sourceVertex, "Mismatched bracketing detection");
bracketSource = tempSource;
splitVertex = sourceVertex;
}
}
// If null != edge then targetVertex is between bracketSource and bracketTarget and SplitEdge returns the
// first half-edge (and weight is ignored as the split uses the edge weight).
var edge = VisGraph.FindEdge(bracketSource.Point, bracketTarget.Point);
edge = (null != edge)
? this.SplitEdge(edge, splitVertex)
: CreateEdge(bracketSource, bracketTarget, weight);
DevTrace_VerifyEdge(edge);
return edge;
}
private void Debug_VerifyNonOverlappedExtension(bool isOverlapped, VisibilityVertex extendVertex, VisibilityVertex nextExtendVertex,
VisibilityVertex spliceSource, VisibilityVertex spliceTarget)
{
if (isOverlapped)
{
return;
}
#if TEST_MSAGL
StaticGraphUtility.Assert(!this.ObstacleTree.SegmentCrossesANonGroupObstacle(extendVertex.Point, nextExtendVertex.Point)
, "extendDir edge crosses an obstacle", this.ObstacleTree, this.VisGraph);
#endif // TEST_MSAGL
if (spliceSource == null)
{
// Only verifying the direct extension.
return;
}
// Verify lateral splices as well.
if ((null == spliceTarget) ||
(null == this.VisGraph.FindEdge(spliceSource.Point, spliceTarget.Point) &&
(null == this.VisGraph.FindEdge(spliceSource.Point, nextExtendVertex.Point))))
{
// If targetVertex isn't null and the proposed edge from nextExtendVertex -> targetVertex
// edge doesn't already exist, then we assert that we're not creating a new edge that
// crosses the obstacle bounds (a bounds-crossing edge may already exist, from a port
// within the obstacle; in that case nextExtendPoint splits that edge). As above, don't
// splice laterally across groups.
StaticGraphUtility.Assert(!this.ObstacleTree.SegmentCrossesAnObstacle(spliceSource.Point, nextExtendVertex.Point)
, "spliceSource->extendVertex edge crosses an obstacle", this.ObstacleTree, this.VisGraph);
// Above we moved spliceTarget over when nextExtendVertex existed, so account
// for that here.
StaticGraphUtility.Assert((null == spliceTarget) ||
(null != this.VisGraph.FindEdge(nextExtendVertex.Point, spliceTarget.Point)) ||
!this.ObstacleTree.SegmentCrossesAnObstacle(nextExtendVertex.Point, spliceTarget.Point)
, "extendVertex->spliceTarget edge crosses an obstacle", this.ObstacleTree, this.VisGraph);
}
}
private SegmentAndCrossings GetSegmentAndCrossings(VisibilityVertex startVertex, Directions dirToExtend, TransientGraphUtility transUtil)
{
var dirIndex = CompassVector.ToIndex(dirToExtend);
var segmentAndCrossings = this.maxVisibilitySegmentsAndCrossings[dirIndex];
if (null == segmentAndCrossings)
{
PointAndCrossingsList pacList;
var maxVisibilitySegment = transUtil.ObstacleTree.CreateMaxVisibilitySegment(startVertex.Point, dirToExtend, out pacList);
segmentAndCrossings = new SegmentAndCrossings(maxVisibilitySegment, pacList);
this.maxVisibilitySegmentsAndCrossings[dirIndex] = segmentAndCrossings;
}
else
{
// For a waypoint this will be a target and then a source, so there may be a different lateral edge to
// connect to. In that case make sure we are consistent in directions - back up the start point if needed.
if (PointComparer.GetDirections(startVertex.Point, segmentAndCrossings.Item1.Start) == dirToExtend)
{
segmentAndCrossings.Item1.Start = startVertex.Point;
}
}
return(segmentAndCrossings);
}
static internal VisibilityVertex FindNextVertex(VisibilityVertex vertex, Directions dir) {
// This function finds the next vertex in the desired direction relative to the
// current vertex, not necessarily the edge orientation, hence it does not use
// EdgeDirection(). This is so the caller can operate on a desired movement
// direction without having to track whether we're going forward or backward
// through the In/OutEdge chain.
int cEdges = vertex.InEdges.Count; // indexing is faster than foreach for Lists
for (int ii = 0; ii < cEdges; ++ii) {
var edge = vertex.InEdges[ii];
if (PointComparer.GetPureDirection(vertex.Point, edge.SourcePoint) == dir) {
return edge.Source;
}
}
// Avoid GetEnumerator overhead.
var outEdgeNode = vertex.OutEdges.IsEmpty() ? null : vertex.OutEdges.TreeMinimum();
for (; outEdgeNode != null; outEdgeNode = vertex.OutEdges.Next(outEdgeNode)) {
var edge = outEdgeNode.Item;
if (PointComparer.GetPureDirection(vertex.Point, edge.TargetPoint) == dir) {
return edge.Target;
}
}
return null;
}
static internal VisibilityVertex FindAdjacentVertex(VisibilityVertex a, Direction dir)
{
// This function finds the next vertex in the desired direction relative to the
// current vertex, not necessarily the edge orientation, hence it does not use
// EdgeDirection(). This is so the caller can operate on a desired movement
// direction without having to track whether we're going forward or backward
// through the In/OutEdge chain.
foreach (var edge in a.InEdges)
{
if (PointComparer.GetPureDirection(a.Point, edge.SourcePoint) == dir)
{
return(edge.Source);
}
}
foreach (var edge in a.OutEdges)
{
if (PointComparer.GetPureDirection(a.Point, edge.TargetPoint) == dir)
{
return(edge.Target);
}
}
return(null);
}
private IEnumerable <VisibilityEdge> GetAllEdgesTest(VisibilityVertex vertex)
{
var processedVertices = new Set <VisibilityVertex>();
var q = new Queue <VisibilityVertex>();
q.Enqueue(vertex);
// Target may not be connected to source.
bool targetFound = (vertex == this.Target);
while (!targetFound)
{
while (q.Count > 0)
{
VisibilityVertex v = q.Dequeue();
if (processedVertices.Contains(v))
{
continue;
}
targetFound |= (v == this.Target);
processedVertices.Insert(v);
foreach (var u in VertsToGo(v).Where(u => !processedVertices.Contains(u)))
{
q.Enqueue(u);
}
foreach (VisibilityEdge edge in v.OutEdges)
{
yield return(edge);
}
}
q.Enqueue(this.Target);
targetFound = true;
}
}
private bool InitEntryDirectionsAtTarget(VisibilityVertex vert)
{
EntryDirectionsToTarget = Directions.None;
// This routine is only called once so don't worry about optimizing foreach.
foreach (var edge in vert.OutEdges)
{
#if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with |= operator
EntryDirectionsToTarget = EntryDirectionsToTarget | CompassVector.DirectionsFromPointToPoint(edge.TargetPoint, vert.Point);
#else
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.TargetPoint, vert.Point);
#endif
}
foreach (var edge in vert.InEdges)
{
#if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with |= operator
EntryDirectionsToTarget = EntryDirectionsToTarget | CompassVector.DirectionsFromPointToPoint(edge.SourcePoint, vert.Point);
#else
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.SourcePoint, vert.Point);
#endif
}
// If this returns false then the target is isolated.
return(EntryDirectionsToTarget != Directions.None);
}
void DevTraceShowAllPartialPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) {
#if DEVTRACE
if (ssstTrace.IsLevel(3))
{
this.TestShowAllPaths(source, mostRecentlyExtendedPath);
}
#endif // DEVTRACE
}
private static VisibilityVertex GetCrossingInteriorVertex(VisibilityGraph vg, VisibilityVertex crossingVertex,
GroupBoundaryCrossing crossing)
{
Point interiorPoint = crossing.GetInteriorVertexPoint(crossingVertex.Point);
return(vg.FindVertex(interiorPoint) ?? vg.AddVertex(interiorPoint));
}
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;
}
void FindBeforeAfterV(VisibilityVertex v, RbTree<VisibilityVertex> nodeBoundaryRbTree,
out VisibilityVertex beforeV, out VisibilityVertex afterV, Point center) {
Point xDir=new Point(1,0);
var vAngle = Point.Angle(xDir, v.Point - center);
var rNode = nodeBoundaryRbTree.FindLast(w => Point.Angle(xDir, w.Point - center) <= vAngle);
beforeV = rNode!=null? rNode.Item : nodeBoundaryRbTree.TreeMaximum().Item;
rNode = nodeBoundaryRbTree.FindFirst(w => Point.Angle(xDir, w.Point - center) >= vAngle);
afterV = rNode != null ? rNode.Item : nodeBoundaryRbTree.TreeMinimum().Item;
}
internal List<Point> GetPath(VisibilityVertex vs, VisibilityVertex vt,
bool shrinkDistances) {
var pathPoints = new List<Point>();
vs.IsShortestPathTerminal = vt.IsShortestPathTerminal = true;
_visGraph.ClearPrevEdgesTable();
var router = new SingleSourceSingleTargetShortestPathOnVisibilityGraph(_visGraph, vs, vt)
{
LengthMultiplier = 0.8,
LengthMultiplierForAStar = 0.3
};
var vpath = router.GetPath(shrinkDistances);
if (vpath == null) {
Console.WriteLine("seeing a null path");
vs.IsShortestPathTerminal = vt.IsShortestPathTerminal = false;
return pathPoints;
}
var path = vpath.ToList();
for (int i = 0; i < path.Count(); i++)
pathPoints.Add(path[i].Point);
vs.IsShortestPathTerminal = vt.IsShortestPathTerminal = false;
return pathPoints;
}
static internal Directions GetPureDirection(VisibilityVertex first, VisibilityVertex second)
{
Assert_Rounded(first.Point);
Assert_Rounded(second.Point);
return(GetPureDirection(first.Point, second.Point));
}
VisibilityVertex GlueOrAddToPolylineAndVisGraph(Point[] polySplitArray, int i, VisibilityVertex v, Polyline poly) {
var ip = polySplitArray[i];
if (ApproximateComparer.Close(v.Point, ip))
return v; // gluing ip to the previous point on the polyline
if (ApproximateComparer.Close(ip, poly.StartPoint.Point)) {
var vv = VisGraph.FindVertex(poly.StartPoint.Point);
Debug.Assert(vv != null);
return vv;
}
poly.AddPoint(ip);
return VisGraph.AddVertex(ip);
}
static VisibilityVertex OtherVertex(VisibilityEdge axisEdge, VisibilityVertex v)
{
return(axisEdge.Source == v?axisEdge.Target:axisEdge.Source);
}
private bool InitEntryDirectionsAtTarget(VisibilityVertex vert) {
EntryDirectionsToTarget = Directions. None;
// This routine is only called once so don't worry about optimizing foreach.
foreach (var edge in vert.OutEdges) {
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.TargetPoint, vert.Point);
}
foreach (var edge in vert.InEdges) {
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.SourcePoint, vert.Point);
}
// If this returns false then the target is isolated.
return EntryDirectionsToTarget != Directions. None;
}
bool AngleIsTooSmallAfterShortcutAtVertex(VisibilityVertex v1, VisibilityVertex v2, VisibilityVertex v3)
{
foreach (var edge in v1.OutEdges)
{
var t = edge.Target;
if (t == v3 || t == v2)
{
continue;
}
//LayoutAlgorithmSettings.ShowDebugCurves(new DebugCurve(1, "red", new LineSegment(v3.Point, v1.Point)),
//new DebugCurve(1, "blue", new LineSegment(v1.Point, t.Point)));
if (AngleIsTooSmall(Point.Angle(v3.Point, v1.Point, t.Point)))
{
return(true);
}
}
foreach (var edge in v1.InEdges)
{
var t = edge.Source;
if (t == v3 || t == v2)
{
continue;
}
if (AngleIsTooSmall(Point.Angle(v3.Point, v1.Point, t.Point)))
{
return(true);
}
}
return(false);
}
bool ShortcutIntersectsNewVerticesOrCuttingInsideNodes(VisibilityVertex v0, VisibilityVertex v, VisibilityVertex v1)
{
Set <LgNodeInfo> oldIntersected = IntersectedByLineSegNodeInfos(v0.Point, v.Point) +
IntersectedByLineSegNodeInfos(v.Point, v1.Point);
Set <LgNodeInfo> newIntersected = IntersectedByLineSegNodeInfos(v0.Point, v1.Point);
if (CuttingInsideOfNodes(newIntersected, v0.Point, v1.Point))
{
return(true);
}
return(!oldIntersected.Contains(newIntersected));
}
bool AngleIsTooSmallAfterShortcut(VisibilityVertex v1, VisibilityVertex v2, VisibilityVertex v3)
{
return(AngleIsTooSmallAfterShortcutAtVertex(v1, v2, v3) || AngleIsTooSmallAfterShortcutAtVertex(v3, v2, v1));
}
static internal VisibilityEdge FindNextEdge(VisibilityGraph vg, VisibilityVertex vertex, Directions dir)
{
VisibilityVertex nextVertex = FindNextVertex(vertex, dir);
return((null == nextVertex) ? null : vg.FindEdge(vertex.Point, nextVertex.Point));
}
static internal Directions EdgeDirection(VisibilityVertex source, VisibilityVertex target)
{
return(PointComparer.GetPureDirection(source.Point, target.Point));
}
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
}
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 DevTracePrintSourceAndTarget(VisibilityVertex source, VisibilityVertex target) {
#if DEVTRACE
if (ssstTrace.IsLevel(2))
{
// Don't use this.Target as it hasn't been set yet.
Console.WriteLine("find path: {0} -> {1} distance {2} sourceAdjust {3} targetAdjust {4} bendPenalty {5}",
source.Point, target.Point, ManhattanDistance(source.Point, target.Point),
this.sourceCostAdjustment, this.targetCostAdjustment, this.BendsImportance);
}
#endif // DEVTRACE
}
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 bool IsInBounds(VisibilityVertex vertex) {
return IsInBounds(vertex.Point);
}
private void TestShowPath(VisibilityVertex source, IEnumerable<Point> pathPoints, double cost, double length, int numberOfBends) {
// ReSharper restore UnusedMember.Local
var edges = GetAllEdgesTest(source).Select(e => (ICurve) (new LineSegment(e.SourcePoint, e.TargetPoint))).
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)),
};
List<DebugCurve> pathEdges = GetPathEdgeDebugCurves(pathPoints, "green");
Console.WriteLine("path {0} -> {1} cost = {2} len = {3} nbend = {4} iters = {5}/{6}",
source.Point, this.Target.Point, cost, length, numberOfBends, this.currentIterations, totalIterations);
DevTraceDisplay(edges.Concat(so).Concat(pathEdges));
}
void SnapToAfterBefore(VisibilityVertex v, RbTree<VisibilityVertex> nodeBoundaryRbTree, Point center, Dictionary<VisibilityEdge, VisibilityVertex> ret, VisibilityEdge e) {
VisibilityVertex beforeV, afterV;
FindBeforeAfterV(v, nodeBoundaryRbTree, out beforeV, out afterV, center);
var beforeAngle = Point.Angle(beforeV.Point - center, v.Point - center);
var afterAngle = Point.Angle(v.Point - center, afterV.Point - center);
ret[e] = beforeAngle <= afterAngle ? beforeV : afterV;
}
private IEnumerable<VisibilityEdge> GetAllEdgesTest(VisibilityVertex vertex) {
var processedVertices = new Set<VisibilityVertex>();
var q = new Queue<VisibilityVertex>();
q.Enqueue(vertex);
// Target may not be connected to source.
bool targetFound = (vertex == this.Target);
while (!targetFound) {
while (q.Count > 0) {
VisibilityVertex v = q.Dequeue();
if (processedVertices.Contains(v)) {
continue;
}
targetFound |= (v == this.Target);
processedVertices.Insert(v);
foreach (var u in VertsToGo(v).Where(u => !processedVertices.Contains(u))) {
q.Enqueue(u);
}
foreach (VisibilityEdge edge in v.OutEdges) {
yield return edge;
}
}
q.Enqueue(this.Target);
targetFound = true;
}
}
int CompareByAngleFromNodeCenter(VisibilityVertex a, VisibilityVertex b, Point center) {
var x = new Point(1, 0);
var aAngle = Point.Angle(x, a.Point - center);
var bAngle = Point.Angle(x, b.Point - center);
return aAngle.CompareTo(bAngle);
}
private static IEnumerable<VisibilityVertex> VertsToGo(VisibilityVertex visibilityVertex) {
foreach (var edge in visibilityVertex.OutEdges) {
yield return edge.Target;
}
foreach (var edge in visibilityVertex.InEdges) {
yield return edge.Source;}
}
// internal VisibilityVertex GetExistingVvCloseBy(Point p) {
// var rect = new Rectangle(p);
// rect.Pad(searchEps);
// VisibilityVertex v=null;
//
//
// var crossed = _visGraphVerticesTree.GetAllIntersecting(rect);
// if (crossed.Length == 0) {
// return null;
// }
// double dist = double.PositiveInfinity;
// for (int i = 0; i < crossed.Length; i++) {
// var vv = crossed[i];
// var vvDist = (vv.Point - p).LengthSquared;
// if (vvDist < dist) {
// dist = vvDist;
// v = vv;
// }
// }
// return v;
// }
void RegisterInTree(VisibilityVertex vv) {
var rect = new Rectangle(vv.Point);
VisibilityVertex treeVv;
if (_visGraphVerticesTree.OneIntersecting(rect, out treeVv)) {
Debug.Assert(treeVv == vv);
return;
}
_visGraphVerticesTree.Add(rect, vv);
}
private VisibilityEdge AddEdgeToClosestSegmentEnd(ScanSegment scanSeg, VisibilityVertex segsegVertex, double weight) {
// FindOrAddEdge will walk until it finds the minimal bracketing vertices.
if (PointComparer.IsPureLower(scanSeg.HighestVisibilityVertex.Point, segsegVertex.Point)) {
return this.TransUtil.FindOrAddEdge(scanSeg.HighestVisibilityVertex, segsegVertex, weight);
}
if (PointComparer.IsPureLower(segsegVertex.Point, scanSeg.LowestVisibilityVertex.Point)) {
return this.TransUtil.FindOrAddEdge(segsegVertex, scanSeg.LowestVisibilityVertex, weight);
}
return this.TransUtil.FindOrAddEdge(scanSeg.LowestVisibilityVertex, segsegVertex);
}
