Exemplo n.º 1
0
        /// <summary>
        /// Update the vertices on the straight path
        /// </summary>
        /// <param name="startIdx">Original path's starting index</param>
        /// <param name="endIdx">Original path's end index</param>
        /// <param name="endPos">The end position</param>
        /// <param name="path">The original path of polygon references</param>
        /// <param name="straightPath">An array of points on the straight path</param>
        /// <param name="straightPathFlags">An array of flags</param>
        /// <param name="straightPathRefs">An array of polygon references</param>
        /// <param name="straightPathCount">The number of points on the path</param>
        /// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
        /// <param name="options">Options flag</param>
        /// <returns></returns>
        public bool AppendPortals(int startIdx, int endIdx, Vector3 endPos, PolyId[] path, Vector3[] straightPath, int[] straightPathFlags, PolyId[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
        {
            Vector3 startPos = straightPath[straightPathCount - 1];

            //append or update last vertex
            bool stat = false;
            for (int i = startIdx; i < endIdx; i++)
            {
                //calculate portal
                PolyId from = path[i];
                MeshTile fromTile;
                Poly fromPoly;
                if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
                    return false;

                PolyId to = path[i + 1];
                MeshTile toTile;
                Poly toPoly;
                if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
                    return false;

                Vector3 left = new Vector3();
                Vector3 right = new Vector3();
                if (GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right) == false)
                    break;

                if ((options & PathBuildFlags.AreaCrossingVertices) != 0)
                {
                    //skip intersection if only area crossings are requested
                    if (fromPoly.Area == toPoly.Area)
                        continue;
                }

                //append intersection
                float s, t;
                if (Intersection.SegmentSegment2D(ref startPos, ref endPos, ref left, ref right, out s, out t))
                {
                    Vector3 pt = Vector3.Lerp(left, right, t);

                    stat = AppendVertex(pt, 0, path[i + 1], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

                    if (stat != true)
                        return true;
                }
            }

            return true;
        }
Exemplo n.º 2
0
        /// <summary>
        /// Add vertices and portals to a regular path computed from the method FindPath().
        /// </summary>
        /// <param name="startPos">Starting position</param>
        /// <param name="endPos">Ending position</param>
        /// <param name="path">Path of polygon references</param>
        /// <param name="pathSize">Length of path</param>
        /// <param name="straightPath">An array of points on the straight path</param>
        /// <param name="straightPathFlags">An array of flags</param>
        /// <param name="straightPathRefs">An array of polygon references</param>
        /// <param name="straightPathCount">The number of points on the path</param>
        /// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
        /// <param name="options">Options flag</param>
        /// <returns>True, if path found. False, if otherwise.</returns>
        public bool FindStraightPath(Vector3 startPos, Vector3 endPos, PolyId[] path, int pathSize, Vector3[] straightPath, int[] straightPathFlags, PolyId[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
        {
            straightPathCount = 0;

            if (path.Length == 0)
                return false;

            bool stat = false;

            Vector3 closestStartPos = new Vector3();
            ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos);

            Vector3 closestEndPos = new Vector3();
            ClosestPointOnPolyBoundary(path[pathSize - 1], endPos, ref closestEndPos);

            stat = AppendVertex(closestStartPos, PathfindingCommon.STRAIGHTPATH_START, path[0], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

            if (!stat)
                return true;

            if (pathSize > 1)
            {
                Vector3 portalApex = closestStartPos;
                Vector3 portalLeft = portalApex;
                Vector3 portalRight = portalApex;
                int apexIndex = 0;
                int leftIndex = 0;
                int rightIndex = 0;

                PolygonType leftPolyType = 0;
                PolygonType rightPolyType = 0;

                PolyId leftPolyRef = path[0];
                PolyId rightPolyRef = path[0];

                for (int i = 0; i < pathSize; i++)
                {
                    Vector3 left = new Vector3();
                    Vector3 right = new Vector3();
                    PolygonType fromType = 0, toType = 0;

                    if (i + 1 < pathSize)
                    {
                        //next portal
                        if (GetPortalPoints(path[i], path[i + 1], ref left, ref right, ref fromType, ref toType) == false)
                        {
                            //failed to get portal points means path[i + 1] is an invalid polygon
                            //clamp end point to path[i] and return path so far
                            if (ClosestPointOnPolyBoundary(path[i], endPos, ref closestEndPos) == false)
                            {
                                //first polygon is invalid
                                return false;
                            }

                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                //append portals
                                stat = AppendPortals(apexIndex, i, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
                            }

                            stat = AppendVertex(closestEndPos, 0, path[i], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

                            return true;
                        }

                        //if starting really close to the portal, advance
                        if (i == 0)
                        {
                            float t;
                            if (Distance.PointToSegment2DSquared(ref portalApex, ref left, ref right, out t) < 0.001 * 0.001)
                                continue;
                        }
                    }
                    else
                    {
                        //end of the path
                        left = closestEndPos;
                        right = closestEndPos;

                        fromType = toType = PolygonType.Ground;
                    }

                    //right vertex
                    float triArea2D;
                    Triangle3.Area2D(ref portalApex, ref portalRight, ref right, out triArea2D);
                    if (triArea2D <= 0.0)
                    {
                        Triangle3.Area2D(ref portalApex, ref portalLeft, ref right, out triArea2D);
                        if (portalApex == portalRight || triArea2D > 0.0)
                        {
                            portalRight = right;
                            rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : PolyId.Null;
                            rightPolyType = toType;
                            rightIndex = i;
                        }
                        else
                        {
                            //append portals along current straight path segment
                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

                                if (stat != true)
                                    return true;
                            }

                            portalApex = portalLeft;
                            apexIndex = leftIndex;

                            int flags = 0;
                            if (leftPolyRef == PolyId.Null)
                                flags = PathfindingCommon.STRAIGHTPATH_END;
                            else if (leftPolyType == PolygonType.OffMeshConnection)
                                flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;

                            PolyId reference = leftPolyRef;

                            //append or update vertex
                            stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

                            if (stat != true)
                                return true;

                            portalLeft = portalApex;
                            portalRight = portalApex;
                            leftIndex = apexIndex;
                            rightIndex = apexIndex;

                            //restart
                            i = apexIndex;

                            continue;
                        }
                    }

                    //left vertex
                    Triangle3.Area2D(ref portalApex, ref portalLeft, ref left, out triArea2D);
                    if (triArea2D >= 0.0)
                    {
                        Triangle3.Area2D(ref portalApex, ref portalRight, ref left, out triArea2D);
                        if (portalApex == portalLeft || triArea2D < 0.0f)
                        {
                            portalLeft = left;
                            leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : PolyId.Null;
                            leftPolyType = toType;
                            leftIndex = i;
                        }
                        else
                        {
                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

                                if (stat != true)
                                    return true;
                            }

                            portalApex = portalRight;
                            apexIndex = rightIndex;

                            int flags = 0;
                            if (rightPolyRef == PolyId.Null)
                                flags = PathfindingCommon.STRAIGHTPATH_END;
                            else if (rightPolyType == PolygonType.OffMeshConnection)
                                flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;

                            PolyId reference = rightPolyRef;

                            //append or update vertex
                            stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

                            if (stat != true)
                                return true;

                            portalLeft = portalApex;
                            portalRight = portalApex;
                            leftIndex = apexIndex;
                            rightIndex = apexIndex;

                            //restart
                            i = apexIndex;

                            continue;
                        }
                    }
                }

                //append portals along the current straight line segment
                if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                {
                    stat = AppendPortals(apexIndex, pathSize - 1, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

                    if (stat != true)
                        return true;
                }
            }

            stat = AppendVertex(closestEndPos, PathfindingCommon.STRAIGHTPATH_END, PolyId.Null, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

            return true;
        }
Exemplo n.º 3
0
        /// <summary>
        /// Add vertices and portals to a regular path computed from the method FindPath().
        /// </summary>
        /// <param name="startPos">Starting position</param>
        /// <param name="endPos">Ending position</param>
        /// <param name="path">Path of polygon references</param>
        /// <param name="pathSize">Length of path</param>
        /// <param name="straightPath">An array of points on the straight path</param>
        /// <param name="straightPathFlags">An array of flags</param>
        /// <param name="straightPathRefs">An array of polygon references</param>
        /// <param name="straightPathCount">The number of points on the path</param>
        /// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
        /// <param name="options">Options flag</param>
        /// <returns>True, if path found. False, if otherwise.</returns>
        public bool FindStraightPath(Vector3 startPos, Vector3 endPos, Path path, StraightPath straightPath, PathBuildFlags options)
        {
            straightPath.Clear();

            if (path.Count == 0)
                return false;

            bool stat = false;

            Vector3 closestStartPos = new Vector3();
            ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos);

            Vector3 closestEndPos = new Vector3();
            ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, ref closestEndPos);

            stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[0], closestStartPos), StraightPathFlags.Start));

            if (!stat)
                return true;

            if (path.Count > 1)
            {
                Vector3 portalApex = closestStartPos;
                Vector3 portalLeft = portalApex;
                Vector3 portalRight = portalApex;
                int apexIndex = 0;
                int leftIndex = 0;
                int rightIndex = 0;

                NavPolyType leftPolyType = 0;
                NavPolyType rightPolyType = 0;

                NavPolyId leftPolyRef = path[0];
                NavPolyId rightPolyRef = path[0];

                for (int i = 0; i < path.Count; i++)
                {
                    Vector3 left = new Vector3();
                    Vector3 right = new Vector3();
                    NavPolyType fromType = 0, toType = 0;

                    if (i + 1 < path.Count)
                    {
                        //next portal
                        if (GetPortalPoints(path[i], path[i + 1], ref left, ref right, ref fromType, ref toType) == false)
                        {
                            //failed to get portal points means path[i + 1] is an invalid polygon
                            //clamp end point to path[i] and return path so far
                            if (ClosestPointOnPolyBoundary(path[i], endPos, ref closestEndPos) == false)
                            {
                                //first polygon is invalid
                                return false;
                            }

                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                //append portals
                                stat = AppendPortals(apexIndex, i, closestEndPos, path, straightPath, options);
                            }

                            stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i], closestEndPos), StraightPathFlags.None));

                            return true;
                        }

                        //if starting really close to the portal, advance
                        if (i == 0)
                        {
                            float t;
                            if (Distance.PointToSegment2DSquared(ref portalApex, ref left, ref right, out t) < 0.001 * 0.001)
                                continue;
                        }
                    }
                    else
                    {
                        //end of the path
                        left = closestEndPos;
                        right = closestEndPos;

                        fromType = toType = NavPolyType.Ground;
                    }

                    //right vertex
                    float triArea2D;
                    Triangle3.Area2D(ref portalApex, ref portalRight, ref right, out triArea2D);
                    if (triArea2D <= 0.0)
                    {
                        Triangle3.Area2D(ref portalApex, ref portalLeft, ref right, out triArea2D);
                        if (portalApex == portalRight || triArea2D > 0.0)
                        {
                            portalRight = right;
                            rightPolyRef = (i + 1 < path.Count) ? path[i + 1] : NavPolyId.Null;
                            rightPolyType = toType;
                            rightIndex = i;
                        }
                        else
                        {
                            //append portals along current straight path segment
                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, options);

                                if (stat != true)
                                    return true;
                            }

                            portalApex = portalLeft;
                            apexIndex = leftIndex;

                            StraightPathFlags flags = 0;
                            if (leftPolyRef == NavPolyId.Null)
                                flags = StraightPathFlags.End;
                            else if (leftPolyType == NavPolyType.OffMeshConnection)
                                flags = StraightPathFlags.OffMeshConnection;

                            NavPolyId reference = leftPolyRef;

                            //append or update vertex
                            stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags));

                            if (stat != true)
                                return true;

                            portalLeft = portalApex;
                            portalRight = portalApex;
                            leftIndex = apexIndex;
                            rightIndex = apexIndex;

                            //restart
                            i = apexIndex;

                            continue;
                        }
                    }

                    //left vertex
                    Triangle3.Area2D(ref portalApex, ref portalLeft, ref left, out triArea2D);
                    if (triArea2D >= 0.0)
                    {
                        Triangle3.Area2D(ref portalApex, ref portalRight, ref left, out triArea2D);
                        if (portalApex == portalLeft || triArea2D < 0.0f)
                        {
                            portalLeft = left;
                            leftPolyRef = (i + 1 < path.Count) ? path[i + 1] : NavPolyId.Null;
                            leftPolyType = toType;
                            leftIndex = i;
                        }
                        else
                        {
                            if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                            {
                                stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, options);

                                if (stat != true)
                                    return true;
                            }

                            portalApex = portalRight;
                            apexIndex = rightIndex;

                            StraightPathFlags flags = 0;
                            if (rightPolyRef == NavPolyId.Null)
                                flags = StraightPathFlags.End;
                            else if (rightPolyType == NavPolyType.OffMeshConnection)
                                flags = StraightPathFlags.OffMeshConnection;

                            NavPolyId reference = rightPolyRef;

                            //append or update vertex
                            stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags));

                            if (stat != true)
                                return true;

                            portalLeft = portalApex;
                            portalRight = portalApex;
                            leftIndex = apexIndex;
                            rightIndex = apexIndex;

                            //restart
                            i = apexIndex;

                            continue;
                        }
                    }
                }

                //append portals along the current straight line segment
                if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
                {
                    stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, straightPath, options);

                    if (stat != true)
                        return true;
                }
            }

            stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(NavPolyId.Null, closestEndPos), StraightPathFlags.End));

            return true;
        }