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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
|
using UnityEngine;
using System.Collections.Generic;
using Pathfinding.Serialization;
using Pathfinding.Util;
using UnityEngine.Assertions;
namespace Pathfinding {
using System;
using Pathfinding.Drawing;
using Unity.Jobs;
/// <summary>
/// Graph for off-mesh links.
///
/// This is an internal graph type which is used to store off-mesh links.
/// An off-mesh link between two nodes A and B is represented as: <code> A <--> N1 <--> N2 <--> B </code>.
/// where N1 and N2 are two special nodes added to this graph at the exact start and endpoints of the link.
///
/// This graph is not persistent. So it will never be saved to disk and a new one will be created each time the game starts.
///
/// It is also not possible to query for the nearest node in this graph. The <see cref="GetNearest"/> method will always return an empty result.
/// This is by design, as all pathfinding should start on the navmesh, not on an off-mesh link.
///
/// See: <see cref="OffMeshLinks"/>
/// See: <see cref="NodeLink2"/>
/// </summary>
[JsonOptIn]
[Pathfinding.Util.Preserve]
public class LinkGraph : NavGraph {
LinkNode[] nodes = new LinkNode[0];
int nodeCount;
public override bool isScanned => true;
public override bool persistent => false;
public override bool showInInspector => false;
public override int CountNodes() => nodeCount;
protected override void DestroyAllNodes () {
base.DestroyAllNodes();
nodes = new LinkNode[0];
nodeCount = 0;
}
public override void GetNodes (Action<GraphNode> action) {
if (nodes == null) return;
for (int i = 0; i < nodeCount; i++) action(nodes[i]);
}
internal LinkNode AddNode () {
AssertSafeToUpdateGraph();
if (nodeCount >= nodes.Length) {
Memory.Realloc(ref nodes, Mathf.Max(16, nodeCount * 2));
}
nodeCount++;
return nodes[nodeCount-1] = new LinkNode(active) {
nodeInGraphIndex = nodeCount - 1,
GraphIndex = graphIndex,
Walkable = true,
};
}
internal void RemoveNode (LinkNode node) {
if (nodes[node.nodeInGraphIndex] != node) throw new ArgumentException("Node is not in this graph");
// Remove and swap with the last node
nodeCount--;
nodes[node.nodeInGraphIndex] = nodes[nodeCount];
nodes[node.nodeInGraphIndex].nodeInGraphIndex = node.nodeInGraphIndex;
nodes[nodeCount] = null;
node.Destroy();
}
public override float NearestNodeDistanceSqrLowerBound(Vector3 position, NNConstraint constraint = null) => float.PositiveInfinity;
/// <summary>
/// It's not possible to query for the nearest node in a link graph.
/// This method will always return an empty result.
/// </summary>
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, float maxDistanceSqr) => default;
public override void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) {
// We rely on the link components themselves to draw the links
// TODO
base.OnDrawGizmos(gizmos, drawNodes, redrawScope);
}
class LinkGraphUpdatePromise : IGraphUpdatePromise {
public LinkGraph graph;
public void Apply (IGraphUpdateContext ctx) {
// Destroy all previous nodes (if any)
graph.DestroyAllNodes();
}
public IEnumerator<JobHandle> Prepare() => null;
}
protected override IGraphUpdatePromise ScanInternal () => new LinkGraphUpdatePromise { graph = this };
}
public class LinkNode : PointNode {
public OffMeshLinks.OffMeshLinkSource linkSource;
public OffMeshLinks.OffMeshLinkConcrete linkConcrete;
public int nodeInGraphIndex;
public LinkNode() { }
public LinkNode(AstarPath active) : base(active) {}
public override void RemovePartialConnection (GraphNode node) {
linkConcrete.staleConnections = true;
// Mark the link as dirty so that it will be recalculated.
// Ensure that this does not immediately schedule an update.
// Nodes should only be updated during work items and while graphs are scanned,
// and in those cases node links will be refreshed anyway.
// However, this can also trigger when the AstarPath component is being destroyed,
// or when a graph is removed. In those cases, we don't want to schedule an update.
AstarPath.active.offMeshLinks.DirtyNoSchedule(linkSource);
base.RemovePartialConnection(node);
}
public override void Open (Path path, uint pathNodeIndex, uint gScore) {
// Note: Not calling path.OpenCandidateConnectionsToEndNode here, because link nodes should never be the end node of a path
if (connections == null) return;
var pathHandler = (path as IPathInternals).PathHandler;
var pn = pathHandler.pathNodes[pathNodeIndex];
// Check if our parent node was also a link node by checking if it is in the same graph as this node.
// If it is, we are allowed to connect to non-link nodes.
// Otherwise, we are at the start of the link and we must only connect to other link nodes.
// This is to avoid the path going to a link node, and then going directly back to a non-link node
// without actually traversing the link. It would technically be a valid path,
// but it causes confusion for other scripts that look for off-mesh links in the path.
// TODO: Store the other link node as a field to be able to do a more robust check here?
var isEndOfLink = !pathHandler.IsTemporaryNode(pn.parentIndex) && pathHandler.GetNode(pn.parentIndex).GraphIndex == GraphIndex;
var canTraverseNonLinkNodes = isEndOfLink;
for (int i = 0; i < connections.Length; i++) {
GraphNode other = connections[i].node;
if (canTraverseNonLinkNodes == (other.GraphIndex != GraphIndex) && path.CanTraverse(this, other)) {
if (other is PointNode) {
path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position);
} else {
// When connecting to a non-link node, use a special function to open the connection.
// The typical case for this is that we are at the end of an off-mesh link and we are connecting to a navmesh node.
// In that case, this node's position is in the interior of the navmesh node. We let the navmesh node decide how
// that should be handled.
other.OpenAtPoint(path, pathNodeIndex, position, gScore);
}
}
}
}
public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) {
if (path.CanTraverse(this)) {
// Note: Not calling path.OpenCandidateConnectionsToEndNode here, because link nodes should never be the end node of a path
var cost = (uint)(pos - this.position).costMagnitude;
path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, cost, 0, position);
}
}
}
}
|
