public override void Prepare()
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, this.nnConstraint, this.startHint);
this.startPoint = nearest.clampedPosition;
this.endPoint = this.startPoint;
this.startIntPoint = (Int3)this.startPoint;
this.hTarget = (Int3)this.aim;
this.startNode = nearest.node;
this.endNode = this.startNode;
if (this.startNode == null || this.endNode == null)
{
base.LogError("Couldn't find close nodes to the start point");
base.Error();
return;
}
if (!this.startNode.Walkable)
{
base.LogError("The node closest to the start point is not walkable");
base.Error();
return;
}
this.heuristicScale = this.aimStrength;
}
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
public virtual void Prepare()
{
//@pathStartTime = startTime;
//maxAngle = NmaxAngle;
//angleCost = NangleCost;
//stepByStep = NstepByStep;
//unitRadius = 0;//BETA, Not used
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint);
//Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(startNNInfo.node);
}
startPoint = startNNInfo.clampedPosition;
startIntPoint = (Int3)startPoint;
startNode = startNNInfo.node;
if (hasEndPoint)
{
NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint);
endPoint = endNNInfo.clampedPosition;
hTarget = (Int3)endPoint;
endNode = endNNInfo.node;
}
if (startNode == null || (hasEndPoint == true && endNode == null))
{
LogError("Couldn't find close nodes to either the start or the end (start = " + (startNode != null) + " end = " + (endNode != null) + ")");
return;
}
if (!startNode.walkable)
{
Debug.DrawRay(startPoint, Vector3.up, Color.red);
Debug.DrawLine(startPoint, (Vector3)startNode.position, Color.red);
Debug.Break();
LogError("The node closest to the start point is not walkable");
return;
}
if (hasEndPoint && !endNode.walkable)
{
LogError("The node closest to the end point is not walkable");
return;
}
if (hasEndPoint && startNode.area != endNode.area)
{
LogError("There is no valid path to the target (start area: " + startNode.area + ", target area: " + endNode.area + ")");
return;
}
}
private void GetClosestWalkableNodesToChildrenRecursively(Transform tr, List <GraphNode> nodes)
{
IEnumerator enumerator = tr.GetEnumerator();
try
{
while (enumerator.MoveNext())
{
Transform current = (Transform)enumerator.Current;
NNInfo nearest = AstarPath.active.GetNearest(current.position, NNConstraint.Default);
if ((nearest.node != null) && nearest.node.Walkable)
{
nodes.Add(nearest.node);
}
this.GetClosestWalkableNodesToChildrenRecursively(current, nodes);
}
}
finally
{
IDisposable disposable = enumerator as IDisposable;
if (disposable == null)
{
}
disposable.Dispose();
}
}
protected override void Prepare()
{
if (this.startNode == null)
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.originalStartPoint, this.nnConstraint);
this.startPoint = nearest.position;
this.startNode = nearest.node;
}
else
{
this.startPoint = (Vector3)this.startNode.position;
}
if (this.startNode == null)
{
base.Error();
base.LogError("Couldn't find a close node to the start point");
return;
}
if (!base.CanTraverse(this.startNode))
{
base.Error();
base.LogError("The node closest to the start point could not be traversed");
return;
}
}
// Token: 0x06000767 RID: 1895 RVA: 0x00048238 File Offset: 0x00046638
public override void Prepare()
{
if (this.startNode == null)
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.originalStartPoint, this.nnConstraint);
this.startPoint = nearest.clampedPosition;
this.startNode = nearest.node;
}
else
{
this.startPoint = (Vector3)this.startNode.position;
}
if (this.startNode == null)
{
base.Error();
base.LogError("Couldn't find a close node to the start point");
return;
}
if (!this.startNode.Walkable)
{
base.Error();
base.LogError("The node closest to the start point is not walkable");
return;
}
}
public static NNInfo GetNearestForce(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
NNInfo nearestForceBoth = NavMeshGraph.GetNearestForceBoth(graph, navmesh, position, constraint, accurateNearestNode);
nearestForceBoth.node = nearestForceBoth.constrainedNode;
nearestForceBoth.clampedPosition = nearestForceBoth.constClampedPosition;
return nearestForceBoth;
}
/** Queries the tree for the best node, searching within a circle around \a p with the specified radius */
public NNInfo QueryCircle(Vector3 p, float radius, NNConstraint constraint)
{
BBTreeBox c = root;
if (c == null)
{
return(null);
}
#if DEBUG
Vector3 prev = new Vector3(1, 0, 0) * radius + p;
for (double i = 0; i < Math.PI * 2; i += Math.PI / 50.0)
{
Vector3 cpos = new Vector3((float)Math.Cos(i), 0, (float)Math.Sin(i)) * radius + p;
Debug.DrawLine(prev, cpos, Color.yellow);
prev = cpos;
}
#endif
NNInfo nnInfo = new NNInfo(null);
SearchBoxCircle(c, p, radius, constraint, ref nnInfo);
nnInfo.UpdateInfo();
return(nnInfo);
}
protected override void Prepare()
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, this.nnConstraint);
this.startPoint = nearest.position;
this.endPoint = this.startPoint;
this.startIntPoint = (Int3)this.startPoint;
this.hTarget = (Int3)this.aim;
this.startNode = nearest.node;
this.endNode = this.startNode;
if (this.startNode == null || this.endNode == null)
{
base.LogError("Couldn't find close nodes to the start point");
base.Error();
return;
}
if (!base.CanTraverse(this.startNode))
{
base.LogError("The node closest to the start point could not be traversed");
base.Error();
return;
}
this.heuristicScale = this.aimStrength;
}
public override void Prepare()
{
AstarProfiler.StartProfile("Get Nearest");
//Initialize the NNConstraint
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest(originalStartPoint, nnConstraint);
startPoint = startNNInfo.clampedPosition;
startNode = startNNInfo.node;
AstarProfiler.EndProfile();
#if ASTARDEBUG
Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
#endif
if (startNode == null)
{
Error();
LogError("Couldn't find a close node to the start point");
return;
}
if (!startNode.walkable)
{
#if ASTARDEBUG
Debug.DrawRay(startPoint, Vector3.up, Color.red);
Debug.DrawLine(startPoint, (Vector3)startNode.position, Color.red);
#endif
Error();
LogError("The node closest to the start point is not walkable");
return;
}
}
public override void Prepare()
{
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint);
startPoint = startNNInfo.clampedPosition;
endPoint = startPoint;
startIntPoint = (Int3)startPoint;
hTarget = (Int3)aim; //startIntPoint;
startNode = startNNInfo.node;
endNode = startNode;
#if ASTARDEBUG
Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
Debug.DrawLine((Vector3)endNode.position, endPoint, Color.blue);
#endif
if (startNode == null || endNode == null)
{
LogError("Couldn't find close nodes to the start point");
Error();
return;
}
if (!startNode.Walkable)
{
LogError("The node closest to the start point is not walkable");
Error();
return;
}
heuristicScale = aimStrength;
}
public static NNInfo GetNearestForce(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
NNInfo info = GetNearestForceBoth(graph, navmesh, position, constraint, accurateNearestNode);
info.node = info.constrainedNode;
info.clampedPosition = info.constClampedPosition;
return(info);
}
/** This performs a linear search through all polygons returning the closest one */
public static NNInfo GetNearestForce(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
NNInfo nn = GetNearestForceBoth(nodes, vertices, position, constraint, accurateNearestNode);
nn.node = nn.constrainedNode;
nn.clampedPosition = nn.constClampedPosition;
return(nn);
}
/// <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;
}
public override void Prepare()
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, this.nnConstraint, this.startHint);
PathNNConstraint pathNNConstraint = this.nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(nearest.node);
}
this.startPoint = nearest.position;
this.startIntPoint = (Int3)this.startPoint;
this.startNode = nearest.node;
if (this.startNode == null)
{
base.Error();
return;
}
if (!this.startNode.Walkable)
{
base.Error();
return;
}
if (this.hasEndPoint)
{
NNInfo nearest2 = AstarPath.active.GetNearest(this.endPoint, this.nnConstraint, this.endHint);
this.endPoint = nearest2.position;
this.endNode = nearest2.node;
if (this.startNode == null && this.endNode == null)
{
base.Error();
return;
}
if (this.endNode == null)
{
base.Error();
return;
}
if (!this.endNode.Walkable)
{
base.Error();
return;
}
if (this.startNode.Area != this.endNode.Area)
{
base.Error();
return;
}
if (!this.EndPointGridGraphSpecialCase(nearest2.node))
{
this.hTarget = (Int3)this.endPoint;
this.hTargetNode = this.endNode;
base.pathHandler.GetPathNode(this.endNode).flag1 = true;
}
}
}
public override void Prepare () {
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest (startPoint,nnConstraint);
startNode = startNNInfo.node;
if (startNode == null) {
Error ();
LogError ("Could not find close node to the start point");
return;
}
}
public override void Prepare()
{
base.nnConstraint.tags = base.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, base.nnConstraint);
this.startNode = nearest.node;
if (this.startNode == null)
{
base.Error();
}
}
// Token: 0x06002727 RID: 10023 RVA: 0x001AFCE0 File Offset: 0x001ADEE0
protected override void Prepare()
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, this.nnConstraint);
this.startNode = nearest.node;
if (this.startNode == null)
{
base.FailWithError("Could not find close node to the start point");
return;
}
}
public override void Prepare()
{
base.nnConstraint.tags = base.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, base.nnConstraint);
this.startNode = nearest.node;
if (this.startNode == null)
{
base.Error();
base.LogError("Could not find close node to the start point");
}
}
private void GetClosestWalkableNodesToChildrenRecursively(Transform tr, List <GraphNode> nodes)
{
foreach (Transform transform in tr)
{
NNInfo nearest = AstarPath.active.GetNearest(transform.position, NNConstraint.Default);
if (nearest.node != null && nearest.node.Walkable)
{
nodes.Add(nearest.node);
}
this.GetClosestWalkableNodesToChildrenRecursively(tr, nodes);
}
}
public NNInfo Query(Vector3 p, NNConstraint constraint)
{
if (this.count == 0)
{
return(new NNInfo(null));
}
NNInfo result = default(NNInfo);
this.SearchBox(0, p, constraint, ref result);
result.UpdateInfo();
return(result);
}
public NNInfo QueryCircle(Vector3 p, float radius, NNConstraint constraint)
{
if (this.count == 0)
{
return(new NNInfo(null));
}
NNInfo result = new NNInfo(null);
this.SearchBoxCircle(0, p, radius, constraint, ref result);
result.UpdateInfo();
return(result);
}
public void Apply(bool forceNewCheck)
{
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
NNConstraint nn = NNConstraint.None;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
startNode.SetPosition((Int3)StartTransform.position);
endNode.SetPosition((Int3)EndTransform.position);
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
if (connectedNode1 == null || forceNewCheck)
{
NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
connectedNode1 = n1.node;
clamped1 = n1.clampedPosition;
}
if (connectedNode2 == null || forceNewCheck)
{
NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
connectedNode2 = n2.node;
clamped2 = n2.clampedPosition;
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
if (!oneWay)
{
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
}
if (!oneWay)
{
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
}
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
}
public NNInfo Query(Vector3 p, NNConstraint constraint)
{
if (this.count == 0)
{
return(new NNInfo(null));
}
NNInfo nnInfo = new NNInfo();
this.SearchBox(0, p, constraint, ref nnInfo);
nnInfo.UpdateInfo();
return(nnInfo);
}
public override void Prepare()
{
NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint);
startPoint = startNNInfo.constFixedClampedPosition;
startNode = startNNInfo.node;
NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint);
endPoint = endNNInfo.constFixedClampedPosition;
endNode = endNNInfo.node;
hTarget = Vector3d.ToInt3(endPoint);
}
// Token: 0x06002173 RID: 8563 RVA: 0x0018E130 File Offset: 0x0018C330
public override void Teleport(Vector3 newPosition, bool clearPath = true)
{
NNInfo nninfo = (AstarPath.active != null) ? AstarPath.active.GetNearest(newPosition) : default(NNInfo);
float elevation;
this.movementPlane.ToPlane(newPosition, out elevation);
newPosition = this.movementPlane.ToWorld(this.movementPlane.ToPlane((nninfo.node != null) ? nninfo.position : newPosition), elevation);
if (clearPath)
{
this.richPath.Clear();
}
base.Teleport(newPosition, clearPath);
}
void GetClosestWalkableNodesToChildrenRecursively(Transform tr, List <GraphNode> nodes)
{
foreach (Transform ch in tr)
{
NNInfo info = AstarPath.active.GetNearest(ch.position, NNConstraint.Default);
if (info.node != null && info.node.Walkable)
{
nodes.Add(info.node);
}
GetClosestWalkableNodesToChildrenRecursively(tr, nodes);
}
}
/** 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());
}
var maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
var minDist = float.PositiveInfinity;
GraphNode minNode = null;
var minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
for (var i = 0; i < nodeCount; i++)
{
var node = nodes[i];
var 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);
}
// Token: 0x0600271A RID: 10010 RVA: 0x001AF4E8 File Offset: 0x001AD6E8
protected override void Prepare()
{
this.nnConstraint.tags = this.enabledTags;
NNInfo nearest = AstarPath.active.GetNearest(this.startPoint, this.nnConstraint);
PathNNConstraint pathNNConstraint = this.nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(nearest.node);
}
this.startPoint = nearest.position;
this.startIntPoint = (Int3)this.startPoint;
this.startNode = nearest.node;
if (this.startNode == null)
{
base.FailWithError("Couldn't find a node close to the start point");
return;
}
if (!base.CanTraverse(this.startNode))
{
base.FailWithError("The node closest to the start point could not be traversed");
return;
}
if (this.hasEndPoint)
{
NNInfo nearest2 = AstarPath.active.GetNearest(this.endPoint, this.nnConstraint);
this.endPoint = nearest2.position;
this.endNode = nearest2.node;
if (this.endNode == null)
{
base.FailWithError("Couldn't find a node close to the end point");
return;
}
if (!base.CanTraverse(this.endNode))
{
base.FailWithError("The node closest to the end point could not be traversed");
return;
}
if (this.startNode.Area != this.endNode.Area)
{
base.FailWithError("There is no valid path to the target");
return;
}
if (!this.EndPointGridGraphSpecialCase(nearest2.node))
{
this.hTarget = (Int3)this.endPoint;
this.hTargetNode = this.endNode;
this.pathHandler.GetPathNode(this.endNode).flag1 = true;
}
}
}
public void Teleport(Vector3 newPosition)
{
this.CancelCurrentPathRequest();
NNInfo nearest = AstarPath.active.GetNearest(newPosition);
float elevation;
this.movementPlane.ToPlane(newPosition, out elevation);
this.prevPosition = (this.tr.position = this.movementPlane.ToWorld(this.movementPlane.ToPlane((nearest.node != null) ? nearest.position : newPosition), elevation));
this.richPath.Clear();
if (this.rvoController != null)
{
this.rvoController.Move(Vector3.zero);
}
}
public override void Prepare()
{
base.nnConstraint.tags = base.enabledTags;
NNInfo info = AstarPath.active.GetNearest(this.startPoint, base.nnConstraint, this.startHint);
PathNNConstraint nnConstraint = base.nnConstraint as PathNNConstraint;
if (nnConstraint != null)
{
nnConstraint.SetStart(info.node);
}
this.startPoint = info.position;
this.startIntPoint = (Int3)this.startPoint;
this.startNode = info.node;
if (this.startNode == null)
{
base.Error();
}
else if (!this.startNode.Walkable)
{
base.Error();
}
else if (this.hasEndPoint)
{
NNInfo info2 = AstarPath.active.GetNearest(this.endPoint, base.nnConstraint, this.endHint);
this.endPoint = info2.position;
this.endNode = info2.node;
if ((this.startNode == null) && (this.endNode == null))
{
base.Error();
}
else if (this.endNode == null)
{
base.Error();
}
else if (!this.endNode.Walkable)
{
base.Error();
}
else if (this.startNode.Area != this.endNode.Area)
{
base.Error();
}
else if (!this.EndPointGridGraphSpecialCase(info2.node))
{
base.hTarget = (Int3)this.endPoint;
base.hTargetNode = this.endNode;
base.pathHandler.GetPathNode(this.endNode).flag1 = true;
}
}
}
public NNInfo Query (Vector3 p, NNConstraint constraint) {
BBTreeBox c = root;
if (c == null) {
return new NNInfo();
}
NNInfo nnInfo = new NNInfo ();
SearchBox (c,p, constraint, ref nnInfo);
nnInfo.UpdateInfo ();
return nnInfo;
}
/** Queries the tree for the best node, searching within a circle around \a p with the specified radius.
* Will fill in both the constrained node and the not constrained node in the NNInfo.
*
* \see QueryClosest
*/
public NNInfo QueryCircle (Vector3 p, float radius, NNConstraint constraint) {
BBTreeBox c = root;
if (c == null) {
return new NNInfo();
}
#if ASTARDEBUG
Vector3 prev = new Vector3 (1,0,0)*radius+p;
for (double i=0;i< Math.PI*2; i += Math.PI/50.0) {
Vector3 cpos = new Vector3 ((float)Math.Cos (i),0,(float)Math.Sin (i))*radius+p;
Debug.DrawLine (prev,cpos,Color.yellow);
prev = cpos;
}
#endif
NNInfo nnInfo = new NNInfo (null);
SearchBoxCircle (c,p, radius, constraint, ref nnInfo);
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, 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 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 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 NNInfo QueryCircle(Vector3 p, float radius, NNConstraint constraint)
{
if (this.count == 0)
{
return new NNInfo(null);
}
NNInfo result = new NNInfo(null);
this.SearchBoxCircle(0, p, radius, constraint, ref result);
result.UpdateInfo();
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;
}
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);
}
}
/** 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;
}
/** 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;
}
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);
}
}
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
//return NavMeshGraph.GetNearest (this, nodes,position, constraint, accurateNearestNode);
if (tiles == null) return new NNInfo ();
Vector3 localPosition = position - forcedBounds.min;
int tx = Mathf.FloorToInt (localPosition.x / (cellSize*tileSizeX));
int tz = Mathf.FloorToInt (localPosition.z / (cellSize*tileSizeZ));
//Clamp to graph borders
tx = Mathf.Clamp (tx, 0, tileXCount-1);
tz = Mathf.Clamp (tz, 0, tileZCount-1);
int wmax = System.Math.Max (tileXCount, tileZCount);
NNInfo best = new NNInfo();
float bestDistance = float.PositiveInfinity;
bool xzSearch = nearestSearchOnlyXZ || ( constraint != null && constraint.distanceXZ );
//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min, Vector3.up*15, Color.blue);
//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.forward*5, Vector3.back*10, Color.blue);
//Debug.DrawRay (new Vector3(tx,0,tz)*cellSize*tileSize + forcedBounds.min + Vector3.left*5, Vector3.right*10, Color.blue);
//Search outwards in a diamond pattern from the closest tile
for (int w=0;w<wmax;w++) {
if (!xzSearch && bestDistance < (w-1)*cellSize*System.Math.Max (tileSizeX, tileSizeZ)) break;
int zmax = System.Math.Min (w+tz +1, tileZCount);
for (int z=System.Math.Max(-w+tz, 0); z < zmax; z++) {
//Solve for z such that abs(x-tx) + abs(z-tx) == w
//Delta X coordinate
int dx = System.Math.Abs( w - System.Math.Abs(z-tz));
//Solution is dx + tx and -dx + tx
//First solution negative delta x
if (-dx + tx >= 0) {
//Absolute x coordinate
int x = -dx + tx;
NavmeshTile tile = tiles[x + z*tileXCount];
if (tile != null) {
if (xzSearch) {
best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best);
if ( bestDistance < float.PositiveInfinity ) break;
} else {
best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best);
}
}
}
//Other solution, make sure it is not the same solution by checking x != 0
if (dx != 0 && dx + tx < tileXCount) {
//Absolute x coordinate
int x = dx + tx;
NavmeshTile tile = tiles[x + z*tileXCount];
if (tile != null) {
if (xzSearch) {
best = tile.bbTree.QueryClosestXZ (position, constraint, ref bestDistance, best);
if ( bestDistance < float.PositiveInfinity ) break;
} else {
best = tile.bbTree.QueryClosest (position, constraint, ref bestDistance, best);
}
}
}
}
}
best.node = best.constrainedNode;
best.constrainedNode = null;
best.clampedPosition = best.constClampedPosition;
return best;
//return GetNearestForce (position, constraint);
}
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);
}
}
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);
}
}
}
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);
}
}
/** 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) 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;
}
/** 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;
}
/** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution.
* Note that this function will, unlike QueryCircle, only fill in the constrained node.
* If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
*
* \param p Point to search around
* \param constraint Optionally set to constrain which nodes to return
* \param distance The best distance for the \a previous solution. Will be updated with the best distance
* after this search. Will be positive infinity if no node could be found.
* Set to positive infinity if there was no previous solution.
* \param previous This search will start from the \a previous NNInfo and improve it if possible.
* Even if the search fails on this call, the solution will never be worse than \a previous.
*
* \see QueryCircle
*/
public NNInfo QueryClosest (Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) {
if ( count == 0 ) return previous;
SearchBoxClosest (0,p, ref distance, constraint, ref previous);
return previous;
}
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;
}
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);
}
}
}
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;
}
}
}
//public void GenerateBounds () {
//bounds.center = offset+new Vector3 (0,height*0.5F,0);
//bounds.size = new Vector3 (width*scale,height,depth*scale);
//}
/** \todo Set clamped position for Grid Graph */
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
if (nodes == null || depth*width != nodes.Length) {
return new NNInfo ();
}
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);
NNInfo nn = new NNInfo(nodes[z*width+x]);
float y = inverseMatrix.MultiplyPoint3x4((Vector3)nodes[z*width+x].position).y;
nn.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));
//Set clamped position
//nn.clampedPosition = new Vector3(Mathf.Clamp (xf,x-0.5f,x+0.5f),position.y,Mathf.Clamp (zf,z-0.5f,z+0.5f));
//nn.clampedPosition = matrix.MultiplyPoint3x4 (nn.clampedPosition);
return nn;
}
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);
}
}
}
/** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution.
* Note that this function will, unlike QueryCircle, only fill in the constrained node.
* If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
*
* This search will start from the \a previous NNInfo and improve it if possible.
* Even if the search fails on this call, the solution will never be worse than \a previous.
*
* This method will completely ignore any Y-axis differences in positions.
*
* \param distance The best distance for the \a previous solution. Will be updated with the best distance
* after this search. Will be positive infinity if no node could be found.
* Set to positive infinity if there was no previous solution.
*
*
* \see QueryCircle
*/
public NNInfo QueryClosestXZ (Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) {
BBTreeBox c = root;
if (c == null) {
return previous;
}
SearchBoxClosestXZ (c,p, ref distance, constraint, ref previous);
return previous;
}
/** 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;
}
public NNInfo Query (Vector3 p, NNConstraint constraint) {
if ( count == 0 ) return new NNInfo(null);
var nnInfo = new NNInfo ();
SearchBox (0,p, constraint, ref nnInfo);
nnInfo.UpdateInfo ();
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 ();
}
/** Queries the tree for the best node, searching within a circle around \a p with the specified radius.
* Will fill in both the constrained node and the not constrained node in the NNInfo.
*
* \see QueryClosest
*/
public NNInfo QueryCircle (Vector3 p, float radius, NNConstraint constraint) {
if ( count == 0 ) return new NNInfo(null);
var nnInfo = new NNInfo (null);
SearchBoxCircle (0,p, radius, constraint, ref nnInfo);
nnInfo.UpdateInfo ();
return nnInfo;
}
