C# Pathfinding.NNConstraint类代码示例

        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 bool GetPositionIsSlime(Vector3 point, float toleranceRadius)
 {
     point.y = 0;
     NNConstraint slimeConstraint = new NNConstraint ();
     slimeConstraint.constrainTags = true;
     slimeConstraint.tags = ~slimeTag;
     NNInfo nn = AstarPath.active.GetNearest (point, slimeConstraint);
     if (AstarMath.SqrMagnitudeXZ (nn.clampedPosition, point) <= toleranceRadius * toleranceRadius) {
         Debug.DrawLine (nn.clampedPosition, point);
         return true;
     }
     return false;
 }

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

    protected override void Start() {
        base.Start();
        _pathStart = _transform.position;
        D.Log("Path start = {0}, target = {1}.", _pathStart, pathDestination);
        Debug.DrawLine(_pathStart, pathDestination, Color.yellow, 20F, false);
        //Path path = new Path(startPosition, targetPosition, null);    // Path is now abstract
        //Path path = PathPool<ABPath>.GetPath();   // don't know how to assign start and target points
        Path path = ABPath.Construct(_pathStart, pathDestination, null);

        // Node qualifying constraint instance that checks that nodes are walkable, and within the seeker-specified
        // max search distance. Tags and area testing are turned off, primarily because I don't yet understand them
        NNConstraint constraint = new NNConstraint();
        constraint.constrainTags = false;
        path.nnConstraint = constraint;

        _seeker.StartPath(path);

        // this simple default version uses a constraint that has tags enabled which made finding close nodes problematic
        //_seeker.StartPath(startPosition, targetPosition); 
    }

		/** 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.
	 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) {
		return GetNearest(position,constraint,null);
	}

        public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint = null)
        {
            if (nodes == null || depth*width*layerCount != nodes.Length) {
                //Debug.LogError ("NavGraph hasn't been generated yet");
                return new NNInfo ();
            }

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

            int index = width*z+x;
            float minDist = float.PositiveInfinity;
            Node minNode = null;
            for (int i=0;i<layerCount;i++) {
                Node node = nodes[index + width*depth*i];
                if (node != null) {
                    float dist =  ((Vector3)node.position - position).sqrMagnitude;
                    if (dist < minDist) {
                        minDist = dist;
                        minNode = node;
                    }
                }
            }

            return new NNInfo(minNode);
        }

		/** 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 (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 = Pathfinding.Util.ListPool<GraphNode>.Claim();
			vectorPath = Pathfinding.Util.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;
		}

		/// <summary>
		/// Returns the nearest node to a position using the specified <see cref="NNConstraint">constraint</see>.
		/// </summary>
		/// <param name="position">
		/// A <see cref="Vector3"/>
		/// </param>
		/// <param name="constraint">
		/// A <see cref="NNConstraint"/>
		/// </param>
		/// <returns>
		/// A <see cref="NNInfo"/>. This function will only return an empty NNInfo if there is no nodes which comply with the specified constraint.
		/// </returns>
		public virtual NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
			return GetNearest (position, constraint);
			//Debug.LogError ("This should not be called if not GetNearest has been overriden, and if GetNearest has been overriden, you should override this function too, always return a node which returns true when passed to constraint.Suitable (node)");
			//return new NNInfo ();
		}

		public static NNInfo GetNearest (INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint) {
				
			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;
			
			
			return GetNearestForce (nodes,graph.vertices, position, constraint);
			
		}

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

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

    private void GenerateCourse() {
        Vector3 start = _data.Position;
        string replot = _isCourseReplot ? "replotting" : "plotting";
        D.Log("{0} is {1} course to {2}. Start = {3}, Destination = {4}.", _fleet.FullName, replot, Target.FullName, start, Destination);
        //Debug.DrawLine(start, Destination, Color.yellow, 20F, false);
        //Path path = new Path(startPosition, targetPosition, null);    // Path is now abstract
        //Path path = PathPool<ABPath>.GetPath();   // don't know how to assign start and target points
        Path path = ABPath.Construct(start, Destination, null);

        // Node qualifying constraint instance that checks that nodes are walkable, and within the seeker-specified
        // max search distance. Tags and area testing are turned off, primarily because I don't yet understand them
        NNConstraint constraint = new NNConstraint();
        constraint.constrainTags = false;
        path.nnConstraint = constraint;

        _seeker.StartPath(path);
        // this simple default version uses a constraint that has tags enabled which made finding close nodes problematic
        //_seeker.StartPath(startPosition, targetPosition); 
    }