FindOptimalLinksToCrossovers(
IntVector2 p,
int[] candidateWaypoints = null,
IReadOnlyDictionary <DoubleLineSegment2, IntLineSegment2[]> candidateBarriersByDestinationSegment = null
)
{
Interlocked.Increment(ref FindOptimalLinksToCrossoversInvocationCount);
//var links = new List<PathLink>(128);
//links.Resize(crossoverPoints.Count);
//return (new PathLink[0], 0, new PathLink[waypoints.Length], links);
int visibleWaypointLinksLength;
PathLink[] optimalLinkToWaypoints;
var visibleWaypointLinks = FindVisibleWaypointLinks(p, candidateWaypoints, out visibleWaypointLinksLength, out optimalLinkToWaypoints);
// Cost from p to other crossoverPoints...
var optimalLinkToCrossovers = new ExposedArrayList <PathLink>(Math.Max(128, crossoverPoints.Count));
optimalLinkToCrossovers.size = crossoverPoints.Count;
void ProcessCpi(int cpi, IntLineSegment2[] candidateBarriers)
{
// for bench
optimalLinkToCrossovers[cpi] = new PathLink {
PriorIndex = PathLink.ErrorInvalidIndex,
TotalCost = float.PositiveInfinity
};
return;
Interlocked.Increment(ref ProcessCpiInvocationCount);
bool isDirectPath;
if (p == crossoverPoints[cpi])
{
isDirectPath = true; // degenerate segment
}
else if (candidateBarriers == null)
{
// Console.WriteLine($"Try intersect {cpi}: {p} to {crossoverPoints[cpi]}");
var isDirectPath1 = !landPolyNode.FindContourAndChildHoleBarriersBvh().Intersects(new IntLineSegment2(p, crossoverPoints[cpi]));
// var isDirectPath2 = landPolyNode.SegmentInLandPolygonNonrecursive(p, crossoverPoints[cpi]);
// Console.WriteLine($" => res {isDirectPath1} vs {isDirectPath2}");
isDirectPath = isDirectPath1;
}
else
{
// below is equivalent to (and shaved off 22% execution time relative to):
// isDirectPath = candidateBarriers.None(new ILS2(p, crossoverPoints[cpi]).Intersects)
isDirectPath = true;
//var seg = new IntLineSegment2(p, crossoverPoints[cpi]);
for (var bi = 0; bi < candidateBarriers.Length && isDirectPath; bi++)
{
Interlocked.Increment(ref ProcessCpiInvocation_CandidateBarrierIntersectCount);
if (IntLineSegment2.Intersects(
p.X, p.Y, crossoverPoints[cpi].X, crossoverPoints[cpi].Y,
candidateBarriers[bi].X1, candidateBarriers[bi].Y1, candidateBarriers[bi].X2, candidateBarriers[bi].Y2))
{
isDirectPath = false;
}
//if (seg.Intersects(ref candidateBarriers[bi])) isDirectPath = false;
}
}
if (isDirectPath)
{
Interlocked.Increment(ref ProcessCpiInvocation_DirectCount);
var totalCost = p.To(crossoverPoints[cpi]).Norm2F();
Trace.Assert(!float.IsNaN(totalCost));
optimalLinkToCrossovers[cpi] = new PathLink {
PriorIndex = PathLink.DirectPathIndex,
TotalCost = totalCost
};
}
else
{
Interlocked.Increment(ref ProcessCpiInvocation_IndirectCount);
var otherOptimalLinkByWaypointIndex = optimalLinkToWaypointsByCrossoverPointIndex[cpi];
//--
// Below is equivalent to (and shaved off 14% execution time relative to):
// visibleWaypointLinks.MinBy(cpwl => cpwl.TotalCost + otherOptimalLinkByWaypointIndex[cpwl.PriorIndex].TotalCost);
var optimalLinkToOtherCrossoverPointIndex = -1;
var optimalLinkToOtherCrossoverPointCost = float.PositiveInfinity;
for (var vwli = 0; vwli < visibleWaypointLinksLength; vwli++)
{
ref var vwl = ref visibleWaypointLinks[vwli];
var cost = vwl.TotalCost + otherOptimalLinkByWaypointIndex[vwl.PriorIndex].TotalCost;
if (cost < optimalLinkToOtherCrossoverPointCost)
{
optimalLinkToOtherCrossoverPointIndex = vwli;
optimalLinkToOtherCrossoverPointCost = cost;
}
}
if (optimalLinkToOtherCrossoverPointIndex == -1)
{
// Todo: This shouldn't happen!
optimalLinkToCrossovers[cpi] = new PathLink {
PriorIndex = PathLink.ErrorInvalidIndex,
TotalCost = float.PositiveInfinity
};
}
else
{
ref var optimalLinkToOtherCrossoverPoint = ref visibleWaypointLinks[optimalLinkToOtherCrossoverPointIndex];
//--
var optimalLinkFromOtherCrossoverPoint = otherOptimalLinkByWaypointIndex[optimalLinkToOtherCrossoverPoint.PriorIndex];
var totalCost = optimalLinkToOtherCrossoverPoint.TotalCost + optimalLinkFromOtherCrossoverPoint.TotalCost;
Trace.Assert(!float.IsNaN(totalCost));
optimalLinkToCrossovers[cpi] = new PathLink {
PriorIndex = optimalLinkToOtherCrossoverPoint.PriorIndex,
TotalCost = totalCost
};
}
}