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