1 files changed, 76 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs
new file mode 100644
index 0000000..6f62660
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs
@@ -0,0 +1,76 @@
+namespace Pathfinding.Graphs.Grid.Rules {
+	using Pathfinding.Jobs;
+	using Unity.Jobs;
+	using Unity.Collections;
+	using Unity.Burst;
+	using UnityEngine;
+	using Unity.Mathematics;
+
+	/// <summary>
+	/// Applies penalty based on the elevation of the node.
+	///
+	/// This is useful if you for example want to discourage agents from walking high up in mountain regions.
+	///
+	/// The penalty applied is equivalent to:
+	///
+	/// <code>
+	/// penalty = curve.evaluate(Mathf.Clamp01(Mathf.InverseLerp(lower elevation range, upper elevation range, elevation))) * penaltyScale
+	/// </code>
+	///
+	/// [Open online documentation to see images]
+	///
+	/// See: grid-rules (view in online documentation for working links)
+	/// </summary>
+	[Pathfinding.Util.Preserve]
+	public class RuleElevationPenalty : GridGraphRule {
+		public float penaltyScale = 10000;
+		public Vector2 elevationRange = new Vector2(0, 100);
+		public AnimationCurve curve = AnimationCurve.Linear(0, 0, 1, 1);
+		NativeArray<float> elevationToPenalty;
+
+		public override void Register (GridGraphRules rules) {
+			if (!elevationToPenalty.IsCreated) elevationToPenalty = new NativeArray<float>(64, Allocator.Persistent, NativeArrayOptions.UninitializedMemory);
+			for (int i = 0; i < elevationToPenalty.Length; i++) {
+				elevationToPenalty[i] = Mathf.Max(0, penaltyScale * curve.Evaluate(i * 1.0f / (elevationToPenalty.Length - 1)));
+			}
+
+			var clampedElevationRange = new Vector2(math.max(0, elevationRange.x), math.max(1, elevationRange.y));
+			rules.AddJobSystemPass(Pass.BeforeConnections, context => {
+				//var elevationRangeScale = Matrix4x4.TRS(new Vector3(0, -clampedElevationRange.x, 0), Quaternion.identity, new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1));
+				var elevationRangeScale = Matrix4x4.Scale(new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1)) * Matrix4x4.Translate(new Vector3(0, -clampedElevationRange.x, 0));
+				new JobElevationPenalty {
+					elevationToPenalty = elevationToPenalty,
+					nodePositions = context.data.nodes.positions,
+					worldToGraph = elevationRangeScale * context.data.transform.matrix.inverse,
+					penalty = context.data.nodes.penalties,
+				}.Schedule(context.tracker);
+			});
+		}
+
+		public override void DisposeUnmanagedData () {
+			if (elevationToPenalty.IsCreated) elevationToPenalty.Dispose();
+		}
+
+		[BurstCompile(FloatMode = FloatMode.Fast)]
+		public struct JobElevationPenalty : IJob {
+			[ReadOnly]
+			public NativeArray<float> elevationToPenalty;
+
+			[ReadOnly]
+			public NativeArray<Vector3> nodePositions;
+
+			public Matrix4x4 worldToGraph;
+			public NativeArray<uint> penalty;
+
+			public void Execute () {
+				for (int i = 0; i < penalty.Length; i++) {
+					float y = math.clamp(worldToGraph.MultiplyPoint3x4(nodePositions[i]).y, 0, 1) * (elevationToPenalty.Length - 1);
+					int iy = (int)y;
+					float p1 = elevationToPenalty[iy];
+					float p2 = elevationToPenalty[math.min(iy + 1, elevationToPenalty.Length - 1)];
+					penalty[i] += (uint)math.lerp(p1, p2, y - iy);
+				}
+			}
+		}
+	}
+}