internal void SetVertexEntry(VertexEntry entry)
{
if (this.VertexEntries == null)
{
this.VertexEntries = new VertexEntry[4];
}
this.VertexEntries[CompassVector.ToIndex(entry.Direction)] = entry;
}
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 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 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);
}
