/** Queries the tree for the best node, searching within a circle around \a p with the specified radius. * Will fill in both the constrained node and the not constrained node in the NNInfo. * * \see QueryClosest */ public NNInfo QueryCircle (Vector3 p, float radius, NNConstraint constraint) { if ( count == 0 ) return new NNInfo(null); var nnInfo = new NNInfo (null); SearchBoxCircle (0,p, radius, constraint, ref nnInfo); nnInfo.UpdateInfo (); return nnInfo; }
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { return GetNearest (this, nodes,position, constraint, accurateNearestNode); }
public NNInfo Query (Vector3 p, NNConstraint constraint) { if ( count == 0 ) return new NNInfo(null); var nnInfo = new NNInfo (); SearchBox (0,p, constraint, ref nnInfo); nnInfo.UpdateInfo (); return nnInfo; }
void SearchBox (int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) {//, int intendentLevel = 0) { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (box.node.ContainsPoint ((Int3)p)) { //Update the NNInfo if (nnInfo.node == null) { nnInfo.node = box.node; } else if (Mathf.Abs(((Vector3)box.node.position).y - p.y) < Mathf.Abs (((Vector3)nnInfo.node.position).y - p.y)) { nnInfo.node = box.node; } if (constraint.Suitable (box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; } else if (Mathf.Abs(box.node.position.y - p.y) < Mathf.Abs (nnInfo.constrainedNode.position.y - p.y)) { nnInfo.constrainedNode = box.node; } } } return; } //Search children if (arr[box.left].Contains(p)) { SearchBox (box.left,p, constraint, ref nnInfo); } if (arr[box.right].Contains(p)) { SearchBox (box.right,p, constraint, ref nnInfo); } }
/** Returns the nearest node to a position using the specified NNConstraint. * \param position The position to try to find a close node to * \param hint Can be passed to enable some graph generators to find the nearest node faster. * \param constraint Can for example tell the function to try to return a walkable node. If you do not get a good node back, consider calling GetNearestForce. */ public virtual NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { // This is a default implementation and it is pretty slow // Graphs usually override this to provide faster and more specialised implementations float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; float minDist = float.PositiveInfinity; GraphNode minNode = null; float minConstDist = float.PositiveInfinity; GraphNode minConstNode = null; // Loop through all nodes and find the closest suitable node GetNodes (node => { float dist = (position-(Vector3)node.position).sqrMagnitude; if (dist < minDist) { minDist = dist; minNode = node; } if (dist < minConstDist && dist < maxDistSqr && constraint.Suitable (node)) { minConstDist = dist; minConstNode = node; } return true; }); var nnInfo = new NNInfo (minNode); nnInfo.constrainedNode = minConstNode; if (minConstNode != null) { nnInfo.constClampedPosition = (Vector3)minConstNode.position; } else if (minNode != null) { nnInfo.constrainedNode = minNode; nnInfo.constClampedPosition = (Vector3)minNode.position; } return nnInfo; }
void SearchBoxClosest (int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (NodeIntersectsCircle (box.node,p,closestDist)) { //Update the NNInfo #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,Color.red); #endif Vector3 closest = box.node.ClosestPointOnNode (p); if (constraint == null || constraint.Suitable (box.node)) { float dist = (closest-p).sqrMagnitude; if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)Math.Sqrt (dist); } else if (dist < closestDist*closestDist) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)Math.Sqrt (dist); } } } else { #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.blue); #endif } } else { #if ASTARDEBUG Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMin),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMax),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMin,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMax,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); #endif //Search children if (RectIntersectsCircle (arr[box.left].rect,p,closestDist)) { SearchBoxClosest (box.left,p, ref closestDist, constraint, ref nnInfo); } if (RectIntersectsCircle (arr[box.right].rect,p,closestDist)) { SearchBoxClosest (box.right,p, ref closestDist, constraint, ref nnInfo); } } }
MeshNode SearchBoxInside (int boxi, Vector3 p, NNConstraint constraint) { BBTreeBox box = arr[boxi]; if (box.node != null) { if (box.node.ContainsPoint ((Int3)p)) { //Update the NNInfo #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,Color.red); #endif if (constraint == null || constraint.Suitable (box.node)) { return box.node; } } else { #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.blue); #endif } } else { #if ASTARDEBUG Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMin),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMax),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMin,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMax,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); #endif //Search children MeshNode g; if (arr[box.left].Contains (p)) { g = SearchBoxInside (box.left,p, constraint); if (g != null) return g; } if (arr[box.right].Contains (p)) { g = SearchBoxInside (box.right, p, constraint); if (g != null) return g; } } return null; }
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { //return NavMeshGraph.GetNearest (this, nodes,position, constraint, accurateNearestNode); if (tiles == null) return new NNInfo (); Vector3 localPosition = position - forcedBounds.min; int tx = Mathf.FloorToInt (localPosition.x / (cellSize*tileSizeX)); int tz = Mathf.FloorToInt (localPosition.z / (cellSize*tileSizeZ)); //Clamp to graph borders tx = Mathf.Clamp (tx, 0, tileXCount-1); tz = Mathf.Clamp (tz, 0, tileZCount-1); int wmax = Math.Max (tileXCount, tileZCount); var best = new NNInfo(); float bestDistance = float.PositiveInfinity; bool xzSearch = nearestSearchOnlyXZ || ( constraint != null && constraint.distanceXZ ); //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min, Vector3.up*15, Color.blue); //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.forward*5, Vector3.back*10, Color.blue); //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.left*5, Vector3.right*10, Color.blue); //Search outwards in a diamond pattern from the closest tile for (int w=0;w<wmax;w++) { if (!xzSearch && bestDistance < (w-1)*cellSize*Math.Max (tileSizeX, tileSizeZ)) break; int zmax = Math.Min (w+tz +1, tileZCount); for (int z=Math.Max(-w+tz, 0); z < zmax; z++) { //Solve for z such that abs(x-tx) + abs(z-tx) == w //Delta X coordinate int dx = Math.Abs( w - Math.Abs(z-tz)); //Solution is dx + tx and -dx + tx //First solution negative delta x if (-dx + tx >= 0) { //Absolute x coordinate int x = -dx + tx; NavmeshTile tile = tiles[x + z*tileXCount]; if (tile != null) { if (xzSearch) { best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best); if ( bestDistance < float.PositiveInfinity ) break; } else { best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best); } } } //Other solution, make sure it is not the same solution by checking x != 0 if (dx != 0 && dx + tx < tileXCount) { //Absolute x coordinate int x = dx + tx; NavmeshTile tile = tiles[x + z*tileXCount]; if (tile != null) { if (xzSearch) { best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best); if ( bestDistance < float.PositiveInfinity ) break; } else { best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best); } } } } } best.node = best.constrainedNode; best.constrainedNode = null; best.clampedPosition = best.constClampedPosition; return best; //return GetNearestForce (position, constraint); }
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { if (nodes == null || depth*width*layerCount != nodes.Length) { //Debug.LogError ("NavGraph hasn't been generated yet"); return new NNInfo (); } var graphPosition = inverseMatrix.MultiplyPoint3x4 (position); int x = Mathf.Clamp (Mathf.RoundToInt (graphPosition.x-0.5F) , 0, width-1); int z = Mathf.Clamp (Mathf.RoundToInt (graphPosition.z-0.5F) , 0, depth-1); var minNode = GetNearestNode (position, x, z, null); return new NNInfo(minNode); }
/** Finds the first node which contains \a position. * "Contains" is defined as \a position is inside the triangle node when seen from above. So only XZ space matters. * In case of a multilayered environment, which node of the possibly several nodes * containing the point is undefined. * * Returns null if there was no node containing the point. This serves as a quick * check for "is this point on the navmesh or not". * * Note that the behaviour of this method is distinct from the GetNearest method. * The GetNearest method will return the closest node to a point, * which is not necessarily the one which contains it in XZ space. * * \see GetNearest */ public GraphNode PointOnNavmesh (Vector3 position, NNConstraint constraint) { //return NavMeshGraph.GetNearest (this, nodes,position, constraint, accurateNearestNode); if (tiles == null) return null; Vector3 localPosition = position - forcedBounds.min; int tx = Mathf.FloorToInt (localPosition.x / (cellSize*tileSizeX)); int tz = Mathf.FloorToInt (localPosition.z / (cellSize*tileSizeZ)); // Graph borders if (tx < 0 || tz < 0 || tx >= tileXCount || tz >= tileZCount) return null; //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min, Vector3.up*15, Color.blue); //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.forward*5, Vector3.back*10, Color.blue); //Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.left*5, Vector3.right*10, Color.blue); NavmeshTile tile = tiles[tx + tz*tileXCount]; if (tile != null) { GraphNode node = tile.bbTree.QueryInside (position, constraint); return node; } return null; }
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { NNInfo info = GetNearestForce (position, null); //info.node = info.constrainedNode; //info.constrainedNode = null; //info.clampedPosition = info.constClampedPosition; return info; }
/** This performs a linear search through all polygons returning the closest one. * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node * complying with the NNConstraint. * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool) */ public static NNInfo GetNearestForceBoth (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { var pos = (Int3)position; float minDist = -1; GraphNode minNode = null; float minConstDist = -1; GraphNode minConstNode = null; float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; GraphNodeDelegateCancelable del = delegate (GraphNode _node) { var node = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 closest = node.ClosestPointOnNode (position); float dist = ((Vector3)pos-closest).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable (node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { if (!node.ContainsPoint ((Int3)position)) { float dist = (node.position-pos).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable (node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { int dist = AstarMath.Abs (node.position.y-pos.y); if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable (node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } } return true; }; graph.GetNodes (del); var nninfo = new NNInfo (minNode); //Find the point closest to the nearest triangle if (nninfo.node != null) { var node = nninfo.node as TriangleMeshNode;//minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode (position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minConstNode; if (nninfo.constrainedNode != null) { var node = nninfo.constrainedNode as TriangleMeshNode;//minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode (position); nninfo.constClampedPosition = clP; } return nninfo; }
/** This performs a linear search through all polygons returning the closest one */ public static NNInfo GetNearestForce (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { NNInfo nn = GetNearestForceBoth (graph, navmesh,position,constraint,accurateNearestNode); nn.node = nn.constrainedNode; nn.clampedPosition = nn.constClampedPosition; return nn; }
/** This performs a linear search through all polygons returning the closest one. * This is usually only called in the Free version of the A* Pathfinding Project since the Pro one supports BBTrees and will do another query */ public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { return GetNearestForce (this, this,position,constraint, accurateNearestNode); //Debug.LogWarning ("This function shouldn't be called since constrained nodes are sent back in the GetNearest call"); //return new NNInfo (); }
/** Queries the tree for the closest node to \a p constrained by the NNConstraint. * Note that this function will, unlike QueryCircle, only fill in the constrained node. * If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None * * \see QueryCircle */ public NNInfo QueryClosest (Vector3 p, NNConstraint constraint, out float distance) { distance = float.PositiveInfinity; return QueryClosest (p, constraint, ref distance, new NNInfo (null)); }
private LevelGridNode GetNearestNode (Vector3 position, int x, int z, NNConstraint constraint) { int index = width*z+x; float minDist = float.PositiveInfinity; LevelGridNode minNode = null; for (int i=0;i<layerCount;i++) { LevelGridNode node = nodes[index + width*depth*i]; if (node != null) { float dist = ((Vector3)node.position - position).sqrMagnitude; if (dist < minDist && (constraint == null || constraint.Suitable (node))) { minDist = dist; minNode = node; } } } return minNode; }
/** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution. * Note that this function will, unlike QueryCircle, only fill in the constrained node. * If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None * * \param p Point to search around * \param constraint Optionally set to constrain which nodes to return * \param distance The best distance for the \a previous solution. Will be updated with the best distance * after this search. Will be positive infinity if no node could be found. * Set to positive infinity if there was no previous solution. * \param previous This search will start from the \a previous NNInfo and improve it if possible. * Even if the search fails on this call, the solution will never be worse than \a previous. * * \see QueryCircle */ public NNInfo QueryClosest (Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) { if ( count == 0 ) return previous; SearchBoxClosest (0,p, ref distance, constraint, ref previous); return previous; }
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { if (nodes == null || depth*width*layerCount != nodes.Length || layerCount == 0) { return new NNInfo (); } Vector3 globalPosition = position; position = inverseMatrix.MultiplyPoint3x4 (position); int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F), 0, width-1); int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F), 0, depth-1); LevelGridNode minNode; float minDist = float.PositiveInfinity; int overlap = getNearestForceOverlap; minNode = GetNearestNode (globalPosition,x,z,constraint); if (minNode != null) { minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude; } if (minNode != null) { if (overlap == 0) return new NNInfo(minNode); overlap--; } float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity; float maxDistSqr = maxDist*maxDist; for (int w = 1;;w++) { int nx; int nz = z+w; // Check if the nodes are within distance limit if (nodeSize*w > maxDist) { return new NNInfo(minNode); } for (nx = x-w;nx <= x+w;nx++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint); if (node != null) { float dist = ((Vector3)node.position-globalPosition).sqrMagnitude; //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; } } } nz = z-w; for (nx = x-w;nx <= x+w;nx++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint); if (node != null) { float dist = ((Vector3)node.position-globalPosition).sqrMagnitude; //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; } } } nx = x-w; for (nz = z-w+1;nz <= z+w-1; nz++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint); if (node != null) { float dist = ((Vector3)node.position-globalPosition).sqrMagnitude; //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; } } } nx = x+w; for (nz = z-w+1;nz <= z+w-1; nz++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint); if (node != null) { float dist = ((Vector3)node.position-globalPosition).sqrMagnitude; //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; } } } if (minNode != null) { if (overlap == 0) return new NNInfo(minNode); overlap--; } } }
public MeshNode QueryInside (Vector3 p, NNConstraint constraint) { return count != 0 ? SearchBoxInside (0,p, constraint) : null; }
/** \todo Set clamped position for Grid Graph */ public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { if (nodes == null || depth*width != nodes.Length) { return new NNInfo (); } position = inverseMatrix.MultiplyPoint3x4 (position); float xf = position.x-0.5F; float zf = position.z-0.5f; int x = Mathf.Clamp (Mathf.RoundToInt (xf) , 0, width-1); int z = Mathf.Clamp (Mathf.RoundToInt (zf) , 0, depth-1); var nn = new NNInfo(nodes[z*width+x]); float y = inverseMatrix.MultiplyPoint3x4((Vector3)nodes[z*width+x].position).y; nn.clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f,y,Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f)); return nn; }
void SearchBoxCircle (int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo) {//, int intendentLevel = 0) { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (NodeIntersectsCircle (box.node,p,radius)) { //Update the NNInfo #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.red); Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.red); #endif Vector3 closest = box.node.ClosestPointOnNode (p);//NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p); float dist = (closest-p).sqrMagnitude; if (nnInfo.node == null) { nnInfo.node = box.node; nnInfo.clampedPosition = closest; } else if (dist < (nnInfo.clampedPosition - p).sqrMagnitude) { nnInfo.node = box.node; nnInfo.clampedPosition = closest; } if (constraint == null || constraint.Suitable (box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } else if (dist < (nnInfo.constClampedPosition - p).sqrMagnitude) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } } } else { #if ASTARDEBUG Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.blue); Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.blue); #endif } return; } #if ASTARDEBUG Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMin),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMax),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMin,0,box.rect.yMax),Color.white); Debug.DrawLine (new Vector3 (box.rect.xMax,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white); #endif //Search children if (RectIntersectsCircle (arr[box.left].rect,p,radius)) { SearchBoxCircle (box.left,p, radius, constraint, ref nnInfo); } if (RectIntersectsCircle (arr[box.right].rect,p,radius)) { SearchBoxCircle (box.right,p, radius, constraint, ref nnInfo); } }
/** \todo Set clamped position for Grid Graph */ public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { if (nodes == null || depth*width != nodes.Length) { return new NNInfo (); } // Position in global space Vector3 globalPosition = position; // Position in graph space position = inverseMatrix.MultiplyPoint3x4 (position); // Find the coordinates of the closest node float xf = position.x-0.5F; float zf = position.z-0.5f; int x = Mathf.Clamp (Mathf.RoundToInt (xf) , 0, width-1); int z = Mathf.Clamp (Mathf.RoundToInt (zf) , 0, depth-1); // Closest node GridNode node = nodes[x+z*width]; GridNode minNode = null; float minDist = float.PositiveInfinity; int overlap = getNearestForceOverlap; Vector3 clampedPosition = Vector3.zero; var nn = new NNInfo(null); // If the closest node was suitable if (constraint.Suitable (node)) { minNode = node; minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude; float y = inverseMatrix.MultiplyPoint3x4((Vector3)node.position).y; clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f, y, Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f)); } if (minNode != null) { nn.node = minNode; nn.clampedPosition = clampedPosition; // We have a node, and we don't need to search more, so just return if (overlap == 0) return nn; overlap--; } // Search up to this distance float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity; float maxDistSqr = maxDist*maxDist; // Search a square/spiral pattern around the point for (int w = 1;;w++) { //Check if the nodes are within distance limit if (nodeSize*w > maxDist) { nn.node = minNode; nn.clampedPosition = clampedPosition; return nn; } bool anyInside = false; int nx; int nz = z+w; int nz2 = nz*width; // Side 1 on the square for (nx = x-w;nx <= x+w;nx++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; anyInside = true; if (constraint.Suitable (nodes[nx+nz2])) { float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude; if (dist < minDist && dist < maxDistSqr) { // Minimum distance so far minDist = dist; minNode = nodes[nx+nz2]; // Closest point on the node if the node is treated as a square clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f)); } } } nz = z-w; nz2 = nz*width; // Side 2 on the square for (nx = x-w;nx <= x+w;nx++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; anyInside = true; if (constraint.Suitable (nodes[nx+nz2])) { float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz2]; clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f)); } } } nx = x-w; // Side 3 on the square for (nz = z-w+1;nz <= z+w-1; nz++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; anyInside = true; if (constraint.Suitable (nodes[nx+nz*width])) { float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz*width]; clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f)); } } } nx = x+w; // Side 4 on the square for (nz = z-w+1;nz <= z+w-1; nz++) { if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue; anyInside = true; if (constraint.Suitable (nodes[nx+nz*width])) { float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude; if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz*width]; clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f)); } } } // We found a suitable node if (minNode != null) { // If we don't need to search more, just return // Otherwise search for 'overlap' iterations more if (overlap == 0) { nn.node = minNode; nn.clampedPosition = clampedPosition; return nn; } overlap--; } // No nodes were inside grid bounds // We will not be able to find any more valid nodes // so just return if (!anyInside) { nn.node = minNode; nn.clampedPosition = clampedPosition; return nn; } } }
/** Returns the nearest node to a position using the specified NNConstraint. * \param position The position to try to find a close node to * \param constraint Can for example tell the function to try to return a walkable node. If you do not get a good node back, consider calling GetNearestForce. */ public NNInfo GetNearest (Vector3 position, NNConstraint constraint) { return GetNearest (position, constraint, null); }
/** Reset all values to their default values. * * \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to * override this function, resetting ALL their variables to enable recycling of paths. * If this is not done, trying to use that path type for pooling might result in weird behaviour. * The best way is to reset to default values the variables declared in the extended path type and then * call this base function in inheriting types with base.Reset (). * * \warning This function should not be called manually. */ public virtual void Reset () { #if ASTAR_POOL_DEBUG pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n"; pathTraceInfo += System.Environment.StackTrace; #endif if (System.Object.ReferenceEquals (AstarPath.active, null)) throw new System.NullReferenceException ("No AstarPath object found in the scene. " + "Make sure there is one or do not create paths in Awake"); hasBeenReset = true; state = (int)PathState.Created; releasedNotSilent = false; pathHandler = null; callback = null; _errorLog = ""; pathCompleteState = PathCompleteState.NotCalculated; path = ListPool<GraphNode>.Claim(); vectorPath = ListPool<Vector3>.Claim(); currentR = null; duration = 0; searchIterations = 0; searchedNodes = 0; //calltime nnConstraint = PathNNConstraint.Default; next = null; heuristic = AstarPath.active.heuristic; heuristicScale = AstarPath.active.heuristicScale; enabledTags = -1; tagPenalties = null; callTime = System.DateTime.UtcNow; pathID = AstarPath.active.GetNextPathID (); hTarget = Int3.zero; hTargetNode = null; }
/** * Returns the nearest node to a position using the specified \link Pathfinding.NNConstraint constraint \endlink. * \returns an NNInfo. This method will only return an empty NNInfo if there are no nodes which comply with the specified constraint. */ public virtual NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { return GetNearest (position, constraint); }
public static NNInfo GetNearest (NavMeshGraph graph, GraphNode[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { if (nodes == null || nodes.Length == 0) { Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes"); return new NNInfo (); } if (constraint == null) constraint = NNConstraint.None; Int3[] vertices = graph.vertices; //Query BBTree if (graph.bbTree == null) { /** \todo Change method to require a navgraph */ return GetNearestForce (graph, graph, position, constraint, accurateNearestNode); } //Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree float w = (graph.bbTree.Size.width + graph.bbTree.Size.height)*0.5F*0.02F; NNInfo query = graph.bbTree.QueryCircle (position,w,constraint);//graph.bbTree.Query (position,constraint); if (query.node == null) { for (int i=1;i<=8;i++) { query = graph.bbTree.QueryCircle (position, i*i*w, constraint); if (query.node != null || (i-1)*(i-1)*w > AstarPath.active.maxNearestNodeDistance*2) { // *2 for a margin break; } } } if (query.node != null) { query.clampedPosition = ClosestPointOnNode (query.node as TriangleMeshNode,vertices,position); } if (query.constrainedNode != null) { if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) { query.constrainedNode = null; } else { query.constClampedPosition = ClosestPointOnNode (query.constrainedNode as TriangleMeshNode, vertices, position); } } return query; }