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