/// <summary>
/// Gets the id of the node.
/// </summary>
/// <param name="node">The node</param>
/// <returns>The id</returns>
public int GetNodeIdx(NavNode node)
{
if (node == null)
{
return(0);
}
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] == node)
{
return(i + 1);
}
}
return(0);
}
/// <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 FindLocalNeighborhood(ref NavPoint centerPoint, float radius, NavPolyId[] resultRef, NavPolyId[] resultParent, ref int resultCount, int maxResult)
{
resultCount = 0;
//validate input
if (centerPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(centerPoint.Polygon))
return false;
int MAX_STACK = 48;
NavNode[] stack = new NavNode[MAX_STACK];
int nstack = 0;
tinyNodePool.Clear();
NavNode startNode = tinyNodePool.GetNode(centerPoint.Polygon);
startNode.ParentIndex = 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] = NavPolyId.Null;
++n;
}
while (nstack > 0)
{
//pop front
NavNode curNode = stack[0];
for (int i = 0; i < nstack - 1; i++)
stack[i] = stack[i + 1];
nstack--;
//get poly and tile
NavPolyId curRef = curNode.Id;
NavTile curTile;
NavPoly curPoly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
foreach (Link link in curPoly.Links)
{
NavPolyId neighborRef = link.Reference;
//skip invalid neighbors
if (neighborRef == NavPolyId.Null)
continue;
//skip if cannot allocate more nodes
NavNode neighborNode = tinyNodePool.GetNode(neighborRef);
if (neighborNode == null)
continue;
//skip visited
if ((neighborNode.Flags & NodeFlags.Closed) != 0)
continue;
//expand to neighbor
NavTile neighborTile;
NavPoly neighborPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);
//skip off-mesh connections
if (neighborPoly.PolyType == NavPolyType.OffMeshConnection)
continue;
//find edge and calculate distance to edge
Vector3 va = new Vector3();
Vector3 vb = new Vector3();
if (!GetPortalPoints(curRef, curPoly, curTile, neighborRef, neighborPoly, neighborTile, 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
neighborNode.Flags |= NodeFlags.Closed;
neighborNode.ParentIndex = tinyNodePool.GetNodeIdx(curNode);
//check that the polygon doesn't collide with existing polygons
//collect vertices of the neighbor poly
int npa = neighborPoly.VertCount;
for (int k = 0; k < npa; k++)
pa[k] = neighborTile.Verts[neighborPoly.Verts[k]];
bool overlap = false;
for (int j = 0; j < n; j++)
{
NavPolyId pastRef = resultRef[j];
//connected polys do not overlap
bool connected = false;
foreach (Link link2 in curPoly.Links)
{
if (link2.Reference == pastRef)
{
connected = true;
break;
}
}
if (connected)
continue;
//potentially overlapping
NavTile pastTile;
NavPoly 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] = neighborRef;
resultParent[n] = curRef;
++n;
}
if (nstack < MAX_STACK)
{
stack[nstack++] = neighborNode;
}
}
}
resultCount = n;
return true;
}
public void SetNodeFlagOpen(ref NavNode node)
{
node.Flags |= NodeFlags.Open;
}
public void SetNodeFlagClosed(ref NavNode node)
{
node.Flags &= ~NodeFlags.Open;
node.Flags |= NodeFlags.Closed;
}
public NodeFlags RemoveNodeFlagClosed(NavNode node)
{
return node.Flags & ~(NodeFlags.Closed | NodeFlags.ParentDetached);
}
public bool IsInOpenList(NavNode node)
{
return (node.Flags & NodeFlags.Open) != 0;
}
public bool IsInClosedList(NavNode node)
{
return (node.Flags & NodeFlags.Closed) != 0;
}
/// <summary>
/// Try to find the node. If it doesn't exist, create a new node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node</returns>
public NavNode GetNode(NavPolyId id)
{
NavNode node;
if (nodeDict.TryGetValue(id, out node))
{
return node;
}
if (nodes.Count >= maxNodes)
return null;
NavNode newNode = new NavNode();
newNode.ParentIndex = 0;
newNode.PolyCost = 0;
newNode.TotalCost = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return newNode;
}
/// <summary>
/// Gets the id of the node.
/// </summary>
/// <param name="node">The node</param>
/// <returns>The id</returns>
public int GetNodeIdx(NavNode node)
{
if (node == null)
return 0;
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] == node)
return i + 1;
}
return 0;
}
