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