387 lines
17 KiB
C#
387 lines
17 KiB
C#
using Pathfinding.Sync;
|
|
using Unity.Collections;
|
|
using Unity.Burst;
|
|
using Unity.Jobs;
|
|
using Unity.Mathematics;
|
|
using UnityEngine;
|
|
using UnityEngine.Profiling;
|
|
using Unity.Collections.LowLevel.Unsafe;
|
|
using Unity.Profiling;
|
|
|
|
namespace Pathfinding {
|
|
using Pathfinding.Util;
|
|
using Pathfinding.RVO;
|
|
using Pathfinding.Jobs;
|
|
using Pathfinding.Drawing;
|
|
using Pathfinding.Collections;
|
|
|
|
[BurstCompile]
|
|
public class NavmeshEdges {
|
|
public RVO.SimulatorBurst.ObstacleData obstacleData;
|
|
NativeReference<SpinLock> allocationLock;
|
|
const int JobRecalculateObstaclesBatchCount = 32;
|
|
RWLock rwLock = new RWLock();
|
|
public HierarchicalGraph hierarchicalGraph;
|
|
int gizmoVersion = 0;
|
|
|
|
public void Dispose () {
|
|
// Waits for any jobs to finish
|
|
rwLock.WriteSync().Unlock();
|
|
obstacleData.Dispose();
|
|
allocationLock.Dispose();
|
|
}
|
|
|
|
void Init () {
|
|
obstacleData.Init(Allocator.Persistent);
|
|
if (!allocationLock.IsCreated) allocationLock = new NativeReference<SpinLock>(Allocator.Persistent);
|
|
}
|
|
|
|
public JobHandle RecalculateObstacles (NativeList<int> dirtyHierarchicalNodes, NativeReference<int> numHierarchicalNodes, JobHandle dependency) {
|
|
Init();
|
|
|
|
unsafe {
|
|
// Resize the obstacle data arrays if necessary.
|
|
// We need to do this in a separate single-threaded job before we branch out to multiple threads.
|
|
var writeLock = rwLock.Write();
|
|
var lastJob = new JobResizeObstacles {
|
|
numHierarchicalNodes = numHierarchicalNodes,
|
|
obstacles = obstacleData.obstacles,
|
|
}.Schedule(JobHandle.CombineDependencies(dependency, writeLock.dependency));
|
|
lastJob = new JobCalculateObstacles {
|
|
hGraphGC = hierarchicalGraph.gcHandle,
|
|
obstacleVertices = obstacleData.obstacleVertices,
|
|
obstacleVertexGroups = obstacleData.obstacleVertexGroups,
|
|
obstacles = obstacleData.obstacles.AsDeferredJobArray(),
|
|
bounds = hierarchicalGraph.bounds.AsDeferredJobArray(),
|
|
dirtyHierarchicalNodes = dirtyHierarchicalNodes,
|
|
allocationLock = allocationLock,
|
|
}.ScheduleBatch(JobRecalculateObstaclesBatchCount, 1, lastJob);
|
|
writeLock.UnlockAfter(lastJob);
|
|
gizmoVersion++;
|
|
return lastJob;
|
|
}
|
|
}
|
|
|
|
public void OnDrawGizmos (DrawingData gizmos, RedrawScope redrawScope) {
|
|
if (!obstacleData.obstacleVertices.IsCreated) return;
|
|
|
|
var hasher = new NodeHasher(AstarPath.active);
|
|
hasher.Add(12314127); // Some random constant to avoid hash collisions with other systems
|
|
hasher.Add(gizmoVersion);
|
|
|
|
if (!gizmos.Draw(hasher, redrawScope)) {
|
|
var readLock = rwLock.ReadSync();
|
|
try {
|
|
using (var builder = gizmos.GetBuilder(hasher, redrawScope)) {
|
|
for (int i = 1; i < obstacleData.obstacles.Length; i++) {
|
|
var ob = obstacleData.obstacles[i];
|
|
var vertices = obstacleData.obstacleVertices.GetSpan(ob.verticesAllocation);
|
|
var groups = obstacleData.obstacleVertexGroups.GetSpan(ob.groupsAllocation);
|
|
var vertexOffset = 0;
|
|
for (int g = 0; g < groups.Length; g++) {
|
|
var group = groups[g];
|
|
builder.PushLineWidth(2f);
|
|
for (int j = 0; j < group.vertexCount - 1; j++) {
|
|
builder.ArrowRelativeSizeHead(vertices[vertexOffset + j], vertices[vertexOffset + j + 1], new float3(0, 1, 0), 0.05f, Color.black);
|
|
}
|
|
if (group.type == RVO.ObstacleType.Loop) {
|
|
builder.Arrow(vertices[vertexOffset + group.vertexCount - 1], vertices[vertexOffset], new float3(0, 1, 0), 0.05f, Color.black);
|
|
}
|
|
builder.PopLineWidth();
|
|
vertexOffset += group.vertexCount;
|
|
builder.WireBox(0.5f*(group.boundsMn + group.boundsMx), group.boundsMx - group.boundsMn, Color.white);
|
|
}
|
|
}
|
|
}
|
|
} finally {
|
|
readLock.Unlock();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Obstacle data for navmesh edges.
|
|
///
|
|
/// Can be queried in burst jobs.
|
|
/// </summary>
|
|
public NavmeshBorderData GetNavmeshEdgeData (out RWLock.CombinedReadLockAsync readLock) {
|
|
Init();
|
|
var readLock1 = rwLock.Read();
|
|
var hierarchicalNodeData = hierarchicalGraph.GetHierarhicalNodeData(out var readLock2);
|
|
readLock = new RWLock.CombinedReadLockAsync(readLock1, readLock2);
|
|
return new NavmeshBorderData {
|
|
hierarhicalNodeData = hierarchicalNodeData,
|
|
obstacleData = obstacleData,
|
|
};
|
|
}
|
|
|
|
[BurstCompile]
|
|
struct JobResizeObstacles : IJob {
|
|
public NativeList<UnmanagedObstacle> obstacles;
|
|
public NativeReference<int> numHierarchicalNodes;
|
|
|
|
public void Execute () {
|
|
var prevLength = obstacles.Length;
|
|
var newLength = numHierarchicalNodes.Value;
|
|
obstacles.Resize(newLength, NativeArrayOptions.UninitializedMemory);
|
|
for (int i = prevLength; i < obstacles.Length; i++) obstacles[i] = new RVO.UnmanagedObstacle { verticesAllocation = SlabAllocator<float3>.ZeroLengthArray, groupsAllocation = SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray };
|
|
// First hierarchical node is always invalid
|
|
if (obstacles.Length > 0) obstacles[0] = new RVO.UnmanagedObstacle { verticesAllocation = SlabAllocator<float3>.InvalidAllocation, groupsAllocation = SlabAllocator<ObstacleVertexGroup>.InvalidAllocation };
|
|
}
|
|
}
|
|
|
|
struct JobCalculateObstacles : IJobParallelForBatch {
|
|
public System.Runtime.InteropServices.GCHandle hGraphGC;
|
|
public SlabAllocator<float3> obstacleVertices;
|
|
public SlabAllocator<ObstacleVertexGroup> obstacleVertexGroups;
|
|
[NativeDisableParallelForRestriction]
|
|
public NativeArray<UnmanagedObstacle> obstacles;
|
|
[NativeDisableParallelForRestriction]
|
|
public NativeArray<Bounds> bounds;
|
|
[ReadOnly]
|
|
public NativeList<int> dirtyHierarchicalNodes;
|
|
[NativeDisableParallelForRestriction]
|
|
public NativeReference<SpinLock> allocationLock;
|
|
|
|
public void Execute (int startIndex, int count) {
|
|
var hGraph = hGraphGC.Target as HierarchicalGraph;
|
|
var stepMultiplier = (dirtyHierarchicalNodes.Length + JobRecalculateObstaclesBatchCount - 1) / JobRecalculateObstaclesBatchCount;
|
|
startIndex *= stepMultiplier;
|
|
count *= stepMultiplier;
|
|
var finalIndex = math.min(startIndex + count, dirtyHierarchicalNodes.Length);
|
|
var edges = new NativeList<RVO.RVOObstacleCache.ObstacleSegment>(Allocator.Temp);
|
|
for (int i = startIndex; i < finalIndex; i++) {
|
|
edges.Clear();
|
|
var hNode = dirtyHierarchicalNodes[i];
|
|
UnityEngine.Assertions.Assert.IsTrue(hNode > 0 && hNode < obstacles.Length);
|
|
// These tasks are independent, but they benefit a lot from running at the same time
|
|
// due to cache locality (they use mostly the same data).
|
|
CalculateBoundingBox(hGraph, hNode);
|
|
CalculateObstacles(hGraph, hNode, obstacleVertexGroups, obstacleVertices, obstacles, edges);
|
|
}
|
|
}
|
|
|
|
private static readonly ProfilerMarker MarkerBBox = new ProfilerMarker("HierarchicalBBox");
|
|
private static readonly ProfilerMarker MarkerObstacles = new ProfilerMarker("CalculateObstacles");
|
|
private static readonly ProfilerMarker MarkerCollect = new ProfilerMarker("Collect");
|
|
private static readonly ProfilerMarker MarkerTrace = new ProfilerMarker("Trace");
|
|
|
|
void CalculateBoundingBox (HierarchicalGraph hGraph, int hierarchicalNode) {
|
|
var nodes = hGraph.children[hierarchicalNode];
|
|
MarkerBBox.Begin();
|
|
var b = new Bounds();
|
|
// We know that all nodes in an hierarchical node only belongs to a single graph,
|
|
// so we can branch on the type of the first node, and use optimized code for each node type.
|
|
if (nodes.Count == 0) {
|
|
// NOOP
|
|
} else if (nodes[0] is TriangleMeshNode) {
|
|
var mn = new Int3(int.MaxValue, int.MaxValue, int.MaxValue);
|
|
var mx = new Int3(int.MinValue, int.MinValue, int.MinValue);
|
|
for (int i = 0; i < nodes.Count; i++) {
|
|
var node = nodes[i] as TriangleMeshNode;
|
|
node.GetVertices(out var v0, out var v1, out var v2);
|
|
mn = Int3.Min(Int3.Min(Int3.Min(mn, v0), v1), v2);
|
|
mx = Int3.Max(Int3.Max(Int3.Max(mx, v0), v1), v2);
|
|
}
|
|
b.SetMinMax((Vector3)mn, (Vector3)mx);
|
|
} else {
|
|
var mn = new Int3(int.MaxValue, int.MaxValue, int.MaxValue);
|
|
var mx = new Int3(int.MinValue, int.MinValue, int.MinValue);
|
|
for (int i = 0; i < nodes.Count; i++) {
|
|
var node = nodes[i];
|
|
mn = Int3.Min(mn, node.position);
|
|
mx = Int3.Max(mx, node.position);
|
|
}
|
|
if (nodes[0] is GridNodeBase) {
|
|
float nodeSize;
|
|
if (nodes[0] is LevelGridNode) nodeSize = LevelGridNode.GetGridGraph(nodes[0].GraphIndex).nodeSize;
|
|
else
|
|
nodeSize = GridNode.GetGridGraph(nodes[0].GraphIndex).nodeSize;
|
|
// Grid nodes have a surface. We don't know how it is oriented, so we pad conservatively in all directions.
|
|
// The surface can extend at most nodeSize*sqrt(2)/2 in any direction.
|
|
const float SQRT2_DIV_2 = 0.70710678f;
|
|
var padding = nodeSize*SQRT2_DIV_2*Vector3.one;
|
|
b.SetMinMax((Vector3)mn - padding, (Vector3)mx + padding);
|
|
} else {
|
|
// Point node, or other custom node type
|
|
b.SetMinMax((Vector3)mn, (Vector3)mx);
|
|
}
|
|
}
|
|
bounds[hierarchicalNode] = b;
|
|
MarkerBBox.End();
|
|
}
|
|
|
|
void CalculateObstacles (HierarchicalGraph hGraph, int hierarchicalNode, SlabAllocator<ObstacleVertexGroup> obstacleVertexGroups, SlabAllocator<float3> obstacleVertices, NativeArray<UnmanagedObstacle> obstacles, NativeList<RVO.RVOObstacleCache.ObstacleSegment> edgesScratch) {
|
|
MarkerObstacles.Begin();
|
|
MarkerCollect.Begin();
|
|
RVO.RVOObstacleCache.CollectContours(hGraph.children[hierarchicalNode], edgesScratch);
|
|
MarkerCollect.End();
|
|
var prev = obstacles[hierarchicalNode];
|
|
unsafe {
|
|
ref var allocationLockRef = ref UnsafeUtility.AsRef<SpinLock>(allocationLock.GetUnsafePtr());
|
|
if (prev.groupsAllocation != SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray) {
|
|
unsafe {
|
|
allocationLockRef.Lock();
|
|
obstacleVertices.Free(prev.verticesAllocation);
|
|
obstacleVertexGroups.Free(prev.groupsAllocation);
|
|
allocationLockRef.Unlock();
|
|
}
|
|
}
|
|
unsafe {
|
|
// Find the graph's natural movement plane.
|
|
// This is used to simplify almost colinear segments into a single segment.
|
|
var children = hGraph.children[hierarchicalNode];
|
|
NativeMovementPlane movementPlane;
|
|
bool simplifyObstacles = true;
|
|
if (children.Count > 0) {
|
|
if (children[0] is GridNodeBase) {
|
|
movementPlane = new NativeMovementPlane((children[0].Graph as GridGraph).transform.rotation);
|
|
} else if (children[0] is TriangleMeshNode) {
|
|
var graph = children[0].Graph as NavmeshBase;
|
|
movementPlane = new NativeMovementPlane(graph.transform.rotation);
|
|
// If normal recalculation is disabled, the graph may have very a strange shape, like a spherical world.
|
|
// In that case we should not simplify the obstacles, as there is no well defined movement plane.
|
|
simplifyObstacles = graph.RecalculateNormals;
|
|
} else {
|
|
movementPlane = new NativeMovementPlane(quaternion.identity);
|
|
simplifyObstacles = false;
|
|
}
|
|
} else {
|
|
movementPlane = default;
|
|
}
|
|
MarkerTrace.Begin();
|
|
var edgesSpan = edgesScratch.AsUnsafeSpan();
|
|
RVO.RVOObstacleCache.TraceContours(
|
|
ref edgesSpan,
|
|
ref movementPlane,
|
|
hierarchicalNode,
|
|
(UnmanagedObstacle*)obstacles.GetUnsafePtr(),
|
|
ref obstacleVertices,
|
|
ref obstacleVertexGroups,
|
|
ref allocationLockRef,
|
|
simplifyObstacles
|
|
);
|
|
MarkerTrace.End();
|
|
}
|
|
}
|
|
MarkerObstacles.End();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Burst-accessible data about borders in the navmesh.
|
|
///
|
|
/// Can be queried from burst, and from multiple threads in parallel.
|
|
/// </summary>
|
|
// TODO: Change to a quadtree/kdtree/aabb tree that stored edges as { index: uint10, prev: uint10, next: uint10 }, with a natural max of 1024 vertices per obstacle (hierarchical node). This is fine because hnodes have at most 256 nodes, which cannot create more than 1024 edges.
|
|
public struct NavmeshBorderData {
|
|
public HierarchicalGraph.HierarhicalNodeData hierarhicalNodeData;
|
|
public RVO.SimulatorBurst.ObstacleData obstacleData;
|
|
|
|
/// <summary>
|
|
/// An empty set of edges.
|
|
///
|
|
/// Must be disposed using <see cref="DisposeEmpty"/>.
|
|
/// </summary>
|
|
public static NavmeshBorderData CreateEmpty (Allocator allocator) {
|
|
return new NavmeshBorderData {
|
|
hierarhicalNodeData = new HierarchicalGraph.HierarhicalNodeData {
|
|
connectionAllocator = default,
|
|
connectionAllocations = new NativeList<int>(0, allocator),
|
|
bounds = new NativeList<Bounds>(0, allocator),
|
|
},
|
|
obstacleData = new RVO.SimulatorBurst.ObstacleData {
|
|
obstacleVertexGroups = default,
|
|
obstacleVertices = default,
|
|
obstacles = new NativeList<UnmanagedObstacle>(0, allocator),
|
|
}
|
|
};
|
|
}
|
|
|
|
public void DisposeEmpty (JobHandle dependsOn) {
|
|
if (hierarhicalNodeData.connectionAllocator.IsCreated) throw new System.InvalidOperationException("NavmeshEdgeData was not empty");
|
|
hierarhicalNodeData.connectionAllocations.Dispose(dependsOn);
|
|
hierarhicalNodeData.bounds.Dispose(dependsOn);
|
|
obstacleData.obstacles.Dispose(dependsOn);
|
|
}
|
|
|
|
static void GetHierarchicalNodesInRangeRec (int hierarchicalNode, Bounds bounds, SlabAllocator<int> connectionAllocator, [NoAlias] NativeList<int> connectionAllocations, NativeList<Bounds> nodeBounds, [NoAlias] NativeList<int> indices) {
|
|
indices.Add(hierarchicalNode);
|
|
var conns = connectionAllocator.GetSpan(connectionAllocations[hierarchicalNode]);
|
|
for (int i = 0; i < conns.Length; i++) {
|
|
var neighbour = conns[i];
|
|
if (nodeBounds[neighbour].Intersects(bounds) && !indices.Contains(neighbour)) {
|
|
GetHierarchicalNodesInRangeRec(neighbour, bounds, connectionAllocator, connectionAllocations, nodeBounds, indices);
|
|
}
|
|
}
|
|
}
|
|
|
|
static unsafe void ConvertObstaclesToEdges (ref RVO.SimulatorBurst.ObstacleData obstacleData, NativeList<int> obstacleIndices, Bounds localBounds, NativeList<float2> edgeBuffer, NativeMovementPlane movementPlane) {
|
|
var globalBounds = movementPlane.ToWorld(localBounds);
|
|
var worldToMovementPlane = movementPlane.AsWorldToPlaneMatrix();
|
|
var globalMn = (float3)globalBounds.min;
|
|
var globalMx = (float3)globalBounds.max;
|
|
var localMn = (float3)localBounds.min;
|
|
var localMx = (float3)localBounds.max;
|
|
int vertexCount = 0;
|
|
for (int obstacleIndex = 0; obstacleIndex < obstacleIndices.Length; obstacleIndex++) {
|
|
var obstacle = obstacleData.obstacles[obstacleIndices[obstacleIndex]];
|
|
vertexCount += obstacleData.obstacleVertices.GetSpan(obstacle.verticesAllocation).Length;
|
|
}
|
|
edgeBuffer.ResizeUninitialized(vertexCount*3);
|
|
int edgeVertexOffset = 0;
|
|
for (int obstacleIndex = 0; obstacleIndex < obstacleIndices.Length; obstacleIndex++) {
|
|
var obstacle = obstacleData.obstacles[obstacleIndices[obstacleIndex]];
|
|
if (obstacle.verticesAllocation != SlabAllocator<float3>.ZeroLengthArray) {
|
|
var vertices = obstacleData.obstacleVertices.GetSpan(obstacle.verticesAllocation);
|
|
var groups = obstacleData.obstacleVertexGroups.GetSpan(obstacle.groupsAllocation);
|
|
int offset = 0;
|
|
for (int i = 0; i < groups.Length; i++) {
|
|
var group = groups[i];
|
|
if (!math.all((group.boundsMx >= globalMn) & (group.boundsMn <= globalMx))) {
|
|
offset += group.vertexCount;
|
|
continue;
|
|
}
|
|
|
|
var loop = group.type == RVO.ObstacleType.Loop;
|
|
for (int a = offset + (loop ? group.vertexCount - 1 : 0), b = offset + (loop ? 0 : 1); b < offset + group.vertexCount; a = b, b++) {
|
|
var p1 = vertices[a];
|
|
var p2 = vertices[b];
|
|
var mn = math.min(p1, p2);
|
|
var mx = math.max(p1, p2);
|
|
// Check for intersection with the global bounds (coarse check)
|
|
if (math.all((mx >= globalMn) & (mn <= globalMx))) {
|
|
var p1local = worldToMovementPlane.ToXZPlane(p1);
|
|
var p2local = worldToMovementPlane.ToXZPlane(p2);
|
|
mn = math.min(p1local, p2local);
|
|
mx = math.max(p1local, p2local);
|
|
// Check for intersection with the local bounds (more accurate)
|
|
if (math.all((mx >= localMn) & (mn <= localMx))) {
|
|
edgeBuffer[edgeVertexOffset++] = p1local.xz;
|
|
edgeBuffer[edgeVertexOffset++] = p2local.xz;
|
|
}
|
|
}
|
|
}
|
|
offset += group.vertexCount;
|
|
}
|
|
}
|
|
}
|
|
UnityEngine.Assertions.Assert.IsTrue(edgeVertexOffset <= edgeBuffer.Length);
|
|
edgeBuffer.Length = edgeVertexOffset;
|
|
}
|
|
|
|
public void GetObstaclesInRange (int hierarchicalNode, Bounds bounds, NativeList<int> obstacleIndexBuffer) {
|
|
if (!obstacleData.obstacleVertices.IsCreated) return;
|
|
GetHierarchicalNodesInRangeRec(hierarchicalNode, bounds, hierarhicalNodeData.connectionAllocator, hierarhicalNodeData.connectionAllocations, hierarhicalNodeData.bounds, obstacleIndexBuffer);
|
|
}
|
|
|
|
public void GetEdgesInRange (int hierarchicalNode, Bounds localBounds, NativeList<float2> edgeBuffer, NativeList<int> scratchBuffer, NativeMovementPlane movementPlane) {
|
|
if (!obstacleData.obstacleVertices.IsCreated) return;
|
|
|
|
GetObstaclesInRange(hierarchicalNode, movementPlane.ToWorld(localBounds), scratchBuffer);
|
|
ConvertObstaclesToEdges(ref obstacleData, scratchBuffer, localBounds, edgeBuffer, movementPlane);
|
|
}
|
|
}
|
|
}
|
|
}
|