private static Index3D __maxAllowedSectorGridSizeToScan = new Index3D(4, 4, 4); // limit to divisible by 2
#region Archived
/// <summary>
/// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
/// Use this for any initialization code which can't be placed in Scan
/// </summary>
//public override void Awake() {
// base.Awake();
//}
/// <summary>
/// This will be called on the same time as OnDisable on the gameObject which the AstarPath script is attached to (remember, not in the editor)
/// Use for any cleanup code such as cleaning up static variables which otherwise might prevent resources from being collected
/// Use by creating a function overriding this one in a graph class, but always call base.OnDestroy () in that function.
/// </summary>
//public override void OnDestroy() {
// base.OnDestroy();
//}
#endregion
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint) {
if (nodes == null) return new NNInfo();
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
for (int i = 0; i < nodeCount; i++) {
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node))) {
minConstDist = dist;
minConstNode = node;
}
}
NNInfo nnInfo = new NNInfo(minNode);
nnInfo.constrainedNode = minConstNode;
if (minConstNode != null) {
nnInfo.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null) {
nnInfo.constrainedNode = minNode;
nnInfo.constClampedPosition = (Vector3)minNode.position;
}
#region Debugging
//D.Log("Constraint: GraphMask: {0}, ConstrainArea: {1}, Area: {2}, ConstrainWalkability: {3}, \nWalkable: {4}, ConstrainTags: {5}, Tags: {6}, ConstrainDistance: {7}.",
// constraint.graphMask, constraint.constrainArea, constraint.area, constraint.constrainWalkability, constraint.walkable,
// constraint.constrainTags, constraint.tags, constraint.constrainDistance);
//if (minConstNode != null) {
// D.Log("Constraint criteria met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.constClampedPosition, Vector3.Distance(nnInfo.constClampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
//else {
// D.Log("Constraint criteria NOT met. Closest Node is at {0}, {1} from {2}. \nNodeConstrainDistance = {3}, DistanceConstraint = {4}.",
// nnInfo.clampedPosition, Vector3.Distance(nnInfo.clampedPosition, position), position,
// constraint.constrainDistance, Mathf.Sqrt(maxDistSqr));
//}
#endregion
return nnInfo;
}
/// <summary>
/// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
/// Use this for any initialization code which can't be placed in Scan
/// </summary>
//public override void Awake() {
// base.Awake();
//}
// IMPROVE not really necessary. I just override this NavGraph method to add the debug line at the bottom. Otherwise its identical
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint) {
if (nodes == null) {
return new NNInfo();
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
Node minNode = null;
float minConstDist = float.PositiveInfinity;
Node minConstNode = null;
for (int i = 0; i < nodes.Length; i++) {
Node node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
if (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)) {
minConstDist = dist;
minConstNode = node;
}
}
NNInfo nnInfo = new NNInfo(minNode);
nnInfo.constrainedNode = minConstNode;
if (minConstNode != null) {
nnInfo.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null) {
nnInfo.constrainedNode = minNode;
nnInfo.constClampedPosition = (Vector3)minNode.position;
}
#region Debugging
if (nnInfo.constrainedNode == null) {
float closestDistance;
Node closestNode = __FindClosestNode(position, out closestDistance);
D.Warn("Can't find node close enough to {0}. ClosestNode is {1} away.", position, closestDistance);
}
else {
D.Log("Closest Node is at {0}, {1} from {2}.", (Vector3)nnInfo.constrainedNode.position, Mathf.Sqrt(minDist), position);
}
//D.Log("GetNearest() constraint.Suitable = {0}.", constraint.Suitable(nnInfo.node));
#endregion
return nnInfo;
}
public bool GetPositionIsSlime(Vector3 point, float toleranceRadius)
{
point.y = 0;
NNConstraint slimeConstraint = new NNConstraint ();
slimeConstraint.constrainTags = true;
slimeConstraint.tags = ~slimeTag;
NNInfo nn = AstarPath.active.GetNearest (point, slimeConstraint);
if (AstarMath.SqrMagnitudeXZ (nn.clampedPosition, point) <= toleranceRadius * toleranceRadius) {
Debug.DrawLine (nn.clampedPosition, point);
return true;
}
return false;
}
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;
}
static int set_nnConstraint(IntPtr L)
{
object o = null;
try
{
o = ToLua.ToObject(L, 1);
Pathfinding.Path obj = (Pathfinding.Path)o;
Pathfinding.NNConstraint arg0 = (Pathfinding.NNConstraint)ToLua.CheckObject <Pathfinding.NNConstraint>(L, 2);
obj.nnConstraint = arg0;
return(0);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e, o, "attempt to index nnConstraint on a nil value"));
}
}
static int get_nnConstraint(IntPtr L)
{
object o = null;
try
{
o = ToLua.ToObject(L, 1);
Pathfinding.Path obj = (Pathfinding.Path)o;
Pathfinding.NNConstraint ret = obj.nnConstraint;
ToLua.PushObject(L, ret);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e, o, "attempt to index nnConstraint on a nil value"));
}
}
protected override void Start() {
base.Start();
_pathStart = _transform.position;
D.Log("Path start = {0}, target = {1}.", _pathStart, pathDestination);
Debug.DrawLine(_pathStart, pathDestination, Color.yellow, 20F, false);
//Path path = new Path(startPosition, targetPosition, null); // Path is now abstract
//Path path = PathPool<ABPath>.GetPath(); // don't know how to assign start and target points
Path path = ABPath.Construct(_pathStart, pathDestination, null);
// Node qualifying constraint instance that checks that nodes are walkable, and within the seeker-specified
// max search distance. Tags and area testing are turned off, primarily because I don't yet understand them
NNConstraint constraint = new NNConstraint();
constraint.constrainTags = false;
path.nnConstraint = constraint;
_seeker.StartPath(path);
// this simple default version uses a constraint that has tags enabled which made finding close nodes problematic
//_seeker.StartPath(startPosition, targetPosition);
}
/** 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;
}
/** 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;
}
public void Apply(bool forceNewCheck)
{
NNConstraint none = NNConstraint.None;
none.distanceXZ = true;
int graphIndex = (int)this.startNode.GraphIndex;
none.graphMask = ~(1 << graphIndex);
bool flag = true;
NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none);
flag &= (nearest.node == this.connectedNode1 && nearest.node != null);
this.connectedNode1 = (nearest.node as MeshNode);
this.clamped1 = nearest.clampedPosition;
if (this.connectedNode1 != null)
{
Debug.DrawRay((Vector3)this.connectedNode1.position, Vector3.up * 5f, Color.red);
}
NNInfo nearest2 = AstarPath.active.GetNearest(this.EndTransform.position, none);
flag &= (nearest2.node == this.connectedNode2 && nearest2.node != null);
this.connectedNode2 = (nearest2.node as MeshNode);
this.clamped2 = nearest2.clampedPosition;
if (this.connectedNode2 != null)
{
Debug.DrawRay((Vector3)this.connectedNode2.position, Vector3.up * 5f, Color.cyan);
}
if (this.connectedNode2 == null || this.connectedNode1 == null)
{
return;
}
this.startNode.SetPosition((Int3)this.StartTransform.position);
this.endNode.SetPosition((Int3)this.EndTransform.position);
if (flag && !forceNewCheck)
{
return;
}
this.RemoveConnections(this.startNode);
this.RemoveConnections(this.endNode);
uint cost = (uint)Mathf.RoundToInt((float)((Int3)(this.StartTransform.position - this.EndTransform.position)).costMagnitude * this.costFactor);
this.startNode.AddConnection(this.endNode, cost);
this.endNode.AddConnection(this.startNode, cost);
Int3 rhs = this.connectedNode2.position - this.connectedNode1.position;
for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++)
{
Int3 vertex = this.connectedNode1.GetVertex(i);
Int3 vertex2 = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount());
if (Int3.DotLong((vertex2 - vertex).Normal2D(), rhs) <= 0L)
{
for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++)
{
Int3 vertex3 = this.connectedNode2.GetVertex(j);
Int3 vertex4 = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount());
if (Int3.DotLong((vertex4 - vertex3).Normal2D(), rhs) >= 0L)
{
if ((double)Int3.Angle(vertex4 - vertex3, vertex2 - vertex) > 2.9670598109563189)
{
float num = 0f;
float num2 = 1f;
num2 = Math.Min(num2, AstarMath.NearestPointFactor(vertex, vertex2, vertex3));
num = Math.Max(num, AstarMath.NearestPointFactor(vertex, vertex2, vertex4));
if (num2 >= num)
{
Vector3 vector = (Vector3)(vertex2 - vertex) * num + (Vector3)vertex;
Vector3 vector2 = (Vector3)(vertex2 - vertex) * num2 + (Vector3)vertex;
this.startNode.portalA = vector;
this.startNode.portalB = vector2;
this.endNode.portalA = vector2;
this.endNode.portalB = vector;
this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
return;
}
Debug.LogError(string.Concat(new object[]
{
"Wait wut!? ",
num,
" ",
num2,
" ",
vertex,
" ",
vertex2,
" ",
vertex3,
" ",
vertex4,
"\nTODO, fix this error"
}));
}
}
}
}
}
}
/** This performs a linear search through all polygons returning the closest one.
* This is usually only called in the Free version of the A* Pathfinding Project since the Pro one supports BBTrees and will do another query
*/
public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
return GetNearestForce (this, this,position,constraint, accurateNearestNode);
//Debug.LogWarning ("This function shouldn't be called since constrained nodes are sent back in the GetNearest call");
//return new NNInfo ();
}
/// <summary>
/// Connects the start and end points using a link or refreshes the existing link.
///
/// If you have moved the link or otherwise modified it you need to call this method.
///
/// Warning: This must only be done when it is safe to update the graph structure.
/// The easiest is to do it inside a work item. See <see cref="AstarPath.active.AddWorkItem"/>.
/// </summary>
public void Apply(bool forceNewCheck)
{
CreateLinkNodes();
if (connectedNode1 != null && connectedNode1.Destroyed)
{
connectedNode1 = null;
}
if (connectedNode2 != null && connectedNode2.Destroyed)
{
connectedNode2 = null;
}
if (startNode == null || endNode == null)
{
return;
}
//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.position = (Int3)StartTransform.position;
endNode.position = (Int3)EndTransform.position;
// Clear connections to and from the start and end nodes
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)
{
var info = AstarPath.active.GetNearest(StartTransform.position, nn);
connectedNode1 = info.node;
clamped1 = info.position;
}
if (connectedNode2 == null || forceNewCheck)
{
var info = AstarPath.active.GetNearest(EndTransform.position, nn);
connectedNode2 = info.node;
clamped2 = info.position;
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
// Add connections between nodes, or replace old connections if they exist
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 static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
VInt3 pos = (VInt3)position;
double minDist = -1.0;
GraphNode minNode = null;
double minConstDist = -1.0;
GraphNode minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
GraphNodeDelegateCancelable del = delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode3 = _node as TriangleMeshNode;
if (accurateNearestNode)
{
Vector3 b = triangleMeshNode3.ClosestPointOnNode(position);
float sqrMagnitude = ((Vector3)pos - b).sqrMagnitude;
if (minNode == null || (double)sqrMagnitude < minDist)
{
minDist = (double)sqrMagnitude;
minNode = triangleMeshNode3;
}
if (sqrMagnitude < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (double)sqrMagnitude < minConstDist))
{
minConstDist = (double)sqrMagnitude;
minConstNode = triangleMeshNode3;
}
}
else if (!triangleMeshNode3.ContainsPoint((VInt3)position))
{
double sqrMagnitude2 = (triangleMeshNode3.position - pos).sqrMagnitude;
if (minNode == null || sqrMagnitude2 < minDist)
{
minDist = sqrMagnitude2;
minNode = triangleMeshNode3;
}
if (sqrMagnitude2 < (double)maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude2 < minConstDist))
{
minConstDist = sqrMagnitude2;
minConstNode = triangleMeshNode3;
}
}
else
{
int num = AstarMath.Abs(triangleMeshNode3.position.y - pos.y);
if (minNode == null || (double)num < minDist)
{
minDist = (double)num;
minNode = triangleMeshNode3;
}
if ((float)num < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (double)num < minConstDist))
{
minConstDist = (double)num;
minConstNode = triangleMeshNode3;
}
}
return(true);
};
graph.GetNodes(del);
NNInfo result = new NNInfo(minNode);
if (result.node != null)
{
TriangleMeshNode triangleMeshNode = result.node as TriangleMeshNode;
Vector3 clampedPosition = triangleMeshNode.ClosestPointOnNode(position);
result.clampedPosition = clampedPosition;
}
result.constrainedNode = minConstNode;
if (result.constrainedNode != null)
{
TriangleMeshNode triangleMeshNode2 = result.constrainedNode as TriangleMeshNode;
Vector3 constClampedPosition = triangleMeshNode2.ClosestPointOnNode(position);
result.constClampedPosition = constClampedPosition;
}
return(result);
}
/** Reset all values to their default values.
*
* \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
* override this function, resetting ALL their variables to enable recycling of paths.
* If this is not done, trying to use that path type for pooling might result in weird behaviour.
* The best way is to reset to default values the variables declared in the extended path type and then
* call this base function in inheriting types with base.Reset ().
*
* \warning This function should not be called manually.
*/
public virtual void Reset () {
if (System.Object.ReferenceEquals (AstarPath.active, null))
throw new System.NullReferenceException ("No AstarPath object found in the scene. " +
"Make sure there is one or do not create paths in Awake");
hasBeenReset = true;
state = (int)PathState.Created;
releasedNotSilent = false;
pathHandler = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = Pathfinding.Util.ListPool<GraphNode>.Claim();
vectorPath = Pathfinding.Util.ListPool<Vector3>.Claim();
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
//calltime
nnConstraint = PathNNConstraint.Default;
next = null;
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
enabledTags = -1;
tagPenalties = null;
callTime = System.DateTime.UtcNow;
pathID = AstarPath.active.GetNextPathID ();
hTarget = Int3.zero;
hTargetNode = null;
}
/** 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) {
return GetNearest(position,constraint,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);
}
}
}
public MeshNode QueryInside(Vector3 p, NNConstraint constraint)
{
return(count != 0 ? SearchBoxInside(0, p, constraint) : null);
}
public NNInfoInternal GetNearest(Vector3 position, NNConstraint constraint)
{
return(this.GetNearest(position, constraint, null));
}
/** Queries the tree for the closest node to \a p constrained by the NNConstraint.
* 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
*
* \see QueryCircle
*/
public NNInfo QueryClosest(Vector3 p, NNConstraint constraint, out float distance)
{
distance = float.PositiveInfinity;
return(QueryClosest(p, constraint, ref distance, new NNInfo(null)));
}
public virtual NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
{
return(this.GetNearest(position, constraint));
}
/// <summary>
/// Returns the nearest node to a position using the specified <see cref="NNConstraint">constraint</see>.
/// </summary>
/// <param name="position">
/// A <see cref="Vector3"/>
/// </param>
/// <param name="constraint">
/// A <see cref="NNConstraint"/>
/// </param>
/// <returns>
/// A <see cref="NNInfo"/>. This function will only return an empty NNInfo if there is no nodes which comply with the specified constraint.
/// </returns>
public virtual NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
return(GetNearest(position, constraint));
//Debug.LogError ("This should not be called if not GetNearest has been overriden, and if GetNearest has been overriden, you should override this function too, always return a node which returns true when passed to constraint.Suitable (node)");
//return new NNInfo ();
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (nodes == null)
{
return(new NNInfo());
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
if (optimizeForSparseGraph)
{
Int3 lookupStart = WorldToLookupSpace((Int3)position);
Int3 size = lookupStart - minLookup;
int mw = 0;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
size = lookupStart - maxLookup;
mw = System.Math.Max(mw, System.Math.Abs(size.x));
mw = System.Math.Max(mw, System.Math.Abs(size.y));
mw = System.Math.Max(mw, System.Math.Abs(size.z));
var searcher = new GetNearestHelper(position, maxDistSqr, constraint, nodeLookup);
searcher.Search(lookupStart);
for (int w = 1; w <= mw; w++)
{
if (w >= 20)
{
Debug.LogWarning("Aborting GetNearest call at maximum distance because it has iterated too many times.\n" +
"If you get this regularly, check your settings for PointGraph -> <b>Optimize For Sparse Graph</b> and " +
"PointGraph -> <b>Optimize For 2D</b>.\nThis happens when the closest node was very far away (20*link distance between nodes). " +
"When optimizing for sparse graphs, getting the nearest node from far away positions is <b>very slow</b>.\n");
break;
}
if (lookupCellSize.y == 0)
{
Int3 reference = lookupStart + new Int3(-w, 0, -w);
for (int x = 0; x <= 2 * w; x++)
{
searcher.Search(reference + new Int3(x, 0, 0));
searcher.Search(reference + new Int3(x, 0, 2 * w));
}
for (int z = 1; z < 2 * w; z++)
{
searcher.Search(reference + new Int3(0, 0, z));
searcher.Search(reference + new Int3(2 * w, 0, z));
}
}
else
{
Int3 reference = lookupStart + new Int3(-w, -w, -w);
for (int x = 0; x <= 2 * w; x++)
{
for (int y = 0; y <= 2 * w; y++)
{
searcher.Search(reference + new Int3(x, y, 0));
searcher.Search(reference + new Int3(x, y, 2 * w));
}
}
for (int z = 1; z < 2 * w; z++)
{
for (int y = 0; y <= 2 * w; y++)
{
searcher.Search(reference + new Int3(0, y, z));
searcher.Search(reference + new Int3(2 * w, y, z));
}
}
for (int x = 1; x < 2 * w; x++)
{
for (int z = 1; z < 2 * w; z++)
{
searcher.Search(reference + new Int3(x, 0, z));
searcher.Search(reference + new Int3(x, 2 * w, z));
}
}
}
minConstNode = searcher.minConstNode;
minNode = searcher.minNode;
minDist = searcher.minDist;
minConstDist = searcher.minConstDist;
if (minConstNode != null)
{
// Only search one more layer
mw = System.Math.Min(mw, w + 1);
}
}
}
else
{
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minNode = node;
}
if (constraint == null || (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
}
}
var nnInfo = new NNInfo(minNode);
nnInfo.constrainedNode = minConstNode;
if (minConstNode != null)
{
nnInfo.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null)
{
nnInfo.constrainedNode = minNode;
nnInfo.constClampedPosition = (Vector3)minNode.position;
}
return(nnInfo);
}
/// <summary>
/// Returns the nearest node to a position using the specified <see cref="NNConstraint">constraint</see>.
/// </summary>
/// <param name="position">
/// A <see cref="Vector3"/>
/// </param>
/// <param name="constraint">
/// A <see cref="NNConstraint"/>
/// </param>
/// <returns>
/// A <see cref="NNInfo"/>. This function will only return an empty NNInfo if there is no nodes which comply with the specified constraint.
/// </returns>
public virtual NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
return GetNearest (position, constraint);
//Debug.LogError ("This should not be called if not GetNearest has been overriden, and if GetNearest has been overriden, you should override this function too, always return a node which returns true when passed to constraint.Suitable (node)");
//return new NNInfo ();
}
private void GetNearestConnectionInternal(int index, Int3 point, NNConstraint constraint, ref GraphNode best,
ref long bestSqrDist, long distanceThresholdOffset)
{
var data = tree[index].data;
if (data != null)
{
var pointv3 = (UnityEngine.Vector3)point;
for (var i = tree[index].count - 1; i >= 0; i--)
{
var dist = (data[i].position - point).sqrMagnitudeLong;
// Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue
if (dist - distanceThresholdOffset < bestSqrDist &&
(constraint == null || constraint.Suitable(data[i])))
{
// This node may contains the closest connection
// Check all connections
var conns = (data[i] as PointNode).connections;
if (conns != null)
{
var nodePos = (UnityEngine.Vector3)data[i].position;
for (var j = 0; j < conns.Length; j++)
{
// Find the closest point on the connection, but only on this node's side of the connection
// This ensures that we will find the closest node with the closest connection.
var connectionMidpoint =
((UnityEngine.Vector3)conns[j].node.position + nodePos) * 0.5f;
var sqrConnectionDistance =
VectorMath.SqrDistancePointSegment(nodePos, connectionMidpoint, pointv3);
// Convert to Int3 space
var sqrConnectionDistanceInt =
(long)(sqrConnectionDistance * Int3.FloatPrecision * Int3.FloatPrecision);
if (sqrConnectionDistanceInt < bestSqrDist)
{
bestSqrDist = sqrConnectionDistanceInt;
best = data[i];
}
}
}
// Also check if the node itself is close enough.
// This is important if the node has no connections at all.
if (dist < bestSqrDist)
{
bestSqrDist = dist;
best = data[i];
}
}
}
}
else
{
var dist = (long)(point[tree[index].splitAxis] - tree[index].split);
var childIndex = 2 * index + (dist < 0 ? 0 : 1);
GetNearestConnectionInternal(childIndex, point, constraint, ref best, ref bestSqrDist,
distanceThresholdOffset);
// Try the other one if it is possible to find a valid node on the other side
// Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue
if (dist * dist - distanceThresholdOffset < bestSqrDist
) // childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even
{
GetNearestConnectionInternal(childIndex ^ 0x1, point, constraint, ref best, ref bestSqrDist,
distanceThresholdOffset);
}
}
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (this.nodes == null || this.depth * this.width * this.layerCount != this.nodes.Length || this.layerCount == 0)
{
return(default(NNInfo));
}
Vector3 vector = position;
position = this.inverseMatrix.MultiplyPoint3x4(position);
int num = Mathf.Clamp(Mathf.RoundToInt(position.x - 0.5f), 0, this.width - 1);
int num2 = Mathf.Clamp(Mathf.RoundToInt(position.z - 0.5f), 0, this.depth - 1);
float num3 = float.PositiveInfinity;
int num4 = 2;
LevelGridNode levelGridNode = this.GetNearestNode(vector, num, num2, constraint);
if (levelGridNode != null)
{
num3 = ((Vector3)levelGridNode.position - vector).sqrMagnitude;
}
if (levelGridNode != null)
{
if (num4 == 0)
{
return(new NNInfo(levelGridNode));
}
num4--;
}
float num5 = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistance;
float num6 = num5 * num5;
int num7 = 1;
while (true)
{
int i = num2 + num7;
if (this.nodeSize * (float)num7 > num5)
{
break;
}
int j;
for (j = num - num7; j <= num + num7; j++)
{
if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
{
LevelGridNode nearestNode = this.GetNearestNode(vector, j, i, constraint);
if (nearestNode != null)
{
float sqrMagnitude = ((Vector3)nearestNode.position - vector).sqrMagnitude;
if (sqrMagnitude < num3 && sqrMagnitude < num6)
{
num3 = sqrMagnitude;
levelGridNode = nearestNode;
}
}
}
}
i = num2 - num7;
for (j = num - num7; j <= num + num7; j++)
{
if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
{
LevelGridNode nearestNode2 = this.GetNearestNode(vector, j, i, constraint);
if (nearestNode2 != null)
{
float sqrMagnitude2 = ((Vector3)nearestNode2.position - vector).sqrMagnitude;
if (sqrMagnitude2 < num3 && sqrMagnitude2 < num6)
{
num3 = sqrMagnitude2;
levelGridNode = nearestNode2;
}
}
}
}
j = num - num7;
for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++)
{
if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
{
LevelGridNode nearestNode3 = this.GetNearestNode(vector, j, i, constraint);
if (nearestNode3 != null)
{
float sqrMagnitude3 = ((Vector3)nearestNode3.position - vector).sqrMagnitude;
if (sqrMagnitude3 < num3 && sqrMagnitude3 < num6)
{
num3 = sqrMagnitude3;
levelGridNode = nearestNode3;
}
}
}
}
j = num + num7;
for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++)
{
if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
{
LevelGridNode nearestNode4 = this.GetNearestNode(vector, j, i, constraint);
if (nearestNode4 != null)
{
float sqrMagnitude4 = ((Vector3)nearestNode4.position - vector).sqrMagnitude;
if (sqrMagnitude4 < num3 && sqrMagnitude4 < num6)
{
num3 = sqrMagnitude4;
levelGridNode = nearestNode4;
}
}
}
}
if (levelGridNode != null)
{
if (num4 == 0)
{
goto Block_37;
}
num4--;
}
num7++;
}
return(new NNInfo(levelGridNode));
Block_37:
return(new NNInfo(levelGridNode));
}
/** 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 override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint = null)
{
if (nodes == null || depth*width*layerCount != nodes.Length) {
//Debug.LogError ("NavGraph hasn't been generated yet");
return new NNInfo ();
}
position = inverseMatrix.MultiplyPoint3x4 (position);
int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F) , 0, depth-1);
int index = width*z+x;
float minDist = float.PositiveInfinity;
Node minNode = null;
for (int i=0;i<layerCount;i++) {
Node node = nodes[index + width*depth*i];
if (node != null) {
float dist = ((Vector3)node.position - position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
}
}
return new NNInfo(minNode);
}
public static NNInfo GetNearest (INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint) {
if (nodes == null || nodes.Length == 0) {
Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
return new NNInfo ();
}
if (constraint == null) constraint = NNConstraint.None;
return GetNearestForce (nodes,graph.vertices, position, constraint);
}
//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;
}
/** 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;
}
/** This performs a linear search through all polygons returning the closest one.
* This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
* complying with the NNConstraint.
* \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
*/
public static NNInfo GetNearestForceBoth(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
Int3 pos = (Int3)position;
float minDist = -1;
Node minNode = null;
float minConstDist = -1;
Node minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
if (nodes == null || nodes.Length == 0)
{
return(new NNInfo());
}
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (accurateNearestNode)
{
Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
float dist = ((Vector3)pos - closest).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos))
{
float dist = (node.position - pos).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
int dist = Mathfx.Abs(node.position.y - pos.y);
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
}
}
NNInfo nninfo = new NNInfo(minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null)
{
MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null)
{
MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
public static NNInfo GetNearest (NavMeshGraph graph, GraphNode[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
if (nodes == null || nodes.Length == 0) {
Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
return new NNInfo ();
}
if (constraint == null) constraint = NNConstraint.None;
Int3[] vertices = graph.vertices;
//Query BBTree
if (graph.bbTree == null) {
/** \todo Change method to require a navgraph */
return GetNearestForce (graph as NavGraph, graph as INavmeshHolder, position, constraint, accurateNearestNode);
//Debug.LogError ("No Bounding Box Tree has been assigned");
//return new NNInfo ();
}
//Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
float w = (graph.bbTree.root.rect.width + graph.bbTree.root.rect.height)*0.5F*0.02F;
NNInfo query = graph.bbTree.QueryCircle (position,w,constraint);//graph.bbTree.Query (position,constraint);
if (query.node == null) {
for (int i=1;i<=8;i++) {
query = graph.bbTree.QueryCircle (position, i*i*w, constraint);
if (query.node != null || (i-1)*(i-1)*w > AstarPath.active.maxNearestNodeDistance*2) { // *2 for a margin
break;
}
}
}
if (query.node != null) {
query.clampedPosition = ClosestPointOnNode (query.node as TriangleMeshNode,vertices,position);
}
if (query.constrainedNode != null) {
if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) {
query.constrainedNode = null;
} else {
query.constClampedPosition = ClosestPointOnNode (query.constrainedNode as TriangleMeshNode, vertices, position);
}
}
return query;
}
MeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint)
{
BBTreeBox box = arr[boxi];
if (box.node != null)
{
if (box.node.ContainsPoint((Int3)p))
{
//Update the NNInfo
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, Color.red);
#endif
if (constraint == null || constraint.Suitable(box.node))
{
return(box.node);
}
}
else
{
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
}
}
else
{
#if ASTARDEBUG
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMin), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMax), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMin, 0, box.rect.yMax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMax, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white);
#endif
//Search children
MeshNode g;
if (arr[box.left].Contains(p))
{
g = SearchBoxInside(box.left, p, constraint);
if (g != null)
{
return(g);
}
}
if (arr[box.right].Contains(p))
{
g = SearchBoxInside(box.right, p, constraint);
if (g != null)
{
return(g);
}
}
}
return(null);
}
public MeshNode QueryInside(Vector3 p, NNConstraint constraint)
{
return((this.count == 0) ? null : this.SearchBoxInside(0, p, constraint));
}
private void GenerateCourse() {
Vector3 start = _data.Position;
string replot = _isCourseReplot ? "replotting" : "plotting";
D.Log("{0} is {1} course to {2}. Start = {3}, Destination = {4}.", _fleet.FullName, replot, Target.FullName, start, Destination);
//Debug.DrawLine(start, Destination, Color.yellow, 20F, false);
//Path path = new Path(startPosition, targetPosition, null); // Path is now abstract
//Path path = PathPool<ABPath>.GetPath(); // don't know how to assign start and target points
Path path = ABPath.Construct(start, Destination, null);
// Node qualifying constraint instance that checks that nodes are walkable, and within the seeker-specified
// max search distance. Tags and area testing are turned off, primarily because I don't yet understand them
NNConstraint constraint = new NNConstraint();
constraint.constrainTags = false;
path.nnConstraint = constraint;
_seeker.StartPath(path);
// this simple default version uses a constraint that has tags enabled which made finding close nodes problematic
//_seeker.StartPath(startPosition, targetPosition);
}
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);
}
}
/**
* Returns the nearest node to a position using the specified \link Pathfinding.NNConstraint constraint \endlink.
* \returns an NNInfo. This method will only return an empty NNInfo if there are no nodes which comply with the specified constraint.
*/
public virtual NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
return GetNearest (position, constraint);
}
/** Returns the nearest node to a position using the specified NNConstraint.
* \param position The position to try to find a close node to
* \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 NNInfo GetNearest(Vector3 position, NNConstraint constraint)
{
return(GetNearest(position, constraint, null));
}
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, Node hint = null) {
return GetNearest (this, nodes,position, constraint);
}
/**
* Returns the nearest node to a position using the specified \link Pathfinding.NNConstraint constraint \endlink.
* \returns an NNInfo. This method will only return an empty NNInfo if there are no nodes which comply with the specified constraint.
*/
public virtual NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
return(GetNearest(position, constraint));
}
void StartupNode()
{
//Get a random node to update
NNConstraint constr = new NNConstraint();
constr.graphMask = 1<<0;
NNInfo node = AstarPath.active.GetNearest(gameObject.transform.position,constr);
Debug.Log(node.clampedPosition);
AstarPath.RegisterSafeUpdate(delegate()
{
//Move the node a bit
node.clampedPosition = gameObject.transform.position;//+= (Int3)(Random.insideUnitSphere * 0.1f);
//Recalculate the area around the node.
//No functions for updating a single node is available, so we create
//a bounds object which contains that node and only a small volume around it
AstarPath.active.UpdateGraphs(new GraphUpdateObject(new Bounds((Vector3)node.node.position, new Vector3(0.1f, 0.1f, 0.1f))));
//Make sure the above graph update is done now
AstarPath.active.FlushGraphUpdates();
//The update area functions of the built-in graphs generally assumes that the position of the node
//has not changed, so some connection costs might be wrong
node.node.RecalculateConnectionCosts(true);
}, true);
//If you want to make sure the update is called directly instead of
//at the end of this frame or after a few frames.
//AstarPath.active.FlushThreadSafeCallbacks ();
}
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;
nn.distanceXZ = true;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
bool same = true;
{
var info = AstarPath.active.GetNearest(StartTransform.position, nn);
same &= info.node == connectedNode1 && info.node != null;
connectedNode1 = info.node as MeshNode;
clamped1 = info.position;
if (connectedNode1 != null)
{
Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
}
}
{
var info = AstarPath.active.GetNearest(EndTransform.position, nn);
same &= info.node == connectedNode2 && info.node != null;
connectedNode2 = info.node as MeshNode;
clamped2 = info.position;
if (connectedNode2 != null)
{
Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
}
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
startNode.SetPosition((Int3)StartTransform.position);
endNode.SetPosition((Int3)EndTransform.position);
if (same && !forceNewCheck)
{
return;
}
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
Int3 dir = connectedNode2.position - connectedNode1.position;
for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
{
Int3 va1 = connectedNode1.GetVertex(a);
Int3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());
if (Int3.DotLong((va2 - va1).Normal2D(), dir) > 0)
{
continue;
}
for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
{
Int3 vb1 = connectedNode2.GetVertex(b);
Int3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());
if (Int3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
{
continue;
}
if (Int3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
{
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));
if (t2 < t1)
{
Debug.LogError("Something went wrong! " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, how can this happen?");
}
else
{
Vector3 pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
Vector3 pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
return;
}
}
}
}
}
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
return GetNearest (this, nodes,position, constraint, accurateNearestNode);
}
List <Vector3> ApplyGreedy(Path p, List <Vector3> points, System.Func <GraphNode, bool> filter, NNConstraint nnConstraint)
{
bool canBeOriginalNodes = points.Count == p.path.Count;
int startIndex = 0;
while (startIndex < points.Count)
{
Vector3 start = points[startIndex];
var startNode = canBeOriginalNodes && points[startIndex] == (Vector3)p.path[startIndex].position ? p.path[startIndex] : null;
buffer.Add(start);
// Do a binary search to find the furthest node we can see from this node
int mn = 1, mx = 2;
while (true)
{
int endIndex = startIndex + mx;
if (endIndex >= points.Count)
{
mx = points.Count - startIndex;
break;
}
Vector3 end = points[endIndex];
var endNode = canBeOriginalNodes && end == (Vector3)p.path[endIndex].position ? p.path[endIndex] : null;
if (!ValidateLine(startNode, endNode, start, end, filter, nnConstraint))
{
break;
}
mn = mx;
mx *= 2;
}
while (mn + 1 < mx)
{
int mid = (mn + mx) / 2;
int endIndex = startIndex + mid;
Vector3 end = points[endIndex];
var endNode = canBeOriginalNodes && end == (Vector3)p.path[endIndex].position ? p.path[endIndex] : null;
if (ValidateLine(startNode, endNode, start, end, filter, nnConstraint))
{
mn = mid;
}
else
{
mx = mid;
}
}
startIndex += mn;
}
Memory.Swap(ref buffer, ref points);
buffer.ClearFast();
return(points);
}
/** This performs a linear search through all polygons returning the closest one */
public static NNInfo GetNearestForce (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
NNInfo nn = GetNearestForceBoth (graph, navmesh,position,constraint,accurateNearestNode);
nn.node = nn.constrainedNode;
nn.clampedPosition = nn.constClampedPosition;
return nn;
}
List <Vector3> ApplyDP(Path p, List <Vector3> points, System.Func <GraphNode, bool> filter, NNConstraint nnConstraint)
{
if (DPCosts.Length < points.Count)
{
DPCosts = new float[points.Count];
DPParents = new int[points.Count];
}
for (int i = 0; i < DPParents.Length; i++)
{
DPCosts[i] = DPParents[i] = -1;
}
bool canBeOriginalNodes = points.Count == p.path.Count;
for (int i = 0; i < points.Count; i++)
{
float d = DPCosts[i];
Vector3 start = points[i];
var startIsOriginalNode = canBeOriginalNodes && start == (Vector3)p.path[i].position;
for (int j = i + 1; j < points.Count; j++)
{
// Total distance from the start to this point using the best simplified path
// The small additive constant is to make sure that the number of points is kept as small as possible
// even when the total distance is the same (which can happen with e.g multiple colinear points).
float d2 = d + (points[j] - start).magnitude + 0.0001f;
if (DPParents[j] == -1 || d2 < DPCosts[j])
{
var endIsOriginalNode = canBeOriginalNodes && points[j] == (Vector3)p.path[j].position;
if (j == i + 1 || ValidateLine(startIsOriginalNode ? p.path[i] : null, endIsOriginalNode ? p.path[j] : null, start, points[j], filter, nnConstraint))
{
DPCosts[j] = d2;
DPParents[j] = i;
}
else
{
break;
}
}
}
}
int c = points.Count - 1;
while (c != -1)
{
buffer.Add(points[c]);
c = DPParents[c];
}
buffer.Reverse();
Memory.Swap(ref buffer, ref points);
buffer.ClearFast();
return(points);
}
/** 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;
}
/// <summary>
/// Check if a straight path between v1 and v2 is valid.
/// If both n1 and n2 are supplied it is assumed that the line goes from the center of n1 to the center of n2 and a more optimized graph linecast may be done.
/// </summary>
protected bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2, System.Func <GraphNode, bool> filter, NNConstraint nnConstraint)
{
if (useRaycasting)
{
// Use raycasting to check if a straight path between v1 and v2 is valid
if (use2DPhysics)
{
if (thickRaycast && thickRaycastRadius > 0 && Physics2D.CircleCast(v1 + raycastOffset, thickRaycastRadius, v2 - v1, (v2 - v1).magnitude, mask))
{
return(false);
}
if (Physics2D.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask))
{
return(false);
}
}
else
{
// Perform a normal raycast
// This is done even if a thick raycast is also done because thick raycasts do not report collisions for
// colliders that overlapped the (imaginary) sphere at the origin of the thick raycast.
// If this raycast was not done then some obstacles could be missed.
// This is done before the normal raycast for performance.
// Normal raycasts are cheaper, so if it can be used to rule out a line earlier that's good.
if (Physics.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask))
{
return(false);
}
// Perform a thick raycast (if enabled)
if (thickRaycast && thickRaycastRadius > 0)
{
// Sphere cast doesn't detect collisions which are inside the start position of the sphere.
// That's why we do an additional check sphere which is slightly ahead of the start and which will catch most
// of these omissions. It's slightly ahead to avoid false positives that are actuall behind the agent.
if (Physics.CheckSphere(v1 + raycastOffset + (v2 - v1).normalized * thickRaycastRadius, thickRaycastRadius, mask))
{
return(false);
}
if (Physics.SphereCast(new Ray(v1 + raycastOffset, v2 - v1), thickRaycastRadius, (v2 - v1).magnitude, mask))
{
return(false);
}
}
}
}
if (useGraphRaycasting)
{
#if !ASTAR_NO_GRID_GRAPH
bool betweenNodeCenters = n1 != null && n2 != null;
#endif
if (n1 == null)
{
n1 = AstarPath.active.GetNearest(v1, nnConstraint).node;
}
if (n2 == null)
{
n2 = AstarPath.active.GetNearest(v2, nnConstraint).node;
}
if (n1 != null && n2 != null)
{
// Use graph raycasting to check if a straight path between v1 and v2 is valid
NavGraph graph = n1.Graph;
NavGraph graph2 = n2.Graph;
if (graph != graph2)
{
return(false);
}
var rayGraph = graph as IRaycastableGraph;
#if !ASTAR_NO_GRID_GRAPH
GridGraph gg = graph as GridGraph;
if (betweenNodeCenters && gg != null)
{
// If the linecast is exactly between the centers of two nodes on a grid graph then a more optimized linecast can be used.
// This method is also more stable when raycasting along a diagonal when the line just touches an obstacle.
// The normal linecast method may or may not detect that as a hit depending on floating point errors
// however this method never detect it as an obstacle (and that is very good for this component as it improves the simplification).
return(!gg.Linecast(n1 as GridNodeBase, n2 as GridNodeBase, filter));
}
else
#endif
if (rayGraph != null)
{
return(!rayGraph.Linecast(v1, v2, out GraphHitInfo _, null, filter));
}
}
}
return(true);
}
public virtual void Reset()
{
if (object.ReferenceEquals(AstarPath.active, null))
{
throw new NullReferenceException("No AstarPath object found in the scene. Make sure there is one or do not create paths in Awake");
}
this.hasBeenReset = true;
this.state = PathState.Created;
this.releasedNotSilent = false;
this.pathHandler = null;
this.callback = null;
this._errorLog = string.Empty;
this.pathCompleteState = PathCompleteState.NotCalculated;
this.path = ListPool<GraphNode>.Claim();
this.vectorPath = ListPool<Vector3>.Claim();
this.currentR = null;
this.duration = 0f;
this.searchIterations = 0;
this.searchedNodes = 0;
this.nnConstraint = PathNNConstraint.Default;
this.next = null;
this.heuristic = AstarPath.active.heuristic;
this.heuristicScale = AstarPath.active.heuristicScale;
this.enabledTags = -1;
this.tagPenalties = null;
this.callTime = DateTime.UtcNow;
this.pathID = AstarPath.active.GetNextPathID();
this.hTarget = Int3.zero;
this.hTargetNode = null;
}
public override NNInfoInternal GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
return(GetNearestInternal(position, constraint, true));
}
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
if (nodes == null || depth*width*layerCount != nodes.Length) {
//Debug.LogError ("NavGraph hasn't been generated yet");
return new NNInfo ();
}
var graphPosition = inverseMatrix.MultiplyPoint3x4 (position);
int x = Mathf.Clamp (Mathf.RoundToInt (graphPosition.x-0.5F) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (graphPosition.z-0.5F) , 0, depth-1);
var minNode = GetNearestNode (position, x, z, null);
return new NNInfo(minNode);
}
public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
{
return(GetNearestInternal(position, constraint, false));
}
private Node GetNearestNode(Vector3 position, int x, int z, NNConstraint constraint)
{
int index = width*z+x;
float minDist = float.PositiveInfinity;
Node minNode = null;
for (int i=0;i<layerCount;i++) {
Node node = nodes[index + width*depth*i];
if (node != null) {
float dist = ((Vector3)node.position - position).sqrMagnitude;
if (dist < minDist && constraint.Suitable (node)) {
minDist = dist;
minNode = node;
}
}
}
return minNode;
}
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
return(GetNearestForce(position, null));
}
public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
{
if (nodes == null || depth*width*layerCount != nodes.Length || layerCount == 0) {
return new NNInfo ();
}
Vector3 globalPosition = position;
position = inverseMatrix.MultiplyPoint3x4 (position);
int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F) , 0, depth-1);
Node minNode = null;
float minDist = float.PositiveInfinity;
int overlap = getNearestForceOverlap;
minNode = GetNearestNode (globalPosition,x,z,constraint);
if (minNode != null) {
minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
}
/*if (constraint.Suitable (firstBestNode)) {
minNode = firstBestNode;
minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
}*/
if (minNode != null) {
if (overlap == 0) return new NNInfo(minNode);
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++) {
int nx = x;
int nz = z+w;
//int nz2 = nz*width;
//Check if the nodes are within distance limit
if (nodeSize*w > maxDist) {
return new NNInfo(minNode);
}
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
Node node = GetNearestNode (globalPosition, nx,nz, constraint);
if (node != null) {
float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
}
}
nz = z-w;
//nz2 = nz*width;
for (nx = x-w;nx <= x+w;nx++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
Node node = GetNearestNode (globalPosition, nx,nz, constraint);
if (node != null) {
float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
}
}
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;
Node node = GetNearestNode (globalPosition, nx,nz, constraint);
if (node != null) {
float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
}
}
nx = x+w;
for (nz = z-w+1;nz <= z+w-1; nz++) {
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
Node node = GetNearestNode (globalPosition, nx,nz, constraint);
if (node != null) {
float dist = ((Vector3)node.position-globalPosition).sqrMagnitude;
//Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = node; }
}
}
if (minNode != null) {
if (overlap == 0) return new NNInfo(minNode);
else overlap--;
}
}
//return new NNInfo ();
}
public static NNInfo GetNearest(INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
if (nodes == null || nodes.Length == 0)
{
Debug.LogError("NavGraph hasn't been generated yet or does not contain any nodes");
return(new NNInfo());
}
if (constraint == null)
{
constraint = NNConstraint.None;
}
return(GetNearestForceBoth(nodes, graph.vertices, position, NNConstraint.None, accurateNearestNode));
}
/** 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 override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint)
{
return(GetNearest(this, nodes, position, constraint, accurateNearestNode));
}
/** \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 ();
}
/** 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);
}