public TerrainOverlayNetworkEdgeGroup(TerrainOverlayNetworkNode source, TerrainOverlayNetworkNode destination, EdgeJob edgeJob, TerrainOverlayNetworkEdge[] edges) { Source = source; Destination = destination; EdgeJob = edgeJob; Edges = edges; }
public static void DrawCrossSectorVisibilityPolygon( this IDebugCanvas canvas, TerrainOverlayNetworkNode terrainNode, IntVector2 visibilityPolygonOrigin, FillStyle fillStyle = null ) { fillStyle = fillStyle ?? kDefaultFillStyle; canvas.Transform = terrainNode.SectorNodeDescription.WorldTransform; // canvas.DrawPoint(visibilityPolygonOrigin, StrokeStyle.RedThick25Solid); var visibilityPolygon = VisibilityPolygon.Create(visibilityPolygonOrigin.ToDoubleVector2(), terrainNode.LandPolyNode.FindContourAndChildHoleBarriers()); var visibleCrossoverSegmentsByNeighbor = FindVisibleCrossoverSegmentsByNeighborAndClearLocalAt(canvas, terrainNode, visibilityPolygon, visibilityPolygonOrigin); canvas.DrawVisibilityPolygon(visibilityPolygon, fillStyle: fillStyle ?? kDefaultFillStyle, angleBoundaryStrokeStyle: StrokeStyle.None, visibleWallStrokeStyle: StrokeStyle.None); var visibilityPolygonOriginWorld = Vector3.Transform(new Vector3(visibilityPolygonOrigin.ToDotNetVector(), 0), terrainNode.SectorNodeDescription.WorldTransform); foreach (var(neighbor, inboundCrossoverSegments) in visibleCrossoverSegmentsByNeighbor) { var neighborPolygonOrigin = Vector3.Transform(visibilityPolygonOriginWorld, neighbor.SectorNodeDescription.WorldTransformInv); Z(canvas, new IntVector2((int)neighborPolygonOrigin.X, (int)neighborPolygonOrigin.Y), neighbor, inboundCrossoverSegments, new HashSet <TerrainOverlayNetworkNode> { terrainNode }, fillStyle); } }
public bool IsDestinationReachable(TerrainOverlayNetworkNode sourceNode, TerrainOverlayNetworkNode destinationNode) { var visited = new HashSet <TerrainOverlayNetworkNode>(); bool Visit(TerrainOverlayNetworkNode n) { if (!visited.Add(n)) { return(false); } if (n == destinationNode) { return(true); } foreach (var neighbor in n.OutboundEdgeGroups.Keys) { Visit(neighbor); } return(false); } return(Visit(sourceNode)); }
public bool TryFindPath(TerrainOverlayNetworkNode sourceNode, IntVector2 sourcePoint, TerrainOverlayNetworkNode destinationNode, IntVector2 destinationPoint, out MotionRoadmap result, IDebugCanvas debugCanvas = null) { if (debugCanvas != null) { debugCanvas.Transform = Matrix4x4.Identity; } if (sourceNode == destinationNode) { var roadmap = new MotionRoadmap(); if (sourcePoint == destinationPoint || sourceNode.LandPolyNode.SegmentInLandPolygonNonrecursive(sourcePoint, destinationPoint)) { roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourcePoint, destinationPoint)); result = roadmap; return(true); } var sourceVisibleWaypointLinks = sourceNode.CrossoverPointManager.FindVisibleWaypointLinks(sourcePoint, null, out var sourceVisibleWaypointLinksLength, out var sourceOptimalLinkToWaypoints); var destinationVisibleWaypointLinks = sourceNode.CrossoverPointManager.FindVisibleWaypointLinks(destinationPoint, null, out var destinationVisibleWaypointLinksLength, out var destinationOptimalLinkToWaypoints); var bestFirstWaypoint = -1; var bestFirstWaypointCost = double.PositiveInfinity; for (var i = 0; i < sourceVisibleWaypointLinksLength; i++) { var link = sourceVisibleWaypointLinks[i]; var firstWaypoint = link.PriorIndex; var cost = link.TotalCost + destinationOptimalLinkToWaypoints[firstWaypoint].TotalCost; if (cost < bestFirstWaypointCost) { bestFirstWaypoint = firstWaypoint; bestFirstWaypointCost = cost; } } roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourcePoint, sourceNode.CrossoverPointManager.Waypoints[bestFirstWaypoint])); AddInterTerrainOverlayNetworkNodeWaypointToWaypointRoadmapActions(roadmap, sourceNode, bestFirstWaypoint, destinationOptimalLinkToWaypoints[bestFirstWaypoint].PriorIndex); roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourceNode.CrossoverPointManager.Waypoints[destinationOptimalLinkToWaypoints[bestFirstWaypoint].PriorIndex], destinationPoint)); result = roadmap; return(true); } const int SOURCE_POINT_CPI = -100; const int DESTINATION_POINT_CPI = -200; // todo: special-case if src is dst node // Console.WriteLine("Src had " + sourceNode.CrossoverPointManager.CrossoverPoints.Count + " : " + string.Join(", ", sourceNode.CrossoverPointManager.CrossoverPoints)); var(_, _, _, sourceOptimalLinkToCrossovers) = sourceNode.CrossoverPointManager.FindOptimalLinksToCrossovers(sourcePoint); var(_, _, _, destinationOptimalLinkToCrossovers) = destinationNode.CrossoverPointManager.FindOptimalLinksToCrossovers(destinationPoint); var q = new PriorityQueue <ValueTuple <float, float, TerrainOverlayNetworkNode, int, TerrainOverlayNetworkNode, int, TerrainOverlayNetworkEdge> >((a, b) => a.Item1.CompareTo(b.Item1)); var priorityUpperBounds = new Dictionary <(TerrainOverlayNetworkNode, int), float>(); var predecessor = new Dictionary <(TerrainOverlayNetworkNode, int), (TerrainOverlayNetworkNode, int, TerrainOverlayNetworkEdge, float)>(); // visited foreach (var kvp in sourceNode.OutboundEdgeGroups) { foreach (var g in kvp.Value) { foreach (var edge in g.Edges) { var cpiLink = sourceOptimalLinkToCrossovers[edge.SourceCrossoverIndex]; var worldCpiLinkCost = cpiLink.TotalCost * sourceNode.SectorNodeDescription.LocalToWorldScalingFactor; priorityUpperBounds[(sourceNode, edge.SourceCrossoverIndex)] = worldCpiLinkCost;
private static MultiValueDictionary <TerrainOverlayNetworkNode, IntLineSegment2> FindVisibleCrossoverSegmentsByNeighborAndClearLocalAt( IDebugCanvas canvas, TerrainOverlayNetworkNode terrainNode, VisibilityPolygon visibilityPolygon, IntVector2 visibilityPolygonOrigin, HashSet <TerrainOverlayNetworkNode> visited = null) { var visibleCrossoverSegmentsByNeighbor = MultiValueDictionary <TerrainOverlayNetworkNode, IntLineSegment2> .Create(() => new HashSet <IntLineSegment2>()); foreach (var outboundEdgeGroup in terrainNode.OutboundEdgeGroups) { var otherTerrainNode = outboundEdgeGroup.Key; if (visited?.Contains(otherTerrainNode) ?? false) { continue; } foreach (var outboundEdge in outboundEdgeGroup.Value) { var ranges = visibilityPolygon.Get(); (IntLineSegment2, bool) FlipMaybeSorta(IntLineSegment2 x) => GeometryOperations.Clockness(visibilityPolygonOrigin, x.First, x.Second) == Clockness.CounterClockwise ? (new IntLineSegment2(x.Second, x.First), true) : (x, false); var(localCrossoverSegment, lcsFlipped) = FlipMaybeSorta(outboundEdge.EdgeJob.EdgeDescription.SourceSegment); var(remoteCrossoverSegment, rcsFlipped) = FlipMaybeSorta(outboundEdge.EdgeJob.EdgeDescription.DestinationSegment); // todo: clamp visibleStartT, visibleEndT to account for agent radius eroding crossover segmetmentnt var rangeIndexIntervals = visibilityPolygon.RangeStab(localCrossoverSegment); var locallyClearedSegments = new List <IntLineSegment2>(); foreach (var(startIndexInclusive, endIndexInclusive) in rangeIndexIntervals) { for (var i = startIndexInclusive; i <= endIndexInclusive; i++) { if (ranges[i].Id == VisibilityPolygon.RANGE_ID_INFINITELY_FAR || ranges[i].Id == VisibilityPolygon.RANGE_ID_INFINITESIMALLY_NEAR) { continue; } var seg = ranges[i].Segment; var rstart = DoubleVector2.FromRadiusAngle(100, ranges[i].ThetaStart) * 100; var rend = DoubleVector2.FromRadiusAngle(100, ranges[i].ThetaEnd) * 100; double visibleStartT, visibleEndT; if (!GeometryOperations.TryFindNonoverlappingLineLineIntersectionT(localCrossoverSegment.First.ToDoubleVector2(), localCrossoverSegment.Second.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2() + rstart, out visibleStartT) || !GeometryOperations.TryFindNonoverlappingLineLineIntersectionT(localCrossoverSegment.First.ToDoubleVector2(), localCrossoverSegment.Second.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2(), visibilityPolygonOrigin.ToDoubleVector2() + rend, out visibleEndT)) { // wtf? Console.WriteLine("???"); continue; } // Todo: I don't actually understand why visibleEndT > 1 is a thing? // t values are for parameterization of crossover line segment, so must be within [0, 1] if ((visibleStartT < 0 && visibleEndT < 0) || (visibleStartT > 1 && visibleEndT > 1)) { continue; } visibleStartT = Math.Min(1.0, Math.Max(0.0, visibleStartT)); visibleEndT = Math.Min(1.0, Math.Max(0.0, visibleEndT)); if (visibilityPolygon.SegmentComparer.Compare(localCrossoverSegment, seg) < 0) { var localVisibleStart = localCrossoverSegment.PointAt(visibleStartT).LossyToIntVector2(); var localVisibleEnd = localCrossoverSegment.PointAt(visibleEndT).LossyToIntVector2(); var remoteVisibleStart = remoteCrossoverSegment.PointAt(lcsFlipped == rcsFlipped ? visibleStartT : 1.0 - visibleStartT).LossyToIntVector2(); var remoteVisibleEnd = remoteCrossoverSegment.PointAt(lcsFlipped == rcsFlipped ? visibleEndT : 1.0 - visibleEndT).LossyToIntVector2(); if (localVisibleStart == localVisibleEnd) { continue; } if (remoteVisibleStart == remoteVisibleEnd) { continue; } var locallyClearedSegment = new IntLineSegment2(localVisibleStart, localVisibleEnd); locallyClearedSegments.Add(locallyClearedSegment); visibleCrossoverSegmentsByNeighbor.Add(otherTerrainNode, new IntLineSegment2(remoteVisibleStart, remoteVisibleEnd)); } } } foreach (var locallyClearedSegment in locallyClearedSegments) { visibilityPolygon.ClearBefore(locallyClearedSegment); } } } return(visibleCrossoverSegmentsByNeighbor); }
private static void Z(this IDebugCanvas canvas, IntVector2 visibilityPolygonOrigin, TerrainOverlayNetworkNode terrainNode, IReadOnlyCollection <IntLineSegment2> inboundCrossoverSegments, HashSet <TerrainOverlayNetworkNode> visited, FillStyle fillStyle) { canvas.Transform = terrainNode.SectorNodeDescription.WorldTransform; // canvas.DrawPoint(visibilityPolygonOrigin, StrokeStyle.RedThick25Solid); var visibilityPolygon = new VisibilityPolygon( visibilityPolygonOrigin.ToDoubleVector2(), new[] { new VisibilityPolygon.IntervalRange { Id = VisibilityPolygon.RANGE_ID_INFINITESIMALLY_NEAR, ThetaStart = 0, ThetaEnd = VisibilityPolygon.TwoPi }, }); foreach (var inboundCrossoverSegment in inboundCrossoverSegments) { visibilityPolygon.ClearBefore(inboundCrossoverSegment); } // Console.WriteLine("===="); foreach (var seg in terrainNode.LandPolyNode.FindContourAndChildHoleBarriers()) { if (GeometryOperations.Clockness(visibilityPolygon.Origin, seg.First.ToDoubleVector2(), seg.Second.ToDoubleVector2()) == Clockness.CounterClockwise) { continue; } visibilityPolygon.Insert(seg); // Console.WriteLine(seg); } // Console.WriteLine("===="); var visibleCrossoverSegmentsByNeighbor = FindVisibleCrossoverSegmentsByNeighborAndClearLocalAt(canvas, terrainNode, visibilityPolygon, visibilityPolygonOrigin, visited); canvas.DrawVisibilityPolygon(visibilityPolygon, fillStyle: fillStyle ?? kDefaultFillStyle, angleBoundaryStrokeStyle: StrokeStyle.None, visibleWallStrokeStyle: StrokeStyle.None); var visibilityPolygonOriginWorld = Vector3.Transform(new Vector3(visibilityPolygonOrigin.ToDotNetVector(), 0), terrainNode.SectorNodeDescription.WorldTransform); foreach (var(neighbor, nextInboundCrossoverSegments) in visibleCrossoverSegmentsByNeighbor) { var neighborPolygonOrigin = Vector3.Transform(visibilityPolygonOriginWorld, neighbor.SectorNodeDescription.WorldTransformInv); //visibilityPolygonOrigin Z(canvas, new IntVector2((int)neighborPolygonOrigin.X, (int)neighborPolygonOrigin.Y), neighbor, nextInboundCrossoverSegments, visited.Concat(new[] { terrainNode }).ToHashSet(), fillStyle); } }
public MotionRoadmapWalkAction(TerrainOverlayNetworkNode node, IntVector2 source, IntVector2 destination) { Node = node; Source = source; Destination = destination; }