private void GetEdges()
{
var halfWidth = _boxCollider.size.x / 2.0f;
var halfHeight = _boxCollider.size.y / 2.0f;
var upperLeft = new Vector3(-halfWidth, halfHeight);
var lowerLeft = new Vector3(-halfWidth, -halfHeight);
var upperRight = new Vector3(halfWidth, halfHeight);
var lowerRight = new Vector3(halfWidth, -halfHeight);
var leftEdge = new VisibilityEdge(lowerLeft, upperLeft);
var topEdge = new VisibilityEdge(upperLeft, upperRight);
var rightEdge = new VisibilityEdge(upperRight, lowerRight);
var bottomEdge = new VisibilityEdge(lowerRight, lowerLeft);
leftEdge.NextEdge = topEdge;
leftEdge.PreviousEdge = bottomEdge;
topEdge.NextEdge = rightEdge;
topEdge.PreviousEdge = leftEdge;
rightEdge.NextEdge = bottomEdge;
rightEdge.PreviousEdge = topEdge;
bottomEdge.NextEdge = leftEdge;
bottomEdge.PreviousEdge = rightEdge;
Edges = new List <VisibilityEdge>()
{
leftEdge,
topEdge,
rightEdge,
bottomEdge
};
}
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);
}
void AddEdge(Point a, Point b)
{
Debug.Assert(PortLocations.Contains(a));
/*********************
* A complication arises when we have overlaps. Loose obstacles become large enough to contain several
* ports. We need to avoid a situation when a port has degree more than one.
* To avoid this situation we redirect to b every edge incoming into a.
* Notice that we create a new graph for each AddDiriction call, so all this edges point roughly to the
* direction of the sweep and the above procedure just alignes the edges better.
* In the resulting graph, which contains the sum of the graphs passed to AddDirection, of course
* a port can have an incoming and outcoming edge at the same time
*******************/
VisibilityEdge ab = visibilityGraph.AddEdge(a, b);
VisibilityVertex av = ab.Source;
Debug.Assert(av.Point == a && ab.TargetPoint == b);
//all edges adjacent to a which are different from ab
VisibilityEdge[] edgesToFix =
av.InEdges.Where(e => e != ab).Concat(av.OutEdges.Where(e => e != ab)).ToArray();
foreach (VisibilityEdge edge in edgesToFix)
{
Point c = (edge.Target == av ? edge.Source : edge.Target).Point;
VisibilityGraph.RemoveEdge(edge);
visibilityGraph.AddEdge(c, b);
}
}
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);
}
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 != sources && _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 bool IsEdgeUsed(VisibilityEdge e)
{
int usage;
if (!_usedEdges.TryGetValue(e, out usage))
{
return(false);
}
return(usage > 0);
}
internal void ConnectVertexToTargetEdge(VisibilityVertex sourceVertex, VisibilityEdge targetEdge,
Point targetIntersect, Directions finalEdgeDir, double weight)
{
VisibilityVertex targetVertex = FindOrAddVertex(targetIntersect);
if ((targetVertex != targetEdge.Source) && (targetVertex != targetEdge.Target))
{
SplitEdge(targetEdge, targetVertex);
}
ConnectVertexToTargetVertex(sourceVertex, targetVertex, finalEdgeDir, weight);
}
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));
}
}
// ReSharper disable InconsistentNaming
void DevTrace_VerifyEdge(VisibilityEdge edge)
{
#if DEVTRACE
if (transGraphVerify.IsLevel(1))
{
Debug.Assert(PointComparer.IsPureLower(edge.SourcePoint, edge.TargetPoint), "non-ascending edge");
// For A -> B -> C, make sure there is no A -> C, and vice-versa. Simplest way to do
// this is to ensure that there is only one edge in any given direction for each vertex.
DevTrace_VerifyVertex(edge.Source);
DevTrace_VerifyVertex(edge.Target);
}
#endif // DEVTRACE
}
private VisibilityEdge AddVisibilityEdge(VisibilityVertex source, VisibilityVertex target)
{
Debug.Assert(source.Point != target.Point, "Self-edges are not allowed");
Debug.Assert(PointComparer.IsPureLower(source.Point, target.Point), "Impure or reversed direction encountered");
// Make sure we aren't adding two edges in the same direction to the same vertex.
Debug.Assert(null == StaticGraphUtility.FindAdjacentVertex(source, StaticGraphUtility.EdgeDirection(source, target))
, "Duplicate outEdge from Source vertex");
Debug.Assert(null == StaticGraphUtility.FindAdjacentVertex(target, StaticGraphUtility.EdgeDirection(target, source))
, "Duplicate inEdge to Target vertex");
var edge = new VisibilityEdge(source, target, this.Weight);
VisibilityGraph.AddEdge(edge);
return(edge);
}
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));
}
// 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);
}
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;
}
internal void ModifySkeletonWithNewBoundaryOnLayer(LgNodeInfo nodeInfo)
{
Dictionary <VisibilityEdge, VisibilityVertex> edgeSnapMap = GetEdgeSnapAtVertexMap(nodeInfo);
foreach (var t in edgeSnapMap)
{
VisibilityEdge edge = t.Key;
VisibilityGraph.RemoveEdge(edge);
var midleV = edgeSnapMap[edge];
if (nodeInfo.Center != edge.SourcePoint)
{
VisibilityGraph.AddEdge(edge.Source, midleV);
}
else
{
VisibilityGraph.AddEdge(midleV, edge.Target);
}
}
}
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);
}
static VisibilityVertex OtherVertex(VisibilityEdge axisEdge, VisibilityVertex v)
{
return(axisEdge.Source == v?axisEdge.Target:axisEdge.Source);
}
// Determine the direction of an edge.
static internal Directions EdgeDirection(VisibilityEdge edge)
{
return(EdgeDirection(edge.Source, edge.Target));
}
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 bool DevTrace_IsEndOfEdge(VisibilityEdge e, Point x)
{
return(PointComparer.Equal(e.SourcePoint, x) || PointComparer.Equal(e.TargetPoint, x));
}
internal SdBoneEdge(VisibilityEdge visibilityEdge, SdVertex source, SdVertex target)
{
VisibilityEdge = visibilityEdge;
Source = source;
Target = target;
}
bool PassableInEdge(VisibilityEdge e)
{
return(e.Source == target || e.Target == source || !IsForbidden(e));
}
private static bool IsPassable(VisibilityEdge edge)
{
return(edge.IsPassable == null || edge.IsPassable());
}
internal bool EdgeIsPassable(VisibilityEdge e)
{
return(((!e.Source.IsTerminal) && (!e.Target.IsTerminal)) ||
(e.Source.IsShortestPathTerminal || e.Target.IsShortestPathTerminal));
}
bool PassableOutEdge(VisibilityEdge e)
{
return(e.Source == sources ||
targets.Contains(e.Target) ||
!IsForbidden(e));
}
static internal Point SegmentIntersection(VisibilityEdge edge, Point from)
{
return(SegmentIntersection(edge.SourcePoint, edge.TargetPoint, from));
}
bool PassableInEdge(VisibilityEdge e)
{
return(targets.Contains(e.Source) || e.Target == sources || !IsForbidden(e));
}
static internal bool IsVertical(VisibilityEdge edge)
{
return(IsVertical(PointComparer.GetPureDirection(edge.SourcePoint, edge.TargetPoint)));
}
void ProcessNeighbor(GenericBinaryHeapPriorityQueue <VisibilityVertex> pq, VisibilityVertex u, VisibilityEdge l, VisibilityVertex v)
{
var len = l.Length;
var c = u.Distance + len;
if (v != source && _visGraph.PreviosVertex(v) == null)
{
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
if (v != target)
{
pq.Enqueue(v, H(v));
}
}
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 influence other nodes.
//Changing v.Prev is fine since we come up with the path with an insignificantly
//smaller distance.
{
v.Distance = c;
_visGraph.SetPreviousEdge(v, l);
if (v != target)
{
pq.DecreasePriority(v, H(v));
}
}
}
static bool IsSkippableSpliceSourceEdgeWithNullTarget(VisibilityEdge spliceSourceEdge)
{
return((null != spliceSourceEdge) &&
(null != spliceSourceEdge.IsPassable) &&
(PointComparer.Equal(spliceSourceEdge.Length, GroupBoundaryCrossing.BoundaryWidth)));
}
