234 lines
9.8 KiB
C#
234 lines
9.8 KiB
C#
using UnityEngine;
|
||
using Unity.Burst;
|
||
using Unity.Collections;
|
||
using Unity.Mathematics;
|
||
using Pathfinding.Jobs;
|
||
using Pathfinding.Util;
|
||
using Pathfinding.Collections;
|
||
|
||
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;
|
||
public float4x4 graphToWorld;
|
||
public IntBounds bounds;
|
||
public int3 arrayBounds;
|
||
public NumNeighbours neighbours;
|
||
public float characterHeight;
|
||
public bool use2D;
|
||
public bool cutCorners;
|
||
public bool maxStepUsesSlope;
|
||
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;
|
||
|
||
public static bool IsValidConnection (float y, float y2, float maxStepHeight) {
|
||
return math.abs(y - y2) <= maxStepHeight;
|
||
}
|
||
|
||
public static bool IsValidConnection (float2 yRange, float2 yRange2, float maxStepHeight, float characterHeight) {
|
||
if (!IsValidConnection(yRange.x, yRange2.x, maxStepHeight)) return false;
|
||
|
||
// Find the overlap between the two spans to check if the character could pass through the vertical gap
|
||
float bottom = math.max(yRange.x, yRange2.x);
|
||
float top = math.min(yRange.y, yRange2.y);
|
||
return top-bottom >= characterHeight;
|
||
}
|
||
|
||
static float ConnectionY (UnsafeSpan<float3> nodePositions, UnsafeSpan<float4> nodeNormals, NativeArray<float4> normalToHeightOffset, int nodeIndex, int dir, float4 up, bool reverse) {
|
||
Unity.Burst.CompilerServices.Hint.Assume(nodeIndex >= 0 && nodeIndex < nodePositions.length);
|
||
Unity.Burst.CompilerServices.Hint.Assume(nodeIndex >= 0 && nodeIndex < nodeNormals.length);
|
||
Unity.Burst.CompilerServices.Hint.Assume(dir >= 0 && dir < normalToHeightOffset.Length);
|
||
float4 pos = new float4(nodePositions[(uint)nodeIndex], 0);
|
||
return math.dot(up, pos) + (reverse ? -1 : 1) * math.dot(nodeNormals[nodeIndex], normalToHeightOffset[dir]);
|
||
}
|
||
|
||
static float2 ConnectionYRange (UnsafeSpan<float3> nodePositions, UnsafeSpan<float4> nodeNormals, NativeArray<float4> normalToHeightOffset, int nodeIndex, int layerStride, int y, int maxY, int dir, float4 up, bool reverse) {
|
||
var floor = ConnectionY(nodePositions, nodeNormals, normalToHeightOffset, nodeIndex, dir, up, reverse);
|
||
|
||
float ceiling;
|
||
var aboveNodeIndex = nodeIndex + layerStride;
|
||
if ((uint)aboveNodeIndex < nodeNormals.length && math.any(nodeNormals[(uint)aboveNodeIndex])) {
|
||
ceiling = ConnectionY(nodePositions, nodeNormals, normalToHeightOffset, aboveNodeIndex, dir, up, reverse);
|
||
} else {
|
||
ceiling = float.PositiveInfinity;
|
||
}
|
||
return new float2(floor, ceiling);
|
||
}
|
||
|
||
static NativeArray<float4> HeightOffsetProjections (float4x4 graphToWorldTranform, bool maxStepUsesSlope) {
|
||
var normalToHeightOffset = new NativeArray<float4>(8, Allocator.Temp, NativeArrayOptions.ClearMemory);
|
||
if (maxStepUsesSlope) {
|
||
for (int dir = 0; dir < normalToHeightOffset.Length; dir++) {
|
||
//
|
||
// |\
|
||
// | \
|
||
// H | \ 1 _ N = Normal
|
||
// | \ _/ α |
|
||
// | α \ _/ |
|
||
// D<---|-----x ---------
|
||
// 1/2 \
|
||
// \
|
||
// .
|
||
//
|
||
// Assume we have a node at x viewed from the side, with a surface normal N.
|
||
// We want to find the height difference (H) between the node, and the point where it touches the adjacent node.
|
||
//
|
||
// This can be calculated as
|
||
// H = tan(α) * 1/2
|
||
//
|
||
// We can approximate this for small angles as:
|
||
// H = tan(α) * 1/2 ≈ sin(α) * 1/2 = N.x * 1/2
|
||
//
|
||
// This approximation is also desirable, because it doesn't allow extremely sloped nodes to connect to nodes arbitrarily far up or down.
|
||
//
|
||
// To calculate N.x, we need to take into account that the whole graph can be rotated, and it is also in 3D space.
|
||
// Instead we calculate it as N.x = -N . D (where . is the dot product, and D = flatDir is the direction to the adjacent node along the ground plane).
|
||
var flatDir = GridGraph.neighbourXOffsets[dir] * graphToWorldTranform.c0.xyz + GridGraph.neighbourZOffsets[dir] * graphToWorldTranform.c2.xyz;
|
||
|
||
// Lastly, we create a linear transform that maps any node normal to H for a given direction
|
||
// dot(normal, normalToHeightOffset[dir]) = H
|
||
normalToHeightOffset[dir] = -new float4(flatDir, 0) * 0.5f;
|
||
}
|
||
}
|
||
return normalToHeightOffset;
|
||
}
|
||
|
||
public void Execute (int start, int count) {
|
||
if (nodePositions.Length != nodeNormals.Length) throw new System.Exception("nodePositions and nodeNormals must have the same length");
|
||
if (nodePositions.Length != nodeWalkable.Length) throw new System.Exception("nodePositions and nodeWalkable must have the same length");
|
||
if (nodePositions.Length != nodeConnections.Length) throw new System.Exception("nodePositions and nodeConnections must have the same length");
|
||
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 = graphToWorld.c1;
|
||
|
||
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>();
|
||
var normalToHeightOffset = HeightOffsetProjections(graphToWorld, maxStepUsesSlope);
|
||
|
||
// 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));
|
||
|
||
for (int dir = 0; dir < 8; dir++) {
|
||
float y = ConnectionY(nodePositions, nodeNormals, normalToHeightOffset, nodeIndex, dir, up, false);
|
||
int neighbourIndex = nodeIndex + neighbourOffsets[dir];
|
||
if ((conns & (1 << dir)) != 0) {
|
||
float y2 = ConnectionY(nodePositions, nodeNormals, normalToHeightOffset, neighbourIndex, dir, up, true);
|
||
if (!nodeWalkable[neighbourIndex] || !IsValidConnection(y, y2, maxStepHeight)) {
|
||
// Disable connection
|
||
conns &= ~(1 << dir);
|
||
}
|
||
}
|
||
}
|
||
|
||
nodeConnections[nodeIndex] = (ulong)GridNode.FilterDiagonalConnections(conns, neighbours, cutCorners);
|
||
}
|
||
}
|
||
}
|
||
|
||
public void ExecuteLayered (int start, int count) {
|
||
if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity;
|
||
|
||
float4 up = graphToWorld.c1;
|
||
|
||
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>();
|
||
var normalToHeightOffset = HeightOffsetProjections(graphToWorld, maxStepUsesSlope);
|
||
|
||
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;
|
||
|
||
if (nodeWalkable[nodeIndex]) {
|
||
for (int dir = 0; dir < 8; dir++) {
|
||
int nx = x + GridGraph.neighbourXOffsets[dir];
|
||
int nz = z + GridGraph.neighbourZOffsets[dir];
|
||
|
||
int conn = LevelGridNode.NoConnection;
|
||
if (nx >= 0 && nz >= 0 && nx < arrayBounds.x && nz < arrayBounds.z) {
|
||
float2 yRange = ConnectionYRange(nodePositions, nodeNormals, normalToHeightOffset, nodeIndex, layerStride, y, arrayBounds.y, dir, up, false);
|
||
|
||
int neighbourStartIndex = nodeIndexXZ + neighbourOffsets[dir];
|
||
for (int y2 = 0; y2 < arrayBounds.y; y2++) {
|
||
var neighbourIndex = neighbourStartIndex + y2 * layerStride;
|
||
if (!nodeWalkable[neighbourIndex]) continue;
|
||
|
||
float2 yRange2 = ConnectionYRange(nodePositions, nodeNormals, normalToHeightOffset, neighbourIndex, layerStride, y2, arrayBounds.y, dir, up, true);
|
||
if (IsValidConnection(yRange, yRange2, maxStepHeight, characterHeight)) {
|
||
conn = y2;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
conns |= (ulong)conn << LevelGridNode.ConnectionStride*dir;
|
||
}
|
||
} else {
|
||
conns = LevelGridNode.AllConnectionsMask;
|
||
}
|
||
|
||
nodeConnections[nodeIndex] = conns;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|