private static Index3D __maxAllowedSectorGridSizeToScan = new Index3D(4, 4, 4); // limit to divisible by 2
#region Archived
/// <summary>
/// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
/// Use this for any initialization code which can't be placed in Scan
/// </summary>
//public override void Awake() {
// base.Awake();
//}
/// <summary>
/// This will be called on the same time as OnDisable on the gameObject which the AstarPath script is attached to (remember, not in the editor)
/// Use for any cleanup code such as cleaning up static variables which otherwise might prevent resources from being collected
/// Use by creating a function overriding this one in a graph class, but always call base.OnDestroy () in that function.
/// </summary>
//public override void OnDestroy() {
// base.OnDestroy();
//}
#endregion
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint) {
if (nodes == null) return new NNInfo();
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
for (int i = 0; i < nodeCount; i++) {
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) {
minConstDist = dist;
minConstNode = node;
}
}
NNInfo 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;
}
#region Debugging
//D.Log("Constraint: GraphMask: {0}, ConstrainArea: {1}, Area: {2}, ConstrainWalkability: {3}, \nWalkable: {4}, ConstrainTags: {5}, Tags: {6}, ConstrainDistance: {7}.",
// constraint.graphMask, constraint.constrainArea, constraint.area, constraint.constrainWalkability, constraint.walkable,
// constraint.constrainTags, constraint.tags, constraint.constrainDistance);
//if (minConstNode != null) {
// D.Log("Constraint criteria met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.constClampedPosition, Vector3.Distance(nnInfo.constClampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
//else {
// D.Log("Constraint criteria NOT met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.clampedPosition, Vector3.Distance(nnInfo.clampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
#endregion
return nnInfo;
}
/// <summary>
/// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
/// Use this for any initialization code which can't be placed in Scan
/// </summary>
//public override void Awake() {
// base.Awake();
//}
// IMPROVE not really necessary. I just override this NavGraph method to add the debug line at the bottom. Otherwise its identical
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint) {
if (nodes == null) {
return new NNInfo();
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
Node minNode = null;
float minConstDist = float.PositiveInfinity;
Node minConstNode = null;
for (int i = 0; i < nodes.Length; i++) {
Node node = nodes[i];
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;
}
}
NNInfo 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;
}
#region Debugging
if (nnInfo.constrainedNode == null) {
float closestDistance;
Node closestNode = __FindClosestNode(position, out closestDistance);
D.Warn("Can't find node close enough to {0}. ClosestNode is {1} away.", position, closestDistance);
}
else {
D.Log("Closest Node is at {0}, {1} from {2}.", (Vector3)nnInfo.constrainedNode.position, Mathf.Sqrt(minDist), position);
}
//D.Log("GetNearest() constraint.Suitable = {0}.", constraint.Suitable(nnInfo.node));
#endregion
return nnInfo;
}
/** 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 NNInfoInternal 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 == null || 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 == null || constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
});
var nnInfo = new NNInfoInternal(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);
}
public void Search(Int3 p)
{
PointNode node;
if (nodeLookup.TryGetValue(p, out node))
{
while (node != null)
{
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist; minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)))
{
minConstDist = dist; minConstNode = node;
}
node = node.next;
}
}
}
/** Returns the nearest node to a position using the specified NNConstraint.
Searches through all graphs for their nearest nodes to the specified position and picks the closest one.
The NNConstraint can be used to specify constraints on which nodes can be chosen such as only picking walkable nodes.
\see Pathfinding.NNConstraint
*/
public NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
if (graphs == null) { return new NNInfo(); }
float minDist = float.PositiveInfinity;//Math.Infinity;
NNInfo nearestNode = new NNInfo ();
int nearestGraph = -1;
for (int i=0;i<graphs.Length;i++) {
NavGraph graph = graphs[i];
if (graph == null) continue;
//Check if this graph should be searched
if (!constraint.SuitableGraph (i,graph)) {
continue;
}
NNInfo nnInfo;
if (fullGetNearestSearch) {
nnInfo = graph.GetNearestForce (position, constraint);
} else {
nnInfo = graph.GetNearest (position, constraint);
}
GraphNode node = nnInfo.node;
if (node == null) {
continue;
}
float dist = ((Vector3)nnInfo.clampedPosition-position).magnitude;
if (prioritizeGraphs && dist < prioritizeGraphsLimit) {
//The node is close enough, choose this graph and discard all others
minDist = dist;
nearestNode = nnInfo;
nearestGraph = i;
break;
} else {
if (dist < minDist) {
minDist = dist;
nearestNode = nnInfo;
nearestGraph = i;
}
}
}
//No matches found
if (nearestGraph == -1) {
return nearestNode;
}
//Check if a constrained node has already been set
if (nearestNode.constrainedNode != null) {
nearestNode.node = nearestNode.constrainedNode;
nearestNode.clampedPosition = nearestNode.constClampedPosition;
}
if (!fullGetNearestSearch && nearestNode.node != null && !constraint.Suitable (nearestNode.node)) {
//Otherwise, perform a check to force the graphs to check for a suitable node
NNInfo nnInfo = graphs[nearestGraph].GetNearestForce (position, constraint);
if (nnInfo.node != null) {
nearestNode = nnInfo;
}
}
if (!constraint.Suitable (nearestNode.node) || (constraint.constrainDistance && (nearestNode.clampedPosition - position).sqrMagnitude > maxNearestNodeDistanceSqr)) {
return new NNInfo();
}
return nearestNode;
}
/** This performs a linear search through all polygons returning the closest one */
public static NNInfo GetNearestForce (Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint) {
Int3 pos = (Int3)position;
//Replacement for Infinity, the maximum value a int can hold
int minDist = -1;
Node minNode = null;
float minDist2 = -1;
Node minNode2 = null;
int minConstDist = -1;
Node minNodeConst = null;
float minConstDist2 = -1;
Node minNodeConst2 = null;
//int rnd = (int)Random.Range (0,10000);
//int skipped = 0;
for (int i=0;i<nodes.Length;i++) {
MeshNode node = nodes[i] as MeshNode;
if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos))
{
//Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],pos) >= 0) {
/*if (minDist2 != -1) {
float d1 = (node.position-vertices[node.v1]).sqrMagnitude;
d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
//The closest distance possible from the current node to 'pos'
d1 = (node.position-pos).sqrMagnitude-d1;
if (d1 > minDist2) {
skipped++;
continue;
}
}*/
/*float dist2 = Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v2].x,vertices[node.v2].z);
dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v3].x,vertices[node.v3].z));
dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v3].x,vertices[node.v3].z,vertices[node.v2].x,vertices[node.v2].z));*/
float dist2 = (node.position-pos).sqrMagnitude;
if (minDist2 == -1 || dist2 < minDist2) {
minDist2 = dist2;
minNode2 = node;
}
if (constraint.Suitable (node)) {
if (minConstDist2 == -1 || dist2 < minConstDist2) {
minConstDist2 = dist2;
minNodeConst2 = node;
}
}
continue;
}
int dist = Mathfx.Abs (node.position.y-pos.y);
if (minDist == -1 || dist < minDist) {
minDist = dist;
minNode = node;
}
if (constraint.Suitable (node)) {
if (minConstDist == -1 || dist < minConstDist) {
minConstDist = dist;
minNodeConst = node;
}
}
}
NNInfo nninfo = new NNInfo (minNode == null ? minNode2 : minNode, minNode == null ? NearestNodePriority.Low : NearestNodePriority.High);
//Find the point closest to the nearest triangle
//if (minNode == null) {
if (nninfo.node != null) {
MeshNode node = nninfo.node as MeshNode;//minNode2 as MeshNode;
Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]};
Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minNodeConst == null ? minNodeConst2 : minNodeConst;
if (nninfo.constrainedNode != null) {
MeshNode node = nninfo.constrainedNode as MeshNode;//minNode2 as MeshNode;
Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]};
Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position);
nninfo.constClampedPosition = clP;
}
return nninfo;
}
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;
}
}
}
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);
}
}
private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo 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)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = constClampedPosition;
closestDist = (float)Math.Sqrt((double)num);
}
else if (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);
}
}
}
public static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
VInt3 pos = (VInt3)position;
double minDist = -1.0;
GraphNode minNode = null;
double minConstDist = -1.0;
GraphNode minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
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 || (double)sqrMagnitude < minDist)
{
minDist = (double)sqrMagnitude;
minNode = triangleMeshNode3;
}
if (sqrMagnitude < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (double)sqrMagnitude < minConstDist))
{
minConstDist = (double)sqrMagnitude;
minConstNode = triangleMeshNode3;
}
}
else if (!triangleMeshNode3.ContainsPoint((VInt3)position))
{
double sqrMagnitude2 = (triangleMeshNode3.position - pos).sqrMagnitude;
if (minNode == null || sqrMagnitude2 < minDist)
{
minDist = sqrMagnitude2;
minNode = triangleMeshNode3;
}
if (sqrMagnitude2 < (double)maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude2 < minConstDist))
{
minConstDist = sqrMagnitude2;
minConstNode = triangleMeshNode3;
}
}
else
{
int num = AstarMath.Abs(triangleMeshNode3.position.y - pos.y);
if (minNode == null || (double)num < minDist)
{
minDist = (double)num;
minNode = triangleMeshNode3;
}
if ((float)num < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (double)num < minConstDist))
{
minConstDist = (double)num;
minConstNode = triangleMeshNode3;
}
}
return(true);
};
graph.GetNodes(del);
NNInfo result = new NNInfo(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);
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (nodes == null)
{
return(new NNInfo());
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
}
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);
}
public void SearchBoxClosestXZ(BBTreeBox box, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo)
{
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.ClosestPointOnNodeXZ(p); //NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
// XZ distance
float dist = (closest.x - p.x) * (closest.x - p.x) + (closest.z - p.z) * (closest.z - p.z);
if (constraint == null || constraint.Suitable(box.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (float)System.Math.Sqrt(dist);
}
else if (dist < closestDist * closestDist)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (float)System.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(box.c1.rect, p, closestDist))
{
SearchBoxClosestXZ(box.c1, p, ref closestDist, constraint, ref nnInfo);
}
if (RectIntersectsCircle(box.c2.rect, p, closestDist))
{
SearchBoxClosestXZ(box.c2, p, ref closestDist, constraint, ref nnInfo);
}
}
}
NNInfoInternal GetNearestInternal(Vector3 position, NNConstraint constraint, bool fastCheck)
{
if (nodes == null)
{
return(new NNInfoInternal());
}
var iposition = (Int3)position;
if (optimizeForSparseGraph)
{
if (nearestNodeDistanceMode == NodeDistanceMode.Node)
{
return(new NNInfoInternal(lookupTree.GetNearest(iposition, fastCheck ? null : constraint)));
}
else
{
var closestNode = lookupTree.GetNearestConnection(iposition, fastCheck ? null : constraint, maximumConnectionLength);
if (closestNode == null)
{
return(new NNInfoInternal());
}
return(FindClosestConnectionPoint(closestNode as PointNode, position));
}
}
float maxDistSqr = constraint == null || constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
maxDistSqr *= Int3.FloatPrecision * Int3.FloatPrecision;
var nnInfo = new NNInfoInternal(null);
long minDist = long.MaxValue;
long minConstDist = long.MaxValue;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
long dist = (iposition - node.position).sqrMagnitudeLong;
if (dist < minDist)
{
minDist = dist;
nnInfo.node = node;
}
if (dist < minConstDist && (float)dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = dist;
nnInfo.constrainedNode = node;
}
}
if (!fastCheck)
{
nnInfo.node = nnInfo.constrainedNode;
}
nnInfo.UpdateInfo();
return(nnInfo);
}
/** Returns the nearest node to a position using the specified NNConstraint.
* Searches through all graphs for their nearest nodes to the specified position and picks the closest one. */
public NNInfo GetNearest (Vector3 position, NNConstraint constraint = null, Node hint = null) {
if (graphs == null) { return null; }
if (constraint == null) {
constraint = NNConstraint.None;
}
float minDist = float.PositiveInfinity;//Math.Infinity;
NNInfo nearestNode = new NNInfo ();
int nearestGraph = 0;
for (int i=0;i<graphs.Length;i++) {
NavGraph graph = graphs[i];
NNInfo nnInfo = graph.GetNearest (position, constraint);
Node node = nnInfo.node;
if (node == null) {
continue;
}
float dist = ((Vector3)nnInfo.clampedPosition-position).magnitude;
if (prioritizeGraphs && dist < prioritizeGraphsLimit) {
//The node is close enough, choose this graph and discard all others
minDist = dist*(int)nnInfo.priority;
nearestNode = nnInfo;
nearestGraph = i;
break;
} else {
if (dist*(int)nnInfo.priority < minDist) {
minDist = dist*(int)nnInfo.priority;
nearestNode = nnInfo;
nearestGraph = i;
}
}
}
if (nearestNode.node != null && !constraint.Suitable (nearestNode.node)) {
//Check if a constrained node has already been set
if (nearestNode.constrainedNode != null) {
nearestNode.node = nearestNode.constrainedNode;
nearestNode.clampedPosition = nearestNode.constClampedPosition;
} else {
//Otherwise, perform a check to force the graphs to check for a suitable node
NNInfo nnInfo = graphs[nearestGraph].GetNearestForce (position, constraint);
if (nnInfo.node != null) {
nearestNode = nnInfo;
}
}
}
return nearestNode;
}
public virtual NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
float maxDistSqr = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
this.GetNodes(delegate(GraphNode node)
{
float sqrMagnitude = (position - (Vector3)node.position).sqrMagnitude;
if (sqrMagnitude < minDist)
{
minDist = sqrMagnitude;
minNode = node;
}
if (sqrMagnitude < minConstDist && sqrMagnitude < maxDistSqr && constraint.Suitable(node))
{
minConstDist = sqrMagnitude;
minConstNode = node;
}
return true;
});
NNInfo result = new NNInfo(minNode);
result.constrainedNode = minConstNode;
if (minConstNode != null)
{
result.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null)
{
result.constrainedNode = minNode;
result.constClampedPosition = (Vector3)minNode.position;
}
return result;
}
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
if (graphNodes == null || depth*width != graphNodes.Length) {
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);
Node node = nodes[x+z*width];
Node minNode = null;
float minDist = float.PositiveInfinity;
int overlap = getNearestForceOverlap;
if (constraint.Suitable (node)) {
minNode = node;
minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
}
if (minNode != null) {
if (overlap == 0) return minNode;
else overlap--;
}
//int counter = 0;
for (int w = 1; w < getNearestForceLimit;w++) {
int nx = x;
int nz = z+w;
int nz2 = nz*width;
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
//
if (constraint.Suitable (nodes[nx+nz2])) {
float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz2]; }
}
}
nz = z-w;
nz2 = nz*width;
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
if (constraint.Suitable (nodes[nx+nz2])) {
float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz2]; }
}
}
nx = x-w;
nz = z-w+1;
for (nz = z-w+1;nz <= z+w-1; nz++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
if (constraint.Suitable (nodes[nx+nz*width])) {
float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz*width]; }
}
}
nx = x+w;
for (nz = z-w+1;nz <= z+w-1; nz++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
if (constraint.Suitable (nodes[nx+nz*width])) {
float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist) { minDist = dist; minNode = nodes[nx+nz*width]; }
}
}
if (minNode != null) {
if (overlap == 0) return minNode;
else overlap--;
}
}
return null;
}
private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfo 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))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = vector;
}
else if (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);
}
}
// Token: 0x060025B4 RID: 9652 RVA: 0x001A1470 File Offset: 0x0019F670
private NNInfoInternal GetNearestInternal(Vector3 position, NNConstraint constraint, bool fastCheck)
{
if (this.nodes == null)
{
return(default(NNInfoInternal));
}
if (this.optimizeForSparseGraph)
{
return(new NNInfoInternal(this.lookupTree.GetNearest((Int3)position, fastCheck ? null : constraint)));
}
float num = (constraint == null || constraint.constrainDistance) ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
NNInfoInternal nninfoInternal = new NNInfoInternal(null);
float num2 = float.PositiveInfinity;
float num3 = float.PositiveInfinity;
for (int i = 0; i < this.nodeCount; i++)
{
PointNode pointNode = this.nodes[i];
float sqrMagnitude = (position - (Vector3)pointNode.position).sqrMagnitude;
if (sqrMagnitude < num2)
{
num2 = sqrMagnitude;
nninfoInternal.node = pointNode;
}
if (sqrMagnitude < num3 && sqrMagnitude < num && (constraint == null || constraint.Suitable(pointNode)))
{
num3 = sqrMagnitude;
nninfoInternal.constrainedNode = pointNode;
}
}
if (!fastCheck)
{
nninfoInternal.node = nninfoInternal.constrainedNode;
}
nninfoInternal.UpdateInfo();
return(nninfoInternal);
}
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);
}
}
}
void GetNearestConnectionInternal(int index, Int3 point, NNConstraint constraint, ref GraphNode best, ref long bestSqrDist, long distanceThresholdOffset)
{
var data = tree[index].data;
if (data != null)
{
var pointv3 = (UnityEngine.Vector3)point;
for (int i = tree[index].count - 1; i >= 0; i--)
{
var dist = (data[i].position - point).sqrMagnitudeLong;
// Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue
if (dist - distanceThresholdOffset < bestSqrDist && (constraint == null || constraint.Suitable(data[i])))
{
// This node may contains the closest connection
// Check all connections
var conns = (data[i] as PointNode).connections;
if (conns != null)
{
var nodePos = (UnityEngine.Vector3)data[i].position;
for (int j = 0; j < conns.Length; j++)
{
// Find the closest point on the connection, but only on this node's side of the connection
// This ensures that we will find the closest node with the closest connection.
var connectionMidpoint = ((UnityEngine.Vector3)conns[j].node.position + nodePos) * 0.5f;
float sqrConnectionDistance = VectorMath.SqrDistancePointSegment(nodePos, connectionMidpoint, pointv3);
// Convert to Int3 space
long sqrConnectionDistanceInt = (long)(sqrConnectionDistance * Int3.FloatPrecision * Int3.FloatPrecision);
if (sqrConnectionDistanceInt < bestSqrDist)
{
bestSqrDist = sqrConnectionDistanceInt;
best = data[i];
}
}
}
// Also check if the node itself is close enough.
// This is important if the node has no connections at all.
if (dist < bestSqrDist)
{
bestSqrDist = dist;
best = data[i];
}
}
}
}
else
{
var dist = (long)(point[tree[index].splitAxis] - tree[index].split);
var childIndex = 2 * index + (dist < 0 ? 0 : 1);
GetNearestConnectionInternal(childIndex, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset);
// Try the other one if it is possible to find a valid node on the other side
// Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue
if (dist * dist - distanceThresholdOffset < bestSqrDist)
{
// childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even
GetNearestConnectionInternal(childIndex ^ 0x1, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset);
}
}
}
/** 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(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
Int3 pos = (Int3)position;
float minDist = -1;
Node minNode = null;
float minConstDist = -1;
Node minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
if (nodes == null || nodes.Length == 0) {
return new NNInfo ();
}
for (int i=0;i<nodes.Length;i++) {
MeshNode node = nodes[i] as MeshNode;
if (accurateNearestNode) {
Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],(Vector3)vertices[node.v3],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 SafeIntMath
if (!Polygon.IsClockwise ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],position) || !Polygon.IsClockwise ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3],position) || !Polygon.IsClockwise ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1],position))
#else
if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos))
#endif
{
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 {
#if ASTARDEBUG
Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.blue);
Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3],Color.blue);
Debug.DrawLine ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1],Color.blue);
#endif
int dist = Mathfx.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;
}
}
}
}
}
NNInfo nninfo = new NNInfo (minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null) {
MeshNode node = nninfo.node as MeshNode;//minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],(Vector3)vertices[node.v3],position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null) {
MeshNode node = nninfo.constrainedNode as MeshNode;//minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],(Vector3)vertices[node.v3],position);
nninfo.constClampedPosition = clP;
}
return nninfo;
}
public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (this.nodes == null)
{
return(default(NNInfoInternal));
}
if (this.optimizeForSparseGraph)
{
return(new NNInfoInternal(this.lookupTree.GetNearest((Int3)position, constraint)));
}
float num = (constraint != null && !constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
NNInfoInternal result = new NNInfoInternal(null);
float num2 = float.PositiveInfinity;
float num3 = float.PositiveInfinity;
for (int i = 0; i < this.nodeCount; i++)
{
PointNode pointNode = this.nodes[i];
float sqrMagnitude = (position - (Vector3)pointNode.position).sqrMagnitude;
if (sqrMagnitude < num2)
{
num2 = sqrMagnitude;
result.node = pointNode;
}
if (sqrMagnitude < num3 && sqrMagnitude < num && (constraint == null || constraint.Suitable(pointNode)))
{
num3 = sqrMagnitude;
result.constrainedNode = pointNode;
}
}
result.UpdateInfo();
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);
}
}
public virtual NNInfoInternal GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
float maxDistSqr = (constraint == null || constraint.constrainDistance) ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
this.GetNodes(delegate(GraphNode node)
{
float sqrMagnitude = (position - (Vector3)node.position).sqrMagnitude;
if (sqrMagnitude < minDist)
{
minDist = sqrMagnitude;
minNode = node;
}
if (sqrMagnitude < minConstDist && sqrMagnitude < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = sqrMagnitude;
minConstNode = node;
}
});
NNInfoInternal result = new NNInfoInternal(minNode);
result.constrainedNode = minConstNode;
if (minConstNode != null)
{
result.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null)
{
result.constrainedNode = minNode;
result.constClampedPosition = (Vector3)minNode.position;
}
return(result);
}
/** 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) {
//Debug.LogError ("This function (GetNearest) is not implemented in the navigation graph generator : Type "+this.GetType ().Name);
var maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
var minDist = float.PositiveInfinity;
GraphNode minNode = null;
var minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
GetNodes (delegate (GraphNode node) {
var 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;
}
/** 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(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
Int3 pos = (Int3)position;
float minDist = -1;
Node minNode = null;
float minConstDist = -1;
Node minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
if (nodes == null || nodes.Length == 0)
{
return(new NNInfo());
}
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (accurateNearestNode)
{
Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], 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 (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos))
{
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 = Mathfx.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;
}
}
}
}
}
NNInfo nninfo = new NNInfo(minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null)
{
MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null)
{
MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
NNInfoInternal GetNearestInternal(Vector3 position, NNConstraint constraint, bool fastCheck)
{
if (nodes == null)
{
return(new NNInfoInternal());
}
if (optimizeForSparseGraph)
{
return(new NNInfoInternal(lookupTree.GetNearest((Int3)position, fastCheck ? null : constraint)));
}
float maxDistSqr = constraint == null || constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
var nnInfo = new NNInfoInternal(null);
float minDist = float.PositiveInfinity;
float minConstDist = float.PositiveInfinity;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
nnInfo.node = node;
}
if (dist < minConstDist && dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = dist;
nnInfo.constrainedNode = node;
}
}
if (!fastCheck)
{
nnInfo.node = nnInfo.constrainedNode;
}
nnInfo.UpdateInfo();
return(nnInfo);
}
NNInfoInternal GetNearestInternal(Vector3 position, NNConstraint constraint, bool fastCheck)
{
if (nodes == null)
{
return(new NNInfoInternal());
}
var iposition = (Int3)position;
float maxDistSqr = constraint == null || constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
maxDistSqr *= Int3.FloatPrecision * Int3.FloatPrecision;
var nnInfo = new NNInfoInternal(null);
long minDist = long.MaxValue;
long minConstDist = long.MaxValue;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
long dist = (iposition - node.position).sqrMagnitudeLong;
if (dist < minDist)
{
minDist = dist;
nnInfo.node = node;
}
if (dist < minConstDist && (float)dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = dist;
nnInfo.constrainedNode = node;
}
}
if (!fastCheck)
{
nnInfo.node = nnInfo.constrainedNode;
}
nnInfo.UpdateInfo();
return(nnInfo);
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (this.nodes == null)
{
return(default(NNInfo));
}
float num = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
float num2 = float.PositiveInfinity;
GraphNode graphNode = null;
float num3 = float.PositiveInfinity;
GraphNode graphNode2 = null;
if (this.optimizeForSparseGraph)
{
Int3 @int = this.WorldToLookupSpace((Int3)position);
Int3 int2 = @int - this.minLookup;
int num4 = 0;
num4 = Math.Max(num4, Math.Abs(int2.x));
num4 = Math.Max(num4, Math.Abs(int2.y));
num4 = Math.Max(num4, Math.Abs(int2.z));
int2 = @int - this.maxLookup;
num4 = Math.Max(num4, Math.Abs(int2.x));
num4 = Math.Max(num4, Math.Abs(int2.y));
num4 = Math.Max(num4, Math.Abs(int2.z));
PointNode next;
if (this.nodeLookup.TryGetValue(@int, out next))
{
while (next != null)
{
float sqrMagnitude = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude < num2)
{
num2 = sqrMagnitude;
graphNode = next;
}
if (constraint == null || (sqrMagnitude < num3 && sqrMagnitude < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude;
graphNode2 = next;
}
next = next.next;
}
}
for (int i = 1; i <= num4; i++)
{
if (i >= 20)
{
Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\nIf you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
break;
}
if (this.lookupCellSize.y == 0)
{
Int3 lhs = @int + new Int3(-i, 0, -i);
for (int j = 0; j <= 2 * i; j++)
{
if (this.nodeLookup.TryGetValue(lhs + new Int3(j, 0, 0), out next))
{
while (next != null)
{
float sqrMagnitude2 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude2 < num2)
{
num2 = sqrMagnitude2;
graphNode = next;
}
if (constraint == null || (sqrMagnitude2 < num3 && sqrMagnitude2 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude2;
graphNode2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(lhs + new Int3(j, 0, 2 * i), out next))
{
while (next != null)
{
float sqrMagnitude3 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude3 < num2)
{
num2 = sqrMagnitude3;
graphNode = next;
}
if (constraint == null || (sqrMagnitude3 < num3 && sqrMagnitude3 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude3;
graphNode2 = next;
}
next = next.next;
}
}
}
for (int k = 1; k < 2 * i; k++)
{
if (this.nodeLookup.TryGetValue(lhs + new Int3(0, 0, k), out next))
{
while (next != null)
{
float sqrMagnitude4 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude4 < num2)
{
num2 = sqrMagnitude4;
graphNode = next;
}
if (constraint == null || (sqrMagnitude4 < num3 && sqrMagnitude4 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude4;
graphNode2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(lhs + new Int3(2 * i, 0, k), out next))
{
while (next != null)
{
float sqrMagnitude5 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude5 < num2)
{
num2 = sqrMagnitude5;
graphNode = next;
}
if (constraint == null || (sqrMagnitude5 < num3 && sqrMagnitude5 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude5;
graphNode2 = next;
}
next = next.next;
}
}
}
}
else
{
Int3 lhs2 = @int + new Int3(-i, -i, -i);
for (int l = 0; l <= 2 * i; l++)
{
for (int m = 0; m <= 2 * i; m++)
{
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(l, m, 0), out next))
{
while (next != null)
{
float sqrMagnitude6 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude6 < num2)
{
num2 = sqrMagnitude6;
graphNode = next;
}
if (constraint == null || (sqrMagnitude6 < num3 && sqrMagnitude6 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude6;
graphNode2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(l, m, 2 * i), out next))
{
while (next != null)
{
float sqrMagnitude7 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude7 < num2)
{
num2 = sqrMagnitude7;
graphNode = next;
}
if (constraint == null || (sqrMagnitude7 < num3 && sqrMagnitude7 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude7;
graphNode2 = next;
}
next = next.next;
}
}
}
}
for (int n = 1; n < 2 * i; n++)
{
for (int num5 = 0; num5 <= 2 * i; num5++)
{
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(0, num5, n), out next))
{
while (next != null)
{
float sqrMagnitude8 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude8 < num2)
{
num2 = sqrMagnitude8;
graphNode = next;
}
if (constraint == null || (sqrMagnitude8 < num3 && sqrMagnitude8 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude8;
graphNode2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(2 * i, num5, n), out next))
{
while (next != null)
{
float sqrMagnitude9 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude9 < num2)
{
num2 = sqrMagnitude9;
graphNode = next;
}
if (constraint == null || (sqrMagnitude9 < num3 && sqrMagnitude9 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude9;
graphNode2 = next;
}
next = next.next;
}
}
}
}
for (int num6 = 1; num6 < 2 * i; num6++)
{
for (int num7 = 1; num7 < 2 * i; num7++)
{
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(num6, 0, num7), out next))
{
while (next != null)
{
float sqrMagnitude10 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude10 < num2)
{
num2 = sqrMagnitude10;
graphNode = next;
}
if (constraint == null || (sqrMagnitude10 < num3 && sqrMagnitude10 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude10;
graphNode2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(lhs2 + new Int3(num6, 2 * i, num7), out next))
{
while (next != null)
{
float sqrMagnitude11 = (position - (Vector3)next.position).sqrMagnitude;
if (sqrMagnitude11 < num2)
{
num2 = sqrMagnitude11;
graphNode = next;
}
if (constraint == null || (sqrMagnitude11 < num3 && sqrMagnitude11 < num && constraint.Suitable(next)))
{
num3 = sqrMagnitude11;
graphNode2 = next;
}
next = next.next;
}
}
}
}
}
if (graphNode2 != null)
{
num4 = Math.Min(num4, i + 1);
}
}
}
else
{
for (int num8 = 0; num8 < this.nodeCount; num8++)
{
PointNode pointNode = this.nodes[num8];
float sqrMagnitude12 = (position - (Vector3)pointNode.position).sqrMagnitude;
if (sqrMagnitude12 < num2)
{
num2 = sqrMagnitude12;
graphNode = pointNode;
}
if (constraint == null || (sqrMagnitude12 < num3 && sqrMagnitude12 < num && constraint.Suitable(pointNode)))
{
num3 = sqrMagnitude12;
graphNode2 = pointNode;
}
}
}
NNInfo result = new NNInfo(graphNode);
result.constrainedNode = graphNode2;
if (graphNode2 != null)
{
result.constClampedPosition = (Vector3)graphNode2.position;
}
else if (graphNode != null)
{
result.constrainedNode = graphNode;
result.constClampedPosition = (Vector3)graphNode.position;
}
return(result);
}
public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (this.nodes == null)
{
return(new NNInfoInternal());
}
float num = !constraint.constrainDistance ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
float positiveInfinity = float.PositiveInfinity;
GraphNode node = null;
float num3 = float.PositiveInfinity;
GraphNode node2 = null;
if (this.optimizeForSparseGraph)
{
PointNode next;
Int3 key = this.WorldToLookupSpace((Int3)position);
Int3 num5 = key - this.minLookup;
int num6 = 0;
int introduced48 = Math.Max(num6, Math.Abs(num5.x));
int introduced49 = Math.Max(introduced48, Math.Abs(num5.y));
num6 = Math.Max(introduced49, Math.Abs(num5.z));
num5 = key - this.maxLookup;
int introduced50 = Math.Max(num6, Math.Abs(num5.x));
int introduced51 = Math.Max(introduced50, Math.Abs(num5.y));
num6 = Math.Max(introduced51, Math.Abs(num5.z));
if (this.nodeLookup.TryGetValue(key, out next))
{
while (next != null)
{
Vector3 vector = position - ((Vector3)next.position);
float sqrMagnitude = vector.sqrMagnitude;
if (sqrMagnitude < positiveInfinity)
{
positiveInfinity = sqrMagnitude;
node = next;
}
if ((constraint == null) || (((sqrMagnitude < num3) && (sqrMagnitude < num)) && constraint.Suitable(next)))
{
num3 = sqrMagnitude;
node2 = next;
}
next = next.next;
}
}
for (int i = 1; i <= num6; i++)
{
if (i >= 20)
{
UnityEngine.Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\nIf you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
break;
}
if (this.lookupCellSize.y == 0)
{
Int3 num9 = key + new Int3(-i, 0, -i);
for (int j = 0; j <= (2 * i); j++)
{
if (this.nodeLookup.TryGetValue(num9 + new Int3(j, 0, 0), out next))
{
while (next != null)
{
Vector3 vector2 = position - ((Vector3)next.position);
float num11 = vector2.sqrMagnitude;
if (num11 < positiveInfinity)
{
positiveInfinity = num11;
node = next;
}
if ((constraint == null) || (((num11 < num3) && (num11 < num)) && constraint.Suitable(next)))
{
num3 = num11;
node2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(num9 + new Int3(j, 0, 2 * i), out next))
{
while (next != null)
{
Vector3 vector3 = position - ((Vector3)next.position);
float num12 = vector3.sqrMagnitude;
if (num12 < positiveInfinity)
{
positiveInfinity = num12;
node = next;
}
if ((constraint == null) || (((num12 < num3) && (num12 < num)) && constraint.Suitable(next)))
{
num3 = num12;
node2 = next;
}
next = next.next;
}
}
}
for (int k = 1; k < (2 * i); k++)
{
if (this.nodeLookup.TryGetValue(num9 + new Int3(0, 0, k), out next))
{
while (next != null)
{
Vector3 vector4 = position - ((Vector3)next.position);
float num14 = vector4.sqrMagnitude;
if (num14 < positiveInfinity)
{
positiveInfinity = num14;
node = next;
}
if ((constraint == null) || (((num14 < num3) && (num14 < num)) && constraint.Suitable(next)))
{
num3 = num14;
node2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(num9 + new Int3(2 * i, 0, k), out next))
{
while (next != null)
{
Vector3 vector5 = position - ((Vector3)next.position);
float num15 = vector5.sqrMagnitude;
if (num15 < positiveInfinity)
{
positiveInfinity = num15;
node = next;
}
if ((constraint == null) || (((num15 < num3) && (num15 < num)) && constraint.Suitable(next)))
{
num3 = num15;
node2 = next;
}
next = next.next;
}
}
}
}
else
{
Int3 num16 = key + new Int3(-i, -i, -i);
for (int m = 0; m <= (2 * i); m++)
{
for (int num18 = 0; num18 <= (2 * i); num18++)
{
if (this.nodeLookup.TryGetValue(num16 + new Int3(m, num18, 0), out next))
{
while (next != null)
{
Vector3 vector6 = position - ((Vector3)next.position);
float num19 = vector6.sqrMagnitude;
if (num19 < positiveInfinity)
{
positiveInfinity = num19;
node = next;
}
if ((constraint == null) || (((num19 < num3) && (num19 < num)) && constraint.Suitable(next)))
{
num3 = num19;
node2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(num16 + new Int3(m, num18, 2 * i), out next))
{
while (next != null)
{
Vector3 vector7 = position - ((Vector3)next.position);
float num20 = vector7.sqrMagnitude;
if (num20 < positiveInfinity)
{
positiveInfinity = num20;
node = next;
}
if ((constraint == null) || (((num20 < num3) && (num20 < num)) && constraint.Suitable(next)))
{
num3 = num20;
node2 = next;
}
next = next.next;
}
}
}
}
for (int n = 1; n < (2 * i); n++)
{
for (int num22 = 0; num22 <= (2 * i); num22++)
{
if (this.nodeLookup.TryGetValue(num16 + new Int3(0, num22, n), out next))
{
while (next != null)
{
Vector3 vector8 = position - ((Vector3)next.position);
float num23 = vector8.sqrMagnitude;
if (num23 < positiveInfinity)
{
positiveInfinity = num23;
node = next;
}
if ((constraint == null) || (((num23 < num3) && (num23 < num)) && constraint.Suitable(next)))
{
num3 = num23;
node2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(num16 + new Int3(2 * i, num22, n), out next))
{
while (next != null)
{
Vector3 vector9 = position - ((Vector3)next.position);
float num24 = vector9.sqrMagnitude;
if (num24 < positiveInfinity)
{
positiveInfinity = num24;
node = next;
}
if ((constraint == null) || (((num24 < num3) && (num24 < num)) && constraint.Suitable(next)))
{
num3 = num24;
node2 = next;
}
next = next.next;
}
}
}
}
for (int num25 = 1; num25 < (2 * i); num25++)
{
for (int num26 = 1; num26 < (2 * i); num26++)
{
if (this.nodeLookup.TryGetValue(num16 + new Int3(num25, 0, num26), out next))
{
while (next != null)
{
Vector3 vector10 = position - ((Vector3)next.position);
float num27 = vector10.sqrMagnitude;
if (num27 < positiveInfinity)
{
positiveInfinity = num27;
node = next;
}
if ((constraint == null) || (((num27 < num3) && (num27 < num)) && constraint.Suitable(next)))
{
num3 = num27;
node2 = next;
}
next = next.next;
}
}
if (this.nodeLookup.TryGetValue(num16 + new Int3(num25, 2 * i, num26), out next))
{
while (next != null)
{
Vector3 vector11 = position - ((Vector3)next.position);
float num28 = vector11.sqrMagnitude;
if (num28 < positiveInfinity)
{
positiveInfinity = num28;
node = next;
}
if ((constraint == null) || (((num28 < num3) && (num28 < num)) && constraint.Suitable(next)))
{
num3 = num28;
node2 = next;
}
next = next.next;
}
}
}
}
}
if (node2 != null)
{
num6 = Math.Min(num6, i + 1);
}
}
}
else
{
for (int num29 = 0; num29 < this.nodeCount; num29++)
{
PointNode node4 = this.nodes[num29];
Vector3 vector12 = position - ((Vector3)node4.position);
float num30 = vector12.sqrMagnitude;
if (num30 < positiveInfinity)
{
positiveInfinity = num30;
node = node4;
}
if ((constraint == null) || (((num30 < num3) && (num30 < num)) && constraint.Suitable(node4)))
{
num3 = num30;
node2 = node4;
}
}
}
NNInfoInternal internal2 = new NNInfoInternal(node);
internal2.constrainedNode = node2;
if (node2 != null)
{
internal2.constClampedPosition = (Vector3)node2.position;
return(internal2);
}
if (node != null)
{
internal2.constrainedNode = node;
internal2.constClampedPosition = (Vector3)node.position;
}
return(internal2);
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint) {
if (nodes == null) return new NNInfo();
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
if (optimizeForSparseGraph) {
Int3 lookupStart = WorldToLookupSpace((Int3)position);
Int3 size = lookupStart - _minLookup;
int mw = 0;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
size = lookupStart - _maxLookup;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
PointNode node;
if (_nodeLookup.TryGetValue(lookupStart, out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
for (int w = 1; w <= mw; w++) {
if (w >= 20) {
Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\n" +
"If you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and " +
"PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). " +
"When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
break;
}
if (_lookupCellSize.y == 0) {
Int3 reference = lookupStart + new Int3(-w, 0, -w);
for (int x = 0; x <= 2 * w; x++) {
if (_nodeLookup.TryGetValue(reference + new Int3(x, 0, 0), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
if (_nodeLookup.TryGetValue(reference + new Int3(x, 0, 2 * w), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
}
for (int x = 1; x < 2 * w; x++) {
if (_nodeLookup.TryGetValue(reference + new Int3(0, 0, x), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
if (_nodeLookup.TryGetValue(reference + new Int3(2 * w, 0, x), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
}
}
else {
Int3 reference = lookupStart + new Int3(-w, -w, -w);
for (int x = 0; x <= 2 * w; x++) {
for (int y = 0; y <= 2 * w; y++) {
if (_nodeLookup.TryGetValue(reference + new Int3(x, y, 0), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
if (_nodeLookup.TryGetValue(reference + new Int3(x, y, 2 * w), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
}
}
for (int x = 1; x < 2 * w; x++) {
for (int y = 0; y <= 2 * w; y++) {
if (_nodeLookup.TryGetValue(reference + new Int3(0, y, x), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
if (_nodeLookup.TryGetValue(reference + new Int3(2 * w, y, x), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
}
}
for (int x = 1; x < 2 * w; x++) {
for (int y = 1; y < 2 * w; y++) {
if (_nodeLookup.TryGetValue(reference + new Int3(x, 0, y), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
if (_nodeLookup.TryGetValue(reference + new Int3(x, 2 * w, y), out node)) {
while (node != null) {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) { minDist = dist; minNode = node; }
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) { minConstDist = dist; minConstNode = node; }
node = node.next;
}
}
}
}
}
if (minConstNode != null) {
// Only search one more layer
mw = System.Math.Min(mw, w + 1);
}
}
}
else {
for (int i = 0; i < nodeCount; i++) {
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) {
minConstDist = dist;
minConstNode = node;
}
}
}
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;
}
#region Debugging
//D.Log("Constraint: GraphMask: {0}, ConstrainArea: {1}, Area: {2}, ConstrainWalkability: {3}, \nWalkable: {4}, ConstrainTags: {5}, Tags: {6}, ConstrainDistance: {7}.",
// constraint.graphMask, constraint.constrainArea, constraint.area, constraint.constrainWalkability, constraint.walkable,
// constraint.constrainTags, constraint.tags, constraint.constrainDistance);
//if (minConstNode != null) {
// D.Log("Constraint criteria met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.constClampedPosition, Vector3.Distance(nnInfo.constClampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
//else {
// D.Log("Constraint criteria NOT met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.clampedPosition, Vector3.Distance(nnInfo.clampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
#endregion
return nnInfo;
}
void SearchBoxClosestXZ(int boxi, Vector3d p, ref long closestDist, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTreeBox box = arr[boxi];
if (box.node != null)
{
//Leaf node
//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
Vector3d closest = box.node.ClosestPointOnNodeXZ(p);
if (constraint == null || constraint.Suitable(box.node))
{
// XZ distance
long dist = (closest.x - p.x).Mul(closest.x - p.x) + (closest.z - p.z).Mul(closest.z - p.z);
long pre = closestDist;
float pre_f = dist.ToFloat();
float clo_f = closestDist.ToFloat();
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
nnInfo.constFixedClampedPosition = closest;
closestDist = FixedMath.Sqrt(dist);
}
else if (dist < closestDist.Mul(closestDist))
{
nnInfo.constrainedNode = box.node;
nnInfo.constFixedClampedPosition = closest;
closestDist = FixedMath.Sqrt(dist);
}
}
#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))
{
SearchBoxClosestXZ(box.left, p, ref closestDist, constraint, ref nnInfo);
}
if (RectIntersectsCircle(arr[box.right].rect, p, closestDist))
{
SearchBoxClosestXZ(box.right, p, ref closestDist, constraint, ref nnInfo);
}
}
}
private void SearchBox(int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
if (bBTreeBox.node != null)
{
if (bBTreeBox.node.ContainsPoint((Int3)p))
{
if (nnInfo.node == null)
{
nnInfo.node = bBTreeBox.node;
}
else if (Mathf.Abs(((Vector3)bBTreeBox.node.position).y - p.y) < Mathf.Abs(((Vector3)nnInfo.node.position).y - p.y))
{
nnInfo.node = bBTreeBox.node;
}
if (constraint.Suitable(bBTreeBox.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = bBTreeBox.node;
}
else if (Mathf.Abs((float)bBTreeBox.node.position.y - p.y) < Mathf.Abs((float)nnInfo.constrainedNode.position.y - p.y))
{
nnInfo.constrainedNode = bBTreeBox.node;
}
}
}
return;
}
if (this.arr[bBTreeBox.left].Contains(p))
{
this.SearchBox(bBTreeBox.left, p, constraint, ref nnInfo);
}
if (this.arr[bBTreeBox.right].Contains(p))
{
this.SearchBox(bBTreeBox.right, p, constraint, ref nnInfo);
}
}
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);
}
}
}
private void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo 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)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = vector;
closestDist = (float)Math.Sqrt((double)sqrMagnitude);
}
else if (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);
}
}
}
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);
}
private MeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint)
{
BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
if (bBTreeBox.node != null)
{
if (bBTreeBox.node.ContainsPoint((Int3)p))
{
if (constraint == null || constraint.Suitable(bBTreeBox.node))
{
return bBTreeBox.node;
}
}
}
else
{
if (this.arr[bBTreeBox.left].Contains(p))
{
MeshNode meshNode = this.SearchBoxInside(bBTreeBox.left, p, constraint);
if (meshNode != null)
{
return meshNode;
}
}
if (this.arr[bBTreeBox.right].Contains(p))
{
MeshNode meshNode = this.SearchBoxInside(bBTreeBox.right, p, constraint);
if (meshNode != null)
{
return meshNode;
}
}
}
return null;
}
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);
}
}
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;
}
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);
}
}
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);
}
}
public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (nodes == null)
{
return(new NNInfoInternal());
}
float maxDistSqr = constraint == null || constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
var nnInfo = new NNInfoInternal(null);
float minDist = float.PositiveInfinity;
float minConstDist = float.PositiveInfinity;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
nnInfo.node = node;
}
if (dist < minConstDist && dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = dist;
nnInfo.constrainedNode = node;
}
}
nnInfo.UpdateInfo();
return(nnInfo);
}
/** 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, Node hint)
{
//Debug.LogError ("This function (GetNearest) is not implemented in the navigation graph generator : Type "+this.GetType ().Name);
if (nodes == null) {
return new NNInfo ();
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
Node minNode = null;
float minConstDist = float.PositiveInfinity;
Node minConstNode = null;
for (int i=0;i<nodes.Length;i++) {
Node node = nodes[i];
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;
}
}
NNInfo 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;
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
//Debug.LogError ("This function (GetNearest) is not implemented in the navigation graph generator : Type "+this.GetType ().Name);
if (nodes == null)
{
return(new NNInfo());
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
}
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);
}
public void SearchBox(BBTreeBox box, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo)
{
//, int intendentLevel = 0) {
if (box.node != null) {
//Leaf node
if (NavMeshGraph.ContainsPoint (box.node,p,graph.vertices)) {
//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 (RectContains (box.c1.rect,p)) {
SearchBox (box.c1,p, constraint, ref nnInfo);
}
if (RectContains (box.c2.rect,p)) {
SearchBox (box.c2,p, constraint, ref nnInfo);
}
}
public MeshNode SearchBoxInside (BBTreeBox box, Vector3 p, NNConstraint constraint) {
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 (box.c1.rect.Contains (new Vector2(p.x,p.z))) {
g = SearchBoxInside (box.c1,p, constraint);
if (g != null) return g;
}
if (box.c2.rect.Contains (new Vector2(p.x,p.z))) {
g = SearchBoxInside (box.c2, p, constraint);
if (g != null) return g;
}
}
return null;
}
/** 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;
}
public void SearchBoxClosestXZ (BBTreeBox box, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) {
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.ClosestPointOnNodeXZ (p);//NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
// XZ distance
float dist = (closest.x-p.x)*(closest.x-p.x)+(closest.z-p.z)*(closest.z-p.z);
if (constraint == null || constraint.Suitable (box.node)) {
if (nnInfo.constrainedNode == null) {
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (float)System.Math.Sqrt (dist);
} else if (dist < closestDist*closestDist) {
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (float)System.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 (box.c1.rect,p,closestDist)) {
SearchBoxClosestXZ (box.c1,p, ref closestDist, constraint, ref nnInfo);
}
if (RectIntersectsCircle (box.c2.rect,p,closestDist)) {
SearchBoxClosestXZ (box.c2,p, ref closestDist, constraint, ref nnInfo);
}
}
}
/** 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) {
Int3 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) {
TriangleMeshNode 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 {
#if ASTARDEBUG
Debug.DrawLine ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],Color.blue);
Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.blue);
Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0],Color.blue);
#endif
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);
NNInfo nninfo = new NNInfo (minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null) {
TriangleMeshNode node = nninfo.node as TriangleMeshNode;//minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode (position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null) {
TriangleMeshNode node = nninfo.constrainedNode as TriangleMeshNode;//minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode (position);
nninfo.constClampedPosition = clP;
}
return nninfo;
}
// Token: 0x060025F9 RID: 9721 RVA: 0x001A379C File Offset: 0x001A199C
private TriangleMeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint)
{
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)
{
break;
}
TriangleMeshNode triangleMeshNode = array[bbtreeBox.nodeOffset + i];
if (triangleMeshNode.ContainsPoint((Int3)p) && (constraint == null || constraint.Suitable(triangleMeshNode)))
{
return(triangleMeshNode);
}
}
}
else
{
if (this.tree[bbtreeBox.left].Contains(p))
{
TriangleMeshNode triangleMeshNode2 = this.SearchBoxInside(bbtreeBox.left, p, constraint);
if (triangleMeshNode2 != null)
{
return(triangleMeshNode2);
}
}
if (this.tree[bbtreeBox.right].Contains(p))
{
TriangleMeshNode triangleMeshNode3 = this.SearchBoxInside(bbtreeBox.right, p, constraint);
if (triangleMeshNode3 != null)
{
return(triangleMeshNode3);
}
}
}
return(null);
}
private Node GetNearestNode(Vector3 position, int x, int z, NNConstraint constraint)
{
int index = width*z+x;
float minDist = float.PositiveInfinity;
Node minNode = null;
for (int i=0;i<layerCount;i++) {
Node node = nodes[index + width*depth*i];
if (node != null) {
float dist = ((Vector3)node.position - position).sqrMagnitude;
if (dist < minDist && constraint.Suitable (node)) {
minDist = dist;
minNode = node;
}
}
}
return minNode;
}
/** 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 = 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 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);
}
/** \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 ();
}
Vector3 globalPosition = position;
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);
GridNode node = nodes[x+z*width];
GridNode minNode = null;
float minDist = float.PositiveInfinity;
int overlap = getNearestForceOverlap;
Vector3 clampedPosition = Vector3.zero;
NNInfo nn = new NNInfo(null);
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;
if (overlap == 0) return nn;
else overlap--;
}
//int counter = 0;
float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
float maxDistSqr = maxDist*maxDist;
//for (int w = 1; w < getNearestForceLimit;w++) {
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 = x;
int nz = z+w;
int nz2 = nz*width;
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;
//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
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));
}
}
}
nz = z-w;
nz2 = nz*width;
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;
//Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
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;
nz = z-w+1;
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;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
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;
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;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
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));
}
}
}
if (minNode != null) {
if (overlap == 0) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
return nn;
}
else overlap--;
}
//No nodes were inside grid bounds
if (!anyInside) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
return nn;
}
}
//return new NNInfo ();
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (nodes == null)
{
return(new NNInfo());
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
if (optimizeForSparseGraph)
{
Int3 lookupStart = WorldToLookupSpace((Int3)position);
Int3 size = lookupStart - minLookup;
int mw = 0;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
size = lookupStart - maxLookup;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
PointNode node;
var searcher = new GetNearestHelper(position, maxDistSqr, constraint, nodeLookup);
searcher.Search(lookupStart);
for (int w = 1; w <= mw; w++)
{
if (w >= 20)
{
Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\n" +
"If you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and " +
"PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). " +
"When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
break;
}
if (lookupCellSize.y == 0)
{
Int3 reference = lookupStart + new Int3(-w, 0, -w);
for (int x = 0; x <= 2 * w; x++)
{
searcher.Search(reference + new Int3(x, 0, 0));
searcher.Search(reference + new Int3(x, 0, 2 * w));
}
for (int z = 1; z < 2 * w; z++)
{
searcher.Search(reference + new Int3(0, 0, z));
searcher.Search(reference + new Int3(2 * w, 0, z));
}
}
else
{
Int3 reference = lookupStart + new Int3(-w, -w, -w);
for (int x = 0; x <= 2 * w; x++)
{
for (int y = 0; y <= 2 * w; y++)
{
searcher.Search(reference + new Int3(x, y, 0));
searcher.Search(reference + new Int3(x, y, 2 * w));
}
}
for (int z = 1; z < 2 * w; z++)
{
for (int y = 0; y <= 2 * w; y++)
{
searcher.Search(reference + new Int3(0, y, z));
searcher.Search(reference + new Int3(2 * w, y, z));
}
}
for (int x = 1; x < 2 * w; x++)
{
for (int z = 1; z < 2 * w; z++)
{
searcher.Search(reference + new Int3(x, 0, z));
searcher.Search(reference + new Int3(x, 2 * w, z));
}
}
}
minConstNode = searcher.minConstNode;
minNode = searcher.minNode;
minDist = searcher.minDist;
minConstDist = searcher.minConstDist;
if (minConstNode != null)
{
// Only search one more layer
mw = System.Math.Min(mw, w + 1);
}
}
}
else
{
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
}
}
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);
}
