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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
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;
}
}
}
|
