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