+ astar project

1 files changed, 99 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs
new file mode 100644
index 0000000..2a2cea4
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs
@@ -0,0 +1,99 @@
+using Unity.Mathematics;
+using Unity.Burst;
+using Pathfinding.Util;
+using Pathfinding.Graphs.Util;
+
+namespace Pathfinding {
+	/// <summary>
+	/// Calculates an estimated cost from the specified point to the target.
+	///
+	/// See: https://en.wikipedia.org/wiki/A*_search_algorithm
+	/// </summary>
+	[BurstCompile]
+	public readonly struct HeuristicObjective {
+		readonly int3 mn;
+		readonly int3 mx;
+		readonly Heuristic heuristic;
+		readonly float heuristicScale;
+		readonly UnsafeSpan<uint> euclideanEmbeddingCosts;
+		readonly uint euclideanEmbeddingPivots;
+		readonly uint targetNodeIndex;
+
+		public HeuristicObjective (int3 point, Heuristic heuristic, float heuristicScale) {
+			this.mn = this.mx = point;
+			this.heuristic = heuristic;
+			this.heuristicScale = heuristicScale;
+			this.euclideanEmbeddingCosts = default;
+			this.euclideanEmbeddingPivots = 0;
+			this.targetNodeIndex = 0;
+		}
+
+		public HeuristicObjective (int3 point, Heuristic heuristic, float heuristicScale, uint targetNodeIndex, EuclideanEmbedding euclideanEmbedding) {
+			this.mn = this.mx = point;
+			this.heuristic = heuristic;
+			this.heuristicScale = heuristicScale;
+			// The euclidean embedding costs are guaranteed to be valid for the duration of the pathfinding request.
+			// We cannot perform checks here, because we may be running in another thread, and Unity does not like that.
+			this.euclideanEmbeddingCosts = euclideanEmbedding != null? euclideanEmbedding.costs.AsUnsafeSpanNoChecks() : default;
+			this.euclideanEmbeddingPivots = euclideanEmbedding != null ? (uint)euclideanEmbedding.pivotCount : 0;
+			this.targetNodeIndex = targetNodeIndex;
+		}
+
+		public HeuristicObjective (int3 mn, int3 mx, Heuristic heuristic, float heuristicScale, uint targetNodeIndex, EuclideanEmbedding euclideanEmbedding) {
+			this.mn = mn;
+			this.mx = mx;
+			this.heuristic = heuristic;
+			this.heuristicScale = heuristicScale;
+			// The euclidean embedding costs are guaranteed to be valid for the duration of the pathfinding request.
+			// We cannot perform checks here, because we may be running in another thread, and Unity does not like that.
+			this.euclideanEmbeddingCosts = euclideanEmbedding != null? euclideanEmbedding.costs.AsUnsafeSpanNoChecks() : default;
+			this.euclideanEmbeddingPivots = euclideanEmbedding != null ? (uint)euclideanEmbedding.pivotCount : 0;
+			this.targetNodeIndex = targetNodeIndex;
+		}
+
+		public int Calculate (int3 point, uint nodeIndex) {
+			return Calculate(in this, ref point, nodeIndex);
+		}
+
+		[BurstCompile]
+		public static int Calculate (in HeuristicObjective objective, ref int3 point, uint nodeIndex) {
+			var closest = math.clamp(point, objective.mn, objective.mx);
+			var diff = point - closest;
+
+			int h;
+			switch (objective.heuristic) {
+			case Heuristic.Euclidean:
+				h = (int)(math.length((float3)diff) * objective.heuristicScale);
+				break;
+			case Heuristic.Manhattan:
+				h = (int)(math.csum(math.abs(diff)) * objective.heuristicScale);
+				break;
+			case Heuristic.DiagonalManhattan:
+				// Octile distance extended to 3D
+				diff = math.abs(diff);
+				var a = diff.x;
+				var b = diff.y;
+				var c = diff.z;
+				// Sort the values so that a <= b <= c
+				if (a > b) Memory.Swap(ref a, ref b);
+				if (b > c) Memory.Swap(ref b, ref c);
+				if (a > b) Memory.Swap(ref a, ref b);
+
+				// This is the same as the Manhattan distance, but with a different weight for the diagonal moves.
+				const float SQRT_3 = 1.7321f;
+				const float SQRT_2 = 1.4142f;
+				h = (int)(objective.heuristicScale * (SQRT_3 * a + SQRT_2 * (b-a) + (c-b-a)));
+				break;
+			case Heuristic.None:
+			default:
+				h = 0;
+				break;
+			}
+
+			if (objective.euclideanEmbeddingPivots > 0) {
+				h = math.max(h, (int)EuclideanEmbedding.GetHeuristic(objective.euclideanEmbeddingCosts, objective.euclideanEmbeddingPivots, nodeIndex, objective.targetNodeIndex));
+			}
+			return h;
+		}
+	}
+}