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