C# Pathfinding.Int3类代码示例

 public static float DistancePointSegment(Int3 a, Int3 b, Int3 p)
 {
     float num = (float)(b.x - a.x);
     float num2 = (float)(b.z - a.z);
     float num3 = (float)(p.x - a.x);
     float num4 = (float)(p.z - a.z);
     float num5 = num * num + num2 * num2;
     float num6 = num * num3 + num2 * num4;
     if (num5 > 0f)
     {
         num6 /= num5;
     }
     if (num6 < 0f)
     {
         num6 = 0f;
     }
     else if (num6 > 1f)
     {
         num6 = 1f;
     }
     num3 = (float)a.x + num6 * num - (float)p.x;
     num4 = (float)a.z + num6 * num2 - (float)p.z;
     return num3 * num3 + num4 * num4;
 }

		public virtual void Open (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) {
			BaseOpen (nodeRunData,nodeR, targetPosition,path);
		}

		//public override Int3 Position {get { return position; } }
		
		public void SetPosition (Int3 value) {
			position = value;
		}

        public new override void Open(NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path)
        {
            BaseOpen (nodeRunData, nodeR, targetPosition, path);

            LayerGridGraph graph = gridGraphs[indices >> 24];
            int[] neighbourOffsets = graph.neighbourOffsets;
            int[] neighbourCosts = graph.neighbourCosts;
            Node[] nodes = graph.nodes;

            int index = GetIndex();//indices & 0xFFFFFF;

            for (int i=0;i<4;i++) {
                int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF;
                if (conn != LevelGridNode.NoConnection) {

                    Node node = nodes[index+neighbourOffsets[i] + graph.width*graph.depth*conn];

                    if (!path.CanTraverse (node)) {
                        continue;
                    }

                    NodeRun nodeR2 = node.GetNodeRun (nodeRunData);

                    if (nodeR2.pathID != nodeRunData.pathID) {

                        nodeR2.parent = nodeR;
                        nodeR2.pathID = nodeRunData.pathID;

                        nodeR2.cost = (uint)neighbourCosts[i];

                        node.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2);
                        node.UpdateG (nodeR2, nodeRunData);

                        nodeRunData.open.Add (nodeR2);

                    } else {
                        //If not we can test if the path from the current node to this one is a better one then the one already used
                        uint tmpCost = (uint)neighbourCosts[i];

                        if (nodeR.g+tmpCost+node.penalty
            #if !NoTagPenalty
                    + path.GetTagPenalty(node.tags)
            #endif
                                < nodeR2.g) {

                            nodeR2.cost = tmpCost;
                            nodeR2.parent = nodeR;

                            //TODO!!!!! ??
                            node.UpdateAllG (nodeR2,nodeRunData);

                            nodeRunData.open.Add (nodeR2);
                        }

                         else if (nodeR2.g+tmpCost+penalty
            #if !NoTagPenalty
                    + path.GetTagPenalty(tags)
            #endif
                                 < nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better

                            bool contains = node.ContainsConnection (this);

                            //Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node.
                            /*if (node.connections != null) {
                                for (int y=0;y<node.connections.Length;y++) {
                                    if (node.connections[y] == this) {
                                        contains = true;
                                        break;
                                    }
                                }
                            }*/

                            if (!contains) {
                                continue;
                            }

                            nodeR.parent = nodeR2;
                            nodeR.cost = tmpCost;

                            //TODO!!!!!!! ??
                            UpdateAllG (nodeR,nodeRunData);

                            nodeRunData.open.Add (nodeR);
                        }
                    }
                }
            }
        }

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

        /** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
        public virtual void Prepare()
        {
            System.DateTime startTime = System.DateTime.Now;

            //@pathStartTime = startTime;

            maxFrameTime = AstarPath.active.maxFrameTime;

            //maxAngle = NmaxAngle;
            //angleCost = NangleCost;
            //stepByStep = NstepByStep;
            //unitRadius = 0;//BETA, Not used
            NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint);

            //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
            PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint;
            if (pathNNConstraint != null)
            {
                pathNNConstraint.SetStart(startNNInfo.node);
            }

            startPoint = startNNInfo.clampedPosition;
            startIntPoint = (Int3)startPoint;
            startNode = startNNInfo.node;

            if (hasEndPoint)
            {
                NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint);
                endPoint = endNNInfo.clampedPosition;
                hTarget = (Int3)endPoint;
                endNode = endNNInfo.node;
            }

            if (startNode == null || (hasEndPoint == true && endNode == null))
            {
                LogError("Couldn't find close nodes to either the start or the end (start = " + (startNode != null) + " end = " + (endNode != null) + ")");
                duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
                return;
            }

            if (!startNode.walkable)
            {
                LogError("The node closest to the start point is not walkable");

                duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
                return;
            }

            if (hasEndPoint && !endNode.walkable)
            {
                LogError("The node closest to the start point is not walkable");

                duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
                return;
            }

            if (hasEndPoint && startNode.area != endNode.area)
            {
                LogError("There is no valid path to the target (start area: " + startNode.area + ", target area: " + endNode.area + ")");
                //Debug.DrawLine (startNode.position,endNode.position,Color.cyan);
                duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
                return;
            }

            duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
        }

		public void SetPosition (Int3 position) {
			this.position = position;
		}

		/** Reset all values to their default values.
		 * All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths.
		 * Call this base function in inheriting types with base.Reset ();
		  */
		public override void Reset () {
			base.Reset ();

			startNode = null;
			endNode = null;
			startHint = null;
			endHint = null;
			originalStartPoint = Vector3.zero;
			originalEndPoint = Vector3.zero;
			startPoint = Vector3.zero;
			endPoint = Vector3.zero;
			calculatePartial = false;
			partialBestTarget = null;
			startIntPoint = new Int3();
			hTarget = new Int3();

			endNodeCosts = null;
		}

		/** Returns if the point is inside the node in XZ space */
		public static bool ContainsPoint (MeshNode node, Vector3 pos, Int3[] vertices) {
			if (Polygon.IsClockwiseMargin ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1], pos)) {
				return true;
			}
			return false;
		}

		/** Returns the closest point of the node */
		public static Vector3 ClosestPointOnNode (MeshNode node, Int3[] vertices, Vector3 pos) {
			return Polygon.ClosesPointOnTriangle (vertices[node[0]],vertices[node[1]],vertices[node[2]],pos);
		}

		/** 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 SetPosition (Int3 p) {
			position = p;
		}

		public override bool ContainsPoint (Int3 p) {
			// Get the object holding the vertex data for this node
			// This is usually a graph or a recast graph tile
			INavmeshHolder navmeshHolder = GetNavmeshHolder(GraphIndex);

			// Get all 3 vertices for this node
			Int3 a = navmeshHolder.GetVertex(v0);
			Int3 b = navmeshHolder.GetVertex(v1);
			Int3 c = navmeshHolder.GetVertex(v2);

			if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false;

			if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false;

			if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false;

			return true;
			// Equivalent code, but the above code is faster
			//return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p);

			//return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p);
		}

        /** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
        public override void Prepare()
        {
            AstarProfiler.StartProfile ("Get Nearest");

            //Initialize the NNConstraint
            nnConstraint.tags = enabledTags;
            NNInfo startNNInfo 	= AstarPath.active.GetNearest (startPoint,nnConstraint, startHint);

            //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
            PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint;
            if (pathNNConstraint != null) {
                pathNNConstraint.SetStart (startNNInfo.node);
            }

            startPoint = startNNInfo.clampedPosition;
            startIntPoint = (Int3)startPoint;
            startNode = startNNInfo.node;

            //If it is declared that this path type has an end point
            //Some path types might want to use most of the ABPath code, but will not have an explicit end point at start
            if (hasEndPoint) {
                NNInfo endNNInfo = AstarPath.active.GetNearest (endPoint,nnConstraint, endHint);
                endPoint = endNNInfo.clampedPosition;
                hTarget = (Int3)endPoint;
                endNode = endNNInfo.node;
            }

            AstarProfiler.EndProfile ();

            if (startNode == null && (hasEndPoint && endNode == null)) {
                Error ();
                LogError ("Couldn't find close nodes to the start point or the end point");
                return;
            }
            if (startNode == null) {
                Error ();
                LogError ("Couldn't find a close node to the start point");
                return;
            }
            if (endNode == null && hasEndPoint) {
                Error ();
                LogError ("Couldn't find a close node to the end point");
                return;
            }

            if (!startNode.walkable) {
                Error ();
                LogError ("The node closest to the start point is not walkable");
                return;
            }

            if (hasEndPoint && !endNode.walkable) {
                Error ();
                LogError ("The node closest to the end point is not walkable");
                return;
            }

            if (hasEndPoint && startNode.area != endNode.area) {
                Error ();
                LogError ("There is no valid path to the target (start area: "+startNode.area+", target area: "+endNode.area+")");
                return;
            }
        }

		/** Opens the nodes connected to this node. This is a base call and can be called by node classes overriding the Open function to open all connections in the #connections array.
		 * \see #connections
		 * \see Open */
		public void BaseOpen (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) {
			
			if (connections == null) return;
			
			for (int i=0;i<connections.Length;i++) {
				Node conNode = connections[i];
				
				if (!path.CanTraverse (conNode)) {
					continue;
				}
				
				NodeRun nodeR2 = conNode.GetNodeRun (nodeRunData);
				
				if (nodeR2.pathID != nodeRunData.pathID) {
					
					nodeR2.parent = nodeR;
					nodeR2.pathID = nodeRunData.pathID;
					
					nodeR2.cost = (uint)connectionCosts[i];
					
					conNode.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2);
					conNode.UpdateG (nodeR2, nodeRunData);
					
					nodeRunData.open.Add (nodeR2);
					
					//Debug.DrawLine (position,node.position,Color.cyan);
					//Debug.Log ("Opening	Node "+node.position.ToString ()+" "+g+" "+node.cost+" "+node.g+" "+node.f);
				} else {
					//If not we can test if the path from the current node to this one is a better one then the one already used
					uint tmpCost = (uint)connectionCosts[i];
					
					if (nodeR.g+tmpCost+conNode.penalty
#if !ASTAR_NoTagPenalty
				+ path.GetTagPenalty(conNode.tags)
#endif
					    	< nodeR2.g) {
						
						nodeR2.cost = tmpCost;
						nodeR2.parent = nodeR;
						
						conNode.UpdateAllG (nodeR2,nodeRunData);
						
						nodeRunData.open.Add (nodeR2);
					}
					
					 else if (nodeR2.g+tmpCost+penalty
#if !ASTAR_NoTagPenalty
				+ path.GetTagPenalty(tags)
#endif
					         < nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better
						
						bool contains = conNode.ContainsConnection (this);
						
						//Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node.
						/*if (node.connections != null) {
							for (int y=0;y<node.connections.Length;y++) {
								if (node.connections[y] == this) {
									contains = true;
									break;
								}
							}
						}*/
						
						if (!contains) {
							continue;
						}
						
						nodeR.parent = nodeR2;
						nodeR.cost = tmpCost;
						
						UpdateAllG (nodeR,nodeRunData);
						
						nodeRunData.open.Add (nodeR);
					}
				}
			}
		}