// navmesh
        public static bool CanSee(actors.area.Zone zone, float x1, float y1, float z1, float x2, float y2, float z2)
        {
            // todo: prolly shouldnt raycast
            var navMesh = zone.tiledNavMesh;

            if (navMesh != null)
            {
                var navMeshQuery = zone.navMeshQuery;

                NavPoint startPt, endPt;
                SharpNav.Pathfinding.Path path = new SharpNav.Pathfinding.Path();

                RaycastHit hit = new RaycastHit();

                SharpNav.Geometry.Vector3 c  = new SharpNav.Geometry.Vector3(x1, y1, z1);
                SharpNav.Geometry.Vector3 ep = new SharpNav.Geometry.Vector3(x2, y2, z2);

                SharpNav.Geometry.Vector3 e = new SharpNav.Geometry.Vector3(5, 5, 5);
                navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
                navMeshQuery.FindNearestPoly(ref ep, ref e, out endPt);


                if (navMeshQuery.Raycast(ref startPt, ref ep, RaycastOptions.None, out hit, path))
                {
                    return(true);
                }
                return(false);
            }
            return(true);
        }
示例#2
0
        public bool GetPathResult(int index, out Path path)
        {
            path = null;

            for (int i = 0; i < MaxQueue; i++)
            {
                if (queue[i].Index == index)
                {
                    PathQuery q = queue[i];

                    //free request for reuse
                    q.Index = 0;
                    q.Status = 0;

                    path = new Path(q.Path);

                    queue[i] = q;

                    return true;
                }
            }

            return false;
        }
示例#3
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, Path path, StraightPath straightPath, PathBuildFlags options)
        {
            Vector3 startPos = straightPath[straightPath.Count - 1].Point.Position;

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

                NavPolyId to = path[i + 1];
                NavTile toTile;
                NavPoly 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 = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i + 1], pt), StraightPathFlags.None));

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

            return true;
        }
示例#4
0
        /// <summary>
        /// Merge two paths when a shorter path is found
        /// </summary>
        /// <param name="path">The current path</param>
        /// <param name="npath">Current path length</param>
        /// <param name="maxPath">Maximum path length allowed</param>
        /// <param name="visited">The visited polygons</param>
        /// <param name="nvisited">Visited path length</param>
        /// <returns>New path length</returns>
        public int MergeCorridorStartShortcut(Path corridorPath, Path visitedPath)
        {
            int furthestPath = -1;
            int furthestVisited = -1;

            //find furthest common polygon
            for (int i = corridorPath.Count - 1; i >= 0; i--)
            {
                bool found = false;
                for (int j = visitedPath.Count - 1; j >= 0; j--)
                {
                    if (path[i] == visitedPath[j])
                    {
                        furthestPath = i;
                        furthestVisited = j;
                        found = true;
                    }
                }

                if (found)
                    break;
            }

            //if no intersection found, return current path
            if (furthestPath == -1 || furthestVisited == -1)
                return corridorPath.Count;

            //concatenate paths
            //adjust beginning of the buffer to include the visited
            int req = furthestVisited;
            if (req <= 0)
                return corridorPath.Count;

            int orig = furthestPath;
            int size = Math.Max(0, corridorPath.Count - orig);
            if (req + size > path.Count)
                size = path.Count - req;
            for (int i = 0; i < size; i++)
                path[req + i] = path[orig + i];

            //store visited
            for (int i = 0; i < req; i++)
                path[i] = visitedPath[i];

            return req + size;
        }
示例#5
0
        private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
                                    ref SVector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
        {
            StraightPath steerPath = new StraightPath();

            navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
            int nsteerPath = steerPath.Count;

            if (nsteerPath == 0)
            {
                return(false);
            }

            //find vertex far enough to steer to
            int ns = 0;

            while (ns < nsteerPath)
            {
                if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
                    !InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
                {
                    break;
                }

                ns++;
            }

            //failed to find good point to steer to
            if (ns >= nsteerPath)
            {
                return(false);
            }

            steerPos     = steerPath[ns].Point.Position;
            steerPos.Y   = startPos.Y;
            steerPosFlag = steerPath[ns].Flags;
            if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
            {
                steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
            }
            steerPosRef = steerPath[ns].Point.Polygon;

            return(true);
        }
示例#6
0
 public Path(Path otherPath)
     : this()
 {
     polys.AddRange(otherPath.polys);
     cost = otherPath.Cost;
 }
示例#7
0
        /// <summary>
        /// Save the sliced path 
        /// </summary>
        /// <param name="path">The path in terms of polygon references</param>
        /// <param name="pathCount">The path length</param>
        /// <param name="maxPath">The maximum path length allowed</param>
        /// <returns>True if the path is saved, false if not</returns>
        public bool FinalizeSlicedFindPath(Path path)
        {
            path.Clear();

            if (query.Status == false)
            {
                query = new QueryData();
                return false;
            }

            int n = 0;

            if (query.Start.Polygon == query.End.Polygon)
            {
                //special case: the search starts and ends at the same poly
                path.Add(query.Start.Polygon);
            }
            else
            {
                //reverse the path
                NavNode prev = null;
                NavNode node = query.LastBestNode;
                NodeFlags prevRay = 0;
                do
                {
                    NavNode next = nodePool.GetNodeAtIdx(node.ParentIndex);
                    node.ParentIndex = nodePool.GetNodeIdx(prev);
                    prev = node;
                    NodeFlags nextRay = node.Flags & NodeFlags.ParentDetached;
                    node.Flags = (node.Flags & ~NodeFlags.ParentDetached) | prevRay;
                    prevRay = nextRay;
                    node = next;
                }
                while (node != null);

                //store path
                node = prev;
                do
                {
                    NavNode next = nodePool.GetNodeAtIdx(node.ParentIndex);
                    if ((node.Flags & NodeFlags.ParentDetached) != 0)
                    {
                        RaycastHit hit;
                        Path m = new Path();
                        NavPoint startPoint = new NavPoint(node.Id, node.Position);
                        bool result = Raycast(ref startPoint, ref next.Position, RaycastOptions.None, out hit, m);
                        path.AppendPath(m);

                        if (path[path.Count - 1] == next.Id)
                            path.RemoveAt(path.Count - 1);
                    }
                    else
                    {
                        path.Add(node.Id);
                    }

                    node = next;
                }
                while (node != null);
            }

            //reset query
            query = new QueryData();

            return true;
        }
示例#8
0
        public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, NavPolyId prevRef, RaycastOptions options, out RaycastHit hit, Path hitPath)
        {
            hit = new RaycastHit();

            if (hitPath != null)
                hitPath.Clear();

            //validate input
            if (startPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon))
                return false;

            if (prevRef != NavPolyId.Null && !nav.IsValidPolyRef(prevRef))
                return false;

            Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];

            NavTile prevTile, curTile, nextTile;
            NavPoly prevPoly, curPoly, nextPoly;

            NavPolyId curRef = startPoint.Polygon;

            nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
            nextTile = prevTile = curTile;
            nextPoly = prevPoly = curPoly;

            if (prevRef != NavPolyId.Null)
                nav.TryGetTileAndPolyByRefUnsafe(prevRef, out prevTile, out prevPoly);

            while (curRef != NavPolyId.Null)
            {
                //collect vertices
                int nv = 0;
                for (int i = 0; i < curPoly.VertCount; i++)
                {
                    verts[nv] = curTile.Verts[curPoly.Verts[i]];
                    nv++;
                }

                float tmin, tmax;
                int segMin, segMax;
                if (!Intersection.SegmentPoly2D(startPoint.Position, endPos, verts, nv, out tmin, out tmax, out segMin, out segMax))
                {
                    //could not hit the polygon, keep the old t and report hit
                    return true;
                }

                hit.EdgeIndex = segMax;

                //keep track of furthest t so far
                if (tmax > hit.T)
                    hit.T = tmax;

                //store visited polygons
                if (hitPath != null)
                    hitPath.Add(curRef);

                //ray end is completely inside the polygon
                if (segMax == -1)
                {
                    hit.T = float.MaxValue;

                    return true;
                }

                //follow neighbors
                NavPolyId nextRef = NavPolyId.Null;

                foreach (Link link in curPoly.Links)
                {
                    //find link which contains the edge
                    if (link.Edge != segMax)
                        continue;

                    //get pointer to the next polygon
                    nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);

                    //skip off-mesh connection
                    if (nextPoly.PolyType == NavPolyType.OffMeshConnection)
                        continue;

                    //TODO QueryFilter

                    //if the link is internal, just return the ref
                    if (link.Side == BoundarySide.Internal)
                    {
                        nextRef = link.Reference;
                        break;
                    }

                    //if the link is at the tile boundary

                    //check if the link spans the whole edge and accept
                    if (link.BMin == 0 && link.BMax == 255)
                    {
                        nextRef = link.Reference;
                        break;
                    }

                    //check for partial edge links
                    int v0 = curPoly.Verts[link.Edge];
                    int v1 = curPoly.Verts[(link.Edge + 1) % curPoly.VertCount];
                    Vector3 left = curTile.Verts[v0];
                    Vector3 right = curTile.Verts[v1];

                    //check that the intersection lies inside the link portal
                    if (link.Side == BoundarySide.PlusX || link.Side == BoundarySide.MinusX)
                    {
                        //calculate link size
                        float s = 1.0f / 255.0f;
                        float lmin = left.Z + (right.Z - left.Z) * (link.BMin * s);
                        float lmax = left.Z + (right.Z - left.Z) * (link.BMax * s);
                        if (lmin > lmax)
                        {
                            //swap
                            float temp = lmin;
                            lmin = lmax;
                            lmax = temp;
                        }

                        //find z intersection
                        float z = startPoint.Position.Z + (endPos.Z - startPoint.Position.Z) * tmax;
                        if (z >= lmin && z <= lmax)
                        {
                            nextRef = link.Reference;
                            break;
                        }
                    }
                    else if (link.Side == BoundarySide.PlusZ || link.Side == BoundarySide.MinusZ)
                    {
                        //calculate link size
                        float s = 1.0f / 255.0f;
                        float lmin = left.X + (right.X - left.X) * (link.BMin * s);
                        float lmax = left.X + (right.X - left.X) * (link.BMax * s);
                        if (lmin > lmax)
                        {
                            //swap
                            float temp = lmin;
                            lmin = lmax;
                            lmax = temp;
                        }

                        //find x intersection
                        float x = startPoint.Position.X + (endPos.X - startPoint.Position.X) * tmax;
                        if (x >= lmin && x <= lmax)
                        {
                            nextRef = link.Reference;
                            break;
                        }
                    }
                }

                if ((options & RaycastOptions.UseCosts) != 0)
                {
                    //TODO add cost
                }

                if (nextRef == NavPolyId.Null)
                {
                    //no neighbor, we hit a wall

                    //calculate hit normal
                    int a = segMax;
                    int b = (segMax + 1) < nv ? segMax + 1 : 0;
                    Vector3 va = verts[a];
                    Vector3 vb = verts[b];
                    float dx = vb.X - va.X;
                    float dz = vb.Z - va.Z;
                    hit.Normal = new Vector3(dz, 0, dx);
                    hit.Normal.Normalize();
                    return true;
                }

                //no hit, advance to neighbor polygon
                prevRef = curRef;
                curRef = nextRef;
                prevTile = curTile;
                curTile = nextTile;
                prevPoly = curPoly;
                curPoly = nextPoly;
            }

            return true;
        }
示例#9
0
        public void GeneratePathfinding()
        {
            /*  生成路径寻找
             *  1. 根据polymesh
             *
             */
            Random         rand   = new Random();
            NavQueryFilter filter = new NavQueryFilter();

            //1.
            buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);

            tiledNavMesh = new TiledNavMesh(buildData);

            OutMesh();


            navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);


            //Find random start and end points on the poly mesh

            /*int startRef;
             *          navMeshQuery.FindRandomPoint(out startRef, out startPos);*/

            SVector3 c = new SVector3(21.4f, 0, -122f);
            SVector3 e = new SVector3(5, 5, 5);

            navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);

            //navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt);
            //endPt = new NavPoint(new NavPolyId(20), new SVector3(-10, -2, 10));

            c = new SVector3(150.7f, 0, -59f);
            e = new SVector3(5, 5, 5);
            navMeshQuery.FindNearestPoly(ref c, ref e, out endPt);

            //calculate the overall path, which contains an array of polygon references
            int MAX_POLYS = 256;

            path = new SharpNav.Pathfinding.Path();
            navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);

            //find a smooth path over the mesh surface
            int      npolys    = path.Count;
            SVector3 iterPos   = new SVector3();
            SVector3 targetPos = new SVector3();

            navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
            navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);

            //smoothPath = new List<SVector3>(2048);
            //smoothPath.Add(iterPos);

            //float STEP_SIZE = 5f;
            //float SLOP = 0.01f;
            //while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
            //{
            //    //find location to steer towards
            //    SVector3 steerPos = new SVector3();
            //    StraightPathFlags steerPosFlag = 0;
            //    NavPolyId steerPosRef = NavPolyId.Null;

            //    if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
            //        break;

            //    bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
            //    bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;

            //    //find movement delta
            //    SVector3 delta = steerPos - iterPos;
            //    float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));

            //    //if steer target is at end of path or off-mesh link

            //    //don't move past location
            //    if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
            //        len = 1;
            //    else
            //        len = STEP_SIZE / len;

            //    SVector3 moveTgt = new SVector3();
            //    VMad(ref moveTgt, iterPos, delta, len);

            //    //move
            //    SVector3 result = new SVector3();
            //    List<NavPolyId> visited = new List<NavPolyId>(16);
            //    NavPoint startPoint = new NavPoint(path[0], iterPos);
            //    navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
            //    path.FixupCorridor(visited);
            //    npolys = path.Count;
            //    float h = 0;
            //    navMeshQuery.GetPolyHeight(path[0], result, ref h);
            //    result.Y = h;
            //    iterPos = result;

            //    //handle end of path when close enough
            //    if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
            //    {
            //        //reached end of path
            //        iterPos = targetPos;
            //        if (smoothPath.Count < smoothPath.Capacity)
            //        {
            //            smoothPath.Add(iterPos);
            //        }
            //        break;
            //    }

            //    //store results
            //    if (smoothPath.Count < smoothPath.Capacity)
            //    {
            //        smoothPath.Add(iterPos);
            //    }
            //}

            StraightPath corners = new StraightPath();

            FindCorners(startPt.Position, endPt.Position, corners, navMeshQuery);
        }
示例#10
0
		public void FindPath( FindPathContext context )
		{
			if( tiledNavMesh != null )
			{
				if( navMeshQuery == null )
					navMeshQuery = new NavMeshQuery( tiledNavMesh, PathfindingMaxNodes );

				try
				{
					var extents = ToSharpNav( context.PolygonPickExtents, true );
					var startPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.Start ), extents );
					var endPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.End ), extents );

					var filter = new NavQueryFilter();

					var path = new SharpNav.Pathfinding.Path();
					var found = navMeshQuery.FindPath( ref startPt, ref endPt, filter, path );

					Vector3[] pathResult = null;
					if( found )
					{
						//find a smooth path over the mesh surface
						int npolys = path.Count;
						SharpNav.Geometry.Vector3 iterPos = new SharpNav.Geometry.Vector3();
						SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3();
						navMeshQuery.ClosestPointOnPoly( startPt.Polygon, startPt.Position, ref iterPos );
						navMeshQuery.ClosestPointOnPoly( path[ npolys - 1 ], endPt.Position, ref targetPos );

						var smoothPath = new List<SharpNav.Geometry.Vector3>( context.MaxSmoothPath );
						smoothPath.Add( iterPos );

						//for( int n = 0; n < path.Count; n++ )
						//{
						//	Vector3 closest = Vector3.Zero;
						//	if( navMeshQuery.ClosestPointOnPoly( path[ n ], startPt.Position, ref closest ) )
						//		smoothPath.Add( closest );
						//}

						float stepSize = (float)context.StepSize;
						float slop = (float)context.Slop;

						while( npolys > 0 && smoothPath.Count < smoothPath.Capacity )
						{
							//find location to steer towards
							SharpNav.Geometry.Vector3 steerPos = new SharpNav.Geometry.Vector3();
							StraightPathFlags steerPosFlag = 0;
							NavPolyId steerPosRef = NavPolyId.Null;

							if( !GetSteerTarget( navMeshQuery, iterPos, targetPos, slop, path, ref steerPos, ref steerPosFlag, ref steerPosRef ) )
								break;

							bool endOfPath = ( steerPosFlag & StraightPathFlags.End ) != 0 ? true : false;
							bool offMeshConnection = ( steerPosFlag & StraightPathFlags.OffMeshConnection ) != 0 ? true : false;

							//find movement delta
							SharpNav.Geometry.Vector3 delta = steerPos - iterPos;
							float len = (float)Math.Sqrt( SharpNav.Geometry.Vector3.Dot( delta, delta ) );

							//if steer target is at end of path or off-mesh link
							//don't move past location
							if( ( endOfPath || offMeshConnection ) && len < stepSize )
								len = 1;
							else
								len = stepSize / len;

							SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3();
							VMad( ref moveTgt, iterPos, delta, len );

							//move
							SharpNav.Geometry.Vector3 result = new SharpNav.Geometry.Vector3();
							List<NavPolyId> visited = new List<NavPolyId>( 16 );
							NavPoint startPoint = new NavPoint( path[ 0 ], iterPos );
							navMeshQuery.MoveAlongSurface( ref startPoint, ref moveTgt, out result, visited );
							path.FixupCorridor( visited );
							npolys = path.Count;
							float h = 0;
							navMeshQuery.GetPolyHeight( path[ 0 ], result, ref h );
							result.Y = h;
							iterPos = result;

							//handle end of path when close enough
							if( endOfPath && InRange( iterPos, steerPos, slop, 1.0f ) )
							{
								//reached end of path
								iterPos = targetPos;
								if( smoothPath.Count < smoothPath.Capacity )
									smoothPath.Add( iterPos );
								break;
							}

							//store results
							if( smoothPath.Count < smoothPath.Capacity )
								smoothPath.Add( iterPos );
						}

						pathResult = new Vector3[ smoothPath.Count ];
						for( int n = 0; n < pathResult.Length; n++ )
							pathResult[ n ] = ToEngine( smoothPath[ n ] );

						//if( pathCount > 0 )
						//{
						//	path = new Vec3[ pathCount ];
						//	//!!!!double support
						//	for( int n = 0; n < pathCount; n++ )
						//		path[ n ] = ToEngineVec3( pathPointer[ n ] );
						//}
						//else
						//	path = new Vec3[] { context.Start };
					}

					context.Path = pathResult;
				}
				catch( Exception e )
				{
					context.Error = e.Message;
				}
			}
			else
				context.Error = "No NavMesh";

			context.Finished = true;
		}
示例#11
0
        private bool GetSteerTarget(
            NavMeshQuery navMeshQuery,
            SVector3 startPos,
            SVector3 endPos,
            float minTargetDist,
            Path path,
            ref SVector3 steerPos,
            ref StraightPathFlags steerPosFlag,
            ref NavPolyId steerPosRef)
        {
            StraightPath steerPath = new StraightPath();
            navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
            int nsteerPath = steerPath.Count;
            if (nsteerPath == 0)
                return false;

            //find vertex far enough to steer to
            int ns = 0;
            while (ns < nsteerPath)
            {
                if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
                    !InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
                    break;

                ns++;
            }

            //failed to find good point to steer to
            if (ns >= nsteerPath)
                return false;

            steerPos = steerPath[ns].Point.Position;
            steerPos.Y = startPos.Y;
            steerPosFlag = steerPath[ns].Flags;
            if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
                steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
            steerPosRef = steerPath[ns].Point.Polygon;

            return true;
        }
示例#12
0
 public PathCorridor()
 {
     this.path = new Path();
 }
示例#13
0
 /// <summary>
 /// The current corridor position is expected to be within the first polygon in the path. The target
 /// is expected to be in the last polygon.
 /// </summary>
 /// <param name="target">The target</param>
 /// <param name="path">The polygon path</param>
 public void SetCorridor(Vector3 target, Path path)
 {
     this.target = target;
     this.path = path;
 }
示例#14
0
        /// <summary>
        /// Use an efficient local visibility search to try to optimize the corridor between the current position and the next.
        /// </summary>
        /// <param name="next">The next postion</param>
        /// <param name="pathOptimizationRange">The range</param>
        /// <param name="navquery">The NavMeshQuery</param>
        public void OptimizePathVisibility(Vector3 next, float pathOptimizationRange, NavMeshQuery navquery)
        {
            //clamp the ray to max distance
            Vector3 goal = next;
            float dist = Vector3Extensions.Distance2D(pos, goal);

            //if too close to the goal, do not try to optimize
            if (dist < 0.01f)
                return;

            dist = Math.Min(dist + 0.01f, pathOptimizationRange);

            //adjust ray length
            Vector3 delta = goal - pos;
            goal = pos + delta * (pathOptimizationRange / dist);

            NavPoint startPoint = new NavPoint(path[0], pos);
            Path raycastPath = new Path();
            RaycastHit hit;

            navquery.Raycast(ref startPoint, ref goal, RaycastOptions.None, out hit, raycastPath);
            if (raycastPath.Count > 1 && hit.T > 0.99f)
            {
                MergeCorridorStartShortcut(raycastPath, raycastPath);
            }
        }
示例#15
0
        /// <summary>
        /// Use a local area path search to try to reoptimize this corridor
        /// </summary>
        /// <param name="navquery">The NavMeshQuery</param>
        /// <returns>True if optimized, false if not</returns>
        public bool OptimizePathTopology(NavMeshQuery navquery, NavQueryFilter filter)
        {
            if (path.Count < 3)
                return false;

            const int MaxIter = 32;
            const int MaxRes = 32;

            Path res = new Path();
            int numRes = 0;
            int tempInt = 0;
            NavPoint startPoint = new NavPoint(path[0], pos);
            NavPoint endPoint = new NavPoint(path[path.Count - 1], target);
            navquery.InitSlicedFindPath(ref startPoint, ref endPoint, filter, FindPathOptions.None);
            navquery.UpdateSlicedFindPath(MaxIter, ref tempInt);
            bool status = navquery.FinalizedSlicedPathPartial(path, res);

            if (status == true && numRes > 0)
            {
                MergeCorridorStartShortcut(path, res);
                return true;
            }

            return false;
        }
示例#16
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;
        }
示例#17
0
 public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, RaycastOptions options, out RaycastHit hit, Path hitPath)
 {
     return Raycast(ref startPoint, ref endPos, NavPolyId.Null, options, out hit, hitPath);
 }
示例#18
0
        //为了使接口适应而附加的
        public List <Vector2> SmothPathfinding(Vector2 start, Vector2 target)
        {
            Random         rand        = new Random();
            SVector3       startVector = new SVector3(start.x_, 0, start.y_);
            SVector3       endVector   = new SVector3(target.x_, 0, target.y_);
            NavQueryFilter filter      = new NavQueryFilter();
            //buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
            //tiledNavMesh = new TiledNavMesh(buildData);
            //navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
            SVector3 extents = new SVector3(5, 5, 5);

            navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
            navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);

            path = new SharpNav.Pathfinding.Path();
            navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);

            //find a smooth path over the mesh surface
            int      npolys    = path.Count;
            SVector3 iterPos   = new SVector3();
            SVector3 targetPos = new SVector3();

            navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
            navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);

            smoothPath = new List <SVector3>(2048);
            smoothPath.Add(iterPos);

            float STEP_SIZE = 2f;
            float SLOP      = 0.1f;

            while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
            {
                //find location to steer towards
                SVector3          steerPos     = new SVector3();
                StraightPathFlags steerPosFlag = 0;
                NavPolyId         steerPosRef  = NavPolyId.Null;

                if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
                {
                    break;
                }

                bool endOfPath         = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
                bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;

                //find movement delta
                SVector3 delta = steerPos - iterPos;
                float    len   = (float)Math.Sqrt(SVector3.Dot(delta, delta));

                //if steer target is at end of path or off-mesh link

                //don't move past location
                if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                {
                    len = 1;
                }
                else
                {
                    len = STEP_SIZE / len;
                }

                SVector3 moveTgt = new SVector3();
                VMad(ref moveTgt, iterPos, delta, len);

                //move
                SVector3         result     = new SVector3();
                List <NavPolyId> visited    = new List <NavPolyId>(16);
                NavPoint         startPoint = new NavPoint(path[0], iterPos);
                navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
                path.FixupCorridor(visited);
                npolys = path.Count;
                float h = 0;
                navMeshQuery.GetPolyHeight(path[0], result, ref h);
                result.Y = h;
                iterPos  = result;

                //handle end of path when close enough
                if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
                {
                    //reached end of path
                    iterPos = targetPos;
                    if (smoothPath.Count < smoothPath.Capacity)
                    {
                        smoothPath.Add(iterPos);
                    }
                    break;
                }

                //store results
                if (smoothPath.Count < smoothPath.Capacity)
                {
                    smoothPath.Add(iterPos);
                }
            }
            List <Vector2> _path = new List <Vector2>();

            for (int i = 0; i < smoothPath.Count; i++)
            {
                _path.Add(new Vector2(smoothPath[i].X, smoothPath[i].Z));
            }
            return(_path);
        }
示例#19
0
        /// <summary>
        /// Update the sliced path as agents move across the path.
        /// </summary>
        /// <param name="maxIter">Maximum iterations</param>
        /// <param name="doneIters">Number of times iterated through</param>
        /// <returns>True if updated, false if not</returns>
        public bool UpdateSlicedFindPath(int maxIter, ref int doneIters)
        {
            if (query.Status != true)
                return query.Status;

            //make sure the request is still valid
            if (!nav.IsValidPolyRef(query.Start.Polygon) || !nav.IsValidPolyRef(query.End.Polygon))
            {
                query.Status = false;
                return false;
            }

            int iter = 0;
            while (iter < maxIter && !openList.Empty())
            {
                iter++;

                //remove node from open list and put it in closed list
                NavNode bestNode = openList.Pop();
                SetNodeFlagClosed(ref bestNode);

                //reached the goal, stop searching
                if (bestNode.Id == query.End.Polygon)
                {
                    query.LastBestNode = bestNode;
                    query.Status = true;
                    doneIters = iter;
                    return query.Status;
                }

                //get current poly and tile
                NavPolyId bestRef = bestNode.Id;
                NavTile bestTile;
                NavPoly bestPoly;
                if (!nav.TryGetTileAndPolyByRef(bestRef, out bestTile, out bestPoly))
                {
                    //the polygon has disappeared during the sliced query, fail
                    query.Status = false;
                    doneIters = iter;
                    return query.Status;
                }

                //get parent poly and tile
                NavPolyId parentRef = NavPolyId.Null;
                NavPolyId grandpaRef = NavPolyId.Null;
                NavTile parentTile = null;
                NavPoly parentPoly = null;
                NavNode parentNode = null;
                if (bestNode.ParentIndex != 0)
                {
                    parentNode = nodePool.GetNodeAtIdx(bestNode.ParentIndex);
                    parentRef = parentNode.Id;
                    if (parentNode.ParentIndex != 0)
                        grandpaRef = nodePool.GetNodeAtIdx(parentNode.ParentIndex).Id;
                }
                if (parentRef != NavPolyId.Null)
                {
                    bool invalidParent = !nav.TryGetTileAndPolyByRef(parentRef, out parentTile, out parentPoly);
                    if (invalidParent || (grandpaRef != NavPolyId.Null && !nav.IsValidPolyRef(grandpaRef)))
                    {
                        //the polygon has disappeared during the sliced query, fail
                        query.Status = false;
                        doneIters = iter;
                        return query.Status;
                    }
                }

                //decide whether to test raycast to previous nodes
                bool tryLOS = false;
                if ((query.Options & FindPathOptions.AnyAngle) != 0)
                {
                    if ((parentRef != NavPolyId.Null) && (parentNode.Position - bestNode.Position).LengthSquared() < query.RaycastLimitSquared)
                        tryLOS = true;
                }

                foreach (Link link in bestPoly.Links)
                {
                    NavPolyId neighborRef = link.Reference;

                    //skip invalid ids and do not expand back to where we came from
                    if (neighborRef == NavPolyId.Null || neighborRef == parentRef)
                        continue;

                    //get neighbor poly and tile
                    NavTile neighborTile;
                    NavPoly neighborPoly;
                    nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);

                    if (!query.Filter.PassFilter(neighborRef, neighborTile, neighborPoly))
                        continue;

                    NavNode neighborNode = nodePool.GetNode(neighborRef);
                    if (neighborNode == null)
                        continue;

                    if (neighborNode.ParentIndex != 0 && neighborNode.ParentIndex == bestNode.ParentIndex)
                        continue;

                    if (neighborNode.Flags == 0)
                    {
                        GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighborRef, neighborPoly, neighborTile, ref neighborNode.Position);
                    }

                    //calculate cost and heuristic
                    float cost = 0;
                    float heuristic = 0;

                    bool foundShortCut = false;
                    RaycastHit hit;
                    Path hitPath = new Path();
                    if (tryLOS)
                    {
                        NavPoint startPoint = new NavPoint(parentRef, parentNode.Position);
                        Raycast(ref startPoint, ref neighborNode.Position, grandpaRef, RaycastOptions.UseCosts, out hit, hitPath);
                        foundShortCut = hit.T >= 1.0f;
                    }

                    if (foundShortCut)
                    {
                        cost = parentNode.PolyCost + hitPath.Cost;
                    }
                    else
                    {
                        float curCost = query.Filter.GetCost(bestNode.Position, neighborNode.Position,
                            parentRef, parentTile, parentPoly,
                            bestRef, bestTile, bestPoly,
                            neighborRef, neighborTile, neighborPoly);

                        cost = bestNode.PolyCost + curCost;
                    }

                    //special case for last node
                    if (neighborRef == query.End.Polygon)
                    {
                        //cost
                        float endCost = query.Filter.GetCost(bestNode.Position, neighborNode.Position,
                            bestRef, bestTile, bestPoly,
                            neighborRef, neighborTile, neighborPoly,
                            NavPolyId.Null, null, null);

                        cost = cost + endCost;
                        heuristic = 0;
                    }
                    else
                    {
                        heuristic = (neighborNode.Position - query.End.Position).Length() * HeuristicScale;
                    }

                    float total = cost + heuristic;

                    //the node is already in open list and new result is worse, skip
                    if (IsInOpenList(neighborNode) && total >= neighborNode.TotalCost)
                        continue;

                    //the node is already visited and processesd, and the new result is worse, skip
                    if (IsInClosedList(neighborNode) && total >= neighborNode.TotalCost)
                        continue;

                    //add or update the node
                    neighborNode.ParentIndex = nodePool.GetNodeIdx(bestNode);
                    neighborNode.Id = neighborRef;
                    neighborNode.Flags = RemoveNodeFlagClosed(neighborNode);
                    neighborNode.PolyCost = cost;
                    neighborNode.TotalCost = total;
                    if (foundShortCut)
                        neighborNode.Flags |= NodeFlags.ParentDetached;

                    if (IsInOpenList(neighborNode))
                    {
                        //already in open, update node location
                        openList.Modify(neighborNode);
                    }
                    else
                    {
                        //put the node in the open list
                        SetNodeFlagOpen(ref neighborNode);
                        openList.Push(neighborNode);
                    }

                    //update nearest node to target so far
                    if (heuristic < query.LastBestNodeCost)
                    {
                        query.LastBestNodeCost = heuristic;
                        query.LastBestNode = neighborNode;
                    }
                }
            }

            //exhausted all nodes, but could not find path
            if (openList.Empty())
            {
                query.Status = true;
            }

            doneIters = iter;

            return query.Status;
        }
示例#20
0
        //只找Corner
        public StraightPath Pathfinding(SVector3 startVector, SVector3 endVector)
        {
            Random         rand   = new Random();
            NavQueryFilter filter = new NavQueryFilter();
            //buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
            //tiledNavMesh = new TiledNavMesh(buildData);
            //navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
            SVector3 extents = new SVector3(5, 5, 5);

            navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
            navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);

            path = new SharpNav.Pathfinding.Path();
            navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);

            ////find a smooth path over the mesh surface
            //int npolys = path.Count;
            //SVector3 iterPos = new SVector3();
            //SVector3 targetPos = new SVector3();
            //navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
            //navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);

            //smoothPath = new List<SVector3>(2048);
            //smoothPath.Add(iterPos);

            //float STEP_SIZE = 1f;
            //float SLOP = 0.01f;
            //while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
            //{
            //    //find location to steer towards
            //    SVector3 steerPos = new SVector3();
            //    StraightPathFlags steerPosFlag = 0;
            //    NavPolyId steerPosRef = NavPolyId.Null;

            //    if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
            //        break;

            //    bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
            //    bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;

            //    //find movement delta
            //    SVector3 delta = steerPos - iterPos;
            //    float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));

            //    //if steer target is at end of path or off-mesh link

            //    //don't move past location
            //    if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
            //        len = 1;
            //    else
            //        len = STEP_SIZE / len;

            //    SVector3 moveTgt = new SVector3();
            //    VMad(ref moveTgt, iterPos, delta, len);

            //    //move
            //    SVector3 result = new SVector3();
            //    List<NavPolyId> visited = new List<NavPolyId>(16);
            //    NavPoint startPoint = new NavPoint(path[0], iterPos);
            //    navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
            //    path.FixupCorridor(visited);
            //    npolys = path.Count;
            //    float h = 0;
            //    navMeshQuery.GetPolyHeight(path[0], result, ref h);
            //    result.Y = h;
            //    iterPos = result;

            //    //handle end of path when close enough
            //    if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
            //    {
            //        //reached end of path
            //        iterPos = targetPos;
            //        if (smoothPath.Count < smoothPath.Capacity)
            //        {
            //            smoothPath.Add(iterPos);
            //        }
            //        break;
            //    }

            //    //store results
            //    if (smoothPath.Count < smoothPath.Capacity)
            //    {
            //        smoothPath.Add(iterPos);
            //    }
            //}
            StraightPath corners = new StraightPath();

            FindCorners(startVector, endVector, corners, navMeshQuery);
            return(corners);
            //return smoothPath;
        }
示例#21
0
        /// <summary>
        /// Find a path from the start polygon to the end polygon.
        /// -If the end polygon can't be reached, the last polygon will be nearest the end polygon
        /// -If the path array is too small, it will be filled as far as possible 
        /// -start and end positions are used to calculate traversal costs
        /// </summary>
        /// <param name="startPt">The start point.</param>
        /// <param name="endPt">The end point.</param>
        /// <param name="filter">A filter for the navmesh data.</param>
        /// <param name="path">The path of polygon references</param>
        /// <returns>True, if path found. False, if otherwise.</returns>
        public bool FindPath(ref NavPoint startPt, ref NavPoint endPt, NavQueryFilter filter, Path path)
        {
            //reset path of polygons
            path.Clear();

            NavPolyId startRef = startPt.Polygon;
            Vector3 startPos = startPt.Position;
            NavPolyId endRef = endPt.Polygon;
            Vector3 endPos = endPt.Position;

            if (startRef == NavPolyId.Null || endRef == NavPolyId.Null)
                return false;

            //validate input
            if (!nav.IsValidPolyRef(startRef) || !nav.IsValidPolyRef(endRef))
                return false;

            //special case: both start and end are in the same polygon
            if (startRef == endRef)
            {
                path.Add(startRef);
                return true;
            }

            nodePool.Clear();
            openList.Clear();

            //initial node is located at the starting position
            NavNode startNode = nodePool.GetNode(startRef);
            startNode.Position = startPos;
            startNode.ParentIndex = 0;
            startNode.PolyCost = 0;
            startNode.TotalCost = (startPos - endPos).Length() * HeuristicScale;
            startNode.Id = startRef;
            startNode.Flags = NodeFlags.Open;
            openList.Push(startNode);

            NavNode lastBestNode = startNode;
            float lastBestTotalCost = startNode.TotalCost;

            while (openList.Count > 0)
            {
                //remove node from open list and put it in closed list
                NavNode bestNode = openList.Pop();
                SetNodeFlagClosed(ref bestNode);

                //reached the goal. stop searching
                if (bestNode.Id == endRef)
                {
                    lastBestNode = bestNode;
                    break;
                }

                //get current poly and tile
                NavPolyId bestRef = bestNode.Id;
                NavTile bestTile;
                NavPoly bestPoly;
                nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);

                //get parent poly and tile
                NavPolyId parentRef = NavPolyId.Null;
                NavTile parentTile = null;
                NavPoly parentPoly = null;
                if (bestNode.ParentIndex != 0)
                    parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIndex).Id;
                if (parentRef != NavPolyId.Null)
                    nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);

                //examine neighbors
                foreach (Link link in bestPoly.Links)
                {
                    NavPolyId neighborRef = link.Reference;

                    //skip invalid ids and do not expand back to where we came from
                    if (neighborRef == NavPolyId.Null || neighborRef == parentRef)
                        continue;

                    //get neighbor poly and tile
                    NavTile neighborTile;
                    NavPoly neighborPoly;
                    nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);

                    NavNode neighborNode = nodePool.GetNode(neighborRef);
                    if (neighborNode == null)
                        continue;

                    //if node is visited the first time, calculate node position
                    if (neighborNode.Flags == 0)
                    {
                        GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighborRef, neighborPoly, neighborTile, ref neighborNode.Position);
                    }

                    //calculate cost and heuristic
                    float cost = 0;
                    float heuristic = 0;

                    //special case for last node
                    if (neighborRef == endRef)
                    {
                        //cost
                        float curCost = filter.GetCost(bestNode.Position, neighborNode.Position,
                            parentRef, parentTile, parentPoly,
                            bestRef, bestTile, bestPoly,
                            neighborRef, neighborTile, neighborPoly);

                        float endCost = filter.GetCost(neighborNode.Position, endPos,
                            bestRef, bestTile, bestPoly,
                            neighborRef, neighborTile, neighborPoly,
                            NavPolyId.Null, null, null);

                        cost = bestNode.PolyCost + curCost + endCost;
                        heuristic = 0;
                    }
                    else
                    {
                        //cost
                        float curCost = filter.GetCost(bestNode.Position, neighborNode.Position,
                            parentRef, parentTile, parentPoly,
                            bestRef, bestTile, bestPoly,
                            neighborRef, neighborTile, neighborPoly);

                        cost = bestNode.PolyCost + curCost;
                        heuristic = (neighborNode.Position - endPos).Length() * HeuristicScale;
                    }

                    float total = cost + heuristic;

                    //the node is already in open list and new result is worse, skip
                    if (IsInOpenList(neighborNode) && total >= neighborNode.TotalCost)
                        continue;

                    //the node is already visited and processesd, and the new result is worse, skip
                    if (IsInClosedList(neighborNode) && total >= neighborNode.TotalCost)
                        continue;

                    //add or update the node
                    neighborNode.ParentIndex = nodePool.GetNodeIdx(bestNode);
                    neighborNode.Id = neighborRef;
                    neighborNode.Flags = RemoveNodeFlagClosed(neighborNode);
                    neighborNode.PolyCost = cost;
                    neighborNode.TotalCost = total;

                    if (IsInOpenList(neighborNode))
                    {
                        //already in open, update node location
                        openList.Modify(neighborNode);
                    }
                    else
                    {
                        //put the node in the open list
                        SetNodeFlagOpen(ref neighborNode);
                        openList.Push(neighborNode);
                    }

                    //update nearest node to target so far
                    if (heuristic < lastBestTotalCost)
                    {
                        lastBestTotalCost = heuristic;
                        lastBestNode = neighborNode;
                    }
                }
            }

            //save path
            NavNode node = lastBestNode;
            do
            {
                path.Add(node.Id);
                node = nodePool.GetNodeAtIdx(node.ParentIndex);
            }
            while (node != null);

            //reverse the path since it's backwards
            path.Reverse();

            return true;
        }
示例#22
0
        /// <summary>
        /// Change the move requests for all the agents
        /// </summary>
        public void UpdateMoveRequest()
        {
            const int PATH_MAX_AGENTS = 8;
            Agent[] queue = new Agent[PATH_MAX_AGENTS];
            int numQueue = 0;
            Status status;

            //fire off new requests
            for (int i = 0; i < maxAgents; i++)
            {
                if (!agents[i].IsActive)
                    continue;
                if (agents[i].State == AgentState.Invalid)
                    continue;
                if (agents[i].TargetState == TargetState.None || agents[i].TargetState == TargetState.Velocity)
                    continue;

                if (agents[i].TargetState == TargetState.Requesting)
                {
                    Path path = agents[i].Corridor.NavPath;

                    Vector3 reqPos = new Vector3();
                    Path reqPath = new Path();

                    //quick search towards the goal
                    const int MAX_ITER = 20;
                    NavPoint startPoint = new NavPoint(path[0], agents[i].Position);
                    NavPoint endPoint = new NavPoint(agents[i].TargetRef, agents[i].TargetPosition);
                    navQuery.InitSlicedFindPath(ref startPoint, ref endPoint, navQueryFilter, FindPathOptions.None);
                    int tempInt = 0;
                    navQuery.UpdateSlicedFindPath(MAX_ITER, ref tempInt);
                    status = Status.Failure;
                    if (agents[i].TargetReplan)
                    {
                        //try to use an existing steady path during replan if possible
                        status = navQuery.FinalizedSlicedPathPartial(path, reqPath).ToStatus();
                    }
                    else
                    {
                        //try to move towards the target when the goal changes
                        status = navQuery.FinalizeSlicedFindPath(reqPath).ToStatus();
                    }

                    if (status != Status.Failure && reqPath.Count > 0)
                    {
                        //in progress or succeed
                        if (reqPath[reqPath.Count - 1] != agents[i].TargetRef)
                        {
                            //partial path, constrain target position in last polygon
                            bool tempBool;
                            status = navQuery.ClosestPointOnPoly(reqPath[reqPath.Count - 1], agents[i].TargetPosition, out reqPos, out tempBool).ToStatus();
                            if (status == Status.Failure)
                                reqPath.Clear();
                        }
                        else
                        {
                            reqPos = agents[i].TargetPosition;
                        }
                    }
                    else
                    {
                        reqPath.Clear();
                    }

                    if (reqPath.Count == 0)
                    {
                        //could not find path, start the request from the current location
                        reqPos = agents[i].Position;
                        reqPath.Add(path[0]);
                    }

                    agents[i].Corridor.SetCorridor(reqPos, reqPath);
                    agents[i].Boundary.Reset();
                    agents[i].IsPartial = false;

                    if (reqPath[reqPath.Count - 1] == agents[i].TargetRef)
                    {
                        agents[i].TargetState = TargetState.Valid;
                        agents[i].TargetReplanTime = 0.0f;
                    }
                    else
                    {
                        //the path is longer or potentially unreachable, full plan
                        agents[i].TargetState = TargetState.WaitingForQueue;
                    }
                }

                if (agents[i].TargetState == TargetState.WaitingForQueue)
                {
                    numQueue = AddToPathQueue(agents[i], queue, numQueue, PATH_MAX_AGENTS);
                }
            }

            for (int i = 0; i < numQueue; i++)
            {
                queue[i].TargetPathQueryIndex = pathq.Request(new NavPoint(queue[i].Corridor.GetLastPoly(), queue[i].Corridor.Target), new NavPoint(queue[i].TargetRef, queue[i].TargetPosition));
                if (queue[i].TargetPathQueryIndex != PathQueue.Invalid)
                    queue[i].TargetState = TargetState.WaitingForPath;
            }

            //update requests
            pathq.Update(MaxItersPerUpdate);

            //process path results
            for (int i = 0; i < maxAgents; i++)
            {
                if (!agents[i].IsActive)
                    continue;
                if (agents[i].TargetState == TargetState.None || agents[i].TargetState == TargetState.Velocity)
                    continue;

                if (agents[i].TargetState == TargetState.WaitingForPath)
                {
                    //poll path queue
                    status = pathq.GetRequestStatus(agents[i].TargetPathQueryIndex);
                    if (status == Status.Failure)
                    {
                        //path find failed, retry if the target location is still valid
                        agents[i].TargetPathQueryIndex = PathQueue.Invalid;
                        if (agents[i].TargetRef != NavPolyId.Null)
                            agents[i].TargetState = TargetState.Requesting;
                        else
                            agents[i].TargetState = TargetState.Failed;
                        agents[i].TargetReplanTime = 0.0f;
                    }
                    else if (status == Status.Success)
                    {
                        Path path = agents[i].Corridor.NavPath;

                        //apply results
                        Vector3 targetPos = new Vector3();
                        targetPos = agents[i].TargetPosition;

                        Path res;
                        bool valid = true;
                        status = pathq.GetPathResult(agents[i].TargetPathQueryIndex, out res).ToStatus();
                        if (status == Status.Failure || res.Count == 0)
                            valid = false;

                        //Merge result and existing path
                        if (valid && path[path.Count - 1] != res[0])
                            valid = false;

                        if (valid)
                        {
                            //put the old path infront of the old path
                            if (path.Count > 1)
                            {
                                //make space for the old path
                                //if ((path.Count - 1) + nres > maxPathResult)
                                    //nres = maxPathResult - (npath - 1);

                                for (int j = 0; j < res.Count; j++)
                                    res[path.Count - 1 + j] = res[j];

                                //copy old path in the beginning
                                for (int j = 0; j < path.Count - 1; j++)
                                    res.Add(path[j]);

                                //remove trackbacks
                                res.RemoveTrackbacks();
                            }

                            //check for partial path
                            if (res[res.Count - 1] != agents[i].TargetRef)
                            {
                                //partial path, constrain target position inside the last polygon
                                Vector3 nearest;
                                bool tempBool = false;
                                status = navQuery.ClosestPointOnPoly(res[res.Count - 1], targetPos, out nearest, out tempBool).ToStatus();
                                if (status == Status.Success)
                                    targetPos = nearest;
                                else
                                    valid = false;
                            }
                        }

                        if (valid)
                        {
                            //set current corridor
                            agents[i].Corridor.SetCorridor(targetPos, res);

                            //forced to update boundary
                            agents[i].Boundary.Reset();
                            agents[i].TargetState = TargetState.Valid;
                        }
                        else
                        {
                            //something went wrong
                            agents[i].TargetState = TargetState.Failed;
                        }

                        agents[i].TargetReplanTime = 0.0f;
                    }
                }
            }
        }
示例#23
0
 public void AppendPath(Path other)
 {
     polys.AddRange(other.polys);
 }
示例#24
0
        // Copyright (c) 2013-2016 Robert Rouhani <*****@*****.**> and other contributors (see CONTRIBUTORS file).
        // Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE

        public static List <Vector3> GetPath(actors.area.Zone zone, Vector3 startVec, Vector3 endVec, float stepSize = 0.70f, int pathSize = 45, float polyRadius = 0.0f, bool skipToTarget = false)
        {
            var navMesh      = zone.tiledNavMesh;
            var navMeshQuery = zone.navMeshQuery;

            // no navmesh loaded, run straight to player
            if (navMesh == null)
            {
                return(new List <Vector3>()
                {
                    endVec
                });
            }

            // no need to waste cycles finding path to same point
            if (startVec.X == endVec.X && startVec.Y == endVec.Y && startVec.Z == endVec.Z && polyRadius == 0.0f)
            {
                return(null);
            }

            var smoothPath = new List <Vector3>(pathSize)
            {
            };

            NavQueryFilter filter = new NavQueryFilter();

            NavPoint startPt, endPt;

            try
            {
                SharpNav.Geometry.Vector3 c  = new SharpNav.Geometry.Vector3(startVec.X, startVec.Y, startVec.Z);
                SharpNav.Geometry.Vector3 ep = new SharpNav.Geometry.Vector3(endVec.X, endVec.Y, endVec.Z);

                SharpNav.Geometry.Vector3 e = new SharpNav.Geometry.Vector3(5, 5, 5);
                navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
                navMeshQuery.FindNearestPoly(ref ep, ref e, out endPt);

                //calculate the overall path, which contains an array of polygon references
                int MAX_POLYS = 256;
                var path      = new SharpNav.Pathfinding.Path();

                navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);

                //find a smooth path over the mesh surface
                int npolys = path.Count;
                SharpNav.Geometry.Vector3 iterPos   = new SharpNav.Geometry.Vector3();
                SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3();
                navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
                navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);

                // set target to random point at end of path
                if (polyRadius != 0.0f)
                {
                    var randPoly = navMeshQuery.FindRandomPointAroundCircle(endPt, polyRadius);
                    targetPos = randPoly.Position;
                }

                if (skipToTarget)
                {
                    return(new List <Vector3>()
                    {
                        new Vector3(targetPos.X, targetPos.Y, targetPos.Z)
                    });
                }
                smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));

                //float STEP_SIZE = 0.70f;
                float SLOP = 0.15f;
                while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
                {
                    //find location to steer towards
                    SharpNav.Geometry.Vector3 steerPos     = new SharpNav.Geometry.Vector3();
                    StraightPathFlags         steerPosFlag = 0;
                    NavPolyId steerPosRef = NavPolyId.Null;

                    if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
                    {
                        break;
                    }

                    bool endOfPath         = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
                    bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;

                    //find movement delta
                    SharpNav.Geometry.Vector3 delta = steerPos - iterPos;
                    float len = (float)Math.Sqrt(SharpNav.Geometry.Vector3.Dot(delta, delta));

                    //if steer target is at end of path or off-mesh link
                    //don't move past location
                    if ((endOfPath || offMeshConnection) && len < stepSize)
                    {
                        len = 1;
                    }
                    else
                    {
                        len = stepSize / len;
                    }

                    SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3();
                    VMad(ref moveTgt, iterPos, delta, len);

                    //move
                    SharpNav.Geometry.Vector3 result  = new SharpNav.Geometry.Vector3();
                    List <NavPolyId>          visited = new List <NavPolyId>(pathSize);
                    NavPoint startPoint = new NavPoint(path[0], iterPos);
                    navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
                    path.FixupCorridor(visited);
                    npolys = path.Count;
                    float h = 0;
                    navMeshQuery.GetPolyHeight(path[0], result, ref h);
                    result.Y = h;
                    iterPos  = result;

                    //handle end of path when close enough
                    if (endOfPath && InRange(iterPos, steerPos, SLOP, 1000.0f))
                    {
                        //reached end of path
                        iterPos = targetPos;
                        if (smoothPath.Count < smoothPath.Capacity)
                        {
                            smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));
                        }
                        break;
                    }

                    //store results
                    if (smoothPath.Count < smoothPath.Capacity)
                    {
                        smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));
                    }
                }
            }
            catch (Exception e)
            {
                Program.Log.Error(e.Message);
                Program.Log.Error("Start pos {0} {1} {2} end pos {3} {4} {5}", startVec.X, startVec.Y, startVec.Z, endVec.X, endVec.Y, endVec.Z);
                // todo: probably log this
                return(new List <Vector3>()
                {
                    endVec
                });
            }
            return(smoothPath);
        }
示例#25
0
        private void GeneratePathfinding()
        {
            if (!hasGenerated)
                return;

            Random rand = new Random();
            NavQueryFilter filter = new NavQueryFilter();

            buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);

            tiledNavMesh = new TiledNavMesh(buildData);
            navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);

            //Find random start and end points on the poly mesh
            /*int startRef;
            navMeshQuery.FindRandomPoint(out startRef, out startPos);*/

            SVector3 c = new SVector3(10, 0, 0);
            SVector3 e = new SVector3(5, 5, 5);
            navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);

            navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt);

            //calculate the overall path, which contains an array of polygon references
            int MAX_POLYS = 256;
            path = new Path();
            navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);

            //find a smooth path over the mesh surface
            int npolys = path.Count;
            SVector3 iterPos = new SVector3();
            SVector3 targetPos = new SVector3();
            navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
            navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);

            smoothPath = new List<SVector3>(2048);
            smoothPath.Add(iterPos);

            float STEP_SIZE = 0.5f;
            float SLOP = 0.01f;
            while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
            {
                //find location to steer towards
                SVector3 steerPos = new SVector3();
                StraightPathFlags steerPosFlag = 0;
                NavPolyId steerPosRef = NavPolyId.Null;

                if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
                    break;

                bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
                bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;

                //find movement delta
                SVector3 delta = steerPos - iterPos;
                float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));

                //if steer target is at end of path or off-mesh link
                //don't move past location
                if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                    len = 1;
                else
                    len = STEP_SIZE / len;

                SVector3 moveTgt = new SVector3();
                VMad(ref moveTgt, iterPos, delta, len);

                //move
                SVector3 result = new SVector3();
                List<NavPolyId> visited = new List<NavPolyId>(16);
                NavPoint startPoint = new NavPoint(path[0], iterPos);
                navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
                path.FixupCorridor(visited);
                npolys = path.Count;
                float h = 0;
                navMeshQuery.GetPolyHeight(path[0], result, ref h);
                result.Y = h;
                iterPos = result;

                //handle end of path when close enough
                if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
                {
                    //reached end of path
                    iterPos = targetPos;
                    if (smoothPath.Count < smoothPath.Capacity)
                    {
                        smoothPath.Add(iterPos);
                    }
                    break;
                }

                //store results
                if (smoothPath.Count < smoothPath.Capacity)
                {
                    smoothPath.Add(iterPos);
                }
            }
        }
示例#26
0
        /// <summary>
        /// Merge two paths after the start is changed
        /// </summary>
        /// <param name="path">The current path</param>
        /// <param name="npath">Current path length</param>
        /// <param name="maxPath">Maximum path length allowed</param>
        /// <param name="visited">The visited polygons</param>
        /// <param name="nvisited">Visited path length</param>
        /// <returns>New path length</returns>
        public int MergeCorridorStartMoved(Path path, List<NavPolyId> visited)
        {
            int furthestPath = -1;
            int furthestVisited = -1;

            //find furthest common polygon
            for (int i = path.Count - 1; i >= 0; i--)
            {
                bool found = false;
                for (int j = visited.Count - 1; j >= 0; j--)
                {
                    if (path[i] == visited[j])
                    {
                        furthestPath = i;
                        furthestVisited = j;
                        found = true;
                    }
                }

                if (found)
                    break;
            }

            //if no intersection found just return current path
            if (furthestPath == -1 || furthestVisited == -1)
                return path.Count;

            //concatenate paths

            //adjust beginning of buffer to include the visited
            int req = visited.Count - furthestVisited;
            int orig = Math.Min(furthestPath + 1, path.Count);
            int size = Math.Max(0, path.Count - orig);
            if (req + size > path.Count)
                size = path.Count - req;
            if (size > 0)
            {
                for (int i = 0; i < size; i++)
                    path[req + i] = path[orig + i];
            }

            //store visited
            for (int i = 0; i < req; i++)
                path[i] = visited[(visited.Count - 1) - i];

            return req + size;
        }