Example #1
0
        private void _CalculateNeighbors()
        {
#if o
            D.WriteLine(" * NavMesh._CalculateNeighbors()");
#endif
            // Fill out the neighbor information for each NavArea
            // Find and create Portals (Portal Nodes) for each NavArea
            #region
            NavArea <TNode, TLink> navArea;
            NavArea <TNode, TLink> otherNavArea;
            AxisAlignedRectangle   navAreaPolygon;
            AxisAlignedRectangle   otherNavAreaPolygon;
            TNode      portalNode;
            SimpleLine portal;
            int        portalId = 1;
            for (int i = 0; i < _NavAreas.Count; i++)
            {
                navArea        = _NavAreas[i];
                navAreaPolygon = navArea.Polygon;
#if o
                D.WriteLine("   navArea: " + navAreaPolygon.Name);
                //(navArea as PositionedNode).CheckedAsMain = true;
#endif
                for (int j = i + 1; j < _NavAreas.Count; j++)
                {
                    otherNavArea        = _NavAreas[j];
                    otherNavAreaPolygon = otherNavArea.Polygon;
#if o
                    D.WriteLine("     otherNavPoly: " + otherNavAreaPolygon.Name);
#endif
                    #region    - Check polygons distance
                    // Check if the other navpoly is within range to touch
                    // Distance between centers
                    var distanceBetweenCenters = RUtils.Distance2D(ref navAreaPolygon.Position, ref otherNavAreaPolygon.Position);
                    // If Distance between centers is bigger than combined radii, they are not in range
                    // If Distance between centers is smaller or equal, they are in range (not necessarily touching)
                    if (distanceBetweenCenters >= navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius)
                    {
                        // Not in range => proceed to another navpoly
#if o
                        D.WriteLine($"       Not in range (distanceBetweenCenters: {distanceBetweenCenters} totalRadii: {navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius})");
#endif
                        continue;
                    }
#if o
                    D.WriteLine($"       In range (distanceBetweenCenters: {distanceBetweenCenters} totalRadii: {navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius})"); \
                    #endif
                    #endregion - Check polygons distance END

                    // The are in range, so check each edge pairing
                    // to find shared edge and shared part of edges = portal
                    #region    - Using common sense
                    // Here I know they do overlap
                    //      Calculate the portal between the two polygons
                    //      - THIS NEEDS TO BE IN CLOCKWISE ORDER, RELATIVE TO EACH POLYGON
                    // _GetSegmentOverlap()
                    //  returns horizontal: always left-to-right
                    //  returns vertical: always bottom-to-top
                    portal = null;
                    // other is above
                    if (navAreaPolygon.Top == otherNavAreaPolygon.Bottom)
                    {
#if o
                        D.WriteLine($"       Other above -- Touching this Top ({navAreaPolygon.Top}) - other Bottom ({otherNavAreaPolygon.Bottom})");
#endif
                        portal = _GetSegmentOverlap(navArea.EdgeTop, otherNavArea.EdgeBottom, true, false);
#if o
                        // Debug visuals
                        if (portal != null)
                        {
                            //_DrawDebugVisualForPortal(portal.Start.X + 3f, portal.Start.Y - 3f, portal.End.X - 3f, portal.End.Y - 3f);
                            Debug.ShowLine(portal, Color.Yellow);
                        }
#endif
                    }
                    // other is below
                    else if (navAreaPolygon.Bottom == otherNavAreaPolygon.Top)
                    {
#if o
                        D.WriteLine($"       Other below -- Touching this Bottom ({navAreaPolygon.Bottom}) - other Top ({otherNavAreaPolygon.Top})");
#endif
                        portal = _GetSegmentOverlap(navArea.EdgeBottom, otherNavArea.EdgeTop, true, true);
#if o
                        // Debug visuals
                        if (portal != null)
                        {
                            //_DrawDebugVisualForPortal(portal.Start.X - 3f, portal.Start.Y + 3f, portal.End.X + 3f, portal.End.Y + 3f);
                            Debug.ShowLine(portal, Color.Yellow);
                        }
#endif
                    }
                    // other is to left
                    else if (navAreaPolygon.Left == otherNavAreaPolygon.Right)
                    {
#if o
                        D.WriteLine($"       Other to left -- Touching this Left ({navAreaPolygon.Left}) - other Right ({otherNavAreaPolygon.Right})");
#endif
                        portal = _GetSegmentOverlap(navArea.EdgeLeft, otherNavArea.EdgeRight, false, false);
#if o
                        // Debug visuals
                        if (portal != null)
                        {
                            //_DrawDebugVisualForPortal(portal.Start.X + 3f, portal.Start.Y + 3f, portal.End.X + 3f, portal.End.Y - 3f);
                            Debug.ShowLine(portal, Color.Yellow);
                        }
#endif
                    }
                    // other is to right
                    else if (navAreaPolygon.Right == otherNavAreaPolygon.Left)
                    {
#if o
                        D.WriteLine($"       Other to right -- Touching this Right ({navAreaPolygon.Right}) - other Left ({otherNavAreaPolygon.Left})");
#endif
                        portal = _GetSegmentOverlap(navArea.EdgeRight, otherNavArea.EdgeLeft, false, true);
#if o
                        // Debug visuals
                        if (portal != null)
                        {
                            //_DrawDebugVisualForPortal(portal.Start.X - 3f, portal.Start.Y - 3f, portal.End.X - 3f, portal.End.Y + 3f);
                            Debug.ShowLine(portal, Color.Yellow);
                        }
#endif
                    }
                    // not touching
                    else
                    {
                        // Not actually touching => proceed to another navpoly
#if o
                        D.WriteLine($"       Not Touching");
#endif
                        continue;
                    }
                    #endregion - Using common sense END


                    if (portal != null) // this check IS needed
                    {
                        // Found portal between 2 NavAreas
                        // - have to add portal Node
                        portalNode = PortalNodeBase <TLink, TNode> .Create(portalId, navArea, otherNavArea, portal);

                        portalId++;
                        //
                        // - have to link this portal node to all other Portal Nodes in both this and other NavArea
                        //   BUT at this point I dont know all Portal Nodes in this or the other NavArea
                        //   => store both portal lines (sides) in this Portal Node
                        //   => store both NavAreas in this Portal Node
                        //   => do the linking at the end of NavMesh creation
                        //
                        //   - assign Portal Node to this NavArea
                        navArea.Portals.Add(portalNode);
                        //
                        //   - assign Portal Node to other NavArea
                        otherNavArea.Portals.Add(portalNode);
                        //

                        //navArea.LinkTo(otherNavArea, portal);

                        _PortalNodes.Add(portalNode);
#if o
                        #region    -- Debug / visuals
                        Debug.ShowText(ref portalNode.Position, portalNode.ID.ToString());

                        /*Debug.ShowLine(portal, Color.Yellow);
                         * var circle = ShapeManager.AddCircle();
                         * circle.Radius = 6f;
                         * circle.Color = Color.Yellow;
                         * circle.X = (float)portal.Start.X;
                         * circle.Y = (float)portal.Start.Y;
                         * Debug.ShowLine(navAreaPolygon.Position, otherNavAreaPolygon.Position, Debug.Gray32);*/
                        #endregion -- Debug / visuals END
#endif
                    }
#if o
                    else
                    {
                        D.WriteLine($"       Not Touching");
                    }
#endif
                } // for other j
            }     // for one i
            #endregion

            // Link Portal Nodes together inside each NavArea
            #region
            navArea    = null;
            portalNode = null;
#if o
            TNode otherPortalNode = null;
            //D.WriteLine("========== Portals' Linking ==========");
#endif
            for (int i = 0; i < _NavAreas.Count; i++)
            {
                navArea = _NavAreas[i];
#if o
                //D.WriteLine("NavArea Portals: " + navArea.Portals.Count + " " + navArea.Polygon.Name);
#endif
                if (navArea.Portals.Count > 1)
                {
                    for (int iPortalNode = 0; iPortalNode < navArea.Portals.Count; iPortalNode++)
                    {
                        portalNode = navArea.Portals[iPortalNode]; // as TNode;
#if o
                        //D.WriteLine("  PortalNode " + portalNode.ID);
#endif
                        for (int iOtherPortalNode = iPortalNode + 1; iOtherPortalNode < navArea.Portals.Count; iOtherPortalNode++)
                        {
#if o
                            otherPortalNode = navArea.Portals[iOtherPortalNode]; // as TNode;

                            //D.WriteLine("    Links to " + otherPortalNode.ID);

                            portalNode.LinkTo(otherPortalNode, navArea);
#else
                            portalNode.LinkTo(navArea.Portals[iOtherPortalNode], navArea);
#endif
                        }
                    }
                }
            }
#if o
            //D.WriteLine("====================================\n");
#endif
            #endregion

            #region    -- Debug / visuals for links
#if o
            //D.WriteLine("========== Portals' Links ==========");
            Xna.Vector3 linkLineShift = new Xna.Vector3(2f, 2f, 0f);
            foreach (var navAreaa in NavAreas)
            {
                //D.WriteLine("NavArea " + navAreaa.Polygon.Name);

                foreach (var portalNodee in navAreaa.Portals)
                {
                    //D.WriteLine("  PortalNode " + portalNodee.ID);

                    foreach (var link in portalNodee.Links)
                    {
                        Debug.ShowLine(portalNodee.Position + linkLineShift, link.NodeLinkingTo.Position + linkLineShift, Debug.Gray32);

                        //D.WriteLine("    Links to " + link.NodeLinkingTo.ID);
                    }
                }
            }
            //D.WriteLine("====================================");

            /*// Debug
             * var sb = new StringBuilder("--------------\n");
             * PositionedNode cNode;
             * foreach (var tNode in _NavPolygons)
             * {
             *  cNode = tNode as PositionedNode;
             *  sb.Append(tNode.Polygon.Name).Append(" ").Append(cNode.CheckedAsMain).Append(" ").Append(cNode.CheckedAsOther)
             *    .Append(" ").Append(tNode.Links.Count)
             *    .AppendLine();
             * }
             * sb.AppendLine("--------------");
             * D.WriteLine(sb.ToString());*/
#endif
            #endregion -- Debug visuals END
        }
Example #2
0
        /// <summary>Find a path from the start point to the end point using this nav mesh.</summary>
        /// <param name="startPoint"></param>
        /// <param name="endPoint"></param>
        /// <param name="drawPolyPath">Whether or not to visualize the path through the polygons - e.g. the path that astar found.</param>
        /// <param name="drawFinalPath">Whether or not to visualize the path through the path that was returned.</param>
        /// <returns>An array of points and nodes if a path is found, or nulls if no path</returns>
        public List <Point> FindPath(Point startPoint, Point endPoint, out List <TNode> portalNodesPath
                                     /*, bool drawPolyPath = false, bool drawFinalPath = false*/)
        {
            #region    --- Find the closest poly for the starting and ending point
            NavArea <TNode, TLink> startArea = FindNavRectFromPoint(ref startPoint);
            NavArea <TNode, TLink> endArea   = FindNavRectFromPoint(ref endPoint);

            // No matching polygons locations for the start or end, so no path found
            // = start or end point not on nav mesh
            if (startArea == null || endArea == null)
            {
                portalNodesPath = null;
                return(null);
            }

            // If the start and end polygons are the same, return a direct path
            if (startArea == endArea)
            {
                List <Point> pointsPath = new List <Point> {
                    startPoint, endPoint
                };
                //if (drawFinalPath) this.debugDrawPath(phaserPath, 0xffd900, 10);
                portalNodesPath = null; // not traversing any Portal Nodes
                return(pointsPath);
            }
            #endregion --- Find the closest poly for the starting and ending point END



            #region    --- A* search
            // --- Search!
            portalNodesPath = new List <TNode>();
            GetPath(
#if DEBUG
                PortalNodeBase <TLink, TNode> .CreateFakeNodeDebug(ref startPoint, startArea.Portals, false, startArea),
                PortalNodeBase <TLink, TNode> .CreateFakeNodeDebug(ref endPoint, endArea.Portals, true, endArea),
#else
                PortalNodeBase <TLink, TNode> .CreateFakeStartNode(ref startPoint, startArea.Portals),
                PortalNodeBase <TLink, TNode> .CreateFakeEndNode(ref endPoint, endArea.Portals, endArea),
#endif
                portalNodesPath
                );

            if (portalNodesPath.Count == 0)
            {
                // While the start and end polygons may be valid, no path between them

                portalNodesPath = null;
                return(null);
            }
            #endregion --- A* search END



            #region    --- Funnel algorithm
            // We have a path, so now time for the funnel algorithm
#if o
            D.WriteLine("======== Path search ========");
            D.WriteLine("  --- Path ---");
            foreach (var pathNode in portalNodesPath)
            {
                D.WriteLine("    " + pathNode.ID);
            }
            D.WriteLine("  --- Channel ---");
#endif
            Channel channel = new Channel();
            channel.Add(startPoint);

            SimpleLine portal;
            int        countLimit = portalNodesPath.Count - 1;
            TNode      pathPortalNode;
            TNode      nextPathPortalNode;
            for (int i = 1; i < countLimit; i++) // skipping first Node - I don't need it's Links - I don't need their Portals
            {
                pathPortalNode     = portalNodesPath[i];
                nextPathPortalNode = portalNodesPath[i + 1];
#if o
                D.WriteLine($"    pathPortalNode: {pathPortalNode.ID} and pathPortalNode: {nextPathPortalNode.ID}");
#endif
                // Find the portal
                portal = null;
                foreach (var link in pathPortalNode.Links)
                {
                    //if ( link.NodeLinkingTo.ReferenceEquals(nextPathPortalNode) )
                    if (link.NodeLinkingTo.ID == nextPathPortalNode.ID)
                    {
#if o
                        D.WriteLine($"      link to {link.NodeLinkingTo.ID} - link found and portal added");
#endif
                        portal = link.Portal;
                        // Push the portal vertices into the channel
                        channel.Add(portal.Start, portal.End);
                        break;
                    }
#if o
                    else
                    {
                        D.WriteLine($"      link to {link.NodeLinkingTo.ID} - link NOT found !");
                    }
#endif
                }
            }

            channel.Add(endPoint);


            // Pull a string along the channel to run the funnel
            channel.StringPull();

            PortalNodeBase <TLink, TNode> .CleanupFakeEndNodeLinks(endArea.Portals);

            // Clone path, excluding duplicates - needed ? @
            Point?       lastPoint       = null;
            List <Point> finalPointsPath = new List <Point>();
            foreach (var point in channel.Path)
            {
                //var newPoint = p.clone();
                var newPoint = point;
                //if (!lastPoint || !newPoint.equals(lastPoint))
                if (lastPoint.HasValue == false || newPoint != lastPoint)
                {
                    finalPointsPath.Add(newPoint);
                }
                lastPoint = newPoint;
            }

            return(finalPointsPath);

            #endregion --- Funnel algorithm END
        }