public PathQueue(int maxSearchNodeCount, ref TiledNavMesh nav) { this.navquery = new NavMeshQuery(nav, maxSearchNodeCount); this.navqueryfilter = new NavQueryFilter(); this.queue = new PathQuery[MaxQueue]; for (int i = 0; i < MaxQueue; i++) { queue[i].Index = 0; queue[i].Path = new Path(); } this.queueHead = 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); } } }
/// <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; }
public QueryData() { Filter = new NavQueryFilter(); }
/// <summary> /// Initialize a sliced path, which is used mostly for crowd pathfinding. /// </summary> /// <param name="startPoint">The start point.</param> /// <param name="endPoint">The end point.</param> /// <param name="filter">A filter for the navigation mesh.</param> /// <param name="options">Options for how the path should be found.</param> /// <returns>True if path initialized, false otherwise</returns> public bool InitSlicedFindPath(ref NavPoint startPoint, ref NavPoint endPoint, NavQueryFilter filter, FindPathOptions options) { //validate input if (startPoint.Polygon == NavPolyId.Null || endPoint.Polygon == NavPolyId.Null) return false; if (!nav.IsValidPolyRef(startPoint.Polygon) || !nav.IsValidPolyRef(endPoint.Polygon)) return false; if (startPoint.Polygon == endPoint.Polygon) { query.Status = true; return true; } //init path state query = new QueryData(); query.Status = false; query.Start = startPoint; query.End = endPoint; nodePool.Clear(); openList.Clear(); NavNode startNode = nodePool.GetNode(startPoint.Polygon); startNode.Position = startPoint.Position; startNode.ParentIndex = 0; startNode.PolyCost = 0; startNode.TotalCost = (endPoint.Position - startPoint.Position).Length() * HeuristicScale; startNode.Id = startPoint.Polygon; startNode.Flags = NodeFlags.Open; openList.Push(startNode); query.Status = true; query.LastBestNode = startNode; query.LastBestNodeCost = startNode.TotalCost; return query.Status; }
/// <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; }