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);
}
}
internal VisibilityEdge AddEdge(Point source, Point target)
{
VisibilityEdge edge;
var sourceV = FindVertex(source);
VisibilityVertex targetV = null;
if (sourceV != null)
{
targetV = FindVertex(target);
if (targetV != null && sourceV.TryGetEdge(targetV, out edge))
{
return(edge);
}
}
if (sourceV == null)
{ //then targetV is also null
sourceV = AddVertex(source);
targetV = AddVertex(target);
}
else if (targetV == null)
{
targetV = AddVertex(target);
}
edge = new VisibilityEdge(sourceV, targetV);
sourceV.OutEdges.Insert(edge);
targetV.InEdges.Add(edge);
return(edge);
}
static internal VisibilityEdge FindEdge(VisibilityEdge edge)
{
if (edge.Source.TryGetEdge(edge.Target, out edge))
{
return(edge);
}
return(null);
}
internal void RemoveEdge(Point p1, Point p2)
{
// the order of p1 and p2 is not important.
VisibilityEdge edge = FindEdge(p1, p2);
if (edge != null)
{
edge.Source.RemoveOutEdge(edge);
edge.Target.RemoveInEdge(edge);
}
}
// 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;
}
static internal VisibilityEdge AddEdge(VisibilityVertex source, VisibilityVertex target)
{
VisibilityEdge visEdge;
if (source.TryGetEdge(target, out visEdge))
{
return(visEdge);
}
Debug.Assert(source != target, "Self-edges are not allowed");
var edge = new VisibilityEdge(source, target);
source.OutEdges.Insert(edge);
target.InEdges.Add(edge);
return(edge);
}
static internal VisibilityEdge AddEdge(VisibilityVertex source, VisibilityVertex target)
{
VisibilityEdge visEdge;
if (source.TryGetEdge(target, out visEdge))
{
return(visEdge);
}
if (source == target)
{
Debug.Assert(false, "Self-edges are not allowed");
throw new InvalidOperationException("Self-edges are not allowed");
}
var edge = new VisibilityEdge(source, target);
source.OutEdges.Insert(edge);
target.InEdges.Add(edge);
return(edge);
}
bool PassableInEdge(VisibilityEdge e) {
return e.Source == _target || e.Target == _source || !IsForbidden(e);
}
static string GetEdgeColor(VisibilityEdge e, Port sourcePort, Port targetPort) {
if (sourcePort == null || targetPort == null)
return "green";
if (ApproximateComparer.Close(e.SourcePoint, sourcePort.Location) ||
ApproximateComparer.Close(e.SourcePoint, targetPort.Location)
|| ApproximateComparer.Close(e.TargetPoint, sourcePort.Location) ||
ApproximateComparer.Close(e.TargetPoint, targetPort.Location))
return "lightgreen";
return e.IsPassable == null || e.IsPassable() ? "green" : "red";
}
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);
}
static internal void RemoveEdge(VisibilityEdge edge)
{
edge.Source.OutEdges.Remove(edge); //not efficient!
edge.Target.InEdges.Remove(edge); //not efficient
}
static bool IsReflectionEdge(VisibilityEdge edge) {
return (null != edge) && (edge.Weight == ScanSegment.ReflectionWeight);
}
internal static bool IsForbidden(VisibilityEdge e) {
return e.IsPassable != null && !e.IsPassable() || e is TollFreeVisibilityEdge;
}
static internal bool IsAscending(VisibilityEdge edge) {
return IsAscending(EdgeDirection(edge));
}
// Determine the direction of an edge.
static internal Directions EdgeDirection(VisibilityEdge edge) {
return EdgeDirection(edge.Source, edge.Target);
}
static internal VisibilityVertex HighVertex(VisibilityEdge edge) {
return IsAscending(edge) ? edge.Target : edge.Source;
}
static internal bool IsVertical(VisibilityEdge edge) {
return IsVertical(PointComparer.GetPureDirection(edge.SourcePoint, edge.TargetPoint));
}
static internal Point SegmentIntersection(VisibilityEdge edge, Point from) {
return SegmentIntersection(edge.SourcePoint, edge.TargetPoint, from);
}
static CubicBezierSegment BezierOnEdge(VisibilityEdge edge) {
return new CubicBezierSegment(edge.SourcePoint, 2.0 / 3.0 * edge.SourcePoint + 1.0 / 3.0 * edge.TargetPoint,
1.0 / 3.0 * edge.SourcePoint + 2.0 / 3.0 * edge.TargetPoint, edge.TargetPoint);
}
void TryToCreateNewEdgeAndSetIsPassable(VisibilityEdge edge, Shape looseShape) {
var e = visGraph.FindEdge(edge.SourcePoint, edge.TargetPoint);
if (e != null) return;
e = visGraph.AddEdge(edge.SourcePoint, edge.TargetPoint);
if (looseShape != null)
e.IsPassable = () => looseShape.IsTransparent;
}
bool PassableOutEdge(VisibilityEdge e) {
return e.Source == source ||
targets.Contains(e.Target) ||
!IsForbidden(e);
}
static internal VisibilityVertex GetVertex(VisibilityEdge edge, Directions dir) {
Directions edgeDir = EdgeDirection(edge);
Debug.Assert(0 != (dir & (edgeDir | CompassVector.OppositeDir(edgeDir))), "dir is orthogonal to edge");
return (dir == edgeDir) ? edge.Target : edge.Source;
}
bool PassableInEdge(VisibilityEdge e) {
return targets.Contains(e.Source) || e.Target == source || !IsForbidden(e);
}
static bool ParallelToDirection(VisibilityEdge edge, Directions direction) {
switch (direction) {
case Directions.North:
case Directions.South:
return ApproximateComparer.Close(edge.SourcePoint.X, edge.TargetPoint.X);
default:
return ApproximateComparer.Close(edge.SourcePoint.Y, edge.TargetPoint.Y);
}
}
static VisibilityVertex OtherVertex(VisibilityEdge axisEdge, VisibilityVertex v) {
return axisEdge.Source==v?axisEdge.Target:axisEdge.Source;
}
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;
}
static internal void AddEdge(VisibilityEdge edge)
{
Debug.Assert(edge.Source != edge.Target);
edge.Source.OutEdges.Insert(edge);
edge.Target.InEdges.Add(edge);
}
internal bool EdgeIsPassable(VisibilityEdge e) {
return ((!e.Source.IsTerminal) && (!e.Target.IsTerminal)) ||
(e.Source.IsShortestPathTerminal || e.Target.IsShortestPathTerminal);
}
static bool IsSkippableSpliceSourceEdgeWithNullTarget(VisibilityEdge spliceSourceEdge) {
return (null != spliceSourceEdge)
&& (null != spliceSourceEdge.IsPassable)
&& (PointComparer.Equal(spliceSourceEdge.Length, GroupBoundaryCrossing.BoundaryWidth));
}
internal VisibilityEdge SplitEdge(VisibilityEdge edge, VisibilityVertex splitVertex) {
// If the edge is NULL it means we could not find an appropriate one, so do nothing.
if (null == edge) {
return null;
}
StaticGraphUtility.Assert(StaticGraphUtility.PointIsOnSegment(edge.SourcePoint, edge.TargetPoint, splitVertex.Point)
, "splitVertex is not on edge", ObstacleTree, VisGraph);
if (PointComparer.Equal(edge.Source.Point, splitVertex.Point) || PointComparer.Equal(edge.Target.Point, splitVertex.Point)) {
// No split needed.
return edge;
}
// Store the original edge, if needed.
if (!(edge is TollFreeVisibilityEdge)) {
edgesToRestore.Add(edge);
}
VisibilityGraph.RemoveEdge(edge);
// If this is an overlapped edge, or we're in sparseVg, then it may be an unpadded->padded edge that crosses
// over another obstacle's padded boundary, and then either a collinear splice from a free point or another
// obstacle in the same cluster starts splicing from that leapfrogged boundary, so we have the edges:
// A -> D | D is unpadded, A is padded border of sourceObstacle
// B -> C -> E -> F | B and C are vertical ScanSegments between A and D
// <-- splice direction is West | F is unpadded, E is padded border of targetObstacle
// Now after splicing F to E to C to B we go A, calling FindOrAddEdge B->A; the bracketing process finds
// A->D which we'll be splitting at B, which would wind up with A->B, B->C, B->D, having to Eastward
// outEdges from B. See RectilinearTests.Reflection_Block1_Big_UseRect for overlapped, and
// RectilinearTests.FreePortLocationRelativeToTransientVisibilityEdgesSparseVg for sparseVg.
// To avoid this we add the edges in each direction from splitVertex with FindOrAddEdge. If we've
// come here from a previous call to FindOrAddEdge, then that call has found the bracketing vertices,
// which are the endpoints of 'edge', and we've removed 'edge', so we will not call SplitEdge again.
if ((this.IsSparseVg || (edge.Weight == ScanSegment.OverlappedWeight)) && (splitVertex.Degree > 0)) {
FindOrAddEdge(splitVertex, edge.Source, edge.Weight);
return FindOrAddEdge(splitVertex, edge.Target, edge.Weight);
}
// Splice it into the graph in place of targetEdge. Return the first half, because
// this may be called from AddEdge, in which case the split vertex is the target vertex.
CreateEdge(splitVertex, edge.Target, edge.Weight);
return CreateEdge(edge.Source, splitVertex, edge.Weight);
}
internal void SetPreviousEdge(VisibilityVertex v, VisibilityEdge e)
{
Debug.Assert(v == e.Source || v == e.Target);
_prevEdgesDictionary[v] = e;
}
// Splits an existing Edge to splice in this.Vertex.
internal void SpliceIntoEdge(TransientGraphUtility transUtil, VisibilityEdge edge) {
transUtil.SplitEdge(edge, this.Vertex);
}
private static bool IsPassable(VisibilityEdge edge) {
return edge.IsPassable == null || edge.IsPassable();
}
void ProcessNeighbor(GenericBinaryHeapPriorityQueue<VisibilityVertex> pq, VisibilityVertex u, VisibilityEdge l, VisibilityVertex v) {
var len = l.Length;
var c = u.Distance + len;
// (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 bool IsOnOldTrajectory(VisibilityEdge e)
{
return _edgesOnOldTrajectories.Contains(e);
}
internal SdBoneEdge(VisibilityEdge visibilityEdge, SdVertex source, SdVertex target) {
VisibilityEdge = visibilityEdge;
Source = source;
Target = target;
}
internal bool IsEdgeUsed(VisibilityEdge e) {
int usage;
if (!_usedEdges.TryGetValue(e, out usage))
return false;
return usage > 0;
}
internal VisibilityEdge SplitEdge(VisibilityEdge edge, Point splitPoint) {
return SplitEdge(edge, FindOrAddVertex(splitPoint));
}