Ejemplo n.º 1
0
		/** Queries the tree for the best node, searching within a circle around \a p with the specified radius.
		  * Will fill in both the constrained node and the not constrained node in the NNInfo.
		  *
		  * \see QueryClosest
		  */
		public NNInfo QueryCircle (Vector3 p, float radius, NNConstraint constraint) {

			if ( count == 0 ) return new NNInfo(null);

			var nnInfo = new NNInfo (null);

			SearchBoxCircle (0,p, radius, constraint, ref nnInfo);

			nnInfo.UpdateInfo ();

			return nnInfo;
		}
Ejemplo n.º 2
0
		public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
			return GetNearest (this, nodes,position, constraint, accurateNearestNode);
		}
Ejemplo n.º 3
0
		public NNInfo Query (Vector3 p, NNConstraint constraint) {

			if ( count == 0 ) return new NNInfo(null);

			var nnInfo = new NNInfo ();

			SearchBox (0,p, constraint, ref nnInfo);

			nnInfo.UpdateInfo ();

			return nnInfo;
		}
Ejemplo n.º 4
0
		void SearchBox (int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) {//, int intendentLevel = 0) {

			BBTreeBox box = arr[boxi];

			if (box.node != null) {
				//Leaf node
				if (box.node.ContainsPoint ((Int3)p)) {
					//Update the NNInfo

					if (nnInfo.node == null) {
						nnInfo.node = box.node;
					} else if (Mathf.Abs(((Vector3)box.node.position).y - p.y) < Mathf.Abs (((Vector3)nnInfo.node.position).y - p.y)) {
						nnInfo.node = box.node;
					}
					if (constraint.Suitable (box.node)) {
						if (nnInfo.constrainedNode == null) {
							nnInfo.constrainedNode = box.node;
						} else if (Mathf.Abs(box.node.position.y - p.y) < Mathf.Abs (nnInfo.constrainedNode.position.y - p.y)) {
							nnInfo.constrainedNode = box.node;
						}
					}
				}
				return;
			}

			//Search children
			if (arr[box.left].Contains(p)) {
				SearchBox (box.left,p, constraint, ref nnInfo);
			}

			if (arr[box.right].Contains(p)) {
				SearchBox (box.right,p, constraint, ref nnInfo);
			}
		}
Ejemplo n.º 5
0
		/** 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;
		}
Ejemplo n.º 6
0
		void SearchBoxClosest (int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) {

			BBTreeBox box = arr[boxi];

			if (box.node != null) {
				//Leaf node
				if (NodeIntersectsCircle (box.node,p,closestDist)) {
					//Update the NNInfo
					#if ASTARDEBUG
					Debug.DrawLine ((Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,Color.red);
					Debug.DrawLine ((Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,Color.red);
					Debug.DrawLine ((Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,Color.red);
					#endif

					Vector3 closest = box.node.ClosestPointOnNode (p);

					if (constraint == null || constraint.Suitable (box.node)) {
						float dist = (closest-p).sqrMagnitude;

						if (nnInfo.constrainedNode == null) {
							nnInfo.constrainedNode = box.node;
							nnInfo.constClampedPosition = closest;
							closestDist = (float)Math.Sqrt (dist);
						} else if (dist < closestDist*closestDist) {
							nnInfo.constrainedNode = box.node;
							nnInfo.constClampedPosition = closest;
							closestDist = (float)Math.Sqrt (dist);
						}
					}
				} else {
					#if ASTARDEBUG
					Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.blue);
					Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.blue);
					Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.blue);
					#endif
				}
			} else {

				#if ASTARDEBUG
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMin),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMax),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMin,0,box.rect.yMax),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMax,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white);
				#endif

				//Search children
				if (RectIntersectsCircle (arr[box.left].rect,p,closestDist)) {
					SearchBoxClosest (box.left,p, ref closestDist, constraint, ref nnInfo);
				}

				if (RectIntersectsCircle (arr[box.right].rect,p,closestDist)) {
					SearchBoxClosest (box.right,p, ref closestDist, constraint, ref nnInfo);
				}
			}
		}
Ejemplo n.º 7
0
		MeshNode SearchBoxInside (int boxi, Vector3 p, NNConstraint constraint) {

			BBTreeBox box = arr[boxi];

			if (box.node != null) {
				if (box.node.ContainsPoint ((Int3)p)) {
					//Update the NNInfo

					#if ASTARDEBUG
					Debug.DrawLine ((Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,Color.red);
					Debug.DrawLine ((Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(1) + Vector3.up*0.2f,Color.red);
					Debug.DrawLine ((Vector3)box.node.GetVertex(2) + Vector3.up*0.2f,(Vector3)box.node.GetVertex(0) + Vector3.up*0.2f,Color.red);
					#endif


					if (constraint == null || constraint.Suitable (box.node)) {
						return box.node;
					}
				} else {
					#if ASTARDEBUG
					Debug.DrawLine ((Vector3)box.node.GetVertex(0),(Vector3)box.node.GetVertex(1),Color.blue);
					Debug.DrawLine ((Vector3)box.node.GetVertex(1),(Vector3)box.node.GetVertex(2),Color.blue);
					Debug.DrawLine ((Vector3)box.node.GetVertex(2),(Vector3)box.node.GetVertex(0),Color.blue);
					#endif
				}
			} else {

				#if ASTARDEBUG
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMin),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMax),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMin,0,box.rect.yMin),new Vector3 (box.rect.xMin,0,box.rect.yMax),Color.white);
				Debug.DrawLine (new Vector3 (box.rect.xMax,0,box.rect.yMin),new Vector3 (box.rect.xMax,0,box.rect.yMax),Color.white);
				#endif

				//Search children
				MeshNode g;
				if (arr[box.left].Contains (p)) {
					g = SearchBoxInside (box.left,p, constraint);
					if (g != null) return g;
				}

				if (arr[box.right].Contains (p)) {
					g = SearchBoxInside (box.right, p, constraint);
					if (g != null) return g;
				}
			}

			return null;
		}
Ejemplo n.º 8
0
		public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
			//return NavMeshGraph.GetNearest (this, nodes,position, constraint, accurateNearestNode);

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

			Vector3 localPosition = position - forcedBounds.min;
			int tx = Mathf.FloorToInt (localPosition.x / (cellSize*tileSizeX));
			int tz = Mathf.FloorToInt (localPosition.z / (cellSize*tileSizeZ));

			//Clamp to graph borders
			tx = Mathf.Clamp (tx, 0, tileXCount-1);
			tz = Mathf.Clamp (tz, 0, tileZCount-1);

			int wmax = Math.Max (tileXCount, tileZCount);

			var best = new NNInfo();
			float bestDistance = float.PositiveInfinity;

			bool xzSearch = nearestSearchOnlyXZ || ( constraint != null && constraint.distanceXZ );

			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min, Vector3.up*15, Color.blue);
			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.forward*5, Vector3.back*10, Color.blue);
			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.left*5, Vector3.right*10, Color.blue);

			//Search outwards in a diamond pattern from the closest tile
			for (int w=0;w<wmax;w++) {

				if (!xzSearch && bestDistance < (w-1)*cellSize*Math.Max (tileSizeX, tileSizeZ)) break;

				int zmax = Math.Min (w+tz +1, tileZCount);
				for (int z=Math.Max(-w+tz, 0); z < zmax; z++) {

					//Solve for z such that abs(x-tx) + abs(z-tx) == w
					//Delta X coordinate
					int dx = Math.Abs( w - Math.Abs(z-tz));
					//Solution is dx + tx and -dx + tx

					//First solution negative delta x
					if (-dx + tx >= 0) {
						//Absolute x coordinate
						int x = -dx + tx;
						NavmeshTile tile = tiles[x + z*tileXCount];

						if (tile != null) {
							if (xzSearch) {
								best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best);
								if ( bestDistance < float.PositiveInfinity ) break;
							} else {
								best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best);
							}
						}
					}

					//Other solution, make sure it is not the same solution by checking x != 0
					if (dx != 0 && dx + tx < tileXCount) {
						//Absolute x coordinate
						int x = dx + tx;
						NavmeshTile tile = tiles[x + z*tileXCount];
						if (tile != null) {
							if (xzSearch) {
								best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best);
								if ( bestDistance < float.PositiveInfinity ) break;
							} else {
								best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best);
							}
						}
					}
				}
			}

			best.node = best.constrainedNode;
			best.constrainedNode = null;

			best.clampedPosition = best.constClampedPosition;

			return best;
			//return GetNearestForce (position, constraint);
		}
		public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {

			if (nodes == null || depth*width*layerCount != nodes.Length) {
				//Debug.LogError ("NavGraph hasn't been generated yet");
				return new NNInfo ();
			}

			var graphPosition = inverseMatrix.MultiplyPoint3x4 (position);

			int x = Mathf.Clamp (Mathf.RoundToInt (graphPosition.x-0.5F)  , 0, width-1);
			int z = Mathf.Clamp (Mathf.RoundToInt (graphPosition.z-0.5F)  , 0, depth-1);

			var minNode = GetNearestNode (position, x, z, null);
			return new NNInfo(minNode);
		}
Ejemplo n.º 10
0
		/** Finds the first node which contains \a position.
		 * "Contains" is defined as \a position is inside the triangle node when seen from above. So only XZ space matters.
		 * In case of a multilayered environment, which node of the possibly several nodes
		 * containing the point is undefined.
		 *
		 * Returns null if there was no node containing the point. This serves as a quick
		 * check for "is this point on the navmesh or not".
		 *
		 * Note that the behaviour of this method is distinct from the GetNearest method.
		 * The GetNearest method will return the closest node to a point,
		 * which is not necessarily the one which contains it in XZ space.
		 *
		 * \see GetNearest
		 */
		public GraphNode PointOnNavmesh (Vector3 position, NNConstraint constraint) {
			//return NavMeshGraph.GetNearest (this, nodes,position, constraint, accurateNearestNode);

			if (tiles == null) return null;

			Vector3 localPosition = position - forcedBounds.min;
			int tx = Mathf.FloorToInt (localPosition.x / (cellSize*tileSizeX));
			int tz = Mathf.FloorToInt (localPosition.z / (cellSize*tileSizeZ));

			// Graph borders
			if (tx < 0 || tz < 0 || tx >= tileXCount || tz >= tileZCount) return null;

			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min, Vector3.up*15, Color.blue);
			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.forward*5, Vector3.back*10, Color.blue);
			//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.left*5, Vector3.right*10, Color.blue);

			NavmeshTile tile = tiles[tx + tz*tileXCount];

			if (tile != null) {
				GraphNode node = tile.bbTree.QueryInside (position, constraint);
				return node;
			}

			return null;
		}
Ejemplo n.º 11
0
		public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
			NNInfo info = GetNearestForce (position, null);
			//info.node = info.constrainedNode;
			//info.constrainedNode = null;

			//info.clampedPosition = info.constClampedPosition;

			return info;
		}
Ejemplo n.º 12
0
		/** This performs a linear search through all polygons returning the closest one.
		  * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
		  * complying with the NNConstraint.
		  * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
		  */
		public static NNInfo GetNearestForceBoth (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
			var pos = (Int3)position;

			float minDist = -1;
			GraphNode minNode = null;

			float minConstDist = -1;
			GraphNode minConstNode = null;

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

			GraphNodeDelegateCancelable del = delegate (GraphNode _node) {
				var node = _node as TriangleMeshNode;

				if (accurateNearestNode) {

					Vector3 closest = node.ClosestPointOnNode (position);
					float dist = ((Vector3)pos-closest).sqrMagnitude;

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

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

				} else {

					if (!node.ContainsPoint ((Int3)position)) {

						float dist = (node.position-pos).sqrMagnitude;
						if (minNode == null || dist < minDist) {
							minDist = dist;
							minNode = node;
						}

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

					} else {
						int dist = AstarMath.Abs (node.position.y-pos.y);

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

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

			graph.GetNodes (del);

			var nninfo = new NNInfo (minNode);

			//Find the point closest to the nearest triangle

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

				Vector3 clP = node.ClosestPointOnNode (position);

				nninfo.clampedPosition = clP;
			}

			nninfo.constrainedNode = minConstNode;
			if (nninfo.constrainedNode != null) {
				var node = nninfo.constrainedNode as TriangleMeshNode;//minNode2 as MeshNode;

				Vector3 clP = node.ClosestPointOnNode (position);

				nninfo.constClampedPosition = clP;
			}

			return nninfo;
		}
Ejemplo n.º 13
0
		/** This performs a linear search through all polygons returning the closest one */
		public static NNInfo GetNearestForce (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
			NNInfo nn = GetNearestForceBoth (graph, navmesh,position,constraint,accurateNearestNode);
			nn.node = nn.constrainedNode;
			nn.clampedPosition = nn.constClampedPosition;
			return nn;
		}
Ejemplo n.º 14
0
		/** This performs a linear search through all polygons returning the closest one.
		 * This is usually only called in the Free version of the A* Pathfinding Project since the Pro one supports BBTrees and will do another query
		 */
		public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {

			return GetNearestForce (this, this,position,constraint, accurateNearestNode);
			//Debug.LogWarning ("This function shouldn't be called since constrained nodes are sent back in the GetNearest call");

			//return new NNInfo ();
		}
Ejemplo n.º 15
0
		/** Queries the tree for the closest node to \a p constrained by the NNConstraint.
		  * Note that this function will, unlike QueryCircle, only fill in the constrained node.
		  * If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
		  *
		  * \see QueryCircle
		  */
		public NNInfo QueryClosest (Vector3 p, NNConstraint constraint, out float distance) {
			distance = float.PositiveInfinity;
			return QueryClosest (p, constraint, ref distance, new NNInfo (null));
		}
		private LevelGridNode GetNearestNode (Vector3 position, int x, int z, NNConstraint constraint) {
			int index = width*z+x;
			float minDist = float.PositiveInfinity;
			LevelGridNode minNode = null;
			for (int i=0;i<layerCount;i++) {
				LevelGridNode node = nodes[index + width*depth*i];
				if (node != null) {
					float dist =  ((Vector3)node.position - position).sqrMagnitude;
					if (dist < minDist && (constraint == null || constraint.Suitable (node))) {
						minDist = dist;
						minNode = node;
					}
				}
			}
			return minNode;
		}
Ejemplo n.º 17
0
		/** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution.
		  * Note that this function will, unlike QueryCircle, only fill in the constrained node.
		  * If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
		  *
		  * \param p Point to search around
		  * \param constraint Optionally set to constrain which nodes to return
		  * \param distance The best distance for the \a previous solution. Will be updated with the best distance
		  * after this search. Will be positive infinity if no node could be found.
		  * Set to positive infinity if there was no previous solution.
		  * \param previous This search will start from the \a previous NNInfo and improve it if possible.
		  * Even if the search fails on this call, the solution will never be worse than \a previous.
		  *
		  * \see QueryCircle
		  */
		public NNInfo QueryClosest (Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) {

			if ( count == 0 ) return previous;

			SearchBoxClosest (0,p, ref distance, constraint, ref previous);

			return previous;
		}
		public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {

			if (nodes == null || depth*width*layerCount != nodes.Length || layerCount == 0) {
				return new NNInfo ();
			}

			Vector3 globalPosition = position;

			position = inverseMatrix.MultiplyPoint3x4 (position);

			int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F), 0, width-1);
			int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F), 0, depth-1);

			LevelGridNode minNode;
			float minDist = float.PositiveInfinity;
			int overlap = getNearestForceOverlap;

			minNode = GetNearestNode (globalPosition,x,z,constraint);
			if (minNode != null) {
				minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
			}

			if (minNode != null) {
				if (overlap == 0) return new NNInfo(minNode);
				overlap--;
			}


			float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
			float maxDistSqr = maxDist*maxDist;

			for (int w = 1;;w++) {
				int nx;
				int nz = z+w;

				// Check if the nodes are within distance limit
				if (nodeSize*w > maxDist) {
					return new NNInfo(minNode);
				}

				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint);
					if (node != null) {
						float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
					}
				}

				nz = z-w;

				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint);
					if (node != null) {
						float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
					}
				}

				nx = x-w;

				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint);
					if (node != null) {
						float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
					}
				}

				nx = x+w;

				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					LevelGridNode node = GetNearestNode (globalPosition, nx,nz, constraint);
					if (node != null) {
						float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
						//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
						if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
					}
				}

				if (minNode != null) {
					if (overlap == 0) return new NNInfo(minNode);
					overlap--;
				}
			}
		}
Ejemplo n.º 19
0
		public MeshNode QueryInside (Vector3 p, NNConstraint constraint) {
			return count != 0 ? SearchBoxInside (0,p, constraint) : null;
		}
Ejemplo n.º 20
0
		/** \todo Set clamped position for Grid Graph */
		public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {

			if (nodes == null || depth*width != nodes.Length) {
				return new NNInfo ();
			}

			position = inverseMatrix.MultiplyPoint3x4 (position);

			float xf = position.x-0.5F;
			float zf = position.z-0.5f;
			int x = Mathf.Clamp (Mathf.RoundToInt (xf)  , 0, width-1);
			int z = Mathf.Clamp (Mathf.RoundToInt (zf)  , 0, depth-1);

			var nn = new NNInfo(nodes[z*width+x]);

			float y = inverseMatrix.MultiplyPoint3x4((Vector3)nodes[z*width+x].position).y;
			nn.clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f,y,Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f));

			return nn;
		}
Ejemplo n.º 21
0
		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);
			}
		}
Ejemplo n.º 22
0
		/** \todo Set clamped position for Grid Graph */
		public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {

			if (nodes == null || depth*width != nodes.Length) {
				return new NNInfo ();
			}

			// Position in global space
			Vector3 globalPosition = position;

			// Position in graph space
			position = inverseMatrix.MultiplyPoint3x4 (position);

			// Find the coordinates of the closest node
			float xf = position.x-0.5F;
			float zf = position.z-0.5f;
			int x = Mathf.Clamp (Mathf.RoundToInt (xf)  , 0, width-1);
			int z = Mathf.Clamp (Mathf.RoundToInt (zf)  , 0, depth-1);

			// Closest node
			GridNode node = nodes[x+z*width];

			GridNode minNode = null;
			float minDist = float.PositiveInfinity;
			int overlap = getNearestForceOverlap;

			Vector3 clampedPosition = Vector3.zero;
			var nn = new NNInfo(null);

			// If the closest node was suitable
			if (constraint.Suitable (node)) {
				minNode = node;
				minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
				float y = inverseMatrix.MultiplyPoint3x4((Vector3)node.position).y;
				clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f, y, Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f));
			}

			if (minNode != null) {
				nn.node = minNode;
				nn.clampedPosition = clampedPosition;

				// We have a node, and we don't need to search more, so just return
				if (overlap == 0) return nn;
				overlap--;
			}

			// Search up to this distance
			float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
			float maxDistSqr = maxDist*maxDist;

			// Search a square/spiral pattern around the point
			for (int w = 1;;w++) {

				//Check if the nodes are within distance limit
				if (nodeSize*w > maxDist) {
					nn.node = minNode;
					nn.clampedPosition = clampedPosition;
					return nn;
				}

				bool anyInside = false;

				int nx;
				int nz = z+w;
				int nz2 = nz*width;

				// Side 1 on the square
				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					anyInside = true;
					if (constraint.Suitable (nodes[nx+nz2])) {
						float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
						if (dist < minDist && dist < maxDistSqr) {
							// Minimum distance so far
							minDist = dist;
							minNode = nodes[nx+nz2];

							// Closest point on the node if the node is treated as a square
							clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
						}
					}
				}

				nz = z-w;
				nz2 = nz*width;

				// Side 2 on the square
				for (nx = x-w;nx <= x+w;nx++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					anyInside = true;
					if (constraint.Suitable (nodes[nx+nz2])) {
						float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
						if (dist < minDist && dist < maxDistSqr) {
							minDist = dist;
							minNode = nodes[nx+nz2];
							clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
						}
					}
				}

				nx = x-w;

				// Side 3 on the square
				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					anyInside = true;
					if (constraint.Suitable (nodes[nx+nz*width])) {
						float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
						if (dist < minDist && dist < maxDistSqr) {
							minDist = dist;
							minNode = nodes[nx+nz*width];
							clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
						}
					}
				}

				nx = x+w;

				// Side 4 on the square
				for (nz = z-w+1;nz <= z+w-1; nz++) {
					if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
					anyInside = true;
					if (constraint.Suitable (nodes[nx+nz*width])) {
						float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
						if (dist < minDist && dist < maxDistSqr) {
							minDist = dist;
							minNode = nodes[nx+nz*width];
							clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
						}
					}
				}

				// We found a suitable node
				if (minNode != null) {
					// If we don't need to search more, just return
					// Otherwise search for 'overlap' iterations more
					if (overlap == 0) {
						nn.node = minNode;
						nn.clampedPosition = clampedPosition;
						return nn;
					}
					overlap--;
				}

				// No nodes were inside grid bounds
				// We will not be able to find any more valid nodes
				// so just return
				if (!anyInside) {
					nn.node = minNode;
					nn.clampedPosition = clampedPosition;
					return nn;
				}
			}
		}
Ejemplo n.º 23
0
		/** Returns the nearest node to a position using the specified NNConstraint.
		  * \param position The position to try to find a close node to
		  * \param constraint Can for example tell the function to try to return a walkable node. If you do not get a good node back, consider calling GetNearestForce. */
		public NNInfo GetNearest (Vector3 position, NNConstraint constraint) {
			return GetNearest (position, constraint, null);
		}
Ejemplo n.º 24
0
		/** Reset all values to their default values.
		 *
		 * \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
		 * override this function, resetting ALL their variables to enable recycling of paths.
		 * If this is not done, trying to use that path type for pooling might result in weird behaviour.
		 * The best way is to reset to default values the variables declared in the extended path type and then
		 * call this base function in inheriting types with base.Reset ().
		 *
		 * \warning This function should not be called manually.
		  */
		public virtual void Reset () {
#if ASTAR_POOL_DEBUG
			pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n";
			pathTraceInfo += System.Environment.StackTrace;
#endif

			if (System.Object.ReferenceEquals (AstarPath.active, null))
				throw new System.NullReferenceException ("No AstarPath object found in the scene. " +
					"Make sure there is one or do not create paths in Awake");

			hasBeenReset = true;
			state = (int)PathState.Created;
			releasedNotSilent = false;

			pathHandler = null;
			callback = null;
			_errorLog = "";
			pathCompleteState = PathCompleteState.NotCalculated;

			path = ListPool<GraphNode>.Claim();
			vectorPath = ListPool<Vector3>.Claim();

			currentR = null;

			duration = 0;
			searchIterations = 0;
			searchedNodes = 0;
			//calltime

			nnConstraint = PathNNConstraint.Default;
			next = null;

			heuristic = AstarPath.active.heuristic;
			heuristicScale = AstarPath.active.heuristicScale;

			enabledTags = -1;
			tagPenalties = null;

			callTime = System.DateTime.UtcNow;
			pathID = AstarPath.active.GetNextPathID ();

			hTarget = Int3.zero;
			hTargetNode = null;
		}
Ejemplo n.º 25
0
		/**
		 * 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);
		}
Ejemplo n.º 26
0
		public static NNInfo GetNearest (NavMeshGraph graph, GraphNode[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
			if (nodes == null || nodes.Length == 0) {
				Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
				return new NNInfo ();
			}

			if (constraint == null) constraint = NNConstraint.None;


			Int3[] vertices = graph.vertices;

			//Query BBTree

			if (graph.bbTree == null) {
				/** \todo Change method to require a navgraph */
				return GetNearestForce (graph, graph, position, constraint, accurateNearestNode);
			}

			//Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
			float w = (graph.bbTree.Size.width + graph.bbTree.Size.height)*0.5F*0.02F;

			NNInfo query = graph.bbTree.QueryCircle (position,w,constraint);//graph.bbTree.Query (position,constraint);

			if (query.node == null) {

				for (int i=1;i<=8;i++) {
					query = graph.bbTree.QueryCircle (position, i*i*w, constraint);

					if (query.node != null || (i-1)*(i-1)*w > AstarPath.active.maxNearestNodeDistance*2) { // *2 for a margin
						break;
					}
				}
			}

			if (query.node != null) {
				query.clampedPosition = ClosestPointOnNode (query.node as TriangleMeshNode,vertices,position);
			}

			if (query.constrainedNode != null) {
				if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) {
					query.constrainedNode = null;
				} else {
					query.constClampedPosition = ClosestPointOnNode (query.constrainedNode as TriangleMeshNode, vertices, position);
				}
			}

			return query;
		}