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(default(NNInfo)); } if (constraint == null) { constraint = NNConstraint.None; } Int3[] vertices = graph.vertices; if (graph.bbTree == null) { return(NavMeshGraph.GetNearestForce(graph, graph, position, constraint, accurateNearestNode)); } float num = (graph.bbTree.Size.width + graph.bbTree.Size.height) * 0.5f * 0.02f; NNInfo result = graph.bbTree.QueryCircle(position, num, constraint); if (result.node == null) { for (int i = 1; i <= 8; i++) { result = graph.bbTree.QueryCircle(position, (float)(i * i) * num, constraint); if (result.node != null || (float)((i - 1) * (i - 1)) * num > AstarPath.active.maxNearestNodeDistance * 2f) { break; } } } if (result.node != null) { result.clampedPosition = NavMeshGraph.ClosestPointOnNode(result.node as TriangleMeshNode, vertices, position); } if (result.constrainedNode != null) { if (constraint.constrainDistance && ((Vector3)result.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) { result.constrainedNode = null; } else { result.constClampedPosition = NavMeshGraph.ClosestPointOnNode(result.constrainedNode as TriangleMeshNode, vertices, position); } } return(result); }
public void SearchBoxCircle(BBTreeBox box, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo) //, int intendentLevel = 0) { { if (box.node != null) { //Leaf node if (NodeIntersectsCircle(box.node, p, radius)) { //Update the NNInfo #if DEBUG Debug.DrawLine(graph.vertices[box.node[0]], graph.vertices[box.node[1]], Color.red); Debug.DrawLine(graph.vertices[box.node[1]], graph.vertices[box.node[2]], Color.red); Debug.DrawLine(graph.vertices[box.node[2]], graph.vertices[box.node[0]], Color.red); #endif Vector3 closest = NavMeshGraph.ClosestPointOnNode(box.node, graph.vertices, p); float dist = (closest - p).sqrMagnitude; if (nnInfo.node == null) { nnInfo.node = box.node; nnInfo.clampedPosition = closest; } else if (dist < (nnInfo.clampedPosition - p).sqrMagnitude) { nnInfo.node = box.node; nnInfo.clampedPosition = closest; } if (constraint.Suitable(box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } else if (dist < (nnInfo.constClampedPosition - p).sqrMagnitude) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } } } return; } #if DEBUG Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMin), Color.white); Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMax), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white); Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMin, 0, box.rect.yMax), Color.white); Debug.DrawLine(new Vector3(box.rect.xMax, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white); #endif //Search children if (RectIntersectsCircle(box.c1.rect, p, radius)) { SearchBoxCircle(box.c1, p, radius, constraint, ref nnInfo); } if (RectIntersectsCircle(box.c2.rect, p, radius)) { SearchBoxCircle(box.c2, p, radius, constraint, ref nnInfo); } }