public static void FillPolygonTriangulation(this IDebugCanvas canvas, Polygon2 poly, FillStyle fillStyle)
{
var cps = new Polygon(poly.Points.Map(p => new PolygonPoint(p.X, p.Y)));
P2T.Triangulate(cps);
foreach (var triangle in cps.Triangles)
{
canvas.FillTriangle(ToDV3(triangle.Points[0]), ToDV3(triangle.Points[1]), ToDV3(triangle.Points[2]), fillStyle);
}
}
public static void DrawRectangle(this IDebugCanvas canvas, IntRect2 nodeRect, float z, StrokeStyle strokeStyle)
{
canvas.DrawLineStrip(
new [] {
new DoubleVector3(nodeRect.Left, nodeRect.Top, z),
new DoubleVector3(nodeRect.Right, nodeRect.Top, z),
new DoubleVector3(nodeRect.Right, nodeRect.Bottom, z),
new DoubleVector3(nodeRect.Left, nodeRect.Bottom, z),
new DoubleVector3(nodeRect.Left, nodeRect.Top, z)
}, strokeStyle);
}
public static void DrawLineStrip(this IDebugCanvas canvas, IReadOnlyList <DoubleVector3> points, StrokeStyle strokeStyle)
{
canvas.BatchDraw(() => {
for (var i = 0; i < points.Count - 1; i++)
{
var a = points[i];
var b = points[i + 1];
canvas.DrawLine(a, b, strokeStyle);
}
});
}
public static void DrawEntityPaths(this IDebugCanvas debugCanvas, EntityService entityService)
{
foreach (var(i, entity) in entityService.EnumerateEntities().Enumerate())
{
var mc = entity.MovementComponent;
if (mc?.PathingRoadmap == null || mc?.Swarm != null || !mc.IsPathfindingEnabled)
{
continue;
}
DrawRoadmap(debugCanvas, mc.PathingRoadmap, mc);
}
}
public static void FillTriangulation(this IDebugCanvas canvas, Triangulation triangulation, FillStyle fillStyle)
{
canvas.BatchDraw(() => {
foreach (var island in triangulation.Islands)
{
foreach (var triangle in island.Triangles)
{
canvas.FillTriangle(triangle, fillStyle);
}
}
});
}
public static void DrawTriangulation(this IDebugCanvas canvas, Triangulation triangulation, StrokeStyle strokeStyle)
{
canvas.BatchDraw(() => {
foreach (var island in triangulation.Islands)
{
foreach (var triangle in island.Triangles)
{
canvas.DrawTriangle(triangle, strokeStyle);
}
}
});
}
private void DrawPathfindingQueryResult(IDebugCanvas debugCanvas, double agentRadius, DoubleVector3 source, DoubleVector3 dest)
{
if (Game.PathfinderCalculator.TryFindPath(agentRadius, source, dest, out var roadmap))
{
Console.WriteLine("Yippee ");
debugCanvas.DrawRoadmap(roadmap);
}
else
{
Console.WriteLine("Nope");
debugCanvas.Transform = Matrix4x4.Identity;
debugCanvas.DrawLine(source, dest, NoPathStroke);
}
}
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 static void DrawLineList(this IDebugCanvas canvas, IReadOnlyList <DoubleVector3> points, StrokeStyle strokeStyle)
{
if (points.Count % 2 != 0)
{
throw new ArgumentException("Line List points must have even length.");
}
canvas.BatchDraw(() => {
for (var i = 0; i < points.Count; i += 2)
{
var a = points[i];
var b = points[i + 1];
canvas.DrawLine(a, b, strokeStyle);
}
});
}
public static void DrawWallPushGrid(this IDebugCanvas canvas, LocalGeometryView lgv, double holeDilationRadius, double xlow = -50, double xhigh = 1100, double xstep = 100, double ylow = -50, double yhigh = 1100, double ystep = 100)
{
for (double x = xlow; x < xhigh; x += xstep)
{
for (double y = ylow; y < yhigh; y += ystep)
{
throw new NotImplementedException();
// var query = new DoubleVector2(x, y);
// DoubleVector2 nearestLandPoint;
// var isInHole = lgv.FindNearestLandPointAndIsInHole(query, out nearestLandPoint);
// canvas.DrawPoint(new DoubleVector3(query), isInHole ? InHoleStrokeStyle : InLandStrokeStyle);
// if (isInHole) {
// canvas.DrawLine(new DoubleVector3(query), new DoubleVector3(nearestLandPoint), NearestLandStrokeStyle);
// }
}
}
}
private void DrawBvhAABB <TValue>(IDebugCanvas debugCanvas, BvhTreeAABB <TValue> bvhRoot)
{
var q = new Queue <(BvhTreeAABB <TValue>, int)>();
q.Enqueue((bvhRoot, 0));
var maxDepth = -1;
while (q.Count > 0)
{
var(node, depth) = q.Dequeue();
if (node.First == null)
{
maxDepth = Math.Max(depth, maxDepth);
}
else
{
q.Enqueue((node.First, depth + 1));
q.Enqueue((node.Second, depth + 1));
}
}
void Helper(BvhTreeAABB <TValue> bvh, int depth = 0)
{
if (bvh.First != null)
{
var r = maxDepth == 0 ? 0 : (int)(255 * depth / (float)maxDepth);
var g = 255 - r;
debugCanvas.DrawAxisAlignedBoundingBox(bvh.Bounds, new StrokeStyle(Color.FromArgb(r, g, 0), 3));
Helper(bvh.First, depth + 1);
Helper(bvh.Second, depth + 1);
}
else
{
for (var i = bvh.StartIndexInclusive; i < bvh.EndIndexExclusive; i++)
{
debugCanvas.DrawAxisAlignedBoundingBox(bvh.BoundingBoxes[i], new StrokeStyle(Color.Black, 3));
}
}
}
Helper(bvhRoot);
}
private void DrawEntityMotionVectors(IDebugCanvas debugCanvas)
{
debugCanvas.Transform = Matrix4x4.Identity;
foreach (var(i, entity) in EntityService.EnumerateEntities().Enumerate())
{
var mc = entity.MovementComponent;
if (mc?.Swarm != null)
{
var local = mc.LocalPosition;
var motionVectorUnnormalized = mc.WeightedSumNBodyForces;
if (motionVectorUnnormalized.Norm2D() < 1E-9)
{
continue;
}
var v = motionVectorUnnormalized.ToUnit() * mc.TerrainOverlayNetworkNode.SectorNodeDescription.WorldToLocalScalingFactor * 100;
var goalWorld = mc.TerrainOverlayNetworkNode.SectorNodeDescription.LocalToWorld(local + v);
debugCanvas.DrawLine(mc.WorldPosition, goalWorld, StrokeStyle.RedHairLineSolid);
}
}
}
public static void DrawPolyNode(this IDebugCanvas canvas, PolyNode polytree, StrokeStyle landStroke = null, StrokeStyle holeStroke = null)
{
landStroke = landStroke ?? new StrokeStyle(Color.Orange);
holeStroke = holeStroke ?? new StrokeStyle(Color.Brown);
canvas.BatchDraw(() => {
var s = new Stack <PolyNode>();
s.Push(polytree);
while (s.Any())
{
var node = s.Pop();
node.Childs.ForEach(s.Push);
if (node.Contour.Any())
{
canvas.DrawPolygonContour(
new Polygon2(node.Contour.Map(p => new IntVector2(p.X, p.Y)).ToList()),
node.IsHole ? holeStroke : landStroke);
}
}
});
}
public static void DrawTriangulationQuadTree(this IDebugCanvas debugCanvas, Triangulation triangulation)
{
foreach (var island in triangulation.Islands)
{
var s = new Stack <Tuple <int, QuadTree <int> .Node> >();
s.Push(Tuple.Create(0, island.TriangleIndexQuadTree.Root));
while (s.Any())
{
var tuple = s.Pop();
var depth = tuple.Item1;
var node = tuple.Item2;
debugCanvas.DrawRectangle(node.Rect, 0.0f, new StrokeStyle(Color.Black));
if (node.TopLeft != null)
{
s.Push(Tuple.Create(depth + 1, node.TopLeft));
s.Push(Tuple.Create(depth + 1, node.TopRight));
s.Push(Tuple.Create(depth + 1, node.BottomLeft));
s.Push(Tuple.Create(depth + 1, node.BottomRight));
}
}
}
}
public static void DrawVisibilityPolygon(this IDebugCanvas debugCanvas, VisibilityPolygon avss, double z = 0.0, FillStyle fillStyle = null, StrokeStyle angleBoundaryStrokeStyle = null, StrokeStyle visibleWallStrokeStyle = null)
{
fillStyle = fillStyle ?? DefaultFillStyle;
var oxy = avss.Origin;
foreach (var range in avss.Get().Where(range => range.Id != VisibilityPolygon.RANGE_ID_INFINITELY_FAR && range.Id != VisibilityPolygon.RANGE_ID_INFINITESIMALLY_NEAR))
{
var rstart = DoubleVector2.FromRadiusAngle(100, range.ThetaStart);
var rend = DoubleVector2.FromRadiusAngle(100, range.ThetaEnd);
var s = range.Segment;
var s1 = s.First.ToDoubleVector2();
var s2 = s.Second.ToDoubleVector2();
DoubleVector2 visibleStart, visibleEnd;
if (!GeometryOperations.TryFindLineLineIntersection(oxy, oxy + rstart, s1, s2, out visibleStart) ||
!GeometryOperations.TryFindLineLineIntersection(oxy, oxy + rend, s1, s2, out visibleEnd))
{
continue;
}
debugCanvas.FillTriangle(oxy, visibleStart, visibleEnd, fillStyle);
debugCanvas.DrawLine(
new DoubleVector3(oxy.X, oxy.Y, z),
new DoubleVector3(visibleStart.X, visibleStart.Y, z),
angleBoundaryStrokeStyle ?? DefaultAngleBoundaryStrokeStyle);
debugCanvas.DrawLine(
new DoubleVector3(oxy.X, oxy.Y, z),
new DoubleVector3(visibleEnd.X, visibleEnd.Y, z),
angleBoundaryStrokeStyle ?? DefaultAngleBoundaryStrokeStyle);
debugCanvas.DrawLine(
new DoubleVector3(visibleStart.X, visibleStart.Y, z),
new DoubleVector3(visibleEnd.X, visibleEnd.Y, z),
visibleWallStrokeStyle ?? DefaultVisibleWallStrokeStyle);
}
}
public static void DrawRoadmap(this IDebugCanvas debugCanvas, MotionRoadmap roadmap, MovementComponent movementComponent = null)
{
var skip = movementComponent?.PathingRoadmapProgressIndex ?? 0;
foreach (var(i, action) in roadmap.Plan.Skip(skip).Enumerate())
{
switch (action)
{
case MotionRoadmapWalkAction walk:
debugCanvas.Transform = Matrix4x4.Identity;
var s = i == 0 && movementComponent != null
? movementComponent.WorldPosition
: Vector3.Transform(new Vector3(walk.Source.X, walk.Source.Y, 0), walk.Node.SectorNodeDescription.WorldTransform).ToOpenMobaVector();
var t = Vector3.Transform(new Vector3(walk.Destination.X, walk.Destination.Y, 0), walk.Node.SectorNodeDescription.WorldTransform).ToOpenMobaVector();
// Console.WriteLine("S: " + s + "\t AND T: " + t);
// for (var i = 0; i < 100; i++) {
// debugCanvas.DrawPoint((s * (100 - i) + t * i) / 100, new StrokeStyle(Color.Cyan, 50));
// }
debugCanvas.DrawLine(s, t, PathStroke);
break;
}
}
}
private void DrawEntities(IDebugCanvas debugCanvas)
{
foreach (var(i, entity) in EntityService.EnumerateEntities().Enumerate())
{
// if (i == 2 || i == 1) continue;
var movementComponent = entity.MovementComponent;
if (movementComponent != null)
{
debugCanvas.Transform = Matrix4x4.Identity;
debugCanvas.DrawPoint(movementComponent.WorldPosition, new StrokeStyle(Color.Black, 2 * movementComponent.BaseRadius));
debugCanvas.DrawPoint(movementComponent.WorldPosition, new StrokeStyle(Color.White, 2 * movementComponent.BaseRadius - 2));
// if (movementComponent.Swarm != null && movementComponent.WeightedSumNBodyForces.Norm2D() > GeometryOperations.kEpsilon) {
// var direction = movementComponent.WeightedSumNBodyForces.ToUnit() * movementComponent.BaseRadius;
// var to = movementComponent.WorldPosition + new DoubleVector3(direction.X, direction.Y, 0.0);
// debugCanvas.DrawLine(movementComponent.WorldPosition, to, new StrokeStyle(Color.Gray));
// }
//if (movementComponent.DebugLines != null)
// debugCanvas.DrawLineList(
// movementComponent.DebugLines.SelectMany(pair => new[] { pair.Item1, pair.Item2 }).ToList(),
// new StrokeStyle(Color.Black));
continue;
var terrainOverlayNetwork = TerrainService.CompileSnapshot().OverlayNetworkManager.CompileTerrainOverlayNetwork(entity.MovementComponent.BaseRadius);
if (terrainOverlayNetwork.TryFindTerrainOverlayNode(movementComponent.WorldPosition, out var node, out var plocal))
{
debugCanvas.Transform = node.SectorNodeDescription.WorldTransform;
//debugCanvas.DrawTriangulation(node.LocalGeometryView.Triangulation, StrokeStyle.BlackHairLineSolid);
if (node.LocalGeometryView.Triangulation.TryIntersect(plocal.X, plocal.Y, out var island, out var triangleIndex))
{
debugCanvas.DrawTriangle(island.Triangles[triangleIndex], StrokeStyle.RedHairLineSolid);
}
}
}
}
}
public static void DrawLine(this IDebugCanvas canvas, DoubleLineSegment2 segment, StrokeStyle strokeStyle)
{
canvas.DrawLine(ToDV3(segment.First), ToDV3(segment.Second), strokeStyle);
}
public static void DrawTriangle(this IDebugCanvas canvas, Triangle3 triangle, StrokeStyle strokeStyle)
{
canvas.DrawLineStrip(
triangle.Points.Concat(new[] { triangle.Points.A }).Select(p => new DoubleVector3(p.X, p.Y, 0)).ToList(),
strokeStyle);
}
public static void FillTriangle(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, DoubleVector2 p3, FillStyle fillStyle)
{
canvas.FillTriangle(ToDV3(p1), ToDV3(p2), ToDV3(p3), fillStyle);
}
public static void DrawLine(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, StrokeStyle strokeStyle)
{
canvas.DrawLine(ToDV3(p1), ToDV3(p2), strokeStyle);
}
private void DrawTestPathfindingQueries(IDebugCanvas debugCanvas, double agentRadius)
{
Console.WriteLine("!@#@!#!@#!@#!");
var pathfinderResultContext = Game.PathfinderCalculator.UniformCostSearch(
agentRadius,
new DoubleVector3(-800, 300, 0),
new[] {
new DoubleVector3(-1200, 300, 0),
new DoubleVector3(-1250, 0, 0),
new DoubleVector3(1250, -80, 0),
new DoubleVector3(1250, -280, 0)
},
true,
null,
debugCanvas);
Console.WriteLine("!@!@#!#@#!@#!@!@!@!@#!#@!#!@#!@#!");
// var prc2 = Game.PathfinderCalculator.UniformCostSearch(
// agentRadius,
// new DoubleVector3(-800, 300, 0),
// new[] {
// new DoubleVector3(-1220, 330, 0),
// new DoubleVector3(-1250, -300, 0),
// new DoubleVector3(1290, -80, 0),
// new DoubleVector3(1250, -380, 0)
// },
// true,
// pathfinderResultContext);
Console.WriteLine("!@#@");
for (var i = 0; i < 4; i++)
{
if (pathfinderResultContext.TryComputeRoadmap(i, out var roadmap))
{
debugCanvas.DrawRoadmap(roadmap);
}
// if (prc2.TryComputeRoadmap(i, out roadmap)) {
// DrawRoadmap(debugCanvas, roadmap);
// }
}
return;
var testPathFindingQueries = new[] {
// Tuple.Create(new DoubleVector3(-600, 300, 0), new DoubleVector3(950, 950, 0)),
// Tuple.Create(new DoubleVector3(900, 750, 0), new DoubleVector3(2100, 800, 0))
Tuple.Create(new DoubleVector3(1250, -80, 0), new DoubleVector3(-800, 300, 0)),
// Tuple.Create(new DoubleVector3(-800, 300, 0), new DoubleVector3(1250, -80, 0))
// Tuple.Create(new DoubleVector3(200, 700, 0), new DoubleVector3(2200, 200, 0))
// Tuple.Create(new DoubleVector3(60, 40, 0), new DoubleVector3(930, 300, 0)),
// Tuple.Create(new DoubleVector3(675, 175, 0), new DoubleVector3(825, 300, 0)),
// Tuple.Create(new DoubleVector3(50, 900, 0), new DoubleVector3(950, 475, 0)),
// Tuple.Create(new DoubleVector3(50, 500, 0), new DoubleVector3(80, 720, 0))
};
foreach (var query in testPathFindingQueries)
{
DrawPathfindingQueryResult(debugCanvas, agentRadius, query.Item1, query.Item2);
}
}
public static void DrawTriangle(this IDebugCanvas canvas, IntVector2 p1, IntVector2 p2, IntVector2 p3, StrokeStyle strokeStyle)
{
canvas.DrawTriangle(ToDV3(p1), ToDV3(p2), ToDV3(p3), strokeStyle);
}
public static void DrawPoints(this IDebugCanvas canvas, IReadOnlyList <DoubleVector2> p, StrokeStyle strokeStyle)
{
canvas.DrawPoints(p.Map(ToDV3), strokeStyle);
}
public static void DrawPoint(this IDebugCanvas canvas, DoubleVector2 p, StrokeStyle strokeStyle)
{
canvas.DrawPoint(ToDV3(p), strokeStyle);
}
public static void DrawText(this IDebugCanvas canvas, string text, IntVector2 point)
{
canvas.DrawText(text, ToDV3(point));
}
public static void DrawLineStrip(this IDebugCanvas canvas, IReadOnlyList <DoubleVector2> points, StrokeStyle strokeStyle)
{
canvas.DrawLineStrip(points.Map(ToDV3), strokeStyle);
}
public bool TryFindPath(double agentRadius, DoubleVector3 sourceWorld, DoubleVector3 destinationWorld, out MotionRoadmap roadmap, IDebugCanvas debugCanvas = null)
{
roadmap = null;
var terrainSnapshot = terrainService.CompileSnapshot();
var terrainOverlayNetwork = terrainSnapshot.OverlayNetworkManager.CompileTerrainOverlayNetwork(agentRadius);
if (!terrainOverlayNetwork.TryFindTerrainOverlayNode(sourceWorld.ToDotNetVector(), out var sourceNode))
{
return(false);
}
if (!terrainOverlayNetwork.TryFindTerrainOverlayNode(destinationWorld.ToDotNetVector(), out var destinationNode))
{
return(false);
}
var sourceLocal = Vector3.Transform(sourceWorld.ToDotNetVector(), sourceNode.SectorNodeDescription.WorldTransformInv);
var destinationLocal = Vector3.Transform(destinationWorld.ToDotNetVector(), destinationNode.SectorNodeDescription.WorldTransformInv);
return(TryFindPath(
sourceNode,
sourceLocal.ToOpenMobaVector().LossyToIntVector3().XY,
destinationNode,
destinationLocal.ToOpenMobaVector().LossyToIntVector3().XY,
out roadmap,
debugCanvas));
}
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;
public static void DrawLineList(this IDebugCanvas canvas, IReadOnlyList <IntLineSegment2> segments, StrokeStyle strokeStyle)
{
canvas.DrawLineList(segments.Map(ToILS3), strokeStyle);
}