diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs')
| -rw-r--r-- | Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs | 484 |
1 files changed, 484 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs new file mode 100644 index 0000000..a99fddc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs @@ -0,0 +1,484 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Jobs; +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + using Pathfinding.Util; + using Unity.Collections.LowLevel.Unsafe; + + public struct RasterizationMesh { + public UnsafeSpan<float3> vertices; + + public UnsafeSpan<int> triangles; + + public int area; + + /// <summary>World bounds of the mesh. Assumed to already be multiplied with the matrix</summary> + public Bounds bounds; + + public Matrix4x4 matrix; + + /// <summary> + /// If true then the mesh will be treated as solid and its interior will be unwalkable. + /// The unwalkable region will be the minimum to maximum y coordinate in each cell. + /// </summary> + public bool solid; + + /// <summary>If true, both sides of the mesh will be walkable. If false, only the side that the normal points towards will be walkable</summary> + public bool doubleSided; + + /// <summary>If true, the <see cref="area"/> will be interpreted as a node tag and applied to the final nodes</summary> + public bool areaIsTag; + + /// <summary> + /// If true, the mesh will be flattened to the base of the graph during rasterization. + /// + /// This is intended for rasterizing 2D meshes which always lie in a single plane. + /// + /// This will also cause unwalkable spans have precedence over walkable ones at all times, instead of + /// only when the unwalkable span is sufficiently high up over a walkable span. Since when flattening, + /// "sufficiently high up" makes no sense. + /// </summary> + public bool flatten; + } + + [BurstCompile(CompileSynchronously = true)] + public struct JobVoxelize : IJob { + [ReadOnly] + public NativeArray<RasterizationMesh> inputMeshes; + + [ReadOnly] + public NativeArray<int> bucket; + + /// <summary>Maximum ledge height that is considered to still be traversable. [Limit: >=0] [Units: vx]</summary> + public int voxelWalkableClimb; + + /// <summary> + /// Minimum floor to 'ceiling' height that will still allow the floor area to + /// be considered walkable. [Limit: >= 3] [Units: vx] + /// </summary> + public uint voxelWalkableHeight; + + /// <summary>The xz-plane cell size to use for fields. [Limit: > 0] [Units: wu]</summary> + public float cellSize; + + /// <summary>The y-axis cell size to use for fields. [Limit: > 0] [Units: wu]</summary> + public float cellHeight; + + /// <summary>The maximum slope that is considered walkable. [Limits: 0 <= value < 90] [Units: Degrees]</summary> + public float maxSlope; + + public Matrix4x4 graphTransform; + public Bounds graphSpaceBounds; + public Vector2 graphSpaceLimits; + public LinkedVoxelField voxelArea; + + public void Execute () { + // Transform from voxel space to graph space. + // then scale from voxel space (one unit equals one voxel) + // Finally add min + Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize)); + + // Transform from voxel space to world space + // add half a voxel to fix rounding + var transform = graphTransform * voxelMatrix * Matrix4x4.Translate(new Vector3(0.5f, 0, 0.5f)); + var world2voxelMatrix = transform.inverse; + + // Cosine of the slope limit in voxel space (some tweaks are needed because the voxel space might be stretched out along the y axis) + float slopeLimit = math.cos(math.atan((cellSize/cellHeight)*math.tan(maxSlope*Mathf.Deg2Rad))); + + // Temporary arrays used for rasterization + var clipperOrig = new VoxelPolygonClipper(); + var clipperX1 = new VoxelPolygonClipper(); + var clipperX2 = new VoxelPolygonClipper(); + var clipperZ1 = new VoxelPolygonClipper(); + var clipperZ2 = new VoxelPolygonClipper(); + + // Find the largest lengths of vertex arrays and check for meshes which can be skipped + int maxVerts = 0; + for (int m = 0; m < bucket.Length; m++) { + maxVerts = math.max(inputMeshes[bucket[m]].vertices.Length, maxVerts); + } + + // Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix + var verts = new NativeArray<float3>(maxVerts, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + int width = voxelArea.width; + int depth = voxelArea.depth; + + // These will be width-1 and depth-1 respectively for all but the last tile row and column of the graph + var cropX = Mathf.Min(width - 1, Mathf.CeilToInt((graphSpaceLimits.x - graphSpaceBounds.min.x) / cellSize)); + var cropZ = Mathf.Min(depth - 1, Mathf.CeilToInt((graphSpaceLimits.y - graphSpaceBounds.min.z) / cellSize)); + + // This loop is the hottest place in the whole rasterization process + // it usually accounts for around 50% of the time + for (int m = 0; m < bucket.Length; m++) { + RasterizationMesh mesh = inputMeshes[bucket[m]]; + var meshMatrix = mesh.matrix; + + // Flip the orientation of all faces if the mesh is scaled in such a way + // that the face orientations would change + // This happens for example if a mesh has a negative scale along an odd number of axes + // e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1) + var flipOrientation = VectorMath.ReversesFaceOrientations(meshMatrix); + + var vs = mesh.vertices; + var tris = mesh.triangles; + + // Transform vertices first to world space and then to voxel space + var localToVoxelMatrix = (float4x4)(world2voxelMatrix * mesh.matrix); + for (int i = 0; i < vs.Length; i++) verts[i] = math.transform(localToVoxelMatrix, vs[i]); + + int mesharea = mesh.area; + if (mesh.areaIsTag) { + mesharea |= VoxelUtilityBurst.TagReg; + } + + var meshBounds = new IntRect(); + + for (int i = 0; i < tris.Length; i += 3) { + float3 p1 = verts[tris[i]]; + float3 p2 = verts[tris[i+1]]; + float3 p3 = verts[tris[i+2]]; + + if (flipOrientation) { + var tmp = p1; + p1 = p3; + p3 = tmp; + } + + int minX = (int)math.min(math.min(p1.x, p2.x), p3.x); + int minZ = (int)math.min(math.min(p1.z, p2.z), p3.z); + + int maxX = (int)math.ceil(math.max(math.max(p1.x, p2.x), p3.x)); + int maxZ = (int)math.ceil(math.max(math.max(p1.z, p2.z), p3.z)); + + // Check if the mesh is completely out of bounds + if (minX > cropX || minZ > cropZ || maxX < 0 || maxZ < 0) continue; + + minX = math.clamp(minX, 0, cropX); + maxX = math.clamp(maxX, 0, cropX); + minZ = math.clamp(minZ, 0, cropZ); + maxZ = math.clamp(maxZ, cropZ, cropZ); + + if (i == 0) meshBounds = new IntRect(minX, minZ, minX, minZ); + meshBounds.xmin = math.min(meshBounds.xmin, minX); + meshBounds.xmax = math.max(meshBounds.xmax, maxX); + meshBounds.ymin = math.min(meshBounds.ymin, minZ); + meshBounds.ymax = math.max(meshBounds.ymax, maxZ); + + // Check max slope + float3 normal = math.cross(p2-p1, p3-p1); + float cosSlopeAngle = math.normalizesafe(normal).y; + if (mesh.doubleSided) cosSlopeAngle = math.abs(cosSlopeAngle); + int area = cosSlopeAngle < slopeLimit ? CompactVoxelField.UnwalkableArea : 1 + mesharea; + + clipperOrig[0] = p1; + clipperOrig[1] = p2; + clipperOrig[2] = p3; + clipperOrig.n = 3; + + for (int x = minX; x <= maxX; x++) { + clipperOrig.ClipPolygonAlongX(ref clipperX1, 1f, -x+0.5f); + + if (clipperX1.n < 3) { + continue; + } + + clipperX1.ClipPolygonAlongX(ref clipperX2, -1F, x+0.5F); + + if (clipperX2.n < 3) { + continue; + } + + float clampZ1, clampZ2; + unsafe { + clampZ1 = clampZ2 = clipperX2.z[0]; + for (int q = 1; q < clipperX2.n; q++) { + float val = clipperX2.z[q]; + clampZ1 = math.min(clampZ1, val); + clampZ2 = math.max(clampZ2, val); + } + } + + int clampZ1I = math.clamp((int)math.round(clampZ1), 0, cropX); + int clampZ2I = math.clamp((int)math.round(clampZ2), 0, cropZ); + + for (int z = clampZ1I; z <= clampZ2I; z++) { + clipperX2.ClipPolygonAlongZWithYZ(ref clipperZ1, 1F, -z+0.5F); + + if (clipperZ1.n < 3) { + continue; + } + + clipperZ1.ClipPolygonAlongZWithY(ref clipperZ2, -1F, z+0.5F); + if (clipperZ2.n < 3) { + continue; + } + + + if (mesh.flatten) { + voxelArea.AddFlattenedSpan(z*width+x, area); + } else { + float sMin, sMax; + unsafe { + var u = clipperZ2.y[0]; + sMin = sMax = u; + for (int q = 1; q < clipperZ2.n; q++) { + float val = clipperZ2.y[q]; + sMin = math.min(sMin, val); + sMax = math.max(sMax, val); + } + } + + int maxi = (int)math.ceil(sMax); + // Make sure mini >= 0 + int mini = (int)sMin; + // Make sure the span is at least 1 voxel high + maxi = math.max(mini+1, maxi); + + voxelArea.AddLinkedSpan(z*width+x, mini, maxi, area, voxelWalkableClimb, m); + } + } + } + } + + if (mesh.solid) { + for (int z = meshBounds.ymin; z <= meshBounds.ymax; z++) { + for (int x = meshBounds.xmin; x <= meshBounds.xmax; x++) { + voxelArea.ResolveSolid(z*voxelArea.width + x, m, voxelWalkableClimb); + } + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildCompactField : IJob { + public LinkedVoxelField input; + public CompactVoxelField output; + + public void Execute () { + output.BuildFromLinkedField(input); + } + } + + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildConnections : IJob { + public CompactVoxelField field; + public int voxelWalkableHeight; + public int voxelWalkableClimb; + + public void Execute () { + int wd = field.width*field.depth; + + // Build voxel connections + for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; i++) { + CompactVoxelSpan s = field.spans[i]; + s.con = 0xFFFFFFFF; + + for (int d = 0; d < 4; d++) { + int nx = x+VoxelUtilityBurst.DX[d]; + int nz = z+VoxelUtilityBurst.DZ[d]*field.width; + + if (nx < 0 || nz < 0 || nz >= wd || nx >= field.width) { + continue; + } + + CompactVoxelCell nc = field.cells[nx+nz]; + + for (int k = nc.index, nk = (int)(nc.index+nc.count); k < nk; k++) { + CompactVoxelSpan ns = field.spans[k]; + + int bottom = System.Math.Max(s.y, ns.y); + + int top = System.Math.Min((int)s.y+(int)s.h, (int)ns.y+(int)ns.h); + + if ((top-bottom) >= voxelWalkableHeight && System.Math.Abs((int)ns.y - (int)s.y) <= voxelWalkableClimb) { + uint connIdx = (uint)k - (uint)nc.index; + + if (connIdx > CompactVoxelField.MaxLayers) { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + throw new System.Exception("Too many layers"); +#else + break; +#endif + } + + s.SetConnection(d, connIdx); + break; + } + } + } + + field.spans[i] = s; + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobErodeWalkableArea : IJob { + public CompactVoxelField field; + public int radius; + + public void Execute () { + var distances = new NativeArray<ushort>(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + VoxelUtilityBurst.CalculateDistanceField(field, distances); + + for (int i = 0; i < distances.Length; i++) { + // Note multiplied with 2 because the distance field increments distance by 2 for each voxel (and 3 for diagonal) + if (distances[i] < radius*2) { + field.areaTypes[i] = CompactVoxelField.UnwalkableArea; + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildDistanceField : IJob { + public CompactVoxelField field; + public NativeList<ushort> output; + + public void Execute () { + var distances = new NativeArray<ushort>(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + VoxelUtilityBurst.CalculateDistanceField(field, distances); + + output.ResizeUninitialized(field.spans.Length); + VoxelUtilityBurst.BoxBlur(field, distances, output.AsArray()); + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobFilterLowHeightSpans : IJob { + public LinkedVoxelField field; + public uint voxelWalkableHeight; + + public void Execute () { + int wd = field.width*field.depth; + //Filter all ledges + var spans = field.linkedSpans; + + for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { + for (int x = 0; x < field.width; x++) { + for (int s = z+x; s != -1 && spans[s].bottom != LinkedVoxelField.InvalidSpanValue; s = spans[s].next) { + uint bottom = spans[s].top; + uint top = spans[s].next != -1 ? spans[spans[s].next].bottom : LinkedVoxelField.MaxHeight; + + if (top - bottom < voxelWalkableHeight) { + var span = spans[s]; + span.area = CompactVoxelField.UnwalkableArea; + spans[s] = span; + } + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobFilterLedges : IJob { + public LinkedVoxelField field; + public uint voxelWalkableHeight; + public int voxelWalkableClimb; + public float cellSize; + public float cellHeight; + + // Code almost completely ripped from Recast + public void Execute () { + // Use an UnsafeSpan to be able to use the ref-return values in order to directly assign fields on spans. + var spans = field.linkedSpans.AsUnsafeSpan(); + int wd = field.width*field.depth; + int width = field.width; + + // Filter all ledges + for (int z = 0, pz = 0; z < wd; z += width, pz++) { + for (int x = 0; x < width; x++) { + if (spans[x+z].bottom == LinkedVoxelField.InvalidSpanValue) continue; + + for (int s = x+z; s != -1; s = spans[s].next) { + // Skip non-walkable spans + if (spans[s].area == CompactVoxelField.UnwalkableArea) { + continue; + } + + // Points on the edge of the voxel field will always have at least 1 out-of-bounds neighbour + if (x == 0 || z == 0 || z == (wd-width) || x == (width-1)) { + spans[s].area = CompactVoxelField.UnwalkableArea; + continue; + } + + int bottom = (int)spans[s].top; + int top = spans[s].next != -1 ? (int)spans[spans[s].next].bottom : (int)LinkedVoxelField.MaxHeight; + + // Find neighbours' minimum height. + int minNeighborHeight = (int)LinkedVoxelField.MaxHeight; + + // Min and max height of accessible neighbours. + int accessibleNeighborMinHeight = (int)spans[s].top; + int accessibleNeighborMaxHeight = accessibleNeighborMinHeight; + + for (int d = 0; d < 4; d++) { + int nx = x + VoxelUtilityBurst.DX[d]; + int nz = z + VoxelUtilityBurst.DZ[d]*width; + + int nsx = nx+nz; + + int nbottom = -voxelWalkableClimb; + int ntop = spans[nsx].bottom != LinkedVoxelField.InvalidSpanValue ? (int)spans[nsx].bottom : (int)LinkedVoxelField.MaxHeight; + + // Skip neighbour if the gap between the spans is too small. + if (math.min(top, ntop) - math.max(bottom, nbottom) > voxelWalkableHeight) { + minNeighborHeight = math.min(minNeighborHeight, nbottom - bottom); + } + + // Loop through the rest of the spans + if (spans[nsx].bottom != LinkedVoxelField.InvalidSpanValue) { + for (int ns = nsx; ns != -1; ns = spans[ns].next) { + ref var nSpan = ref spans[ns]; + nbottom = (int)nSpan.top; + + // Break the loop if it is no longer possible for the spans to overlap. + // This is purely a performance optimization + if (nbottom > top - voxelWalkableHeight) break; + + ntop = nSpan.next != -1 ? (int)spans[nSpan.next].bottom : (int)LinkedVoxelField.MaxHeight; + + // Check the overlap of the ranges (bottom,top) and (nbottom,ntop) + // This is the minimum height when moving from the top surface of span #s to the top surface of span #ns + if (math.min(top, ntop) - math.max(bottom, nbottom) > voxelWalkableHeight) { + minNeighborHeight = math.min(minNeighborHeight, nbottom - bottom); + + // Find min/max accessible neighbour height. + if (math.abs(nbottom - bottom) <= voxelWalkableClimb) { + if (nbottom < accessibleNeighborMinHeight) { accessibleNeighborMinHeight = nbottom; } + if (nbottom > accessibleNeighborMaxHeight) { accessibleNeighborMaxHeight = nbottom; } + } + } + } + } + } + + // The current span is close to a ledge if the drop to any + // neighbour span is less than the walkableClimb. + // Additionally, if the difference between all neighbours is too large, + // we are at steep slope: mark the span as ledge. + if (minNeighborHeight < -voxelWalkableClimb || (accessibleNeighborMaxHeight - accessibleNeighborMinHeight) > voxelWalkableClimb) { + spans[s].area = CompactVoxelField.UnwalkableArea; + } + } + } + } + } + } +} |