예제 #1
0
        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);
            }
        }
예제 #2
0
 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);
         }
     });
 }
예제 #4
0
 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);
     }
 }
예제 #5
0
 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);
             }
         }
     });
 }
예제 #6
0
 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);
             }
         }
     });
 }
예제 #7
0
 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);
     }
 }
예제 #8
0
        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);
                }
            });
        }
예제 #10
0
 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);
             //               }
         }
     }
 }
예제 #11
0
        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);
        }
예제 #12
0
 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);
         }
     }
 }
예제 #13
0
        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));
             }
         }
     }
 }
예제 #15
0
        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);
            }
        }
예제 #16
0
        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;
                }
            }
        }
예제 #17
0
        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);
                        }
                    }
                }
            }
        }
예제 #18
0
 public static void DrawLine(this IDebugCanvas canvas, DoubleLineSegment2 segment, StrokeStyle strokeStyle)
 {
     canvas.DrawLine(ToDV3(segment.First), ToDV3(segment.Second), strokeStyle);
 }
예제 #19
0
 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);
 }
예제 #20
0
 public static void FillTriangle(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, DoubleVector2 p3, FillStyle fillStyle)
 {
     canvas.FillTriangle(ToDV3(p1), ToDV3(p2), ToDV3(p3), fillStyle);
 }
예제 #21
0
 public static void DrawLine(this IDebugCanvas canvas, DoubleVector2 p1, DoubleVector2 p2, StrokeStyle strokeStyle)
 {
     canvas.DrawLine(ToDV3(p1), ToDV3(p2), strokeStyle);
 }
예제 #22
0
        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);
            }
        }
예제 #23
0
 public static void DrawTriangle(this IDebugCanvas canvas, IntVector2 p1, IntVector2 p2, IntVector2 p3, StrokeStyle strokeStyle)
 {
     canvas.DrawTriangle(ToDV3(p1), ToDV3(p2), ToDV3(p3), strokeStyle);
 }
예제 #24
0
 public static void DrawPoints(this IDebugCanvas canvas, IReadOnlyList <DoubleVector2> p, StrokeStyle strokeStyle)
 {
     canvas.DrawPoints(p.Map(ToDV3), strokeStyle);
 }
예제 #25
0
 public static void DrawPoint(this IDebugCanvas canvas, DoubleVector2 p, StrokeStyle strokeStyle)
 {
     canvas.DrawPoint(ToDV3(p), strokeStyle);
 }
예제 #26
0
 public static void DrawText(this IDebugCanvas canvas, string text, IntVector2 point)
 {
     canvas.DrawText(text, ToDV3(point));
 }
예제 #27
0
 public static void DrawLineStrip(this IDebugCanvas canvas, IReadOnlyList <DoubleVector2> points, StrokeStyle strokeStyle)
 {
     canvas.DrawLineStrip(points.Map(ToDV3), strokeStyle);
 }
예제 #28
0
        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));
        }
예제 #29
0
        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;
예제 #30
0
 public static void DrawLineList(this IDebugCanvas canvas, IReadOnlyList <IntLineSegment2> segments, StrokeStyle strokeStyle)
 {
     canvas.DrawLineList(segments.Map(ToILS3), strokeStyle);
 }