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;
}
