/// <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 Node GetNode(PolyId id)
{
Node node;
if (nodeDict.TryGetValue(id, out node))
{
return node;
}
if (nodes.Count >= maxNodes)
return null;
Node newNode = new Node();
newNode.ParentIdx = 0;
newNode.cost = 0;
newNode.total = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return newNode;
}
/// <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 void SetNodeFlagOpen(ref Node node)
{
node.Flags |= NodeFlags.Open;
}
public void SetNodeFlagClosed(ref Node node)
{
node.Flags &= ~NodeFlags.Open;
node.Flags |= NodeFlags.Closed;
}
public NodeFlags RemoveNodeFlagClosed(Node node)
{
return node.Flags & ~NodeFlags.Closed;
}
public bool IsInOpenList(Node node)
{
return (node.Flags & NodeFlags.Open) != 0;
}
public bool IsInClosedList(Node node)
{
return (node.Flags & NodeFlags.Closed) != 0;
}
/// <summary>
/// Gets the id of the node.
/// </summary>
/// <param name="node">The node</param>
/// <returns>The id</returns>
public int GetNodeIdx(Node node)
{
if (node == null)
return 0;
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] == node)
return i + 1;
}
return 0;
}
