path: root/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs

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;
					}
				}
			}
		}
	}
}