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