77 lines
3.0 KiB
C#
77 lines
3.0 KiB
C#
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|