1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
|
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 slope of the surface below the node.
///
/// This is useful if you for example want to discourage agents from walking on steep slopes.
///
/// The penalty applied is equivalent to:
///
/// <code>
/// penalty = curve.evaluate(slope angle in degrees) * penaltyScale
/// </code>
///
/// [Open online documentation to see images]
///
/// See: grid-rules (view in online documentation for working links)
/// </summary>
[Pathfinding.Util.Preserve]
public class RuleAnglePenalty : GridGraphRule {
public float penaltyScale = 10000;
public AnimationCurve curve = AnimationCurve.Linear(0, 0, 90, 1);
NativeArray<float> angleToPenalty;
public override void Register (GridGraphRules rules) {
if (!angleToPenalty.IsCreated) angleToPenalty = new NativeArray<float>(32, Allocator.Persistent, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < angleToPenalty.Length; i++) {
angleToPenalty[i] = Mathf.Max(0, curve.Evaluate(90.0f * i / (angleToPenalty.Length - 1)) * penaltyScale);
}
rules.AddJobSystemPass(Pass.BeforeConnections, context => {
new JobPenaltyAngle {
angleToPenalty = angleToPenalty,
up = context.data.up,
nodeNormals = context.data.nodes.normals,
penalty = context.data.nodes.penalties,
}.Schedule(context.tracker);
});
}
public override void DisposeUnmanagedData () {
if (angleToPenalty.IsCreated) angleToPenalty.Dispose();
}
[BurstCompile(FloatMode = FloatMode.Fast)]
public struct JobPenaltyAngle : IJob {
public Vector3 up;
[ReadOnly]
public NativeArray<float> angleToPenalty;
[ReadOnly]
public NativeArray<float4> nodeNormals;
public NativeArray<uint> penalty;
public void Execute () {
float4 up = new float4(this.up.x, this.up.y, this.up.z, 0);
for (int i = 0; i < penalty.Length; i++) {
float4 normal = nodeNormals[i];
if (math.any(normal)) {
float angle = math.acos(math.dot(normal, up));
// Take the dot product to find out the cosinus of the angle it has
// Add penalty based on the angle from a precalculated array
float x = angle*(angleToPenalty.Length - 1)/math.PI;
int ix = (int)x;
float p1 = angleToPenalty[math.max(ix, 0)];
float p2 = angleToPenalty[math.min(ix + 1, angleToPenalty.Length - 1)];
penalty[i] += (uint)math.lerp(p1, p2, x - ix);
}
}
}
}
}
}
|
