+ astar project

1 files changed, 344 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs
new file mode 100644
index 0000000..5261871
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs
@@ -0,0 +1,344 @@
+namespace Pathfinding.RVO {
+	using Pathfinding;
+	using UnityEngine;
+	using Pathfinding.Util;
+	using Unity.Mathematics;
+	using Unity.Collections;
+	using Unity.Jobs;
+	using System.Collections.Generic;
+	using Unity.Burst;
+	using Unity.Profiling;
+	using Pathfinding.Jobs;
+#if MODULE_COLLECTIONS_2_1_0_OR_NEWER
+	using NativeHashMapIntInt = Unity.Collections.NativeHashMap<int, int>;
+#else
+	using NativeHashMapIntInt = Unity.Collections.NativeParallelHashMap<int, int>;
+#endif
+
+	[BurstCompile]
+	public static class RVOObstacleCache {
+		public struct ObstacleSegment {
+			public float3 vertex1;
+			public float3 vertex2;
+			public int vertex1LinkId;
+			public int vertex2LinkId;
+		}
+
+		static ulong HashKey (GraphNode sourceNode, int traversableTags, SimpleMovementPlane movementPlane) {
+			var hash = (ulong)sourceNode.NodeIndex;
+			hash = hash * 786433 ^ (ulong)traversableTags;
+			// The rotation is not particularly important for the obstacle. It's only used
+			// to simplify the output a bit. So we allow similar rotations to share the same hash.
+			const float RotationQuantization = 4;
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.x*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.y*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.z*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.w*RotationQuantization);
+			return hash;
+		}
+
+		/// <summary>
+		/// Collects an unordered list of contour segments based on the given nodes.
+		///
+		/// Note: All nodes must be from the same graph.
+		/// </summary>
+		public static void CollectContours (List<GraphNode> nodes, NativeList<ObstacleSegment> obstacles) {
+			if (nodes.Count == 0) return;
+			if (nodes[0] is TriangleMeshNode) {
+				for (int i = 0; i < nodes.Count; i++) {
+					var tnode = nodes[i] as TriangleMeshNode;
+					var used = 0;
+					if (tnode.connections != null) {
+						for (int j = 0; j < tnode.connections.Length; j++) {
+							var conn = tnode.connections[j];
+							if (conn.isEdgeShared) {
+								used |= 1 << conn.shapeEdge;
+							}
+						}
+					}
+
+					tnode.GetVertices(out var v0, out var v1, out var v2);
+					for (int edgeIndex = 0; edgeIndex < 3; edgeIndex++) {
+						if ((used & (1 << edgeIndex)) == 0) {
+							// This edge is not shared, therefore it's a contour edge
+							Int3 leftVertex, rightVertex;
+							switch (edgeIndex) {
+							case 0:
+								leftVertex = v0;
+								rightVertex = v1;
+								break;
+							case 1:
+								leftVertex = v1;
+								rightVertex = v2;
+								break;
+							case 2:
+							default:
+								leftVertex = v2;
+								rightVertex = v0;
+								break;
+							}
+							var leftVertexHash = leftVertex.GetHashCode();
+							var rightVertexHash = rightVertex.GetHashCode();
+
+							obstacles.Add(new ObstacleSegment {
+								vertex1 = (Vector3)leftVertex,
+								vertex2 = (Vector3)rightVertex,
+								vertex1LinkId = leftVertexHash,
+								vertex2LinkId = rightVertexHash,
+							});
+						}
+					}
+				}
+			} else if (nodes[0] is GridNodeBase) {
+				GridGraph graph;
+				if (nodes[0] is LevelGridNode) graph = LevelGridNode.GetGridGraph(nodes[0].GraphIndex);
+				else
+					graph = GridNode.GetGridGraph(nodes[0].GraphIndex);
+				unsafe {
+					// Offsets from the center of the node to the corners of the node, in world space
+					// Index dir is the offset to the left corner of the edge in direction dir
+					// See GridNodeBase.GetNeighbourAlongDirection for the direction indices
+					Vector3* offsets = stackalloc Vector3[4];
+					for (int dir = 0; dir < 4; dir++) {
+						var dl = (dir + 1) % 4;
+						offsets[dir] = graph.transform.TransformVector(0.5f * new Vector3(GridGraph.neighbourXOffsets[dir] + GridGraph.neighbourXOffsets[dl], 0, GridGraph.neighbourZOffsets[dir] + GridGraph.neighbourZOffsets[dl]));
+					}
+
+					for (int i = 0; i < nodes.Count; i++) {
+						var gnode = nodes[i] as GridNodeBase;
+						if (gnode.HasConnectionsToAllAxisAlignedNeighbours) continue;
+
+						for (int dir = 0; dir < 4; dir++) {
+							if (!gnode.HasConnectionInDirection(dir)) {
+								// ┌─────────┬─────────┐
+								// │         │         │
+								// │   nl1   │   nl2   │     ^
+								// │         │         │     |
+								// ├────────vL─────────┤     dl
+								// │         │#########│
+								// │   node  │#########│     dir->
+								// │         │#########│
+								// ├────────vR─────────┤     dr
+								// │         │         │      |
+								// │   nr1   │   nr2   │      v
+								// │         │         │
+								// └─────────┴─────────┘
+								var dl = (dir + 1) % 4;
+								var dr = (dir - 1 + 4) % 4;
+								var nl1 = gnode.GetNeighbourAlongDirection(dl);
+								var nr1 = gnode.GetNeighbourAlongDirection(dr);
+
+								// All this hashing code looks slow, but really it's not compared to all the memory accesses to get the node data
+								uint leftVertexHash;
+								if (nl1 != null) {
+									var nl2 = nl1.GetNeighbourAlongDirection(dir);
+									if (nl2 != null) {
+										// Outer corner. Uniquely determined by the 3 nodes that touch the corner.
+										var a = gnode.NodeIndex;
+										var b = nl1.NodeIndex;
+										var c = nl2.NodeIndex;
+										// 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);
+										leftVertexHash = math.hash(new uint3(a, b, c));
+									} else {
+										// Straight wall. Uniquely determined by the 2 nodes that touch the corner and the direction to the wall.
+										var a = gnode.NodeIndex;
+										var b = nl1.NodeIndex;
+										// Sort the values so that a <= b
+										if (a > b) Memory.Swap(ref a, ref b);
+										leftVertexHash = math.hash(new uint3(a, b, (uint)dir));
+									}
+								} else {
+									// Inner corner. Uniquely determined by the single node that touches the corner and the direction to it.
+									var diagonalToCorner = dir + 4;
+									leftVertexHash = math.hash(new uint2(gnode.NodeIndex, (uint)diagonalToCorner));
+								}
+
+								uint rightVertexHash;
+								if (nr1 != null) {
+									var nr2 = nr1.GetNeighbourAlongDirection(dir);
+									if (nr2 != null) {
+										// Outer corner. Uniquely determined by the 3 nodes that touch the corner.
+										var a = gnode.NodeIndex;
+										var b = nr1.NodeIndex;
+										var c = nr2.NodeIndex;
+										// 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);
+										rightVertexHash = math.hash(new uint3(a, b, c));
+									} else {
+										// Straight wall. Uniquely determined by the 2 nodes that touch the corner and the direction to the wall.
+										var a = gnode.NodeIndex;
+										var b = nr1.NodeIndex;
+										// Sort the values so that a <= b
+										if (a > b) Memory.Swap(ref a, ref b);
+										rightVertexHash = math.hash(new uint3(a, b, (uint)dir));
+									}
+								} else {
+									// Inner corner. Uniquely determined by the single node that touches the corner and the direction to it.
+									// Note: It's not a typo that we use `dr+4` here and `dir+4` above. They are different directions.
+									var diagonalToCorner = dr + 4;
+									rightVertexHash = math.hash(new uint2(gnode.NodeIndex, (uint)diagonalToCorner));
+								}
+
+								var nodePos = (Vector3)gnode.position;
+								obstacles.Add(new ObstacleSegment {
+									vertex1 = nodePos + offsets[dir], // Left corner. Yes, it should be dir, not dl, as the offsets array already points to the left corners of each segment.
+									vertex2 = nodePos + offsets[dr], // Right corner
+									vertex1LinkId = (int)leftVertexHash,
+									vertex2LinkId = (int)rightVertexHash,
+								});
+							}
+						}
+					}
+				}
+			}
+		}
+
+		private static readonly ProfilerMarker MarkerAllocate = new ProfilerMarker("Allocate");
+
+		/// <summary>Trace contours generated by CollectContours.</summary>
+		/// <param name="obstaclesSpan">Obstacle segments, typically the borders of the navmesh. In no particular order.
+		///                  Each edge must be oriented the same way (e.g. all clockwise, or all counter-clockwise around the obstacles).</param>
+		/// <param name="movementPlane">The movement plane used for simplification. The up direction will be treated as less important for purposes of simplification.</param>
+		/// <param name="obstacleId">The ID of the obstacle to write into the outputObstacles array.</param>
+		/// <param name="outputObstacles">Array to write the obstacle to.</param>
+		/// <param name="verticesAllocator">Allocator to use for the vertices of the obstacle.</param>
+		/// <param name="obstaclesAllocator">Allocator to use for the obstacle metadata.</param>
+		/// <param name="spinLock">Lock to use when allocating from the allocators.</param>
+		/// <param name="simplifyObstacles">If true, the obstacle will be simplified. This means that colinear vertices (when projected onto the movement plane) will be removed.</param>
+		[BurstCompile]
+		public static unsafe void TraceContours (ref UnsafeSpan<ObstacleSegment> obstaclesSpan, ref NativeMovementPlane movementPlane, int obstacleId, UnmanagedObstacle* outputObstacles, ref SlabAllocator<float3> verticesAllocator, ref SlabAllocator<ObstacleVertexGroup> obstaclesAllocator, ref SpinLock spinLock, bool simplifyObstacles) {
+			var obstacles = obstaclesSpan;
+			if (obstacles.Length == 0) {
+				outputObstacles[obstacleId] = new UnmanagedObstacle {
+					verticesAllocation = SlabAllocator<float3>.ZeroLengthArray,
+					groupsAllocation = SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray,
+				};
+				return;
+			}
+
+			MarkerAllocate.Begin();
+			var traceLookup = new NativeHashMapIntInt(obstacles.Length, Unity.Collections.Allocator.Temp);
+			// For each edge: Will be 0 if the segment should be ignored or if it has been visited, 1 if it has not been visited and has an ingoing edge, and 2 if it has not been visited and has no ingoing edge.
+			var priority = new NativeArray<byte>(obstacles.Length, Unity.Collections.Allocator.Temp, Unity.Collections.NativeArrayOptions.UninitializedMemory);
+			MarkerAllocate.End();
+			for (int i = 0; i < obstacles.Length; i++) {
+				// var obstacle = obstacles[i];
+				// Add the edge to the lookup. But if it already exists, ignore it.
+				// That it already exists is very much a special case that can happen if there is
+				// overlapping geometry (typically added by a NavmeshAdd component).
+				// In that case the "outer edge" that we want to trace is kinda undefined, but we will
+				// do our best with it.
+				if (traceLookup.TryAdd(obstacles[i].vertex1LinkId, i)) {
+					priority[i] = 2;
+				} else {
+					priority[i] = 0;
+				}
+			}
+			for (int i = 0; i < obstacles.Length; i++) {
+				if (traceLookup.TryGetValue(obstacles[i].vertex2LinkId, out var other) && priority[other] > 0) {
+					// The other segment has an ingoing edge. This means it cannot be the start of a contour.
+					// Reduce the priority so that we only consider it when looking for cycles.
+					priority[other] = 1;
+				}
+			}
+
+			var outputMetadata = new NativeList<ObstacleVertexGroup>(16, Allocator.Temp);
+			var outputVertices = new NativeList<float3>(16, Allocator.Temp);
+			// We now have a hashmap of directed edges (vertex1 -> vertex2) such that these edges are directed the same (cw or ccw), and "outer edges".
+			// We can now follow these directed edges to trace out the contours of the mesh.
+			var toPlaneMatrix = movementPlane.AsWorldToPlaneMatrix();
+			for (int allowLoops = 0; allowLoops <= 1; allowLoops++) {
+				var minPriority = allowLoops == 1 ? 1 : 2;
+				for (int i = 0; i < obstacles.Length; i++) {
+					if (priority[i] >= minPriority) {
+						int startVertices = outputVertices.Length;
+						outputVertices.Add(obstacles[i].vertex1);
+
+						var lastAdded = obstacles[i].vertex1;
+						var candidateVertex = obstacles[i].vertex2;
+						var index = i;
+						var obstacleType = ObstacleType.Chain;
+						var boundsMn = lastAdded;
+						var boundsMx = lastAdded;
+						while (true) {
+							if (priority[index] == 0) {
+								// This should not happen for a regular navmesh.
+								// But it can happen if there are degenerate edges or overlapping triangles.
+								// In that case we will just stop here
+								break;
+							}
+							priority[index] = 0;
+
+							float3 nextVertex;
+							if (traceLookup.TryGetValue(obstacles[index].vertex2LinkId, out int nextIndex)) {
+								nextVertex = 0.5f * (obstacles[index].vertex2 + obstacles[nextIndex].vertex1);
+							} else {
+								nextVertex = obstacles[index].vertex2;
+								nextIndex = -1;
+							}
+
+							// Try to simplify and see if we even need to add the vertex C.
+							var p1 = lastAdded;
+							var p2 = nextVertex;
+							var p3 = candidateVertex;
+							var d1 = toPlaneMatrix.ToPlane(p2 - p1);
+							var d2 = toPlaneMatrix.ToPlane(p3 - p1);
+							var det = VectorMath.Determinant(d1, d2);
+							if (math.abs(det) < 0.01f && simplifyObstacles) {
+								// We don't need to add candidateVertex. It's just a straight line (p1,p2,p3 are colinear).
+							} else {
+								outputVertices.Add(candidateVertex);
+								boundsMn = math.min(boundsMn, candidateVertex);
+								boundsMx = math.max(boundsMx, candidateVertex);
+								lastAdded = p3;
+							}
+
+							if (nextIndex == i) {
+								// Loop
+								outputVertices[startVertices] = nextVertex;
+								obstacleType = ObstacleType.Loop;
+								break;
+							} else if (nextIndex == -1) {
+								// End of chain
+								outputVertices.Add(nextVertex);
+								boundsMn = math.min(boundsMn, nextVertex);
+								boundsMx = math.max(boundsMx, nextVertex);
+								break;
+							}
+
+							index = nextIndex;
+							candidateVertex = nextVertex;
+						}
+
+						outputMetadata.Add(new ObstacleVertexGroup {
+							type = obstacleType,
+							vertexCount = outputVertices.Length - startVertices,
+							boundsMn = boundsMn,
+							boundsMx = boundsMx,
+						});
+					}
+				}
+			}
+
+			int obstacleSet, vertexSet;
+			if (outputMetadata.Length > 0) {
+				spinLock.Lock();
+				obstacleSet = obstaclesAllocator.Allocate(outputMetadata);
+				vertexSet = verticesAllocator.Allocate(outputVertices);
+				spinLock.Unlock();
+			} else {
+				obstacleSet = SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray;
+				vertexSet = SlabAllocator<float3>.ZeroLengthArray;
+			}
+			outputObstacles[obstacleId] = new UnmanagedObstacle {
+				verticesAllocation = vertexSet,
+				groupsAllocation = obstacleSet,
+			};
+		}
+	}
+}