/// <summary>
/// Finds a random point in a NavMesh within a specified circle.
/// </summary>
/// <param name="center">The center point.</param>
/// <param name="radius">The maximum distance away from the center that the random point can be. If 0, any point on the mesh can be returned.</param>
/// <returns>A random point within the specified circle.</returns>
public NavPoint FindRandomPointAroundCircle(NavPoint center, float radius)
{
NavPoint result;
this.FindRandomPointAroundCircle(center, radius, out result);
return result;
}
/// <summary>
/// Finds a random point in a NavMesh within a specified circle.
/// </summary>
/// <param name="center">The center point.</param>
/// <param name="radius">The maximum distance away from the center that the random point can be. If 0, any point on the mesh can be returned.</param>
/// <param name="randomPoint">A random point within the specified circle.</param>
public void FindRandomPointAroundCircle(NavPoint center, float radius, out NavPoint randomPoint)
{
//TODO fix state
if (nav == null || nodePool == null || openList == null)
throw new InvalidOperationException("Something null");
//validate input
if (center.Polygon == PolyId.Null)
throw new ArgumentOutOfRangeException("startRef", "Null poly reference");
if (!nav.IsValidPolyRef(center.Polygon))
throw new ArgumentException("startRef", "Poly reference is not valid for this navmesh");
MeshTile startTile;
Poly startPoly;
nav.TryGetTileAndPolyByRefUnsafe(center.Polygon, out startTile, out startPoly);
nodePool.Clear();
openList.Clear();
Node startNode = nodePool.GetNode(center.Polygon);
startNode.Pos = center.Position;
startNode.ParentIdx = 0;
startNode.cost = 0;
startNode.total = 0;
startNode.Id = center.Polygon;
startNode.Flags = NodeFlags.Open;
openList.Push(startNode);
bool doRadiusCheck = radius != 0;
float radiusSqr = radius * radius;
float areaSum = 0.0f;
MeshTile randomTile = null;
Poly randomPoly = null;
PolyId randomPolyRef = PolyId.Null;
while (openList.Count > 0)
{
Node bestNode = openList.Pop();
SetNodeFlagClosed(ref bestNode);
//get poly and tile
PolyId bestRef = bestNode.Id;
MeshTile bestTile;
Poly bestPoly;
nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);
//place random locations on ground
if (bestPoly.PolyType == PolygonType.Ground)
{
//calculate area of polygon
float polyArea = 0.0f;
float area;
for (int j = 2; j < bestPoly.VertCount; j++)
{
Triangle3.Area2D(ref bestTile.Verts[bestPoly.Verts[0]], ref bestTile.Verts[bestPoly.Verts[j - 1]], ref bestTile.Verts[bestPoly.Verts[j]], out area);
polyArea += area;
}
//choose random polygon weighted by area using resevoir sampling
areaSum += polyArea;
float u = (float)rand.NextDouble();
if (u * areaSum <= polyArea)
{
randomTile = bestTile;
randomPoly = bestPoly;
randomPolyRef = bestRef;
}
}
//get parent poly and tile
PolyId parentRef = PolyId.Null;
MeshTile parentTile;
Poly parentPoly;
if (bestNode.ParentIdx != 0)
parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIdx).Id;
if (parentRef != PolyId.Null)
nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);
for (int i = bestPoly.FirstLink; i != Link.Null; i = bestTile.Links[i].Next)
{
Link link = bestTile.Links[i];
PolyId neighbourRef = link.Reference;
//skip invalid neighbours and do not follor back to parent
if (neighbourRef == PolyId.Null || neighbourRef == parentRef)
continue;
//expand to neighbour
MeshTile neighbourTile;
Poly neighbourPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);
//find edge and calculate distance to edge
Vector3 va = new Vector3();
Vector3 vb = new Vector3();
if (!GetPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, ref va, ref vb))
continue;
//if circle isn't touching next polygon, skip it
if (doRadiusCheck)
{
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref center.Position, ref va, ref vb, out tseg);
if (distSqr > radiusSqr)
continue;
}
Node neighbourNode = nodePool.GetNode(neighbourRef);
if (neighbourNode == null)
continue;
if (IsInClosedList(neighbourNode))
continue;
//cost
if (neighbourNode.Flags == 0)
neighbourNode.Pos = Vector3.Lerp(va, vb, 0.5f);
float total = bestNode.total + (bestNode.Pos - neighbourNode.Pos).Length();
//node is already in open list and new result is worse, so skip
if (IsInOpenList(neighbourNode) && total >= neighbourNode.total)
continue;
neighbourNode.Id = neighbourRef;
neighbourNode.Flags = RemoveNodeFlagClosed(neighbourNode);
neighbourNode.ParentIdx = nodePool.GetNodeIdx(bestNode);
neighbourNode.total = total;
if (IsInOpenList(neighbourNode))
{
openList.Modify(neighbourNode);
}
else
{
neighbourNode.Flags = NodeFlags.Open;
openList.Push(neighbourNode);
}
}
}
//TODO invalid state.
if (randomPoly == null)
throw new InvalidOperationException("Poly null?");
Vector3 randomPt;
FindRandomPointOnPoly(randomTile, randomPoly, randomPolyRef, out randomPt);
randomPoint = new NavPoint(randomPolyRef, randomPt);
}
/// <summary>
/// Finds a random point somewhere in the navigation mesh.
/// </summary>
/// <param name="randomPoint">Resulting random point.</param>
public void FindRandomPoint(out NavPoint randomPoint)
{
//TODO we're object-oriented, can prevent this state from ever happening.
if (nav == null)
throw new InvalidOperationException("TODO prevent this state from ever occuring");
//randomly pick one tile
//assume all tiles cover roughly the same area
MeshTile tile = null;
float tsum = 0.0f;
for (int i = 0; i < nav.TileCount; i++)
{
MeshTile t = nav[i];
if (t == null || t.Header == null)
continue;
//choose random tile using reservoir sampling
float area = 1.0f;
tsum += area;
float u = (float)rand.NextDouble();
if (u * tsum <= area)
tile = t;
}
//TODO why?
if (tile == null)
throw new InvalidOperationException("No tiles?");
//randomly pick one polygon weighted by polygon area
Poly poly = null;
PolyId polyRef = PolyId.Null;
PolyId polyBase = nav.GetPolyRefBase(tile);
float areaSum = 0.0f;
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly p = tile.Polys[i];
//don't return off-mesh connection polygons
if (p.PolyType != PolygonType.Ground)
continue;
PolyId reference;
PolyId.SetPolyIndex(ref polyBase, i, out reference);
//calculate area of polygon
float polyArea = 0.0f;
float area;
for (int j = 2; j < p.VertCount; j++)
{
Triangle3.Area2D(ref tile.Verts[p.Verts[0]], ref tile.Verts[p.Verts[j - 1]], ref tile.Verts[p.Verts[j]], out area);
polyArea += area;
}
//choose random polygon weighted by area, usig resevoir sampling
areaSum += polyArea;
float u = (float)rand.NextDouble();
if (u * areaSum <= polyArea)
{
poly = p;
polyRef = reference;
}
}
//TODO why?
if (poly == null)
throw new InvalidOperationException("No polys?");
//randomRef = polyRef;
Vector3 randomPt;
FindRandomPointOnPoly(tile, poly, polyRef, out randomPt);
randomPoint = new NavPoint(polyRef, randomPt);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
const int MaxVisited = 16;
Vector3 result = new Vector3();
List<NavPolyId> visited = new List<NavPolyId>(MaxVisited);
NavPoint startPoint = new NavPoint(path[0], pos);
//move along navmesh and update new position
bool status = navquery.MoveAlongSurface(ref startPoint, ref npos, out result, visited);
if (status == true)
{
MergeCorridorStartMoved(path, visited);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return true;
}
return false;
}
/// <summary>
/// Find the nearest poly within a certain range.
/// </summary>
/// <param name="center">Center.</param>
/// <param name="extents">Extents.</param>
/// <param name="nearestPt">The neareast point.</param>
public void FindNearestPoly(ref Vector3 center, ref Vector3 extents, out NavPoint nearestPt)
{
nearestPt = NavPoint.Null;
//TODO error state?
// Get nearby polygons from proximity grid.
List<PolyId> polys = new List<PolyId>(128);
if (!QueryPolygons(ref center, ref extents, polys))
throw new InvalidOperationException("no nearby polys?");
float nearestDistanceSqr = float.MaxValue;
for (int i = 0; i < polys.Count; i++)
{
PolyId reference = polys[i];
Vector3 closestPtPoly;
bool posOverPoly;
ClosestPointOnPoly(reference, center, out closestPtPoly, out posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
Vector3 diff = center - closestPtPoly;
float d = 0;
if (posOverPoly)
{
MeshTile tile;
Poly poly;
nav.TryGetTileAndPolyByRefUnsafe(polys[i], out tile, out poly);
d = Math.Abs(diff.Y) - tile.Header.WalkableClimb;
d = d > 0 ? d * d : 0;
}
else
{
d = diff.LengthSquared();
}
if (d < nearestDistanceSqr)
{
nearestDistanceSqr = d;
nearestPt = new NavPoint(reference, closestPtPoly);
}
}
}
/// <summary>
/// Finds a random point in a NavMesh connected to a specified point on the same mesh.
/// </summary>
/// <param name="connectedTo">The point that the random point will be connected to.</param>
/// <param name="randomPoint">A random point connected to <c>connectedTo</c>.</param>
public void FindRandomConnectedPoint(NavPoint connectedTo, out NavPoint randomPoint)
{
FindRandomPointAroundCircle(connectedTo, 0, out randomPoint);
}
/// <summary>
/// Request an empty slot in the path queue
/// </summary>
/// <param name="start">Start position</param>
/// <param name="end">End position</param>
/// <returns>Index of empty slot</returns>
public int Request(NavPoint start, NavPoint end)
{
//find empty slot
int slot = -1;
for (int i = 0; i < MaxQueue; i++)
{
if (queue[i].Index == 0)
{
slot = i;
break;
}
}
//could not find slot
if (slot == -1)
return PathQueue.Invalid;
int index = nextHandle++;
if (nextHandle == 0) nextHandle++;
PathQuery q = queue[slot];
q.Index = index;
q.Start = start;
q.End = end;
q.Status = 0;
q.PathCount = 0;
q.KeepAlive = 0;
queue[slot] = q;
return index;
}
public bool Raycast(NavPoint startPoint, Vector3 endPos, ref float t, ref Vector3 hitNormal, PolyId[] path, ref int pathCount, int maxPath)
{
t = 0;
pathCount = 0;
//validate input
if (startPoint.Polygon == PolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon))
return false;
PolyId curRef = startPoint.Polygon;
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
int n = 0;
hitNormal = new Vector3(0, 0, 0);
while (curRef != PolyId.Null)
{
//cast ray against current polygon
MeshTile tile;
Poly poly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out tile, out poly);
//collect vertices
int nv = 0;
for (int i = 0; i < poly.VertCount; i++)
{
verts[nv] = tile.Verts[poly.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
pathCount = n;
return true;
}
//keep track of furthest t so far
if (tmax > t)
t = tmax;
//store visited polygons
if (n < maxPath)
path[n++] = curRef;
//ray end is completely inside the polygon
if (segMax == -1)
{
t = float.MaxValue;
pathCount = n;
return true;
}
//follow neighbours
PolyId nextRef = PolyId.Null;
for (int i = poly.FirstLink; i != Link.Null; i = tile.Links[i].Next)
{
Link link = tile.Links[i];
//find link which contains the edge
if (link.Edge != segMax)
continue;
//get pointer to the next polygon
MeshTile nextTile;
Poly nextPoly;
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);
//skip off-mesh connection
if (nextPoly.PolyType == PolygonType.OffMeshConnection)
continue;
//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 = poly.Verts[link.Edge];
int v1 = poly.Verts[(link.Edge + 1) % poly.VertCount];
Vector3 left = tile.Verts[v0];
Vector3 right = tile.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 (nextRef == PolyId.Null)
{
//no neighbour, 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;
hitNormal.X = dz;
hitNormal.Y = 0;
hitNormal.Z = -dx;
hitNormal.Normalize();
pathCount = n;
return true;
}
//no hit, advance to neighbour polygon
curRef = nextRef;
}
pathCount = n;
return true;
}
/// <summary>
/// Examine polygons in the NavMeshQuery and add polygon edges
/// </summary>
/// <param name="reference">The starting polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="collisionQueryRange">Range to query</param>
/// <param name="navquery">The NavMeshQuery</param>
public void Update(NavPolyId reference, Vector3 pos, float collisionQueryRange, NavMeshQuery navquery)
{
const int MAX_SEGS_PER_POLY = PathfindingCommon.VERTS_PER_POLYGON;
if (reference == NavPolyId.Null)
{
this.center = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
this.segCount = 0;
this.numPolys = 0;
return;
}
this.center = pos;
//first query non-overlapping polygons
NavPolyId[] tempArray = new NavPolyId[polys.Length];
NavPoint centerPoint = new NavPoint(reference, pos);
navquery.FindLocalNeighborhood(ref centerPoint, collisionQueryRange, polys, tempArray, ref numPolys, MaxLocalPolys);
//secondly, store all polygon edges
this.segCount = 0;
Segment[] segs = new Segment[MAX_SEGS_PER_POLY];
int numSegs = 0;
for (int j = 0; j < numPolys; j++)
{
tempArray = new NavPolyId[segs.Length];
navquery.GetPolyWallSegments(polys[j], segs, tempArray, ref numSegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < numSegs; k++)
{
//skip too distant segments
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref pos, ref segs[k].Start, ref segs[k].End, out tseg);
if (distSqr > collisionQueryRange * collisionQueryRange)
continue;
AddSegment(distSqr, segs[k]);
}
}
}
/// <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;
}
}
}
}
public StraightPathVertex(NavPoint point, StraightPathFlags flags)
{
Point = point;
Flags = flags;
}
/// <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);
}
}
/// <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;
}
/// <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>
/// <returns>True if path initialized, false otherwise</returns>
public bool InitSlicedFindPath(NavPoint startPoint, NavPoint endPoint)
{
//validate input
if (startPoint.Polygon == PolyId.Null || endPoint.Polygon == PolyId.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();
Node startNode = nodePool.GetNode(startPoint.Polygon);
startNode.Pos = startPoint.Position;
startNode.ParentIdx = 0;
startNode.cost = 0;
startNode.total = (endPoint.Position - startPoint.Position).Length() * H_SCALE;
startNode.Id = startPoint.Polygon;
startNode.Flags = NodeFlags.Open;
openList.Push(startNode);
query.Status = true;
query.LastBestNode = startNode;
query.LastBestNodeCost = startNode.total;
return query.Status;
}
/// <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>
/// This method is optimized for small delta movement and a small number of polygons.
/// If movement distance is too large, the result will form an incomplete path.
/// </summary>
/// <param name="startPoint">The start point.</param>
/// <param name="endPos">End position</param>
/// <param name="resultPos">Intermediate point</param>
/// <param name="visited">Visited polygon references</param>
/// <returns>True, if point found. False, if otherwise.</returns>
public bool MoveAlongSurface(NavPoint startPoint, Vector3 endPos, ref Vector3 resultPos, List<PolyId> visited)
{
if (nav == null)
return false;
if (tinyNodePool == null)
return false;
visited.Clear();
//validate input
if (startPoint.Polygon == PolyId.Null)
return false;
if (!nav.IsValidPolyRef(startPoint.Polygon))
return false;
int MAX_STACK = 48;
Queue<Node> nodeQueue = new Queue<Node>(MAX_STACK);
tinyNodePool.Clear();
Node startNode = tinyNodePool.GetNode(startPoint.Polygon);
startNode.ParentIdx = 0;
startNode.cost = 0;
startNode.total = 0;
startNode.Id = startPoint.Polygon;
startNode.Flags = NodeFlags.Closed;
nodeQueue.Enqueue(startNode);
Vector3 bestPos = startPoint.Position;
float bestDist = float.MaxValue;
Node bestNode = null;
//search constraints
Vector3 searchPos = Vector3.Lerp(startPoint.Position, endPos, 0.5f);
float searchRad = (startPoint.Position - endPos).Length() / 2.0f + 0.001f;
float searchRadSqr = searchRad * searchRad;
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
while (nodeQueue.Count > 0)
{
//pop front
Node curNode = nodeQueue.Dequeue();
//get poly and tile
PolyId curRef = curNode.Id;
MeshTile curTile;
Poly curPoly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
//collect vertices
int nverts = curPoly.VertCount;
for (int i = 0; i < nverts; i++)
verts[i] = curTile.Verts[curPoly.Verts[i]];
//if target is inside poly, stop search
if (Containment.PointInPoly(endPos, verts, nverts))
{
bestNode = curNode;
bestPos = endPos;
break;
}
//find wall edges and find nearest point inside walls
for (int i = 0, j = curPoly.VertCount - 1; i < curPoly.VertCount; j = i++)
{
//find links to neighbors
List<PolyId> neis = new List<PolyId>(8);
if ((curPoly.Neis[j] & Link.External) != 0)
{
//tile border
for (int k = curPoly.FirstLink; k != Link.Null; k = curTile.Links[k].Next)
{
Link link = curTile.Links[k];
if (link.Edge == j)
{
if (link.Reference != PolyId.Null)
{
MeshTile neiTile;
Poly neiPoly;
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out neiTile, out neiPoly);
if (neis.Count < neis.Capacity)
neis.Add(link.Reference);
}
}
}
}
else if (curPoly.Neis[j] != 0)
{
int idx = curPoly.Neis[j] - 1;
PolyId reference = nav.GetPolyRefBase(curTile);
PolyId.SetPolyIndex(ref reference, idx, out reference);
neis.Add(reference); //internal edge, encode id
}
if (neis.Count == 0)
{
//wall edge, calculate distance
float tseg = 0;
float distSqr = Distance.PointToSegment2DSquared(ref endPos, ref verts[j], ref verts[i], out tseg);
if (distSqr < bestDist)
{
//update nearest distance
bestPos = Vector3.Lerp(verts[j], verts[i], tseg);
bestDist = distSqr;
bestNode = curNode;
}
}
else
{
for (int k = 0; k < neis.Count; k++)
{
//skip if no node can be allocated
Node neighbourNode = tinyNodePool.GetNode(neis[k]);
if (neighbourNode == null)
continue;
//skip if already visited
if ((neighbourNode.Flags & NodeFlags.Closed) != 0)
continue;
//skip the link if too far from search constraint
float distSqr = Distance.PointToSegment2DSquared(ref searchPos, ref verts[j], ref verts[i]);
if (distSqr > searchRadSqr)
continue;
//mark the node as visited and push to queue
if (nodeQueue.Count < MAX_STACK)
{
neighbourNode.ParentIdx = tinyNodePool.GetNodeIdx(curNode);
neighbourNode.Flags |= NodeFlags.Closed;
nodeQueue.Enqueue(neighbourNode);
}
}
}
}
}
if ((endPos - bestPos).Length() > 1f)
return false;
if (bestNode != null)
{
//save the path
Node node = bestNode;
do
{
visited.Add(node.Id);
if (visited.Count >= visited.Capacity)
break;
node = tinyNodePool.GetNodeAtIdx(node.ParentIdx);
}
while (node != null);
//reverse the path since it's backwards
visited.Reverse();
}
resultPos = bestPos;
return true;
}
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);
}
/// <summary>
/// Store polygons that are within a certain range from the current polygon
/// </summary>
/// <param name="centerPoint">Starting position</param>
/// <param name="radius">Range to search within</param>
/// <param name="resultRef">All the polygons within range</param>
/// <param name="resultParent">Polygon's parents</param>
/// <param name="resultCount">Number of polygons stored</param>
/// <param name="maxResult">Maximum number of polygons allowed</param>
/// <returns>True, unless input is invalid</returns>
public bool FindLocalNeighbourhood(NavPoint centerPoint, float radius, PolyId[] resultRef, PolyId[] resultParent, ref int resultCount, int maxResult)
{
resultCount = 0;
//validate input
if (centerPoint.Polygon == PolyId.Null || !nav.IsValidPolyRef(centerPoint.Polygon))
return false;
int MAX_STACK = 48;
Node[] stack = new Node[MAX_STACK];
int nstack = 0;
tinyNodePool.Clear();
Node startNode = tinyNodePool.GetNode(centerPoint.Polygon);
startNode.ParentIdx = 0;
startNode.Id = centerPoint.Polygon;
startNode.Flags = NodeFlags.Closed;
stack[nstack++] = startNode;
float radiusSqr = radius * radius;
Vector3[] pa = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
Vector3[] pb = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
int n = 0;
if (n < maxResult)
{
resultRef[n] = startNode.Id;
resultParent[n] = PolyId.Null;
++n;
}
while (nstack > 0)
{
//pop front
Node curNode = stack[0];
for (int i = 0; i < nstack - 1; i++)
stack[i] = stack[i + 1];
nstack--;
//get poly and tile
PolyId curRef = curNode.Id;
MeshTile curTile;
Poly curPoly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
for (int i = curPoly.FirstLink; i != Link.Null; i = curTile.Links[i].Next)
{
Link link = curTile.Links[i];
PolyId neighbourRef = link.Reference;
//skip invalid neighbours
if (neighbourRef == PolyId.Null)
continue;
//skip if cannot allocate more nodes
Node neighbourNode = tinyNodePool.GetNode(neighbourRef);
if (neighbourNode == null)
continue;
//skip visited
if ((neighbourNode.Flags & NodeFlags.Closed) != 0)
continue;
//expand to neighbour
MeshTile neighbourTile;
Poly neighbourPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);
//skip off-mesh connections
if (neighbourPoly.PolyType == PolygonType.OffMeshConnection)
continue;
//find edge and calculate distance to edge
Vector3 va = new Vector3();
Vector3 vb = new Vector3();
if (!GetPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, ref va, ref vb))
continue;
//if the circle is not touching the next polygon, skip it
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref centerPoint.Position, ref va, ref vb, out tseg);
if (distSqr > radiusSqr)
continue;
//mark node visited
neighbourNode.Flags |= NodeFlags.Closed;
neighbourNode.ParentIdx = tinyNodePool.GetNodeIdx(curNode);
//check that the polygon doesn't collide with existing polygons
//collect vertices of the neighbour poly
int npa = neighbourPoly.VertCount;
for (int k = 0; k < npa; k++)
pa[k] = neighbourTile.Verts[neighbourPoly.Verts[k]];
bool overlap = false;
for (int j = 0; j < n; j++)
{
PolyId pastRef = resultRef[j];
//connected polys do not overlap
bool connected = false;
for (int k = curPoly.FirstLink; k != Link.Null; k = curTile.Links[k].Next)
{
if (curTile.Links[k].Reference == pastRef)
{
connected = true;
break;
}
}
if (connected)
continue;
//potentially overlapping
MeshTile pastTile;
Poly pastPoly;
nav.TryGetTileAndPolyByRefUnsafe(pastRef, out pastTile, out pastPoly);
//get vertices and test overlap
int npb = pastPoly.VertCount;
for (int k = 0; k < npb; k++)
pb[k] = pastTile.Verts[pastPoly.Verts[k]];
if (Intersection.PolyPoly2D(pa, npa, pb, npb))
{
overlap = true;
break;
}
}
if (overlap)
continue;
//store poly
if (n < maxResult)
{
resultRef[n] = neighbourRef;
resultParent[n] = curRef;
++n;
}
if (nstack < MAX_STACK)
{
stack[nstack++] = neighbourNode;
}
}
}
resultCount = n;
return true;
}
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;
}
/// <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="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, List<PolyId> path)
{
//reset path of polygons
path.Clear();
PolyId startRef = startPt.Polygon;
Vector3 startPos = startPt.Position;
PolyId endRef = endPt.Polygon;
Vector3 endPos = endPt.Position;
if (startRef == PolyId.Null || endRef == PolyId.Null)
return false;
//path can't store any elements
if (path.Capacity == 0)
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
Node startNode = nodePool.GetNode(startRef);
startNode.Pos = startPos;
startNode.ParentIdx = 0;
startNode.cost = 0;
startNode.total = (startPos - endPos).Length() * H_SCALE;
startNode.Id = startRef;
startNode.Flags = NodeFlags.Open;
openList.Push(startNode);
Node lastBestNode = startNode;
float lastBestTotalCost = startNode.total;
while (openList.Count > 0)
{
//remove node from open list and put it in closed list
Node bestNode = openList.Pop();
SetNodeFlagClosed(ref bestNode);
//reached the goal. stop searching
if (bestNode.Id == endRef)
{
lastBestNode = bestNode;
break;
}
//get current poly and tile
PolyId bestRef = bestNode.Id;
MeshTile bestTile;
Poly bestPoly;
nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);
//get parent poly and tile
PolyId parentRef = PolyId.Null;
MeshTile parentTile;
Poly parentPoly;
if (bestNode.ParentIdx != 0)
parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIdx).Id;
if (parentRef != PolyId.Null)
nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);
//examine neighbors
for (int i = bestPoly.FirstLink; i != Link.Null; i = bestTile.Links[i].Next)
{
PolyId neighbourRef = bestTile.Links[i].Reference;
//skip invalid ids and do not expand back to where we came from
if (neighbourRef == PolyId.Null || neighbourRef == parentRef)
continue;
//get neighbour poly and tile
MeshTile neighbourTile;
Poly neighbourPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);
Node neighbourNode = nodePool.GetNode(neighbourRef);
if (neighbourNode == null)
continue;
//if node is visited the first time, calculate node position
if (neighbourNode.Flags == 0)
{
GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, ref neighbourNode.Pos);
}
//calculate cost and heuristic
float cost = 0;
float heuristic = 0;
//special case for last node
if (neighbourRef == endRef)
{
//cost
float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);
float endCost = GetCost(neighbourNode.Pos, endPos, neighbourPoly);
cost = bestNode.cost + curCost + endCost;
heuristic = 0;
}
else
{
//cost
float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);
cost = bestNode.cost + curCost;
heuristic = (neighbourNode.Pos - endPos).Length() * H_SCALE;
}
float total = cost + heuristic;
//the node is already in open list and new result is worse, skip
if (IsInOpenList(neighbourNode) && total >= neighbourNode.total)
continue;
//the node is already visited and processesd, and the new result is worse, skip
if (IsInClosedList(neighbourNode) && total >= neighbourNode.total)
continue;
//add or update the node
neighbourNode.ParentIdx = nodePool.GetNodeIdx(bestNode);
neighbourNode.Id = neighbourRef;
neighbourNode.Flags = RemoveNodeFlagClosed(neighbourNode);
neighbourNode.cost = cost;
neighbourNode.total = total;
if (IsInOpenList(neighbourNode))
{
//already in open, update node location
openList.Modify(neighbourNode);
}
else
{
//put the node in the open list
SetNodeFlagOpen(ref neighbourNode);
openList.Push(neighbourNode);
}
//update nearest node to target so far
if (heuristic < lastBestTotalCost)
{
lastBestTotalCost = heuristic;
lastBestNode = neighbourNode;
}
}
}
//save path
Node node = lastBestNode;
do
{
path.Add(node.Id);
if (path.Count >= path.Capacity)
break;
node = nodePool.GetNodeAtIdx(node.ParentIdx);
}
while (node != null);
//reverse the path since it's backwards
path.Reverse();
return true;
}
/// <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;
}
/// <summary>
/// Finds a random point in a NavMesh connected to a specified point on the same mesh.
/// </summary>
/// <param name="connectedTo">The point that the random point will be connected to.</param>
/// <returns>A random point connected to <c>connectedTo</c>.</returns>
public NavPoint FindRandomConnectedPoint(NavPoint connectedTo)
{
NavPoint result;
FindRandomConnectedPoint(connectedTo, out result);
return result;
}
/// <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;
}
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);
}
}
}
public StraightPathVertex(NavPoint point, StraightPathFlags flags)
{
Point = point;
Flags = flags;
}
