C# Pathfinding.NNInfo类代码示例

        /// <summary>
        /// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
        /// Use this for any initialization code which can't be placed in Scan
        /// </summary>
        //public override void Awake() {
        //    base.Awake();
        //}


        // IMPROVE not really necessary. I just override this NavGraph method to add the debug line at the bottom. Otherwise its identical
        public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint) {

            if (nodes == null) {
                return new NNInfo();
            }

            float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;

            float minDist = float.PositiveInfinity;
            Node minNode = null;

            float minConstDist = float.PositiveInfinity;
            Node minConstNode = null;

            for (int i = 0; i < nodes.Length; i++) {

                Node node = nodes[i];
                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;
                }
            }

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

            #region Debugging

            if (nnInfo.constrainedNode == null) {
                float closestDistance;
                Node closestNode = __FindClosestNode(position, out closestDistance);
                D.Warn("Can't find node close enough to {0}. ClosestNode is {1} away.", position, closestDistance);
            }
            else {
                D.Log("Closest Node is at {0}, {1} from {2}.", (Vector3)nnInfo.constrainedNode.position, Mathf.Sqrt(minDist), position);
            }

            //D.Log("GetNearest() constraint.Suitable = {0}.", constraint.Suitable(nnInfo.node));
            #endregion

            return nnInfo;
        }

		public NNInfo Query (Vector3 p, NNConstraint constraint) {
			
			BBTreeBox c = root;
			
			if (c == null) {
				return new NNInfo();
			}
			
			NNInfo nnInfo = new NNInfo ();
			
			SearchBox (c,p, constraint, ref nnInfo);
			
			nnInfo.UpdateInfo ();
			
			return nnInfo;
		}

		/** 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) {
			BBTreeBox c = root;
			
			if (c == null) {
				return new NNInfo();
			}
			
#if ASTARDEBUG
			Vector3 prev = new Vector3 (1,0,0)*radius+p;
			for (double i=0;i< Math.PI*2; i += Math.PI/50.0) {
				Vector3 cpos = new Vector3 ((float)Math.Cos (i),0,(float)Math.Sin (i))*radius+p;
				Debug.DrawLine (prev,cpos,Color.yellow);
				prev = cpos;
			}
#endif
			
			NNInfo nnInfo = new NNInfo (null);
			
			SearchBoxCircle (c,p, radius, constraint, ref nnInfo);
			
			nnInfo.UpdateInfo ();
			
			return nnInfo;
		}

		//public void GenerateBounds () {
			//bounds.center = offset+new Vector3 (0,height*0.5F,0);
			//bounds.size = new Vector3 (width*scale,height,depth*scale);
		//}
		
		/** \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);
			
			NNInfo 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));
			
			//Set clamped position
			//nn.clampedPosition = new Vector3(Mathf.Clamp (xf,x-0.5f,x+0.5f),position.y,Mathf.Clamp (zf,z-0.5f,z+0.5f));
			//nn.clampedPosition = matrix.MultiplyPoint3x4 (nn.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 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) {
			//Debug.LogError ("This function (GetNearest) is not implemented in the navigation graph generator : Type "+this.GetType ().Name);
			
			var maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
			
			var minDist = float.PositiveInfinity;
			GraphNode minNode = null;
			
			var minConstDist = float.PositiveInfinity;
			GraphNode minConstNode = null;
			
			GetNodes (delegate (GraphNode node) {
				var 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;
		}

		/** This performs a linear search through all polygons returning the closest one */
		public static NNInfo GetNearestForce (Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint) {
			Int3 pos = (Int3)position;
			//Replacement for Infinity, the maximum value a int can hold
			int minDist = -1;
			Node minNode = null;
			
			float minDist2 = -1;
			Node minNode2 = null;
			
			int minConstDist = -1;
			Node minNodeConst = null;
			
			float minConstDist2 = -1;
			Node minNodeConst2 = null;
			
			//int rnd = (int)Random.Range (0,10000);
			
			//int skipped = 0;
			
			
			for (int i=0;i<nodes.Length;i++) {
				MeshNode node = nodes[i] as MeshNode;
				
				if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos))
				{
				//Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],pos) >= 0) {
					
					/*if (minDist2 != -1) {
						float d1 = (node.position-vertices[node.v1]).sqrMagnitude;
						d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
						d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
						
						//The closest distance possible from the current node to 'pos'
						d1 = (node.position-pos).sqrMagnitude-d1;
						
						if (d1 > minDist2) {
							skipped++;
							continue;
						}
					}*/
					
					/*float dist2 = Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v2].x,vertices[node.v2].z);
					dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v3].x,vertices[node.v3].z));
					dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v3].x,vertices[node.v3].z,vertices[node.v2].x,vertices[node.v2].z));*/
					
					float dist2 = (node.position-pos).sqrMagnitude;
					if (minDist2 == -1 || dist2 < minDist2) {
						minDist2 = dist2;
						minNode2 = node;
					}
					
					if (constraint.Suitable (node)) {
						if (minConstDist2 == -1 || dist2 < minConstDist2) {
							minConstDist2 = dist2;
							minNodeConst2 = node;
						}
					}
					
					continue;
				}
				
				
				int dist = Mathfx.Abs (node.position.y-pos.y);
				
				if (minDist == -1 || dist < minDist) {
					minDist = dist;
					minNode = node;
				}
				
				if (constraint.Suitable (node)) {
					if (minConstDist == -1 || dist < minConstDist) {
						minConstDist = dist;
						minNodeConst = node;
					}
				}
			}
			
			NNInfo nninfo = new NNInfo (minNode == null ? minNode2 : minNode, minNode == null ? NearestNodePriority.Low : NearestNodePriority.High);
			
			//Find the point closest to the nearest triangle
			//if (minNode == null) {
				
			if (nninfo.node != null) {
				MeshNode node = nninfo.node as MeshNode;//minNode2 as MeshNode;
				
				Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]};
				Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position);
				
				nninfo.clampedPosition = clP;
			}
			
			nninfo.constrainedNode = minNodeConst == null ? minNodeConst2 : minNodeConst;
			
			if (nninfo.constrainedNode != null) {
				MeshNode node = nninfo.constrainedNode as MeshNode;//minNode2 as MeshNode;
				
				Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]};
				Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position);
				
//.........这里部分代码省略.........

        public void SearchBoxCircle(BBTreeBox box, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo)
        {
            //, int intendentLevel = 0) {

            if (box.node != null) {
                //Leaf node
                if (NodeIntersectsCircle (box.node,p,radius)) {
                    //Update the NNInfo

            #if DEBUG
                    Debug.DrawLine (graph.vertices[box.node[0]],graph.vertices[box.node[1]],Color.red);
                    Debug.DrawLine (graph.vertices[box.node[1]],graph.vertices[box.node[2]],Color.red);
                    Debug.DrawLine (graph.vertices[box.node[2]],graph.vertices[box.node[0]],Color.red);
            #endif

                    Vector3 closest = 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.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;
                        }
                    }
                }
                return;
            }

            #if DEBUG
            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 (box.c1.rect,p,radius)) {
                SearchBoxCircle (box.c1,p, radius, constraint, ref nnInfo);
            }

            if (RectIntersectsCircle (box.c2.rect,p,radius)) {
                SearchBoxCircle (box.c2,p, radius, constraint, ref nnInfo);
            }
        }

		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
//.........这里部分代码省略.........

        /** 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(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
        {
            Int3 pos = (Int3)position;

            float minDist = -1;
            Node minNode = null;

            float minConstDist = -1;
            Node minConstNode = null;

            float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;

            if (nodes == null || nodes.Length == 0) {
                return new NNInfo ();
            }

            for (int i=0;i<nodes.Length;i++) {
                MeshNode node = nodes[i] as MeshNode;

                if (accurateNearestNode) {

                    Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],(Vector3)vertices[node.v3],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 SafeIntMath
                    if (!Polygon.IsClockwise ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],position) || !Polygon.IsClockwise ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3],position) || !Polygon.IsClockwise ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1],position))
                    #else
                    if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos))
                    #endif
                    {

                        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 {

            #if ASTARDEBUG
                        Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.blue);
                        Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3],Color.blue);
                        Debug.DrawLine ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1],Color.blue);
            #endif

                        int dist = Mathfx.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;
                            }
                        }
                    }
                }
            }

            NNInfo nninfo = new NNInfo (minNode);

            //Find the point closest to the nearest triangle

            if (nninfo.node != null) {
                MeshNode node = nninfo.node as MeshNode;//minNode2 as MeshNode;

                Vector3 clP = Polygon.ClosestPointOnTriangle ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],(Vector3)vertices[node.v3],position);

                nninfo.clampedPosition = clP;
            }

            nninfo.constrainedNode = minConstNode;
//.........这里部分代码省略.........

 private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo)
 {
     BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
     if (bBTreeBox.node != null)
     {
         if (BBTree.NodeIntersectsCircle(bBTreeBox.node, p, radius))
         {
             Vector3 vector = bBTreeBox.node.ClosestPointOnNode(p);
             float sqrMagnitude = (vector - p).sqrMagnitude;
             if (nnInfo.node == null)
             {
                 nnInfo.node = bBTreeBox.node;
                 nnInfo.clampedPosition = vector;
             }
             else if (sqrMagnitude < (nnInfo.clampedPosition - p).sqrMagnitude)
             {
                 nnInfo.node = bBTreeBox.node;
                 nnInfo.clampedPosition = vector;
             }
             if (constraint == null || constraint.Suitable(bBTreeBox.node))
             {
                 if (nnInfo.constrainedNode == null)
                 {
                     nnInfo.constrainedNode = bBTreeBox.node;
                     nnInfo.constClampedPosition = vector;
                 }
                 else if (sqrMagnitude < (nnInfo.constClampedPosition - p).sqrMagnitude)
                 {
                     nnInfo.constrainedNode = bBTreeBox.node;
                     nnInfo.constClampedPosition = vector;
                 }
             }
         }
         return;
     }
     if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.left].rect, p, radius))
     {
         this.SearchBoxCircle(bBTreeBox.left, p, radius, constraint, ref nnInfo);
     }
     if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.right].rect, p, radius))
     {
         this.SearchBoxCircle(bBTreeBox.right, p, radius, constraint, ref nnInfo);
     }
 }

 private void SearchBox(int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo)
 {
     BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
     if (bBTreeBox.node != null)
     {
         if (bBTreeBox.node.ContainsPoint((Int3)p))
         {
             if (nnInfo.node == null)
             {
                 nnInfo.node = bBTreeBox.node;
             }
             else if (Mathf.Abs(((Vector3)bBTreeBox.node.position).y - p.y) < Mathf.Abs(((Vector3)nnInfo.node.position).y - p.y))
             {
                 nnInfo.node = bBTreeBox.node;
             }
             if (constraint.Suitable(bBTreeBox.node))
             {
                 if (nnInfo.constrainedNode == null)
                 {
                     nnInfo.constrainedNode = bBTreeBox.node;
                 }
                 else if (Mathf.Abs((float)bBTreeBox.node.position.y - p.y) < Mathf.Abs((float)nnInfo.constrainedNode.position.y - p.y))
                 {
                     nnInfo.constrainedNode = bBTreeBox.node;
                 }
             }
         }
         return;
     }
     if (this.arr[bBTreeBox.left].Contains(p))
     {
         this.SearchBox(bBTreeBox.left, p, constraint, ref nnInfo);
     }
     if (this.arr[bBTreeBox.right].Contains(p))
     {
         this.SearchBox(bBTreeBox.right, p, constraint, ref nnInfo);
     }
 }

 public NNInfo QueryCircle(Vector3 p, float radius, NNConstraint constraint)
 {
     if (this.count == 0)
     {
         return new NNInfo(null);
     }
     NNInfo result = new NNInfo(null);
     this.SearchBoxCircle(0, p, radius, constraint, ref result);
     result.UpdateInfo();
     return result;
 }

        private static Index3D __maxAllowedSectorGridSizeToScan = new Index3D(4, 4, 4);  // limit to divisible by 2

        #region Archived

        /// <summary>
        /// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
        /// Use this for any initialization code which can't be placed in Scan
        /// </summary>
        //public override void Awake() {
        //    base.Awake();
        //}

        /// <summary>
        ///  This will be called on the same time as OnDisable on the gameObject which the AstarPath script is attached to (remember, not in the editor)
        /// Use for any cleanup code such as cleaning up static variables which otherwise might prevent resources from being collected
        /// Use by creating a function overriding this one in a graph class, but always call base.OnDestroy () in that function.
        /// </summary>
        //public override void OnDestroy() {
        //    base.OnDestroy();
        //}

        #endregion

        public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint) {
            if (nodes == null) return new NNInfo();

            float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;

            float minDist = float.PositiveInfinity;
            GraphNode minNode = null;

            float minConstDist = float.PositiveInfinity;
            GraphNode minConstNode = null;

            for (int i = 0; i < nodeCount; i++) {
                PointNode node = nodes[i];
                float dist = (position - (Vector3)node.position).sqrMagnitude;

                if (dist < minDist) {
                    minDist = dist;
                    minNode = node;
                }

                if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) {
                    minConstDist = dist;
                    minConstNode = node;
                }
            }

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

            #region Debugging

            //D.Log("Constraint: GraphMask: {0}, ConstrainArea: {1}, Area: {2}, ConstrainWalkability: {3}, \nWalkable: {4}, ConstrainTags: {5}, Tags: {6}, ConstrainDistance: {7}.",
            //    constraint.graphMask, constraint.constrainArea, constraint.area, constraint.constrainWalkability, constraint.walkable, 
            //    constraint.constrainTags, constraint.tags, constraint.constrainDistance);

            //if (minConstNode != null) {
            //    D.Log("Constraint criteria met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
            //        nnInfo.constClampedPosition, Vector3.Distance(nnInfo.constClampedPosition, position), position,
            //        constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
            //}
            //else {
            //    D.Log("Constraint criteria NOT met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
            //        nnInfo.clampedPosition, Vector3.Distance(nnInfo.clampedPosition, position), position,
            //        constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
            //}

            #endregion

            return nnInfo;
        }

        public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint) {

            if (nodes == null) return new NNInfo();

            float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;

            float minDist = float.PositiveInfinity;
            GraphNode minNode = null;

            float minConstDist = float.PositiveInfinity;
            GraphNode minConstNode = null;

            if (optimizeForSparseGraph) {
                Int3 lookupStart = WorldToLookupSpace((Int3)position);

                Int3 size = lookupStart - _minLookup;

                int mw = 0;
                mw = System.Math.Max(mw, System.Math.Abs(size.x));
                mw = System.Math.Max(mw, System.Math.Abs(size.y));
                mw = System.Math.Max(mw, System.Math.Abs(size.z));

                size = lookupStart - _maxLookup;
                mw = System.Math.Max(mw, System.Math.Abs(size.x));
                mw = System.Math.Max(mw, System.Math.Abs(size.y));
                mw = System.Math.Max(mw, System.Math.Abs(size.z));

                PointNode node;

                if (_nodeLookup.TryGetValue(lookupStart, out node)) {
                    while (node != null) {
                        float dist = (position - (Vector3)node.position).sqrMagnitude;
                        if (dist < minDist) { minDist = dist; minNode = node; }
                        if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }

                        node = node.next;
                    }
                }

                for (int w = 1; w <= mw; w++) {
                    if (w >= 20) {
                        Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\n" +
                            "If you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and " +
                            "PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). " +
                            "When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
                        break;
                    }

                    if (_lookupCellSize.y == 0) {
                        Int3 reference = lookupStart + new Int3(-w, 0, -w);

                        for (int x = 0; x <= 2 * w; x++) {
                            if (_nodeLookup.TryGetValue(reference + new Int3(x, 0, 0), out node)) {
                                while (node != null) {
                                    float dist = (position - (Vector3)node.position).sqrMagnitude;
                                    if (dist < minDist) { minDist = dist; minNode = node; }
                                    if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }

                                    node = node.next;
                                }
                            }
                            if (_nodeLookup.TryGetValue(reference + new Int3(x, 0, 2 * w), out node)) {
                                while (node != null) {
                                    float dist = (position - (Vector3)node.position).sqrMagnitude;
                                    if (dist < minDist) { minDist = dist; minNode = node; }
                                    if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }

                                    node = node.next;
                                }
                            }
                        }

                        for (int x = 1; x < 2 * w; x++) {
                            if (_nodeLookup.TryGetValue(reference + new Int3(0, 0, x), out node)) {
                                while (node != null) {
                                    float dist = (position - (Vector3)node.position).sqrMagnitude;
                                    if (dist < minDist) { minDist = dist; minNode = node; }
                                    if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }

                                    node = node.next;
                                }
                            }
                            if (_nodeLookup.TryGetValue(reference + new Int3(2 * w, 0, x), out node)) {
                                while (node != null) {
                                    float dist = (position - (Vector3)node.position).sqrMagnitude;
                                    if (dist < minDist) { minDist = dist; minNode = node; }
                                    if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }

                                    node = node.next;
                                }
                            }
                        }
                    }
                    else {
                        Int3 reference = lookupStart + new Int3(-w, -w, -w);

                        for (int x = 0; x <= 2 * w; x++) {
                            for (int y = 0; y <= 2 * w; y++) {
                                if (_nodeLookup.TryGetValue(reference + new Int3(x, y, 0), out node)) {
                                    while (node != null) {
//.........这里部分代码省略.........

		/** 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
		  * 
		  * 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.
		  * 
		  * This method will completely ignore any Y-axis differences in positions.
		  * 
		  * \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.
		  * 
		  * 
		  * \see QueryCircle
		  */
		public NNInfo QueryClosestXZ (Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) {
			BBTreeBox c = root;
			
			if (c == null) {
				return previous;
			}
			
			SearchBoxClosestXZ (c,p, ref distance, constraint, ref previous);
			
			return previous;
		}