/** 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;
}
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);
}
}
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);
}
}
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;
}
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);
}
}
}
/** \todo Set clamped position for Grid Graph */
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
if (nodes == null || depth*width != nodes.Length) {
return new NNInfo ();
}
// Position in global space
Vector3 globalPosition = position;
// Position in graph space
position = inverseMatrix.MultiplyPoint3x4 (position);
// Find the coordinates of the closest node
float xf = position.x-0.5F;
float zf = position.z-0.5f;
int x = Mathf.Clamp (Mathf.RoundToInt (xf) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (zf) , 0, depth-1);
// Closest node
GridNode node = nodes[x+z*width];
GridNode minNode = null;
float minDist = float.PositiveInfinity;
int overlap = getNearestForceOverlap;
Vector3 clampedPosition = Vector3.zero;
var nn = new NNInfo(null);
// If the closest node was suitable
if (constraint.Suitable (node)) {
minNode = node;
minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
float y = inverseMatrix.MultiplyPoint3x4((Vector3)node.position).y;
clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f, y, Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f));
}
if (minNode != null) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
// We have a node, and we don't need to search more, so just return
if (overlap == 0) return nn;
overlap--;
}
// Search up to this distance
float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
float maxDistSqr = maxDist*maxDist;
// Search a square/spiral pattern around the point
for (int w = 1;;w++) {
//Check if the nodes are within distance limit
if (nodeSize*w > maxDist) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
return nn;
}
bool anyInside = false;
int nx;
int nz = z+w;
int nz2 = nz*width;
// Side 1 on the square
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
anyInside = true;
if (constraint.Suitable (nodes[nx+nz2])) {
float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
if (dist < minDist && dist < maxDistSqr) {
// Minimum distance so far
minDist = dist;
minNode = nodes[nx+nz2];
// Closest point on the node if the node is treated as a square
clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
}
}
}
nz = z-w;
nz2 = nz*width;
// Side 2 on the square
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
anyInside = true;
if (constraint.Suitable (nodes[nx+nz2])) {
float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
if (dist < minDist && dist < maxDistSqr) {
minDist = dist;
minNode = nodes[nx+nz2];
clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
}
}
}
nx = x-w;
// Side 3 on the square
for (nz = z-w+1;nz <= z+w-1; nz++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
anyInside = true;
if (constraint.Suitable (nodes[nx+nz*width])) {
float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
if (dist < minDist && dist < maxDistSqr) {
minDist = dist;
minNode = nodes[nx+nz*width];
clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
}
}
}
nx = x+w;
// Side 4 on the square
for (nz = z-w+1;nz <= z+w-1; nz++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
anyInside = true;
if (constraint.Suitable (nodes[nx+nz*width])) {
float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
if (dist < minDist && dist < maxDistSqr) {
minDist = dist;
minNode = nodes[nx+nz*width];
clampedPosition = matrix.MultiplyPoint3x4 (new Vector3 (Mathf.Clamp(xf,nx-0.5f,nx+0.5f)+0.5f, inverseMatrix.MultiplyPoint3x4((Vector3)minNode.position).y, Mathf.Clamp(zf,nz-0.5f,nz+0.5f)+0.5f));
}
}
}
// We found a suitable node
if (minNode != null) {
// If we don't need to search more, just return
// Otherwise search for 'overlap' iterations more
if (overlap == 0) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
return nn;
}
overlap--;
}
// No nodes were inside grid bounds
// We will not be able to find any more valid nodes
// so just return
if (!anyInside) {
nn.node = minNode;
nn.clampedPosition = clampedPosition;
return nn;
}
}
}
private LevelGridNode GetNearestNode (Vector3 position, int x, int z, NNConstraint constraint) {
int index = width*z+x;
float minDist = float.PositiveInfinity;
LevelGridNode minNode = null;
for (int i=0;i<layerCount;i++) {
LevelGridNode node = nodes[index + width*depth*i];
if (node != null) {
float dist = ((Vector3)node.position - position).sqrMagnitude;
if (dist < minDist && (constraint == null || constraint.Suitable (node))) {
minDist = dist;
minNode = node;
}
}
}
return minNode;
}
/** This performs a linear search through all polygons returning the closest one.
* This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
* complying with the NNConstraint.
* \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
*/
public static NNInfo GetNearestForceBoth (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
var pos = (Int3)position;
float minDist = -1;
GraphNode minNode = null;
float minConstDist = -1;
GraphNode minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
GraphNodeDelegateCancelable del = delegate (GraphNode _node) {
var node = _node as TriangleMeshNode;
if (accurateNearestNode) {
Vector3 closest = node.ClosestPointOnNode (position);
float dist = ((Vector3)pos-closest).sqrMagnitude;
if (minNode == null || dist < minDist) {
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable (node)) {
if (minConstNode == null || dist < minConstDist) {
minConstDist = dist;
minConstNode = node;
}
}
} else {
if (!node.ContainsPoint ((Int3)position)) {
float dist = (node.position-pos).sqrMagnitude;
if (minNode == null || dist < minDist) {
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable (node)) {
if (minConstNode == null || dist < minConstDist) {
minConstDist = dist;
minConstNode = node;
}
}
} else {
int dist = AstarMath.Abs (node.position.y-pos.y);
if (minNode == null || dist < minDist) {
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable (node)) {
if (minConstNode == null || dist < minConstDist) {
minConstDist = dist;
minConstNode = node;
}
}
}
}
return true;
};
graph.GetNodes (del);
var nninfo = new NNInfo (minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null) {
var node = nninfo.node as TriangleMeshNode;//minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode (position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null) {
var node = nninfo.constrainedNode as TriangleMeshNode;//minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode (position);
nninfo.constClampedPosition = clP;
}
return nninfo;
}