public static int[] ComputeSegmentSeeingWaypoints(this PolyNode landNode, DoubleLineSegment2 segment)
{
var crossoverSeeingWaypoints = new List <int>();
var waypointVisibilityPolygons = ComputeWaypointVisibilityPolygons(landNode);
for (int waypointIndex = 0; waypointIndex < waypointVisibilityPolygons.Length; waypointIndex++)
{
var waypoint = landNode.visibilityGraphNodeData.AggregateContourWaypoints[waypointIndex];
var waypointVisibilityPolygon = waypointVisibilityPolygons[waypointIndex];
var waypointVisibilityPolygonBarriers = waypointVisibilityPolygon.Get();
var erodedCrossoverSegmentWaypointDistanceSquared = waypoint.ToDoubleVector2().To((segment.First + segment.Second) / 2.0).SquaredNorm2D();
// TODO: If segment matches waypoint this explodes (can't vispoly stab with its origin)
//var segmentsIndices = waypointVisibilityPolygon.RangeStab(segment);
var crossoverSeen = waypointVisibilityPolygon.IsPartiallyVisible(segment);
//for (var i = 0; i < segmentsIndices.Length && !crossoverSeen; i++) {
// var (rangeStartIndex, rangeEndIndex) = segmentsIndices[i];
// for (var j = rangeStartIndex; j <= rangeEndIndex && !crossoverSeen; j++) {
// if (waypointVisibilityPolygonBarriers[j].MidpointDistanceToOriginSquared >= erodedCrossoverSegmentWaypointDistanceSquared) {
// crossoverSeen = true;
// }
// }
//}
if (crossoverSeen)
{
crossoverSeeingWaypoints.Add(waypointIndex);
}
}
return(crossoverSeeingWaypoints.ToArray());
}
public override List<EdgeJob> EmitCrossoverJobs(double crossoverPointSpacing, LocalGeometryView sourceLgv, LocalGeometryView destinationLgv)
{
// return new List<EdgeJob>();
var sourceSegmentVector = SourceSegment.First.To(SourceSegment.Second).ToDoubleVector2();
var destinationSegmentVector = DestinationSegment.First.To(DestinationSegment.Second).ToDoubleVector2();
var edgeJobs = new List<EdgeJob>();
var streamSource = X(SourceSegment, sourceLgv).ToArray();
var streamDest = X(DestinationSegment, destinationLgv).ToArray();
var crossResult = CrossTheStreams(streamSource, streamDest).ToArray();
foreach (var (source, dest, tStart, tEnd) in crossResult) {
var sourceSubSegment = new DoubleLineSegment2(
SourceSegment.First.ToDoubleVector2() + tStart * sourceSegmentVector,
SourceSegment.First.ToDoubleVector2() + tEnd * sourceSegmentVector);
var destinationSubSegment = new DoubleLineSegment2(
DestinationSegment.First.ToDoubleVector2() + tStart * destinationSegmentVector,
DestinationSegment.First.ToDoubleVector2() + tEnd * destinationSegmentVector);
edgeJobs.Add(new EdgeJob {
EdgeDescription = this,
SourcePolyNode = source,
SourceSegment = sourceSubSegment,
DestinationPolyNode = dest,
DestinationSegment = destinationSubSegment
});
}
return edgeJobs;
}
// Todo: Can we support DV2s?
public int[] AddMany(DoubleLineSegment2 edgeSegment, IntVector2[] points)
{
Interlocked.Increment(ref AddManyInvocationCount);
// return points.Map(p => 0);
var segmentSeeingWaypoints = landPolyNode.ComputeSegmentSeeingWaypoints(edgeSegment);
// It's safe to assume <some> point in points will be new, so preprocess which segments are betwen us and other edge segments
var barriers = landPolyNode.FindContourAndChildHoleBarriers();
Dictionary <DoubleLineSegment2, IntLineSegment2[]> barriersBySegment = indicesBySegment.Map((s, _) => {
Interlocked.Increment(ref AddManyConvexHullsComputed);
var hull = GeometryOperations.ConvexHull4(s.First, s.Second, edgeSegment.First, edgeSegment.Second);
return(barriers.Where(b => {
var barrierDv2 = new DoubleLineSegment2(b.First.ToDoubleVector2(), b.Second.ToDoubleVector2());
return GeometryOperations.SegmentIntersectsConvexPolygonInterior(barrierDv2, hull);
}).ToArray());
});
return(indicesBySegment[edgeSegment] = points.Map(p => {
if (TryAdd(p, out var cpi))
{
Interlocked.Increment(ref CrossoverPointsAdded);
segmentByCrossoverPoint[p] = edgeSegment;
}
return cpi;
}));
bool TryAdd(IntVector2 crossoverPoint, out int crossoverPointIndex)
{
if (!crossoverPoints.TryAdd(crossoverPoint, out crossoverPointIndex))
{
return(false);
}
var(visibleWaypointLinks, visibleWaypointLinksLength, optimalLinkToWaypoints, optimalLinkToCrossovers) = FindOptimalLinksToCrossovers(crossoverPoint, segmentSeeingWaypoints, barriersBySegment);
// visibleWaypointLinksByCrossoverPointIndex.Add(visibleWaypointLinks);
optimalLinkToWaypointsByCrossoverPointIndex.Add(optimalLinkToWaypoints);
optimalLinkToOtherCrossoversByCrossoverPointIndex.Add(optimalLinkToCrossovers);
Trace.Assert(optimalLinkToOtherCrossoversByCrossoverPointIndex.Count == optimalLinkToCrossovers.Count);
for (var otherCpi = 0; otherCpi < crossoverPoints.Count - 1; otherCpi++)
{
var linkToOther = optimalLinkToCrossovers[otherCpi];
var linkFromOther = linkToOther.PriorIndex < 0
? new PathLink {
PriorIndex = linkToOther.PriorIndex, TotalCost = linkToOther.TotalCost
}
: new PathLink {
PriorIndex = optimalLinkToWaypointsByCrossoverPointIndex[otherCpi][linkToOther.PriorIndex].PriorIndex,
TotalCost = linkToOther.TotalCost
};
optimalLinkToOtherCrossoversByCrossoverPointIndex[otherCpi].Add(linkFromOther);
}
return(true);
}
}
public static void DrawLine(this IDebugCanvas canvas, DoubleLineSegment2 segment, StrokeStyle strokeStyle)
{
canvas.DrawLine(ToDV3(segment.First), ToDV3(segment.Second), strokeStyle);
}
