1 files changed, 219 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs
new file mode 100644
index 0000000..0a14596
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs
@@ -0,0 +1,219 @@
+using UnityEngine;
+using Unity.Burst;
+using Unity.Collections;
+using Unity.Collections.LowLevel.Unsafe;
+using Unity.Jobs;
+using Unity.Mathematics;
+using Pathfinding.Jobs;
+using Pathfinding.Util;
+using System.Data;
+using UnityEngine.Assertions;
+
+namespace Pathfinding.Graphs.Grid.Jobs {
+	/// <summary>
+	/// Calculates the grid connections for all nodes.
+	///
+	/// This is a IJobParallelForBatch job. Calculating the connections in multiple threads is faster,
+	/// but due to hyperthreading (used on most intel processors) the individual threads will become slower.
+	/// It is still worth it though.
+	/// </summary>
+	[BurstCompile(FloatMode = FloatMode.Fast, CompileSynchronously = true)]
+	public struct JobCalculateGridConnections : IJobParallelForBatched {
+		public float maxStepHeight;
+		/// <summary>Normalized up direction</summary>
+		public Vector3 up;
+		public IntBounds bounds;
+		public int3 arrayBounds;
+		public NumNeighbours neighbours;
+		public bool use2D;
+		public bool cutCorners;
+		public bool maxStepUsesSlope;
+		public float characterHeight;
+		public bool layeredDataLayout;
+
+		[ReadOnly]
+		public UnsafeSpan<bool> nodeWalkable;
+
+		[ReadOnly]
+		public UnsafeSpan<float4> nodeNormals;
+
+		[ReadOnly]
+		public UnsafeSpan<Vector3> nodePositions;
+
+		/// <summary>All bitpacked node connections</summary>
+		[WriteOnly]
+		public UnsafeSpan<ulong> nodeConnections;
+
+		public bool allowBoundsChecks => false;
+
+
+		/// <summary>
+		/// Check if a connection to node B is valid.
+		/// Node A is assumed to be walkable already
+		/// </summary>
+		public static bool IsValidConnection (float4 nodePosA, float4 nodeNormalA, bool nodeWalkableB, float4 nodePosB, float4 nodeNormalB, bool maxStepUsesSlope, float maxStepHeight, float4 up) {
+			if (!nodeWalkableB) return false;
+
+			if (!maxStepUsesSlope) {
+				// Check their differences along the Y coordinate (well, the up direction really. It is not necessarily the Y axis).
+				return math.abs(math.dot(up, nodePosB - nodePosA)) <= maxStepHeight;
+			} else {
+				float4 v = nodePosB - nodePosA;
+				float heightDifference = math.dot(v, up);
+
+				// Check if the step is small enough.
+				// This is a fast path for the common case.
+				if (math.abs(heightDifference) <= maxStepHeight) return true;
+
+				float4 v_flat = (v - heightDifference * up) * 0.5f;
+
+				// Math!
+				// Calculates the approximate offset along the up direction
+				// that the ground will have moved at the midpoint between the
+				// nodes compared to the nodes' center points.
+				float NDotU = math.dot(nodeNormalA, up);
+				float offsetA = -math.dot(nodeNormalA - NDotU * up, v_flat);
+
+				NDotU = math.dot(nodeNormalB, up);
+				float offsetB = math.dot(nodeNormalB - NDotU * up, v_flat);
+
+				// Check the height difference with slopes taken into account.
+				// Note that since we also do the heightDifference check above we will ensure slope offsets do not increase the height difference.
+				// If we allowed this then some connections might not be valid near the start of steep slopes.
+				return math.abs(heightDifference + offsetB - offsetA) <= maxStepHeight;
+			}
+		}
+
+		public void Execute (int start, int count) {
+			if (layeredDataLayout) ExecuteLayered(start, count);
+			else ExecuteFlat(start, count);
+		}
+
+		public void ExecuteFlat (int start, int count) {
+			if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity;
+
+			float4 up = new float4(this.up.x, this.up.y, this.up.z, 0);
+
+			NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i];
+			var nodePositions = this.nodePositions.Reinterpret<float3>();
+
+			// The loop is parallelized over z coordinates
+			start += bounds.min.z;
+			for (int z = start; z < start + count; z++) {
+				var initialConnections = 0xFF;
+
+				// Disable connections to out-of-bounds nodes
+				// See GridNode.HasConnectionInDirection
+				if (z == 0) initialConnections &= ~((1 << 0) | (1 << 7) | (1 << 4));
+				if (z == arrayBounds.z - 1) initialConnections &= ~((1 << 2) | (1 << 5) | (1 << 6));
+
+				for (int x = bounds.min.x; x < bounds.max.x; x++) {
+					int nodeIndex = z * arrayBounds.x + x;
+					if (!nodeWalkable[nodeIndex]) {
+						nodeConnections[nodeIndex] = 0;
+						continue;
+					}
+
+					// Bitpacked connections
+					// bit 0 is set if connection 0 is enabled
+					// bit 1 is set if connection 1 is enabled etc.
+					int conns = initialConnections;
+
+					// Disable connections to out-of-bounds nodes
+					if (x == 0) conns &= ~((1 << 3) | (1 << 6) | (1 << 7));
+					if (x == arrayBounds.x - 1) conns &= ~((1 << 1) | (1 << 4) | (1 << 5));
+
+					float4 pos = new float4(nodePositions[nodeIndex], 0);
+					float4 normal = nodeNormals[nodeIndex];
+
+					for (int i = 0; i < 8; i++) {
+						int neighbourIndex = nodeIndex + neighbourOffsets[i];
+						if ((conns & (1 << i)) != 0 && !IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) {
+							// Enable connection i
+							conns &= ~(1 << i);
+						}
+					}
+
+					nodeConnections[nodeIndex] = (ulong)GridNode.FilterDiagonalConnections(conns, neighbours, cutCorners);
+				}
+			}
+		}
+
+		public void ExecuteLayered (int start, int count) {
+			if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity;
+
+			float4 up = new float4(this.up.x, this.up.y, this.up.z, 0);
+
+			NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i];
+
+			var layerStride = arrayBounds.z*arrayBounds.x;
+			start += bounds.min.z;
+			for (int y = bounds.min.y; y < bounds.max.y; y++) {
+				// The loop is parallelized over z coordinates
+				for (int z = start; z < start + count; z++) {
+					for (int x = bounds.min.x; x < bounds.max.x; x++) {
+						// Bitpacked connections
+						ulong conns = 0;
+						int nodeIndexXZ = z * arrayBounds.x + x;
+						int nodeIndex = nodeIndexXZ + y * layerStride;
+						float4 pos = new float4(nodePositions[nodeIndex], 0);
+						float4 normal = nodeNormals[nodeIndex];
+
+						if (nodeWalkable[nodeIndex]) {
+							var ourY = math.dot(up, pos);
+
+							float ourHeight;
+							if (y == arrayBounds.y-1 || !math.any(nodeNormals[nodeIndex + layerStride])) {
+								ourHeight = float.PositiveInfinity;
+							} else {
+								var nodeAboveNeighbourPos = new float4(nodePositions[nodeIndex + layerStride], 0);
+								ourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - ourY);
+							}
+
+							for (int i = 0; i < 8; i++) {
+								int nx = x + GridGraph.neighbourXOffsets[i];
+								int nz = z + GridGraph.neighbourZOffsets[i];
+
+								// Check if the new position is inside the grid
+								int conn = LevelGridNode.NoConnection;
+								if (nx >= 0 && nz >= 0 && nx < arrayBounds.x && nz < arrayBounds.z) {
+									int neighbourStartIndex = nodeIndexXZ + neighbourOffsets[i];
+									for (int y2 = 0; y2 < arrayBounds.y; y2++) {
+										var neighbourIndex = neighbourStartIndex + y2 * layerStride;
+										float4 nodePosB = new float4(nodePositions[neighbourIndex], 0);
+										var neighbourY = math.dot(up, nodePosB);
+										// Is there a node above this one
+										float neighbourHeight;
+										if (y2 == arrayBounds.y-1 || !math.any(nodeNormals[neighbourIndex + layerStride])) {
+											neighbourHeight = float.PositiveInfinity;
+										} else {
+											var nodeAboveNeighbourPos = new float4(nodePositions[neighbourIndex + layerStride], 0);
+											neighbourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - neighbourY);
+										}
+
+										float bottom = math.max(neighbourY, ourY);
+										float top = math.min(neighbourY + neighbourHeight, ourY + ourHeight);
+
+										float dist = top-bottom;
+
+										if (dist >= characterHeight && IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) {
+											conn = y2;
+										}
+									}
+								}
+
+								conns |= (ulong)conn << LevelGridNode.ConnectionStride*i;
+							}
+						} else {
+							conns = LevelGridNode.AllConnectionsMask;
+						}
+
+						nodeConnections[nodeIndex] = conns;
+					}
+				}
+			}
+		}
+	}
+}