/// <summary> /// Will calculate a number of points around center which are on the graph and are separated by clearance from each other. /// The maximum distance from center to any point will be radius. /// Points will first be tried to be laid out as previousPoints and if that fails, random points will be selected. /// This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position /// this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using /// some kind of local avoidance. /// /// TODO: Write unit tests /// </summary> /// <param name="center">The point to generate points around</param> /// <param name="g">The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph. /// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.</param> /// <param name="previousPoints">The points to use for reference. Note that these should not be in world space. They are treated as relative to center. /// The new points will overwrite the existing points in the list. The result will be in world space, not relative to center.</param> /// <param name="radius">The final points will be at most this distance from center.</param> /// <param name="clearanceRadius">The points will if possible be at least this distance from each other.</param> public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius) { if (g == null) { throw new System.ArgumentNullException("g"); } var graph = g as NavGraph; if (graph == null) { throw new System.ArgumentException("g is not a NavGraph"); } NNInfoInternal nn = graph.GetNearestForce(center, NNConstraint.Default); center = nn.clampedPosition; if (nn.node == null) { // No valid point to start from return; } // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5 radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2)); clearanceRadius *= clearanceRadius; for (int i = 0; i < previousPoints.Count; i++) { Vector3 dir = previousPoints[i]; float magn = dir.magnitude; if (magn > 0) { dir /= magn; } float newMagn = radius; //magn > radius ? radius : magn; dir *= newMagn; GraphHitInfo hit; int tests = 0; while (true) { Vector3 pt = center + dir; if (g.Linecast(center, pt, nn.node, out hit)) { if (hit.point == Vector3.zero) { // Oops, linecast actually failed completely // try again unless we have tried lots of times // then we just continue anyway tests++; if (tests > 8) { previousPoints[i] = pt; break; } } else { pt = hit.point; } } bool worked = false; for (float q = 0.1f; q <= 1.0f; q += 0.05f) { Vector3 qt = Vector3.Lerp(center, pt, q); worked = true; for (int j = 0; j < i; j++) { if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) { worked = false; break; } } // Abort after 8 tests or when we have found a valid point if (worked || tests > 8) { worked = true; previousPoints[i] = qt; break; } } // Break out of nested loop if (worked) { break; } // If we could not find a valid point, reduce the clearance radius slightly to improve // the chances next time clearanceRadius *= 0.9f; // This will pick points in 2D closer to the edge of the circle with a higher probability dir = UnityEngine.Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5); dir.y = 0; tests++; } } }
private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTree.BBTreeBox bbtreeBox = this.tree[boxi]; if (bbtreeBox.IsLeaf) { TriangleMeshNode[] array = this.nodeLookup; for (int i = 0; i < 4; i++) { if (array[bbtreeBox.nodeOffset + i] == null) { return; } TriangleMeshNode triangleMeshNode = array[bbtreeBox.nodeOffset + i]; if (constraint == null || constraint.Suitable(triangleMeshNode)) { Vector3 vector = triangleMeshNode.ClosestPointOnNodeXZ(p); float num = (vector.x - p.x) * (vector.x - p.x) + (vector.z - p.z) * (vector.z - p.z); if (nnInfo.constrainedNode == null || num < closestSqrDist - 1E-06f || (num <= closestSqrDist + 1E-06f && Mathf.Abs(vector.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y))) { nnInfo.constrainedNode = triangleMeshNode; nnInfo.constClampedPosition = vector; closestSqrDist = num; } } } } else { int left = bbtreeBox.left; int right = bbtreeBox.right; float num2; float num3; this.GetOrderedChildren(ref left, ref right, out num2, out num3, p); if (num2 <= closestSqrDist) { this.SearchBoxClosestXZ(left, p, ref closestSqrDist, constraint, ref nnInfo); } if (num3 <= closestSqrDist) { this.SearchBoxClosestXZ(right, p, ref closestSqrDist, constraint, ref nnInfo); } } }
void SearchBoxClosest(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTreeBox box = tree[boxi]; if (box.IsLeaf) { var nodes = nodeLookup; for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++) { var node = nodes[box.nodeOffset + i]; Vector3 closest = node.ClosestPointOnNode(p); float dist = (closest - p).sqrMagnitude; if (dist < closestSqrDist) { DrawDebugNode(node, 0.2f, Color.red); if (constraint == null || constraint.Suitable(node)) { // Update the NNInfo nnInfo.constrainedNode = node; nnInfo.constClampedPosition = closest; closestSqrDist = dist; } } else { DrawDebugNode(node, 0.0f, Color.blue); } } } else { DrawDebugRect(box.rect); int first = box.left, second = box.right; float firstDist, secondDist; GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p); // Search children (closest box first to improve performance) if (firstDist < closestSqrDist) { SearchBoxClosest(first, p, ref closestSqrDist, constraint, ref nnInfo); } if (secondDist < closestSqrDist) { SearchBoxClosest(second, p, ref closestSqrDist, constraint, ref nnInfo); } } }
public static NNInfoInternal GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { Int3 pos = (Int3)position; float minDist = -1f; GraphNode minNode = null; float minConstDist = -1f; GraphNode minConstNode = null; float maxDistSqr = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr; GraphNodeDelegateCancelable del = delegate(GraphNode _node) { TriangleMeshNode triangleMeshNode3 = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 b = triangleMeshNode3.ClosestPointOnNode(position); float sqrMagnitude = ((Vector3)pos - b).sqrMagnitude; if (minNode == null || sqrMagnitude < minDist) { minDist = sqrMagnitude; minNode = triangleMeshNode3; } if (sqrMagnitude < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude < minConstDist)) { minConstDist = sqrMagnitude; minConstNode = triangleMeshNode3; } } else if (!triangleMeshNode3.ContainsPoint((Int3)position)) { float sqrMagnitude2 = (triangleMeshNode3.position - pos).sqrMagnitude; if (minNode == null || sqrMagnitude2 < minDist) { minDist = sqrMagnitude2; minNode = triangleMeshNode3; } if (sqrMagnitude2 < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude2 < minConstDist)) { minConstDist = sqrMagnitude2; minConstNode = triangleMeshNode3; } } else { int num = Math.Abs(triangleMeshNode3.position.y - pos.y); if (minNode == null || (float)num < minDist) { minDist = (float)num; minNode = triangleMeshNode3; } if ((float)num < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (float)num < minConstDist)) { minConstDist = (float)num; minConstNode = triangleMeshNode3; } } return(true); }; graph.GetNodes(del); NNInfoInternal result = new NNInfoInternal(minNode); if (result.node != null) { TriangleMeshNode triangleMeshNode = result.node as TriangleMeshNode; Vector3 clampedPosition = triangleMeshNode.ClosestPointOnNode(position); result.clampedPosition = clampedPosition; } result.constrainedNode = minConstNode; if (result.constrainedNode != null) { TriangleMeshNode triangleMeshNode2 = result.constrainedNode as TriangleMeshNode; Vector3 constClampedPosition = triangleMeshNode2.ClosestPointOnNode(position); result.constClampedPosition = constClampedPosition; } return(result); }
/** 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 NNInfoInternal 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 = System.Math.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 NNInfoInternal(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); }
/// <summary> /// Queries the tree for the closest node to p constrained by the NNConstraint trying to improve an existing solution. /// Note that this function will only fill in the constrained node. /// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None /// </summary> /// <param name="p">Point to search around</param> /// <param name="constraint">Optionally set to constrain which nodes to return</param> /// <param name="distance">The best distance for the 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> /// <param name="previous">This search will start from the previous NNInfo and improve it if possible. /// Even if the search fails on this call, the solution will never be worse than previous.</param> public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous) { var sqrDistance = distance * distance; var origSqrDistance = sqrDistance; if (count > 0 && SquaredRectPointDistance(tree[0].rect, p) < sqrDistance) { SearchBoxClosest(0, p, ref sqrDistance, constraint, ref previous); // Only update the distance if the squared distance changed as otherwise #distance // might change due to rounding errors even if no better solution was found if (sqrDistance < origSqrDistance) { distance = Mathf.Sqrt(sqrDistance); } } return(previous); }
private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTree.BBTreeBox bbtreeBox = this.arr[boxi]; if (bbtreeBox.node != null) { if (BBTree.NodeIntersectsCircle(bbtreeBox.node, p, radius)) { Vector3 vector = bbtreeBox.node.ClosestPointOnNode(p); float sqrMagnitude = (vector - p).sqrMagnitude; if (nnInfo.node == null) { nnInfo.node = bbtreeBox.node; nnInfo.clampedPosition = vector; } else if (sqrMagnitude < (nnInfo.clampedPosition - p).sqrMagnitude) { nnInfo.node = bbtreeBox.node; nnInfo.clampedPosition = vector; } if ((constraint == null || constraint.Suitable(bbtreeBox.node)) && (nnInfo.constrainedNode == null || sqrMagnitude < (nnInfo.constClampedPosition - p).sqrMagnitude)) { nnInfo.constrainedNode = bbtreeBox.node; nnInfo.constClampedPosition = vector; } } return; } if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, radius)) { this.SearchBoxCircle(bbtreeBox.left, p, radius, constraint, ref nnInfo); } if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, radius)) { this.SearchBoxCircle(bbtreeBox.right, p, radius, constraint, ref nnInfo); } }
public static void GetPointsAroundPoint(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius) { if (g == null) { throw new ArgumentNullException("g"); } NavGraph navGraph = g as NavGraph; if (navGraph == null) { throw new ArgumentException("g is not a NavGraph"); } NNInfoInternal nearestForce = navGraph.GetNearestForce(p, NNConstraint.Default); p = nearestForce.clampedPosition; if (nearestForce.node == null) { return; } radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count)); clearanceRadius *= clearanceRadius; for (int i = 0; i < previousPoints.Count; i++) { Vector3 vector = previousPoints[i]; float magnitude = vector.magnitude; if (magnitude > 0f) { vector /= magnitude; } float num = radius; vector *= num; bool flag = false; int num2 = 0; do { Vector3 vector2 = p + vector; GraphHitInfo graphHitInfo; if (g.Linecast(p, vector2, nearestForce.node, out graphHitInfo)) { vector2 = graphHitInfo.point; } for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f) { Vector3 vector3 = (vector2 - p) * num3 + p; flag = true; for (int j = 0; j < i; j++) { if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius) { flag = false; break; } } if (flag) { previousPoints[i] = vector3; break; } } if (!flag) { if (num2 > 8) { flag = true; } else { clearanceRadius *= 0.9f; vector = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5)); vector.y = 0f; num2++; } } }while (!flag); } }
public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous) { if (this.count == 0) { return(previous); } this.SearchBoxClosest(0, p, ref distance, constraint, ref previous); return(previous); }
private void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTree.BBTreeBox bbtreeBox = this.arr[boxi]; if (bbtreeBox.node != null) { if (BBTree.NodeIntersectsCircle(bbtreeBox.node, p, closestDist)) { Vector3 vector = bbtreeBox.node.ClosestPointOnNode(p); if (constraint == null || constraint.Suitable(bbtreeBox.node)) { float sqrMagnitude = (vector - p).sqrMagnitude; if (nnInfo.constrainedNode == null || sqrMagnitude < closestDist * closestDist) { nnInfo.constrainedNode = bbtreeBox.node; nnInfo.constClampedPosition = vector; closestDist = (float)Math.Sqrt((double)sqrMagnitude); } } } } else { if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, closestDist)) { this.SearchBoxClosest(bbtreeBox.left, p, ref closestDist, constraint, ref nnInfo); } if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, closestDist)) { this.SearchBoxClosest(bbtreeBox.right, p, ref closestDist, constraint, ref nnInfo); } } }
private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTree.BBTreeBox bbtreeBox = this.arr[boxi]; if (bbtreeBox.node != null) { Vector3 constClampedPosition = bbtreeBox.node.ClosestPointOnNodeXZ(p); if (constraint == null || constraint.Suitable(bbtreeBox.node)) { float num = (constClampedPosition.x - p.x) * (constClampedPosition.x - p.x) + (constClampedPosition.z - p.z) * (constClampedPosition.z - p.z); if (nnInfo.constrainedNode == null || num < closestDist * closestDist) { nnInfo.constrainedNode = bbtreeBox.node; nnInfo.constClampedPosition = constClampedPosition; closestDist = (float)Math.Sqrt((double)num); } } } else { if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, closestDist)) { this.SearchBoxClosestXZ(bbtreeBox.left, p, ref closestDist, constraint, ref nnInfo); } if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, closestDist)) { this.SearchBoxClosestXZ(bbtreeBox.right, p, ref closestDist, constraint, ref nnInfo); } } }
void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfoInternal nnInfo) //, int intendentLevel = 0) { { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (NodeIntersectsCircle(box.node, p, radius)) { //Update the NNInfo DrawDebugNode(box.node, 0.0f, Color.red); 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 || dist < (nnInfo.constClampedPosition - p).sqrMagnitude) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } } } else { DrawDebugNode(box.node, 0.0f, Color.blue); } return; } DrawDebugRect(box.rect); //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); } }
void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (NodeIntersectsCircle(box.node, p, closestDist)) { DrawDebugNode(box.node, 0.2f, Color.red); Vector3 closest = box.node.ClosestPointOnNode(p); if (constraint == null || constraint.Suitable(box.node)) { float dist = (closest - p).sqrMagnitude; //Update the NNInfo if (nnInfo.constrainedNode == null || dist < closestDist * closestDist) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)Math.Sqrt(dist); } } } else { DrawDebugNode(box.node, 0.0f, Color.blue); } } else { DrawDebugRect(box.rect); //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); } } }
public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous) { float num = distance * distance; float num2 = num; if (this.count > 0 && BBTree.SquaredRectPointDistance(this.tree[0].rect, p) < num) { this.SearchBoxClosest(0, p, ref num, constraint, ref previous); if (num < num2) { distance = Mathf.Sqrt(num); } } return(previous); }
// Token: 0x060024F1 RID: 9457 RVA: 0x0019C554 File Offset: 0x0019A754 public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint) { if (this.nodes == null || this.depth * this.width * this.layerCount != this.nodes.Length || this.layerCount == 0) { return(default(NNInfoInternal)); } Vector3 vector = position; position = base.transform.InverseTransform(position); float x = position.x; float z = position.z; int num = Mathf.Clamp((int)x, 0, this.width - 1); int num2 = Mathf.Clamp((int)z, 0, this.depth - 1); float num3 = float.PositiveInfinity; int num4 = 2; LevelGridNode levelGridNode = this.GetNearestNode(vector, num, num2, constraint); if (levelGridNode != null) { num3 = ((Vector3)levelGridNode.position - vector).sqrMagnitude; } if (levelGridNode != null && num4 > 0) { num4--; } float num5 = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity; float num6 = num5 * num5; int num7 = 1; for (;;) { int i = num2 + num7; if (this.nodeSize * (float)num7 > num5) { break; } int j; for (j = num - num7; j <= num + num7; j++) { if (j >= 0 && i >= 0 && j < this.width && i < this.depth) { LevelGridNode nearestNode = this.GetNearestNode(vector, j, i, constraint); if (nearestNode != null) { float sqrMagnitude = ((Vector3)nearestNode.position - vector).sqrMagnitude; if (sqrMagnitude < num3 && sqrMagnitude < num6) { num3 = sqrMagnitude; levelGridNode = nearestNode; } } } } i = num2 - num7; for (j = num - num7; j <= num + num7; j++) { if (j >= 0 && i >= 0 && j < this.width && i < this.depth) { LevelGridNode nearestNode2 = this.GetNearestNode(vector, j, i, constraint); if (nearestNode2 != null) { float sqrMagnitude2 = ((Vector3)nearestNode2.position - vector).sqrMagnitude; if (sqrMagnitude2 < num3 && sqrMagnitude2 < num6) { num3 = sqrMagnitude2; levelGridNode = nearestNode2; } } } } j = num - num7; for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++) { if (j >= 0 && i >= 0 && j < this.width && i < this.depth) { LevelGridNode nearestNode3 = this.GetNearestNode(vector, j, i, constraint); if (nearestNode3 != null) { float sqrMagnitude3 = ((Vector3)nearestNode3.position - vector).sqrMagnitude; if (sqrMagnitude3 < num3 && sqrMagnitude3 < num6) { num3 = sqrMagnitude3; levelGridNode = nearestNode3; } } } } j = num + num7; for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++) { if (j >= 0 && i >= 0 && j < this.width && i < this.depth) { LevelGridNode nearestNode4 = this.GetNearestNode(vector, j, i, constraint); if (nearestNode4 != null) { float sqrMagnitude4 = ((Vector3)nearestNode4.position - vector).sqrMagnitude; if (sqrMagnitude4 < num3 && sqrMagnitude4 < num6) { num3 = sqrMagnitude4; levelGridNode = nearestNode4; } } } } if (levelGridNode != null) { if (num4 == 0) { break; } num4--; } num7++; } NNInfoInternal result = new NNInfoInternal(levelGridNode); if (levelGridNode != null) { int xcoordinateInGrid = levelGridNode.XCoordinateInGrid; int zcoordinateInGrid = levelGridNode.ZCoordinateInGrid; result.clampedPosition = base.transform.Transform(new Vector3(Mathf.Clamp(x, (float)xcoordinateInGrid, (float)xcoordinateInGrid + 1f), base.transform.InverseTransform((Vector3)levelGridNode.position).y, Mathf.Clamp(z, (float)zcoordinateInGrid, (float)zcoordinateInGrid + 1f))); } return(result); }
private void SearchBoxClosest(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTree.BBTreeBox bbtreeBox = this.tree[boxi]; if (bbtreeBox.IsLeaf) { TriangleMeshNode[] array = this.nodeLookup; for (int i = 0; i < 4; i++) { if (array[bbtreeBox.nodeOffset + i] == null) { return; } TriangleMeshNode triangleMeshNode = array[bbtreeBox.nodeOffset + i]; Vector3 vector = triangleMeshNode.ClosestPointOnNode(p); float sqrMagnitude = (vector - p).sqrMagnitude; if (sqrMagnitude < closestSqrDist && (constraint == null || constraint.Suitable(triangleMeshNode))) { nnInfo.constrainedNode = triangleMeshNode; nnInfo.constClampedPosition = vector; closestSqrDist = sqrMagnitude; } } } else { int left = bbtreeBox.left; int right = bbtreeBox.right; float num; float num2; this.GetOrderedChildren(ref left, ref right, out num, out num2, p); if (num < closestSqrDist) { this.SearchBoxClosest(left, p, ref closestSqrDist, constraint, ref nnInfo); } if (num2 < closestSqrDist) { this.SearchBoxClosest(right, p, ref closestSqrDist, constraint, ref nnInfo); } } }
void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTreeBox box = tree[boxi]; if (box.IsLeaf) { var nodes = nodeLookup; for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++) { var node = nodes[box.nodeOffset + i]; // Update the NNInfo DrawDebugNode(node, 0.2f, Color.red); if (constraint == null || constraint.Suitable(node)) { Vector3 closest = node.ClosestPointOnNodeXZ(p); // XZ squared distance float dist = (closest.x - p.x) * (closest.x - p.x) + (closest.z - p.z) * (closest.z - p.z); // There's a theoretical case when the closest point is on the edge of a node which may cause the // closest point's xz coordinates to not line up perfectly with p's xz coordinates even though they should // (because floating point errors are annoying). So use a tiny margin to cover most of those cases. const float fuzziness = 0.000001f; if (nnInfo.constrainedNode == null || dist < closestSqrDist - fuzziness || (dist <= closestSqrDist + fuzziness && Mathf.Abs(closest.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y))) { nnInfo.constrainedNode = node; nnInfo.constClampedPosition = closest; closestSqrDist = dist; } } } } else { DrawDebugRect(box.rect); int first = box.left, second = box.right; float firstDist, secondDist; GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p); // Search children (closest box first to improve performance) if (firstDist <= closestSqrDist) { SearchBoxClosestXZ(first, p, ref closestSqrDist, constraint, ref nnInfo); } if (secondDist <= closestSqrDist) { SearchBoxClosestXZ(second, p, ref closestSqrDist, constraint, ref nnInfo); } } }
public NNInfo(NNInfoInternal internalInfo) { node = internalInfo.node; position = internalInfo.clampedPosition; }
public static NNInfoInternal 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 NNInfoInternal()); } 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; NNInfoInternal 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); }