GetNearest using ITraversableProvider 4.2.8.patch

From bc65dd6218a3595a2fc80a1fe88b6cf367343b3a Mon Sep 17 00:00:00 2001
From: Wolf Van Herreweghe <Wolf@exiin.com>
Date: Fri, 25 Oct 2019 01:36:05 +0200
Subject: [PATCH] Base Implementation of GetNearest using the
 ITraversableInterface rather than NNConstraint

---
 .../AstarPathfindingProject/Core/AstarPath.cs | 92 +++++++++++++++++++
 .../Generators/Base.cs                        | 54 +++++++++++
 2 files changed, 146 insertions(+)

diff --git a/Assets/AstarPathfindingProject/Core/AstarPath.cs b/Assets/AstarPathfindingProject/Core/AstarPath.cs
index c0b5cd3..8b8f6af 100644
--- a/Assets/AstarPathfindingProject/Core/AstarPath.cs
+++ b/Assets/AstarPathfindingProject/Core/AstarPath.cs
@@ -2128,6 +2128,98 @@ public enum AstarDistribution { WebsiteDownload, AssetStore };
 		return new NNInfo(nearestNode);
 	}
 
+	/// <summary>
+	/// Returns the nearest node to a position using ITraversableProvider interface.
+	/// Searches through all graphs for their nearest nodes to the specified position and picks the closest one.
+	/// The ITraversable can be used to specify constraints on which nodes can be chosen such as only picking walkable nodes.
+	/// See: Utilities for turn-based games -> ITraversableProvider
+	/// </summary>
+	public NNInfo GetNearest (Vector3 position, ITraversalProvider iTraversable) {
+		// Cache property lookup
+		var graphs = this.graphs;
+
+		float minDist = float.PositiveInfinity;
+		NNInfoInternal nearestNode = new NNInfoInternal();
+		int nearestGraph = -1;
+
+		if (graphs != null) {
+			for (int i = 0; i < graphs.Length; i++) {
+				NavGraph graph = graphs[i];
+
+				// Check if this graph should be searched
+				if (graph == null) {
+					continue;
+				}
+
+				NNInfoInternal nnInfo;
+				if (fullGetNearestSearch) {
+					// Slower nearest node search
+					// this will try to find a node which is suitable according to the constraint
+					nnInfo = graph.GetNearestForce(position, iTraversable);
+				} else {
+					// Fast nearest node search
+					// just find a node close to the position without using the constraint that much
+					// (unless that comes essentially 'for free')
+					nnInfo = graph.GetNearest(position, iTraversable);
+				}
+
+				GraphNode node = nnInfo.node;
+
+				// No node found in this graph
+				if (node == null) {
+					continue;
+				}
+
+				// Distance to the closest point on the node from the requested position
+				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 {
+					// Choose the best node found so far
+					if (dist < minDist) {
+						minDist = dist;
+						nearestNode = nnInfo;
+						nearestGraph = i;
+					}
+				}
+			}
+		}
+
+		// No matches found
+		if (nearestGraph == -1) {
+			return new NNInfo();
+		}
+
+		// Check if a constrained node has already been set
+		if (nearestNode.constrainedNode != null) {
+			nearestNode.node = nearestNode.constrainedNode;
+			nearestNode.clampedPosition = nearestNode.constClampedPosition;
+		}
+
+		Path emptyPath = new ConstantPath();
+
+		if (!fullGetNearestSearch && nearestNode.node != null && !iTraversable.CanTraverse(emptyPath, nearestNode.node)) {
+			// Otherwise, perform a check to force the graphs to check for a suitable node
+			NNInfoInternal nnInfo = graphs[nearestGraph].GetNearestForce(position, iTraversable);
+
+			if (nnInfo.node != null) {
+				nearestNode = nnInfo;
+			}
+		}
+
+		if (!iTraversable.CanTraverse(emptyPath, nearestNode.node)) {
+			return new NNInfo();
+		}
+
+		// Convert to NNInfo which doesn't have all the internal fields
+		return new NNInfo(nearestNode);
+	}
+	
 	/// <summary>
 	/// Returns the node closest to the ray (slow).
 	/// Warning: This function is brute-force and very slow, use with caution
diff --git a/Assets/AstarPathfindingProject/Generators/Base.cs b/Assets/AstarPathfindingProject/Generators/Base.cs
index 64db5dc..b3073fb 100644
--- a/Assets/AstarPathfindingProject/Generators/Base.cs
+++ b/Assets/AstarPathfindingProject/Generators/Base.cs
@@ -248,6 +248,52 @@ public abstract class NavGraph : IGraphInternals {
 
 			return nnInfo;
 		}
+		
+		/// <summary>Returns the nearest node to a position using the specified ITraversable.</summary>
+		/// <param name="position">The position to try to find a close node to</param>
+		/// <param name="iTraversable">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.</param>
+		public virtual NNInfoInternal GetNearest (Vector3 position, ITraversalProvider iTraversable) {
+			// This is a default implementation and it is pretty slow
+			// Graphs usually override this to provide faster and more specialised implementations
+
+			float maxDistSqr = AstarPath.active.maxNearestNodeDistanceSqr;
+
+			float minDist = float.PositiveInfinity;
+			GraphNode minNode = null;
+
+			float minConstDist = float.PositiveInfinity;
+			GraphNode minConstNode = null;
+
+			Path emptyPath = new ConstantPath();
+			
+			// 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 && (iTraversable == null || iTraversable.CanTraverse(emptyPath, node))) {
+					minConstDist = dist;
+					minConstNode = node;
+				}
+			});
+
+			var nnInfo = new NNInfoInternal(minNode);
+
+			nnInfo.constrainedNode = minConstNode;
+
+			if (minConstNode != null) {
+				nnInfo.constClampedPosition = (Vector3)minConstNode.position;
+			} else if (minNode != null) {
+				nnInfo.constrainedNode = minNode;
+				nnInfo.constClampedPosition = (Vector3)minNode.position;
+			}
+
+			return nnInfo;
+		}
 
 		/// <summary>
 		/// Returns the nearest node to a position using the specified \link Pathfinding.NNConstraint constraint \endlink.
@@ -256,6 +302,14 @@ public abstract class NavGraph : IGraphInternals {
 		public virtual NNInfoInternal GetNearestForce (Vector3 position, NNConstraint constraint) {
 			return GetNearest(position, constraint);
 		}
+		
+		/// <summary>
+		/// Returns the nearest node to a position using the specified ITraversableProvider .
+		/// Returns: an NNInfo. This method will only return an empty NNInfo if there are no nodes which comply with the specified constraint.
+		/// </summary>
+		public virtual NNInfoInternal GetNearestForce (Vector3 position, ITraversalProvider iTraversable) {
+			return GetNearest(position, iTraversable);
+		}
 
 		/// <summary>
 		/// Function for cleaning up references.