C# NNConstraint.Suitable方法代码示例

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

        /// <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 override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
			
			if (graphNodes == null || depth*width != graphNodes.Length) {
				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);
			
			Node node = nodes[x+z*width];
			
			Node minNode = null;
			float minDist = float.PositiveInfinity;
			int overlap = getNearestForceOverlap;
			
			if (constraint.Suitable (node)) {
				minNode = node;
				minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
			}
			
			if (minNode != null) {
				if (overlap == 0) return minNode;
				else overlap--;
			}
			
			
			//int counter = 0;
			
			for (int w = 1; w < getNearestForceLimit;w++) {
				int nx = x;
				int nz = z+w;
				
				int nz2 = nz*width;
				
				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					//
					if (constraint.Suitable (nodes[nx+nz2])) {
						float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz2]; }
					}
				}
				
				nz = z-w;
				nz2 = nz*width;
				
				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					
					if (constraint.Suitable (nodes[nx+nz2])) {
						float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz2]; }
					}
				}
				
				nx = x-w;
				nz = z-w+1;
				
				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					
					if (constraint.Suitable (nodes[nx+nz*width])) {
						float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz*width]; }
					}
				}
				
				nx = x+w;
				
				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					
					if (constraint.Suitable (nodes[nx+nz*width])) {
						float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz*width]; }
					}
				}
				
				if (minNode != null) {
					if (overlap == 0) return minNode;
					else overlap--;
				}
			}
			return null;
		}

	/** Returns the nearest node to a position using the specified NNConstraint.
	 Searches through all graphs for their nearest nodes to the specified position and picks the closest one.
	 The NNConstraint can be used to specify constraints on which nodes can be chosen such as only picking walkable nodes.
	 \see Pathfinding.NNConstraint
	 */
	public NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
		
		if (graphs == null) { return new NNInfo(); }
		
		float minDist = float.PositiveInfinity;//Math.Infinity;
		NNInfo nearestNode = new NNInfo ();
		int nearestGraph = -1;
		
		for (int i=0;i<graphs.Length;i++) {
			
			NavGraph graph = graphs[i];
			
			if (graph == null) continue;
			
			//Check if this graph should be searched
			if (!constraint.SuitableGraph (i,graph)) {
				continue;
			}
			
			NNInfo nnInfo;
			if (fullGetNearestSearch) {
				nnInfo = graph.GetNearestForce (position, constraint);
			} else {
				nnInfo = graph.GetNearest (position, constraint);
			}
			
			GraphNode node = nnInfo.node;
			
			if (node == null) {
				continue;
			}
			
			float dist = ((Vector3)nnInfo.clampedPosition-position).magnitude;
			
			if (prioritizeGraphs && dist < prioritizeGraphsLimit) {
				//The node is close enough, choose this graph and discard all others
				minDist = dist;
				nearestNode = nnInfo;
				nearestGraph = i;
				break;
			} else {
				if (dist < minDist) {
					minDist = dist;
					nearestNode = nnInfo;
					nearestGraph = i;
				}
			}
		}
		
		//No matches found
		if (nearestGraph == -1) {
			return nearestNode;
		}
		
		//Check if a constrained node has already been set
		if (nearestNode.constrainedNode != null) {
			nearestNode.node = nearestNode.constrainedNode;
			nearestNode.clampedPosition = nearestNode.constClampedPosition;
		}
		
		if (!fullGetNearestSearch && nearestNode.node != null && !constraint.Suitable (nearestNode.node)) {
			
			//Otherwise, perform a check to force the graphs to check for a suitable node
			NNInfo nnInfo = graphs[nearestGraph].GetNearestForce (position, constraint);
			
			if (nnInfo.node != null) {
				nearestNode = nnInfo;
			}
		}
		
		if (!constraint.Suitable (nearestNode.node) || (constraint.constrainDistance && (nearestNode.clampedPosition - position).sqrMagnitude > maxNearestNodeDistanceSqr)) {
			return new NNInfo();
		}
		
		return nearestNode;
	}

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

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

		public void SearchBoxClosestXZ (BBTreeBox box, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) {
			
			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.ClosestPointOnNodeXZ (p);//NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
					
					// XZ distance
					float dist = (closest.x-p.x)*(closest.x-p.x)+(closest.z-p.z)*(closest.z-p.z);
					
					if (constraint == null || constraint.Suitable (box.node)) {
						if (nnInfo.constrainedNode == null) {
							nnInfo.constrainedNode = box.node;
							nnInfo.constClampedPosition = closest;
							closestDist = (float)System.Math.Sqrt (dist);
						} else if (dist < closestDist*closestDist) {
							nnInfo.constrainedNode = box.node;
							nnInfo.constClampedPosition = closest;
							closestDist = (float)System.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 (box.c1.rect,p,closestDist)) {
					SearchBoxClosestXZ (box.c1,p, ref closestDist, constraint, ref nnInfo);
				}
				
				if (RectIntersectsCircle (box.c2.rect,p,closestDist)) {
					SearchBoxClosestXZ (box.c2,p, ref closestDist, constraint, ref nnInfo);
				}
			}
		}

		public MeshNode SearchBoxInside (BBTreeBox box, Vector3 p, NNConstraint constraint) {
			
			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 (box.c1.rect.Contains (new Vector2(p.x,p.z))) {
					g = SearchBoxInside (box.c1,p, constraint);
					if (g != null) return g;
				}
				
				if (box.c2.rect.Contains (new Vector2(p.x,p.z))) {
					g = SearchBoxInside (box.c2, p, constraint);
					if (g != null) return g;
				}
			}
			
			return null;
		}

	/** Returns the nearest node to a position using the specified NNConstraint.
	  * Searches through all graphs for their nearest nodes to the specified position and picks the closest one. */
	public NNInfo GetNearest (Vector3 position, NNConstraint constraint = null, Node hint = null) {
		
		if (graphs == null) { return null; }
		
		if (constraint == null) {
			constraint = NNConstraint.None;
		}
		
		float minDist = float.PositiveInfinity;//Math.Infinity;
		NNInfo nearestNode = new NNInfo ();
		int nearestGraph = 0;
		
		for (int i=0;i<graphs.Length;i++) {
			
			NavGraph graph = graphs[i];
			
			
			NNInfo nnInfo = graph.GetNearest (position, constraint);
			
			Node node = nnInfo.node;
			
			if (node == null) {
				continue;
			}
				
			float dist = ((Vector3)nnInfo.clampedPosition-position).magnitude;
			
			if (prioritizeGraphs && dist < prioritizeGraphsLimit) {
				//The node is close enough, choose this graph and discard all others
				minDist = dist*(int)nnInfo.priority;
				nearestNode = nnInfo;
				nearestGraph = i;
				break;
			} else {
				if (dist*(int)nnInfo.priority < minDist) {
					minDist = dist*(int)nnInfo.priority;
					nearestNode = nnInfo;
					nearestGraph = i;
				}
			}
		}
		
		if (nearestNode.node != null && !constraint.Suitable (nearestNode.node)) {
			
			//Check if a constrained node has already been set
			if (nearestNode.constrainedNode != null) {
				nearestNode.node = nearestNode.constrainedNode;
				nearestNode.clampedPosition = nearestNode.constClampedPosition;
				
			} else {
				//Otherwise, perform a check to force the graphs to check for a suitable node
				NNInfo nnInfo = graphs[nearestGraph].GetNearestForce (position, constraint);
				
				if (nnInfo.node != null) {
					nearestNode = nnInfo;
				}
			}
		}
		
		return nearestNode;
	}

 public virtual NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
 {
     float maxDistSqr = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
     float minDist = float.PositiveInfinity;
     GraphNode minNode = null;
     float minConstDist = float.PositiveInfinity;
     GraphNode minConstNode = null;
     this.GetNodes(delegate(GraphNode node)
     {
         float sqrMagnitude = (position - (Vector3)node.position).sqrMagnitude;
         if (sqrMagnitude < minDist)
         {
             minDist = sqrMagnitude;
             minNode = node;
         }
         if (sqrMagnitude < minConstDist && sqrMagnitude < maxDistSqr && constraint.Suitable(node))
         {
             minConstDist = sqrMagnitude;
             minConstNode = node;
         }
         return true;
     });
     NNInfo result = new NNInfo(minNode);
     result.constrainedNode = minConstNode;
     if (minConstNode != null)
     {
         result.constClampedPosition = (Vector3)minConstNode.position;
     }
     else if (minNode != null)
     {
         result.constrainedNode = minNode;
         result.constClampedPosition = (Vector3)minNode.position;
     }
     return result;
 }

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