|
public GetCost ( Microsoft.Xna.Framework.Vector3 a, Microsoft.Xna.Framework.Vector3 b, NavPolyId prevRef, NavTile prevTile, |
||
| a | Microsoft.Xna.Framework.Vector3 | |
| b | Microsoft.Xna.Framework.Vector3 | |
| prevRef | NavPolyId | |
| prevTile | NavTile | |
| prevPoly | ||
| curRef | NavPolyId | |
| curTile | NavTile | |
| curPoly | ||
| nextRef | NavPolyId | |
| nextTile | NavTile | |
| nextPoly | ||
| Результат | float | |
/// <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;
}
