+ astar project

1 files changed, 577 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs
new file mode 100644
index 0000000..fde39e9
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs
@@ -0,0 +1,577 @@
+using System.Collections.Generic;
+using Pathfinding.Drawing;
+using Pathfinding.Util;
+using Unity.Mathematics;
+using UnityEngine;
+using UnityEngine.Profiling;
+
+namespace Pathfinding {
+	/// <summary>
+	/// Simplifies a path on a grid graph using a string pulling algorithm.
+	/// This is based on a paper called "Toward a String-Pulling Approach to Path Smoothing on Grid Graphs",
+	/// with some optimizations as well as fixes for some edge cases that the paper didn't handle.
+	///
+	/// The result is conceptually similar to the well known funnel string pulling algorithm for navmesh graphs
+	/// but it uses a different algorithm.
+	///
+	/// This class is used by the <see cref="FunnelModifier"/> on grid graphs.
+	///
+	/// See: <see cref="Funnel"/>
+	/// See: <see cref="FunnelModifier"/>
+	/// See: article: https://ojs.aaai.org/index.php/SOCS/article/view/18541
+	/// </summary>
+	public static class GridStringPulling {
+		/// <summary>
+		///         Z
+		///         |
+		///         |
+		///
+		///      3     2
+		///       \ | /
+		/// --    - X -    ----- X
+		///       / | \
+		///      0     1
+		///
+		///         |
+		///         |
+		/// </summary>
+		static int2[] directionToCorners = new int2[] {
+			new int2(0, 0),
+			new int2(FixedPrecisionScale, 0),
+			new int2(FixedPrecisionScale, FixedPrecisionScale),
+			new int2(0, FixedPrecisionScale),
+		};
+
+		static long Cross (int2 lhs, int2 rhs) {
+			return (long)lhs.x*(long)rhs.y - (long)lhs.y*(long)rhs.x;
+		}
+
+		static long Dot (int2 a, int2 b) {
+			return (long)a.x*(long)b.x + (long)a.y*(long)b.y;
+		}
+
+		static bool RightOrColinear (int2 a, int2 b, int2 p) {
+			return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0;
+		}
+
+		static int2 Perpendicular (int2 v) {
+			return new int2(-v.y, v.x);
+		}
+
+		struct TriangleBounds {
+			int2 d1, d2, d3;
+			long t1, t2, t3;
+
+			public TriangleBounds(int2 p1, int2 p2, int2 p3) {
+				if (RightOrColinear(p1, p2, p3)) {
+					var tmp = p3;
+					p3 = p1;
+					p1 = tmp;
+				}
+				d1 = Perpendicular(p2 - p1);
+				d2 = Perpendicular(p3 - p2);
+				d3 = Perpendicular(p1 - p3);
+				t1 = Dot(d1, p1);
+				t2 = Dot(d2, p2);
+				t3 = Dot(d3, p3);
+			}
+
+			public bool Contains (int2 p) {
+				return Dot(d1, p) >= t1 && Dot(d2, p) >= t2 && Dot(d3, p) >= t3;
+			}
+		}
+
+		const int FixedPrecisionScale = 1024;
+
+		static int2 ToFixedPrecision (Vector2 p) {
+			return new int2(math.round(new float2(p)*FixedPrecisionScale));
+		}
+
+		static Vector2 FromFixedPrecision (int2 p) {
+			return (Vector2)(((float2)p) * (1.0f/FixedPrecisionScale));
+		}
+
+		/// <summary>Returns which side of the line a - b that p lies on</summary>
+		static Side Side2D (int2 a, int2 b, int2 p) {
+			var s = Cross(b-a, p-a);
+
+			return s > 0 ? Side.Left : (s < 0 ? Side.Right : Side.Colinear);
+		}
+
+		static Unity.Profiling.ProfilerMarker marker1 = new Unity.Profiling.ProfilerMarker("Linecast hit");
+		static Unity.Profiling.ProfilerMarker marker2 = new Unity.Profiling.ProfilerMarker("Linecast success");
+		static Unity.Profiling.ProfilerMarker marker3 = new Unity.Profiling.ProfilerMarker("Trace");
+		static Unity.Profiling.ProfilerMarker marker4 = new Unity.Profiling.ProfilerMarker("Neighbours");
+		static Unity.Profiling.ProfilerMarker marker5 = new Unity.Profiling.ProfilerMarker("Re-evaluate linecast");
+		static Unity.Profiling.ProfilerMarker marker6 = new Unity.Profiling.ProfilerMarker("Init");
+		static Unity.Profiling.ProfilerMarker marker7 = new Unity.Profiling.ProfilerMarker("Initloop");
+
+		/// <summary>
+		/// Intersection length of the given segment with a square of size Int3.Precision centered at nodeCenter.
+		/// The return value is between 0 and sqrt(2).
+		/// </summary>
+		public static float IntersectionLength (int2 nodeCenter, int2 segmentStart, int2 segmentEnd) {
+			// TODO: Calculations can be hoisted
+			var invNormal = math.rcp((float2)(segmentEnd - segmentStart));
+			var normalMagn = math.length((float2)(segmentEnd - segmentStart));
+
+			float tmin = float.NegativeInfinity, tmax = float.PositiveInfinity;
+
+			var normal = segmentEnd - segmentStart;
+			var bmin = nodeCenter; // - new int2(Int3.Precision/2, Int3.Precision/2);
+			var bmax = nodeCenter + new int2(FixedPrecisionScale, FixedPrecisionScale);
+
+			if (normal.x != 0.0) {
+				float tx1 = (bmin.x - segmentStart.x)*invNormal.x;
+				float tx2 = (bmax.x - segmentStart.x)*invNormal.x;
+
+				tmin = math.max(tmin, math.min(tx1, tx2));
+				tmax = math.min(tmax, math.max(tx1, tx2));
+			} else if (segmentStart.x < bmin.x || segmentStart.x > bmax.x) {
+				return 0.0f;
+			}
+
+			if (normal.y != 0.0) {
+				float ty1 = (bmin.y - segmentStart.y)*invNormal.y;
+				float ty2 = (bmax.y - segmentStart.y)*invNormal.y;
+
+				tmin = math.max(tmin, math.min(ty1, ty2));
+				tmax = math.min(tmax, math.max(ty1, ty2));
+			} else if (segmentStart.y < bmin.y || segmentStart.y > bmax.y) {
+				return 0.0f;
+			}
+
+			tmin = math.max(0, tmin);
+			tmax = math.min(1, tmax);
+			return math.max(tmax - tmin, 0)*normalMagn*(1.0f/FixedPrecisionScale);
+		}
+
+		internal static void TestIntersectionLength () {
+			var s = FixedPrecisionScale;
+
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(math.sqrt(2), IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(2*s, 2*s)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(0.0f, IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(0, 0)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(-1*s, s+1), new int2(2*s, s+1)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(2*s, s), new int2(-1*s, s)));
+
+			// All sides of the square should be included
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s), new int2(s+s, s)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s), new int2(s+s, s+s)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s+s), new int2(s, s+s)));
+			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s+s), new int2(s, s)));
+		}
+
+		/// <summary>
+		/// Cost of moving across all the nodes in the list, along the given segment.
+		/// It is assumed that the segment intersects the nodes. Any potentially intersecting nodes that are not part of the list will be ignored.
+		/// </summary>
+		static uint LinecastCost (List<GraphNode> trace, int2 segmentStart, int2 segmentEnd, GridGraph gg, System.Func<GraphNode, uint> traversalCost) {
+			// Check the cost of the segment compared to not taking this "shortcut"
+			uint cost = 0;
+
+			for (int k = 0; k < trace.Count; k++) {
+				var node = trace[k] as GridNodeBase;
+				// Note: this assumes the default grid connection costs are used. Which is relatively reasonable
+				// since they require changing the code to modify.
+				cost += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid)*FixedPrecisionScale, segmentStart, segmentEnd));
+			}
+			return cost;
+		}
+
+		enum PredicateFailMode {
+			Undefined,
+			Turn,
+			LinecastObstacle,
+			LinecastCost,
+			ReachedEnd,
+		}
+
+		/// <summary>
+		/// Simplifies a path on a grid graph using a string pulling algorithm.
+		/// See the class documentation for more details.
+		/// </summary>
+		/// <param name="pathNodes">A list of input nodes. Only the slice of nodes from nodeStartIndex to nodeEndIndex (inclusive) will be used. These must all be of type GridNodeBase and must form a path (i.e. each node must be a neighbor to the next one in the list).</param>
+		/// <param name="nodeStartIndex">The index in pathNodes to start from.</param>
+		/// <param name="nodeEndIndex">The last index in pathNodes that is used.</param>
+		/// <param name="startPoint">A more exact start point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
+		/// <param name="endPoint">A more exact end point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
+		/// <param name="traversalCost">Can be used to specify how much it costs to traverse each node. If this is null, node penalties and tag penalties will be completely ignored.</param>
+		/// <param name="filter">Can be used to filter out additional nodes that should be treated as unwalkable. It is assumed that all nodes in pathNodes pass this filter.</param>
+		/// <param name="maxCorners">If you only need the first N points of the result, you can specify that here, to avoid unnecessary work.</param>
+		public static List<Vector3> Calculate (List<GraphNode> pathNodes, int nodeStartIndex, int nodeEndIndex, Vector3 startPoint, Vector3 endPoint, System.Func<GraphNode, uint> traversalCost = null, System.Func<GraphNode, bool> filter = null, int maxCorners = int.MaxValue) {
+			Profiler.BeginSample("Funnel");
+			marker6.Begin();
+			// A list of indices into the arrays defined below.
+			// Each index represents a point. But it's more convenient to use indices here and keep all the data separately (also probably faster).
+			var outputPath = ListPool<int>.Claim();
+			outputPath.Add(0);
+
+			var numInputNodes = nodeEndIndex - nodeStartIndex + 1;
+			var gg = pathNodes[nodeStartIndex].Graph as GridGraph;
+			var trace = ListPool<GraphNode>.Claim();
+			var turn = Side.Colinear;
+			int counter = 0;
+
+			// We will add two points, see comments inside the loop.
+			// We may end up adding even more points later, therefore we get arrays that are a bit larger than we need for the initial path.
+			numInputNodes += 2;
+			int numPoints = numInputNodes;
+			var nodes = ArrayPool<GridNodeBase>.Claim(numPoints*2);
+			var points = ArrayPool<int2>.Claim(numPoints*2);
+			var normalizedPoints = ArrayPool<int2>.Claim(numPoints*2);
+			var costs = ArrayPool<uint>.Claim(numPoints*2);
+
+			marker7.Begin();
+			uint costSoFar = 0;
+			// Go through all points and store all relevant data we need about them
+			for (int j = 0; j < numInputNodes; j++) {
+				// After the start-end modifier has adjusted the endpoints of the path, the line from the start point to the center of the second node in the path
+				// might not actually have line of sight.
+				// Assume the path starts at N1 with a diagonal move to node N2.
+				// The start-end modifier adjusts the start point of the path to point S.
+				// This causes the path to cut the corner to the unwalkable node in the bottom right.
+				// ┌─────────┬────────┐
+				// │         │        │
+				// │   N2    │        │
+				// │     \   │        │
+				// │      \  │        │
+				// ├───────\─┼────────┤
+				// │########\│        │
+				// │#########│S  N1   │
+				// │#########│        │
+				// │#########│        │
+				// └─────────┴────────┘
+				// We can solve this case by making the path go from S to N1 and then to N2 instead of directly from S to N2.
+				// We also do the same thing for the end of the path.
+				// The clamping and indexing here is essentially equivalent to one insert at the beginning of the arrays and one at the end.
+				var node = nodes[j] = pathNodes[math.clamp(nodeStartIndex + j-1, nodeStartIndex, nodeEndIndex)] as GridNodeBase;
+				var gridCoordinates = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
+				var point = gridCoordinates * FixedPrecisionScale;
+				int2 normalized;
+				if (j == 0) {
+					normalized = ToFixedPrecision(node.NormalizePoint(startPoint));
+					normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
+				} else if (j == numInputNodes - 1) {
+					normalized = ToFixedPrecision(node.NormalizePoint(endPoint));
+					normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
+				} else {
+					normalized = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2);
+				}
+				points[j] = point + normalized;
+				normalizedPoints[j] = normalized;
+				if (j > 0 && traversalCost != null) {
+					// Calculate the cost of moving along the original path
+					costSoFar += (uint)(((float)traversalCost(nodes[j-1]) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(nodes[j-1].XCoordinateInGrid, nodes[j-1].ZCoordinateInGrid)*FixedPrecisionScale, points[j-1], points[j]));
+					costSoFar += (uint)(((float)traversalCost(nodes[j]) + gg.nodeSize*Int3.Precision) * IntersectionLength(gridCoordinates*FixedPrecisionScale, points[j-1], points[j]));
+				}
+				costs[j] = costSoFar;
+			}
+			marker7.End();
+
+			// We know that there is line of sight from the first point to the second point in the path.
+			var lastSuccessfulStart = 0;
+			var lastSuccessfulEnd = 1;
+			marker6.End();
+
+			int i = 1;
+			while (true) {
+				if (i >= numInputNodes) {
+					// We are done, add the last point
+					outputPath.Add(numInputNodes-1);
+					break;
+				}
+				if (outputPath.Count >= maxCorners) {
+					// We are done with the partial result
+					break;
+				}
+
+				counter++;
+				if (counter > 10000) {
+					Debug.LogError("Inf loop");
+					break;
+				}
+
+				// In the paper, they just use a straight forward loop over the input path.
+				// However, it is better for performance to use a binary search to figure out the next time we need to do something.
+				// We only need an 'i' which succeeds and 'i+1' which fails.
+				// The success in this case is defined by the predicate below. We only need to do stuff if that returns true.
+				var last = outputPath[outputPath.Count-1];
+				var normalizedLast = normalizedPoints[last];
+				var prev = outputPath.Count > 1 ? outputPath[outputPath.Count-2] : -1;
+				var nodeLast = nodes[last];
+				var upperBound = numInputNodes - i - 1;
+
+				// Lower and upper bounds for the binary search
+				int mn = 0;
+				// It is reasonable that most paths can be simplified at least a bit. Assume that seeing 4 or more nodes ahead is common.
+				int mx = math.min(4, upperBound);
+				var mxFailMode = PredicateFailMode.Undefined;
+				uint mxLinecastCost = 0;
+
+				// The calculations are not perfectly accurate. Allow the shortcut's cost to be a tiny bit higher.
+				const uint COST_FUDGE = 5;
+
+				GridHitInfo hit;
+				// First fire off linecasts to nodes exponentially further away until the predicate returns true.
+				while (true) {
+					var idx = i + mx;
+
+					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
+					if (turnPredicate) {
+						mxFailMode = PredicateFailMode.Turn;
+						break;
+					} else {
+						trace.Clear();
+						if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) {
+							mxFailMode = PredicateFailMode.LinecastObstacle;
+							break;
+						} else if (traversalCost != null) {
+							var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost);
+							if (cost > costs[idx] - costs[last] + COST_FUDGE) {
+								// The "shortcut" had such a high penalty that it's not worth taking it
+								mxFailMode = PredicateFailMode.LinecastCost;
+								mxLinecastCost = cost;
+								break;
+							}
+						}
+					}
+
+					if (mx < upperBound) {
+						mn = mx;
+						mx = math.min(mx*2, upperBound);
+					} else {
+						mxFailMode = PredicateFailMode.ReachedEnd;
+						break;
+					}
+				}
+
+				if (mxFailMode == PredicateFailMode.ReachedEnd) {
+					// Reached final node without any hits, we can stop here
+					outputPath.Add(numInputNodes-1);
+					break;
+				}
+
+				// Run a standard binary search
+				while (mx > mn + 1) {
+					int mid = (mn+mx)/2;
+					int idx = i + mid;
+
+					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
+					bool pred = turnPredicate;
+					if (turnPredicate) {
+						mxFailMode = PredicateFailMode.Turn;
+					} else {
+						trace.Clear();
+						if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) {
+							mxFailMode = PredicateFailMode.LinecastObstacle;
+							pred = true;
+						} else if (traversalCost != null) {
+							var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost);
+							if (cost > costs[idx] - costs[last] + COST_FUDGE) {
+								// The "shortcut" had such a high penalty that it's not worth taking it
+								mxFailMode = PredicateFailMode.LinecastCost;
+								mxLinecastCost = cost;
+								pred = true;
+							}
+						}
+					}
+
+					if (pred) {
+						mx = mid;
+					} else {
+						mn = mid;
+					}
+				}
+
+				// i+mn is now a succeeding index, and i+mn+1 (or i+mx) is a failing index
+				if (mn > 0) {
+					lastSuccessfulStart = last;
+					lastSuccessfulEnd = i+mn;
+				} else {
+					// We are not actually completely sure that i+mn is a succeeding index if mn=0
+					// So double check it.
+					// TODO: This is a lot of code duplication. Tidy this up.
+					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[i+mn]) != turn;
+					bool pred = turnPredicate;
+					if (turnPredicate) {
+					} else {
+						trace.Clear();
+						if (gg.Linecast(nodeLast, normalizedLast, nodes[i+mn], normalizedPoints[i+mn], out hit, trace, filter)) {
+							pred = true;
+						} else if (traversalCost != null) {
+							var cost = LinecastCost(trace, points[last], points[i+mn], gg, traversalCost);
+							if (cost > costs[i+mn] - costs[last] + COST_FUDGE) {
+								// The "shortcut" had such a high penalty that it's not worth taking it
+								mxLinecastCost = cost;
+								pred = true;
+							}
+						}
+					}
+
+					if (!pred) {
+						// Success!
+						lastSuccessfulStart = last;
+						lastSuccessfulEnd = i+mn;
+					}
+				}
+
+				// Move to the failing index
+				i += mx;
+				UnityEngine.Assertions.Assert.AreNotEqual(mxFailMode, PredicateFailMode.Undefined);
+
+				marker5.Begin();
+				trace.Clear();
+				trace.Clear();
+				if (mxFailMode == PredicateFailMode.LinecastCost) {
+					outputPath.Add(lastSuccessfulEnd);
+					turn = Side2D(points[last], points[lastSuccessfulEnd], points[i]);
+					// It is guaranteed that there is line of sight from lastSuccessfulStart to lastSuccessfulEnd
+					lastSuccessfulStart = lastSuccessfulEnd;
+					i--;
+					marker5.End();
+					continue;
+				} else if (mxFailMode == PredicateFailMode.LinecastObstacle) {
+					marker5.End();
+					// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.red);
+					marker1.Begin();
+					marker3.Begin();
+					// Re-run a previously successfully linecast to get all nodes it traversed.
+					trace.Clear();
+					int chosenCorner;
+					if (gg.Linecast(nodes[lastSuccessfulStart], normalizedPoints[lastSuccessfulStart], nodes[lastSuccessfulEnd], normalizedPoints[lastSuccessfulEnd], out hit, trace, filter)) {
+						// Weird! This linecast should have succeeded.
+						// Maybe the path crosses some unwalkable nodes it shouldn't cross (the graph could have changed).
+						// Or possibly the linecast implementation doesn't handle some edge case (there are so many!)
+						// In any case, we fall back to just assuming there is a valid line of sight.
+						chosenCorner = lastSuccessfulEnd;
+						Debug.LogError("Inconsistent linecasts");
+					} else {
+						trace.Add(nodes[i]);
+						marker3.End();
+						marker4.Begin();
+
+						GridNodeBase candidateNode = null;
+						var candidateNormalizedPoint = new int2();
+						uint candidateCost = 0;
+						var dirToCandidateCorner = new int2();
+						var lastSuccessfulStartPoint = points[lastSuccessfulStart];
+						var lastSuccessfulEndPoint = points[lastSuccessfulEnd];
+						var dir = lastSuccessfulEndPoint - lastSuccessfulStartPoint;
+						var bounds = new TriangleBounds(
+							lastSuccessfulStartPoint,
+							lastSuccessfulEndPoint,
+							points[i]
+							);
+
+						var desiredSide = System.Math.Sign(Cross(dir, points[i] - lastSuccessfulStartPoint));
+						var candidateCostSoFar = costs[lastSuccessfulStart];
+						for (int j = 0; j < trace.Count; j++) {
+							var node = trace[j] as GridNodeBase;
+							var nodeGridPos = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
+							var nodeCenter = nodeGridPos * FixedPrecisionScale;
+							if (traversalCost != null) {
+								// Not perfectly accurate as it doesn't measure the cost to the exact corner
+								candidateCostSoFar += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(nodeCenter, lastSuccessfulStartPoint, lastSuccessfulEndPoint));
+							}
+							for (int d = 0; d < 4; d++) {
+								if (!node.HasConnectionInDirection(d) || (filter != null && !filter(node.GetNeighbourAlongDirection(d)))) {
+									for (int q = 0; q < 2; q++) {
+										var ncorner = directionToCorners[(d+q)&0x3];
+										var corner = nodeCenter + ncorner;
+
+										if (!bounds.Contains(corner)) {
+											continue;
+										}
+
+										var dirToCorner = corner - lastSuccessfulStartPoint;
+										// We shouldn't pick corners at our current position
+										if (math.all(dirToCorner == 0)) continue;
+										if (math.all(corner == lastSuccessfulEndPoint)) continue;
+
+										var side = Cross(dirToCorner, dirToCandidateCorner);
+										if (candidateNode == null || System.Math.Sign(side) == desiredSide || (side == 0 && math.lengthsq(dirToCorner) > math.lengthsq(dirToCandidateCorner))) {
+											dirToCandidateCorner = dirToCorner;
+											candidateNode = node;
+											candidateNormalizedPoint = ncorner;
+											candidateCost = candidateCostSoFar;
+										}
+									}
+								}
+							}
+						}
+						marker4.End();
+
+						if (candidateNode == null) {
+							// Fall back to adding the lastSuccessfulEnd node. We know there's line of sight to that one.
+							chosenCorner = lastSuccessfulEnd;
+						} else {
+							chosenCorner = numPoints;
+							// TODO: Reallocate
+							nodes[numPoints] = candidateNode;
+							normalizedPoints[numPoints] = candidateNormalizedPoint;
+							var gridCoordinates = new int2(candidateNode.XCoordinateInGrid, candidateNode.ZCoordinateInGrid);
+							points[numPoints] = gridCoordinates * FixedPrecisionScale + candidateNormalizedPoint;
+							costs[numPoints] = candidateCost;
+							numPoints++;
+						}
+					}
+
+					outputPath.Add(chosenCorner);
+					turn = Side2D(points[last], points[chosenCorner], points[i]);
+					// It is guaranteed that there is line of sight from lastSuccessfulStart to chosenCorner because of how we choose the corner.
+					lastSuccessfulStart = chosenCorner;
+					i--;
+					marker1.End();
+					continue;
+				} else {
+					marker5.End();
+					marker2.Begin();
+					lastSuccessfulStart = last;
+					lastSuccessfulEnd = i;
+					// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.green);
+					if (outputPath.Count > 1) {
+						var spPrev = outputPath[outputPath.Count-2];
+						var nextTurn = Side2D(points[spPrev], points[last], points[i]);
+						// Check if the string is no longer taut. If it is not we can remove a previous point.
+						if (turn != nextTurn) {
+							// Draw.SphereOutline(nodes[pts[pts.Count-1]].UnNormalizePoint(FromFixedPrecision(normalizedPoints[pts[pts.Count-1]])), 0.05f, Color.black);
+
+							lastSuccessfulStart = outputPath[outputPath.Count-2];
+							lastSuccessfulEnd = outputPath[outputPath.Count-1];
+
+							outputPath.RemoveAt(outputPath.Count-1);
+							if (outputPath.Count > 1) {
+								last = spPrev;
+								spPrev = outputPath[outputPath.Count-2];
+								turn = Side2D(points[spPrev], points[last], points[i]);
+							} else {
+								// TODO: Should be separate value
+								turn = Side.Colinear;
+							}
+							i--;
+							marker2.End();
+							continue;
+						}
+					}
+					marker2.End();
+				}
+			}
+
+			Profiler.EndSample();
+
+			var result = ListPool<Vector3>.Claim(outputPath.Count);
+			for (int j = 0; j < outputPath.Count; j++) {
+				var idx = outputPath[j];
+				result.Add(nodes[idx].UnNormalizePoint(FromFixedPrecision(normalizedPoints[idx])));
+			}
+
+			ArrayPool<GridNodeBase>.Release(ref nodes);
+			ArrayPool<int2>.Release(ref points);
+			ArrayPool<int2>.Release(ref normalizedPoints);
+			ArrayPool<uint>.Release(ref costs);
+			ListPool<int>.Release(ref outputPath);
+			ListPool<GraphNode>.Release(ref trace);
+			return result;
+		}
+	}
+}