diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels')
14 files changed, 3259 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs new file mode 100644 index 0000000..1d8571e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs @@ -0,0 +1,132 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + /// <summary>Stores a compact voxel field. </summary> + public struct CompactVoxelField : IArenaDisposable { + public const int UnwalkableArea = 0; + public const uint NotConnected = 0x3f; + public readonly int voxelWalkableHeight; + public readonly int width, depth; + public NativeList<CompactVoxelSpan> spans; + public NativeList<CompactVoxelCell> cells; + public NativeList<int> areaTypes; + + /// <summary>Unmotivated variable, but let's clamp the layers at 65535</summary> + public const int MaxLayers = 65535; + + public CompactVoxelField (int width, int depth, int voxelWalkableHeight, Allocator allocator) { + spans = new NativeList<CompactVoxelSpan>(0, allocator); + cells = new NativeList<CompactVoxelCell>(0, allocator); + areaTypes = new NativeList<int>(0, allocator); + this.width = width; + this.depth = depth; + this.voxelWalkableHeight = voxelWalkableHeight; + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(spans); + arena.Add(cells); + arena.Add(areaTypes); + } + + public int GetNeighbourIndex (int index, int direction) { + return index + VoxelUtilityBurst.DX[direction] + VoxelUtilityBurst.DZ[direction] * width; + } + + public void BuildFromLinkedField (LinkedVoxelField field) { + int idx = 0; + + Assert.AreEqual(this.width, field.width); + Assert.AreEqual(this.depth, field.depth); + + int w = field.width; + int d = field.depth; + int wd = w*d; + + int spanCount = field.GetSpanCount(); + spans.Resize(spanCount, NativeArrayOptions.UninitializedMemory); + areaTypes.Resize(spanCount, NativeArrayOptions.UninitializedMemory); + cells.Resize(wd, NativeArrayOptions.UninitializedMemory); + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (this.voxelWalkableHeight >= ushort.MaxValue) { + throw new System.Exception("Too high walkable height to guarantee correctness. Increase voxel height or lower walkable height."); + } +#endif + + var linkedSpans = field.linkedSpans; + for (int z = 0; z < wd; z += w) { + for (int x = 0; x < w; x++) { + int spanIndex = x+z; + if (linkedSpans[spanIndex].bottom == LinkedVoxelField.InvalidSpanValue) { + cells[x+z] = new CompactVoxelCell(0, 0); + continue; + } + + int index = idx; + int count = 0; + + while (spanIndex != -1) { + if (linkedSpans[spanIndex].area != UnwalkableArea) { + int bottom = (int)linkedSpans[spanIndex].top; + int next = linkedSpans[spanIndex].next; + int top = next != -1 ? (int)linkedSpans[next].bottom : LinkedVoxelField.MaxHeightInt; + + // TODO: Why is top-bottom clamped to a ushort range? + spans[idx] = new CompactVoxelSpan((ushort)math.min(bottom, ushort.MaxValue), (uint)math.min(top-bottom, ushort.MaxValue)); + areaTypes[idx] = linkedSpans[spanIndex].area; + idx++; + count++; + } + spanIndex = linkedSpans[spanIndex].next; + } + + cells[x+z] = new CompactVoxelCell(index, count); + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (idx != spanCount) throw new System.Exception("Found span count does not match expected value"); +#endif + } + } + + /// <summary>CompactVoxelCell used for recast graphs.</summary> + public struct CompactVoxelCell { + public int index; + public int count; + + public CompactVoxelCell (int i, int c) { + index = i; + count = c; + } + } + + /// <summary>CompactVoxelSpan used for recast graphs.</summary> + public struct CompactVoxelSpan { + public ushort y; + public uint con; + public uint h; + public int reg; + + public CompactVoxelSpan (ushort bottom, uint height) { + con = 24; + y = bottom; + h = height; + reg = 0; + } + + public void SetConnection (int dir, uint value) { + int shift = dir*6; + + con = (uint)((con & ~(0x3f << shift)) | ((value & 0x3f) << shift)); + } + + public int GetConnection (int dir) { + return ((int)con >> dir*6) & 0x3f; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta new file mode 100644 index 0000000..d833992 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ddc46f5b05337b6ba8eae5dd4906634d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs new file mode 100644 index 0000000..fa2e2cd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs @@ -0,0 +1,295 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + struct CellMinMax { + public int objectID; + public int min; + public int max; + } + + public struct LinkedVoxelField : IArenaDisposable { + public const uint MaxHeight = 65536; + public const int MaxHeightInt = 65536; + /// <summary> + /// Constant for default LinkedVoxelSpan top and bottom values. + /// It is important with the U since ~0 != ~0U + /// This can be used to check if a LinkedVoxelSpan is valid and not just the default span + /// </summary> + public const uint InvalidSpanValue = ~0U; + + /// <summary>Initial estimate on the average number of spans (layers) in the voxel representation. Should be greater or equal to 1</summary> + public const float AvgSpanLayerCountEstimate = 8; + + /// <summary>The width of the field along the x-axis. [Limit: >= 0] [Units: voxels]</summary> + public int width; + + /// <summary>The depth of the field along the z-axis. [Limit: >= 0] [Units: voxels]</summary> + public int depth; + /// <summary>The maximum height coordinate. [Limit: >= 0, <= MaxHeight] [Units: voxels]</summary> + public int height; + public bool flatten; + + public NativeList<LinkedVoxelSpan> linkedSpans; + private NativeList<int> removedStack; + private NativeList<CellMinMax> linkedCellMinMax; + + public LinkedVoxelField (int width, int depth, int height) { + this.width = width; + this.depth = depth; + this.height = height; + this.flatten = true; + linkedSpans = new NativeList<LinkedVoxelSpan>(0, Allocator.Persistent); + removedStack = new NativeList<int>(128, Allocator.Persistent); + linkedCellMinMax = new NativeList<CellMinMax>(0, Allocator.Persistent); + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(linkedSpans); + arena.Add(removedStack); + arena.Add(linkedCellMinMax); + } + + public void ResetLinkedVoxelSpans () { + int len = width * depth; + + LinkedVoxelSpan df = new LinkedVoxelSpan(InvalidSpanValue, InvalidSpanValue, -1, -1); + + linkedSpans.ResizeUninitialized(len); + linkedCellMinMax.Resize(len, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < len; i++) { + linkedSpans[i] = df; + linkedCellMinMax[i] = new CellMinMax { + objectID = -1, + min = 0, + max = 0, + }; + } + removedStack.Clear(); + } + + void PushToSpanRemovedStack (int index) { + removedStack.Add(index); + } + + public int GetSpanCount () { + int count = 0; + + int wd = width*depth; + + for (int x = 0; x < wd; x++) { + for (int s = x; s != -1 && linkedSpans[s].bottom != InvalidSpanValue; s = linkedSpans[s].next) { + count += linkedSpans[s].area != 0 ? 1 : 0; + } + } + return count; + } + + public void ResolveSolid (int index, int objectID, int voxelWalkableClimb) { + var minmax = linkedCellMinMax[index]; + + if (minmax.objectID != objectID) return; + + if (minmax.min < minmax.max - 1) { + // Add a span for the solid part of the object. + // + // This span ends at max-1 (where max is the top of the original object). + // This is to avoid issues when merging spans with different areas. + // Assume we had 3 spans like: + // y=0..5 walkable span from another object, area=2 + // y=9..10 walkable span, area=3 + // and min=0, max=10 for the current object. + // If we added a span for the whole solid range (0..10), then it will first get merged with the 0..5 span, receiving its area (assuming walkable climb was high enough), + // and then get merged with the 9..10 span, replacing its area. This would make the final area be 2, instead of 3 like it should be. + // If we instead add a solid span for the range 0..9, then the tie breaking will ensure that the final area is 3. + // Spans are always at least 1 voxel tall, so the solid span will always get merged with the original span. + AddLinkedSpan(index, minmax.min, minmax.max-1, CompactVoxelField.UnwalkableArea, voxelWalkableClimb, objectID); + } + } + + public void SetWalkableBackground () { + int wd = width*depth; + + for (int i = 0; i < wd; i++) { + linkedSpans[i] = new LinkedVoxelSpan(0, 1, 1); + } + } + + public void AddFlattenedSpan (int index, int area) { + if (linkedSpans[index].bottom == InvalidSpanValue) { + linkedSpans[index] = new LinkedVoxelSpan(0, 1, area); + } else { + // The prioritized area is (in order): + // - the unwalkable area (area=0) + // - the higher valued area + linkedSpans[index] = new LinkedVoxelSpan(0, 1, linkedSpans[index].area == 0 || area == 0 ? 0 : math.max(linkedSpans[index].area, area)); + } + } + + public void AddLinkedSpan (int index, int bottom, int top, int area, int voxelWalkableClimb, int objectID) { + var minmax = linkedCellMinMax[index]; + + if (minmax.objectID != objectID) { + linkedCellMinMax[index] = new CellMinMax { + objectID = objectID, + min = bottom, + max = top, + }; + } else { + minmax.min = math.min(minmax.min, bottom); + minmax.max = math.max(minmax.max, top); + linkedCellMinMax[index] = minmax; + } + + // Clamp to bounding box. If the span was outside the bbox, then bottom will become greater than top. + top = math.min(top, height); + bottom = math.max(bottom, 0); + + // Skip span if below or above the bounding box or if the span is zero voxels tall + if (bottom >= top) return; + + var utop = (uint)top; + var ubottom = (uint)bottom; + + // linkedSpans[index] is the span with the lowest y-coordinate at the position x,z such that index=x+z*width + // i.e linkedSpans is a 2D array laid out in a 1D array (for performance and simplicity) + + // Check if there is a root span, otherwise we can just add a new (valid) span and exit + if (linkedSpans[index].bottom == InvalidSpanValue) { + linkedSpans[index] = new LinkedVoxelSpan(ubottom, utop, area); + return; + } + + int prev = -1; + + // Original index, the first span we visited + int oindex = index; + + while (index != -1) { + var current = linkedSpans[index]; + if (current.bottom > utop) { + // If the current span's bottom higher up than the span we want to insert's top, then they do not intersect + // and we should just insert a new span here + break; + } else if (current.top < ubottom) { + // The current span and the span we want to insert do not intersect + // so just skip to the next span (it might intersect) + prev = index; + index = current.next; + } else { + // Intersection! Merge the spans + + // If two spans have almost the same upper y coordinate then + // we don't just pick the area from the topmost span. + // Instead we pick the maximum of the two areas. + // This ensures that unwalkable spans that end up at the same y coordinate + // as a walkable span (very common for vertical surfaces that meet a walkable surface at a ledge) + // do not end up making the surface unwalkable. + // This is also important for larger distances when there are very small obstacles on the ground. + // For example if a small rock happened to have a surface that was greater than the max slope angle, + // then its surface would be unwalkable. Without this check, even if the rock was tiny, it would + // create a hole in the navmesh. + + // voxelWalkableClimb is flagMergeDistance, when a walkable flag is favored before an unwalkable one + // So if a walkable span intersects an unwalkable span, the walkable span can be up to voxelWalkableClimb + // below the unwalkable span and the merged span will still be walkable. + // If both spans are walkable we use the area from the topmost span. + if (math.abs((int)utop - (int)current.top) < voxelWalkableClimb && (area == CompactVoxelField.UnwalkableArea || current.area == CompactVoxelField.UnwalkableArea)) { + // linkedSpans[index] is the lowest span, but we might use that span's area anyway if it is walkable + area = math.max(area, current.area); + } else { + // Pick the area from the topmost span + if (utop < current.top) area = current.area; + } + + // Find the new bottom and top for the merged span + ubottom = math.min(current.bottom, ubottom); + utop = math.max(current.top, utop); + + // Find the next span in the linked list + int next = current.next; + if (prev != -1) { + // There is a previous span + // Remove this span from the linked list + // TODO: Kinda slow. Check what asm is generated. + var p = linkedSpans[prev]; + p.next = next; + linkedSpans[prev] = p; + + // Add this span index to a list for recycling + PushToSpanRemovedStack(index); + + // Move to the next span in the list + index = next; + } else if (next != -1) { + // This was the root span and there is a span left in the linked list + // Remove this span from the linked list by assigning the next span as the root span + linkedSpans[oindex] = linkedSpans[next]; + + // Recycle the old span index + PushToSpanRemovedStack(next); + + // Move to the next span in the list + // NOP since we just removed the current span, the next span + // we want to visit will have the same index as we are on now (i.e oindex) + } else { + // This was the root span and there are no other spans in the linked list + // Just replace the root span with the merged span and exit + linkedSpans[oindex] = new LinkedVoxelSpan(ubottom, utop, area); + return; + } + } + } + + // We now have a merged span that needs to be inserted + // and connected with the existing spans + + // The new merged span will be inserted right after 'prev' (if it exists, otherwise before index) + + // Take a node from the recycling stack if possible + // Otherwise create a new node (well, just a new index really) + int nextIndex; + if (removedStack.Length > 0) { + // Pop + nextIndex = removedStack[removedStack.Length - 1]; + removedStack.RemoveAtSwapBack(removedStack.Length - 1); + } else { + nextIndex = linkedSpans.Length; + linkedSpans.Resize(linkedSpans.Length + 1, NativeArrayOptions.UninitializedMemory); + } + + if (prev != -1) { + linkedSpans[nextIndex] = new LinkedVoxelSpan(ubottom, utop, area, linkedSpans[prev].next); + // TODO: Check asm + var p = linkedSpans[prev]; + p.next = nextIndex; + linkedSpans[prev] = p; + } else { + linkedSpans[nextIndex] = linkedSpans[oindex]; + linkedSpans[oindex] = new LinkedVoxelSpan(ubottom, utop, area, nextIndex); + } + } + } + + public struct LinkedVoxelSpan { + public uint bottom; + public uint top; + + public int next; + + /*Area + * 0 is an unwalkable span (triangle face down) + * 1 is a walkable span (triangle face up) + */ + public int area; + + public LinkedVoxelSpan (uint bottom, uint top, int area) { + this.bottom = bottom; this.top = top; this.area = area; this.next = -1; + } + + public LinkedVoxelSpan (uint bottom, uint top, int area, int next) { + this.bottom = bottom; this.top = top; this.area = area; this.next = next; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta new file mode 100644 index 0000000..defeb4a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b6e41a3dcfac38cd8910584fc5de0d39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs new file mode 100644 index 0000000..39b49db --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs @@ -0,0 +1,710 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Burst; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + /// <summary>VoxelContour used for recast graphs.</summary> + public struct VoxelContour { + public int nverts; + + /// <summary>Vertex coordinates, each vertex contains 4 components.</summary> + public int vertexStartIndex; + + /// <summary>Region ID of the contour</summary> + public int reg; + + /// <summary>Area ID of the contour.</summary> + public int area; + } + + [BurstCompile(CompileSynchronously = true)] + public struct JobBuildContours : IJob { + public CompactVoxelField field; + public float maxError; + public float maxEdgeLength; + public int buildFlags; + public float cellSize; + public NativeList<VoxelContour> outputContours; + public NativeList<int> outputVerts; + + public void Execute () { + outputContours.Clear(); + outputVerts.Clear(); + + int w = field.width; + int d = field.depth; + int wd = w*d; + + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + + // NOTE: This array may contain uninitialized data, but since we explicitly set all data in it before we use it, it's OK. + var flags = new NativeArray<ushort>(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // Mark boundaries. (@?) + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + ushort res = 0; + CompactVoxelSpan s = field.spans[i]; + + if (s.reg == 0 || (s.reg & BorderReg) == BorderReg) { + flags[i] = 0; + continue; + } + + for (int dir = 0; dir < 4; dir++) { + int r = 0; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ni = field.cells[field.GetNeighbourIndex(x+z, dir)].index + s.GetConnection(dir); + r = field.spans[ni].reg; + } + + //@TODO - Why isn't this inside the previous IF + if (r == s.reg) { + res |= (ushort)(1 << dir); + } + } + + //Inverse, mark non connected edges. + flags[i] = (ushort)(res ^ 0xf); + } + } + } + + + NativeList<int> verts = new NativeList<int>(256, Allocator.Temp); + NativeList<int> simplified = new NativeList<int>(64, Allocator.Temp); + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + if (flags[i] == 0 || flags[i] == 0xf) { + flags[i] = 0; + continue; + } + + int reg = field.spans[i].reg; + + if (reg == 0 || (reg & BorderReg) == BorderReg) { + continue; + } + + int area = field.areaTypes[i]; + + verts.Clear(); + simplified.Clear(); + + WalkContour(x, z, i, flags, verts); + + SimplifyContour(verts, simplified, maxError, buildFlags); + RemoveDegenerateSegments(simplified); + + VoxelContour contour = new VoxelContour { + vertexStartIndex = outputVerts.Length, + nverts = simplified.Length/4, + reg = reg, + area = area, + }; + + outputVerts.AddRange(simplified.AsArray()); + + outputContours.Add(contour); + } + } + } + + verts.Dispose(); + simplified.Dispose(); + + + + // Check and merge droppings. + // Sometimes the previous algorithms can fail and create several outputContours + // per area. This pass will try to merge the holes into the main region. + for (int i = 0; i < outputContours.Length; i++) { + VoxelContour cont = outputContours[i]; + // Check if the contour is would backwards. + var outputVertsArr = outputVerts.AsArray(); + if (CalcAreaOfPolygon2D(outputVertsArr, cont.vertexStartIndex, cont.nverts) < 0) { + // Find another contour which has the same region ID. + int mergeIdx = -1; + for (int j = 0; j < outputContours.Length; j++) { + if (i == j) continue; + if (outputContours[j].nverts > 0 && outputContours[j].reg == cont.reg) { + // Make sure the polygon is correctly oriented. + if (CalcAreaOfPolygon2D(outputVertsArr, outputContours[j].vertexStartIndex, outputContours[j].nverts) > 0) { + mergeIdx = j; + break; + } + } + } + if (mergeIdx == -1) { + // Debug.LogError("rcBuildContours: Could not find merge target for bad contour "+i+"."); + } else { + // Debugging + // Debug.LogWarning ("Fixing contour"); + + VoxelContour mcont = outputContours[mergeIdx]; + // Merge by closest points. + GetClosestIndices(outputVertsArr, mcont.vertexStartIndex, mcont.nverts, cont.vertexStartIndex, cont.nverts, out var ia, out var ib); + + if (ia == -1 || ib == -1) { + // Debug.LogWarning("rcBuildContours: Failed to find merge points for "+i+" and "+mergeIdx+"."); + continue; + } + + + if (!MergeContours(outputVerts, ref mcont, ref cont, ia, ib)) { + //Debug.LogWarning("rcBuildContours: Failed to merge contours "+i+" and "+mergeIdx+"."); + continue; + } + + outputContours[mergeIdx] = mcont; + outputContours[i] = cont; + } + } + } + } + + void GetClosestIndices (NativeArray<int> verts, int vertexStartIndexA, int nvertsa, + int vertexStartIndexB, int nvertsb, + out int ia, out int ib) { + int closestDist = 0xfffffff; + + ia = -1; + ib = -1; + for (int i = 0; i < nvertsa; i++) { + //in is a keyword in C#, so I can't use that as a variable name + int in2 = (i+1) % nvertsa; + int ip = (i+nvertsa-1) % nvertsa; + int va = vertexStartIndexA + i*4; + int van = vertexStartIndexA + in2*4; + int vap = vertexStartIndexA + ip*4; + + for (int j = 0; j < nvertsb; ++j) { + int vb = vertexStartIndexB + j*4; + // vb must be "infront" of va. + if (Ileft(verts, vap, va, vb) && Ileft(verts, va, van, vb)) { + int dx = verts[vb+0] - verts[va+0]; + int dz = (verts[vb+2]/field.width) - (verts[va+2]/field.width); + int d = dx*dx + dz*dz; + if (d < closestDist) { + ia = i; + ib = j; + closestDist = d; + } + } + } + } + } + + public static bool MergeContours (NativeList<int> verts, ref VoxelContour ca, ref VoxelContour cb, int ia, int ib) { + // Note: this will essentially leave junk data in the verts array where the contours were previously. + // This shouldn't be a big problem because MergeContours is normally not called for that many contours (usually none). + int nv = 0; + var startIndex = verts.Length; + + // Copy contour A. + for (int i = 0; i <= ca.nverts; i++) { + int src = ca.vertexStartIndex + ((ia+i) % ca.nverts)*4; + verts.Add(verts[src+0]); + verts.Add(verts[src+1]); + verts.Add(verts[src+2]); + verts.Add(verts[src+3]); + nv++; + } + + // Copy contour B + for (int i = 0; i <= cb.nverts; i++) { + int src = cb.vertexStartIndex + ((ib+i) % cb.nverts)*4; + verts.Add(verts[src+0]); + verts.Add(verts[src+1]); + verts.Add(verts[src+2]); + verts.Add(verts[src+3]); + nv++; + } + + ca.vertexStartIndex = startIndex; + ca.nverts = nv; + + cb.vertexStartIndex = 0; + cb.nverts = 0; + + return true; + } + + public void SimplifyContour (NativeList<int> verts, NativeList<int> simplified, float maxError, int buildFlags) { + // Add initial points. + bool hasConnections = false; + + for (int i = 0; i < verts.Length; i += 4) { + if ((verts[i+3] & VoxelUtilityBurst.ContourRegMask) != 0) { + hasConnections = true; + break; + } + } + + if (hasConnections) { + // The contour has some portals to other regions. + // Add a new point to every location where the region changes. + for (int i = 0, ni = verts.Length/4; i < ni; i++) { + int ii = (i+1) % ni; + bool differentRegs = (verts[i*4+3] & VoxelUtilityBurst.ContourRegMask) != (verts[ii*4+3] & VoxelUtilityBurst.ContourRegMask); + bool areaBorders = (verts[i*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) != (verts[ii*4+3] & VoxelUtilityBurst.RC_AREA_BORDER); + + if (differentRegs || areaBorders) { + simplified.Add(verts[i*4+0]); + simplified.Add(verts[i*4+1]); + simplified.Add(verts[i*4+2]); + simplified.Add(i); + } + } + } + + + if (simplified.Length == 0) { + // If there is no connections at all, + // create some initial points for the simplification process. + // Find lower-left and upper-right vertices of the contour. + int llx = verts[0]; + int lly = verts[1]; + int llz = verts[2]; + int lli = 0; + int urx = verts[0]; + int ury = verts[1]; + int urz = verts[2]; + int uri = 0; + + for (int i = 0; i < verts.Length; i += 4) { + int x = verts[i+0]; + int y = verts[i+1]; + int z = verts[i+2]; + if (x < llx || (x == llx && z < llz)) { + llx = x; + lly = y; + llz = z; + lli = i/4; + } + if (x > urx || (x == urx && z > urz)) { + urx = x; + ury = y; + urz = z; + uri = i/4; + } + } + + simplified.Add(llx); + simplified.Add(lly); + simplified.Add(llz); + simplified.Add(lli); + + simplified.Add(urx); + simplified.Add(ury); + simplified.Add(urz); + simplified.Add(uri); + } + + // Add points until all raw points are within + // error tolerance to the simplified shape. + // This uses the Douglas-Peucker algorithm. + int pn = verts.Length/4; + + //Use the max squared error instead + maxError *= maxError; + + for (int i = 0; i < simplified.Length/4;) { + int ii = (i+1) % (simplified.Length/4); + + int ax = simplified[i*4+0]; + int ay = simplified[i*4+1]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int by = simplified[ii*4+1]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + float maxd = 0; + int maxi = -1; + int ci, cinc, endi; + + // Traverse the segment in lexilogical order so that the + // max deviation is calculated similarly when traversing + // opposite segments. + if (bx > ax || (bx == ax && bz > az)) { + cinc = 1; + ci = (ai+cinc) % pn; + endi = bi; + } else { + cinc = pn-1; + ci = (bi+cinc) % pn; + endi = ai; + Memory.Swap(ref ax, ref bx); + Memory.Swap(ref az, ref bz); + } + + // Tessellate only outer edges or edges between areas. + if ((verts[ci*4+3] & VoxelUtilityBurst.ContourRegMask) == 0 || + (verts[ci*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) == VoxelUtilityBurst.RC_AREA_BORDER) { + while (ci != endi) { + float d2 = VectorMath.SqrDistancePointSegmentApproximate(verts[ci*4+0], verts[ci*4+2]/field.width, ax, az/field.width, bx, bz/field.width); + + if (d2 > maxd) { + maxd = d2; + maxi = ci; + } + ci = (ci+cinc) % pn; + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > maxError) { + // Add space for the new point. + simplified.ResizeUninitialized(simplified.Length + 4); + + // Move all points after this one, to leave space to insert the new point + simplified.AsUnsafeSpan().Move((i+1)*4, (i+2)*4, simplified.Length-(i+2)*4); + + // Add the point. + simplified[(i+1)*4+0] = verts[maxi*4+0]; + simplified[(i+1)*4+1] = verts[maxi*4+1]; + simplified[(i+1)*4+2] = verts[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } else { + i++; + } + } + + // Split too long edges + + float maxEdgeLen = maxEdgeLength / cellSize; + + if (maxEdgeLen > 0 && (buildFlags & (VoxelUtilityBurst.RC_CONTOUR_TESS_WALL_EDGES|VoxelUtilityBurst.RC_CONTOUR_TESS_AREA_EDGES|VoxelUtilityBurst.RC_CONTOUR_TESS_TILE_EDGES)) != 0) { + for (int i = 0; i < simplified.Length/4;) { + if (simplified.Length/4 > 200) { + break; + } + + int ii = (i+1) % (simplified.Length/4); + + int ax = simplified[i*4+0]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + int maxi = -1; + int ci = (ai+1) % pn; + + // Tessellate only outer edges or edges between areas. + bool tess = false; + + // Wall edges. + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_WALL_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.ContourRegMask) == 0) + tess = true; + + // Edges between areas. + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_AREA_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) == VoxelUtilityBurst.RC_AREA_BORDER) + tess = true; + + // Border of tile + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_TILE_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) + tess = true; + + if (tess) { + int dx = bx - ax; + int dz = (bz/field.width) - (az/field.width); + if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) { + // Round based on the segments in lexilogical order so that the + // max tesselation is consistent regardles in which direction + // segments are traversed. + int n = bi < ai ? (bi+pn - ai) : (bi - ai); + if (n > 1) { + if (bx > ax || (bx == ax && bz > az)) { + maxi = (ai + n/2) % pn; + } else { + maxi = (ai + (n+1)/2) % pn; + } + } + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1) { + // Add space for the new point. + //simplified.resize(simplified.size()+4); + simplified.Resize(simplified.Length + 4, NativeArrayOptions.UninitializedMemory); + + simplified.AsUnsafeSpan().Move((i+1)*4, (i+2)*4, simplified.Length-(i+2)*4); + + // Add the point. + simplified[(i+1)*4+0] = verts[maxi*4+0]; + simplified[(i+1)*4+1] = verts[maxi*4+1]; + simplified[(i+1)*4+2] = verts[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } else { + ++i; + } + } + } + + for (int i = 0; i < simplified.Length/4; i++) { + // The edge vertex flag is take from the current raw point, + // and the neighbour region is take from the next raw point. + int ai = (simplified[i*4+3]+1) % pn; + int bi = simplified[i*4+3]; + simplified[i*4+3] = (verts[ai*4+3] & VoxelUtilityBurst.ContourRegMask) | (verts[bi*4+3] & VoxelUtilityBurst.RC_BORDER_VERTEX); + } + } + + public void WalkContour (int x, int z, int i, NativeArray<ushort> flags, NativeList<int> verts) { + // Choose the first non-connected edge + int dir = 0; + + while ((flags[i] & (ushort)(1 << dir)) == 0) { + dir++; + } + + int startDir = dir; + int startI = i; + + int area = field.areaTypes[i]; + + int iter = 0; + + while (iter++ < 40000) { + // Are we facing a region edge + if ((flags[i] & (ushort)(1 << dir)) != 0) { + // Choose the edge corner + bool isBorderVertex = false; + bool isAreaBorder = false; + + int px = x; + int py = GetCornerHeight(x, z, i, dir, ref isBorderVertex); + int pz = z; + + // Offset the vertex to land on the corner of the span. + // The resulting coordinates have an implicit 1/2 voxel offset because all corners + // are in the middle between two adjacent integer voxel coordinates. + switch (dir) { + case 0: pz += field.width; break; + case 1: px++; pz += field.width; break; + case 2: px++; break; + } + + int r = 0; + CompactVoxelSpan s = field.spans[i]; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ni = (int)field.cells[field.GetNeighbourIndex(x+z, dir)].index + s.GetConnection(dir); + r = (int)field.spans[ni].reg; + + if (area != field.areaTypes[ni]) { + isAreaBorder = true; + } + } + + if (isBorderVertex) { + r |= VoxelUtilityBurst.RC_BORDER_VERTEX; + } + if (isAreaBorder) { + r |= VoxelUtilityBurst.RC_AREA_BORDER; + } + + verts.Add(px); + verts.Add(py); + verts.Add(pz); + verts.Add(r); + + flags[i] = (ushort)(flags[i] & ~(1 << dir)); // Remove visited edges + + // & 0x3 is the same as % 4 (for positive numbers) + dir = (dir+1) & 0x3; // Rotate CW + } else { + int ni = -1; + int nx = x + VoxelUtilityBurst.DX[dir]; + int nz = z + VoxelUtilityBurst.DZ[dir]*field.width; + + CompactVoxelSpan s = field.spans[i]; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + CompactVoxelCell nc = field.cells[nx+nz]; + ni = (int)nc.index + s.GetConnection(dir); + } + + if (ni == -1) { + Debug.LogWarning("Degenerate triangles might have been generated.\n" + + "Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case."); + return; + } + x = nx; + z = nz; + i = ni; + + // & 0x3 is the same as % 4 (modulo 4) + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (startI == i && startDir == dir) { + break; + } + } + } + + public int GetCornerHeight (int x, int z, int i, int dir, ref bool isBorderVertex) { + CompactVoxelSpan s = field.spans[i]; + + int cornerHeight = (int)s.y; + + // dir + 1 step in the clockwise direction + int dirp = (dir+1) & 0x3; + + unsafe { + // We need a small buffer to hold regions for each axis aligned neighbour. + // This requires unsafe, though. In future C# versions we can use Span<T>. + // + // dir + // X----> + // dirp | + // v + // + // + // The regs array will contain the regions for the following spans, + // where the 0th span is the current span. + // 'x' signifies the position of the corner we are interested in. + // This is the shared vertex corner the four spans. + // It is conceptually at the current span's position + 0.5*dir + 0.5*dirp + // + // + // 0 --------- 1 -> dir + // | | + // | x | + // | | + // 3 --------- 2 + // + // | dirp + // v + // + var regs = stackalloc uint[] { 0, 0, 0, 0 }; + + regs[0] = (uint)field.spans[i].reg | ((uint)field.areaTypes[i] << 16); + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int neighbourCell = field.GetNeighbourIndex(x+z, dir); + int ni = (int)field.cells[neighbourCell].index + s.GetConnection(dir); + + CompactVoxelSpan ns = field.spans[ni]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns.y); + regs[1] = (uint)ns.reg | ((uint)field.areaTypes[ni] << 16); + + if (ns.GetConnection(dirp) != CompactVoxelField.NotConnected) { + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, dirp); + int ni2 = (int)field.cells[neighbourCell2].index + ns.GetConnection(dirp); + + CompactVoxelSpan ns2 = field.spans[ni2]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns2.y); + regs[2] = (uint)ns2.reg | ((uint)field.areaTypes[ni2] << 16); + } + } + + if (s.GetConnection(dirp) != CompactVoxelField.NotConnected) { + int neighbourCell = field.GetNeighbourIndex(x+z, dirp); + int ni = (int)field.cells[neighbourCell].index + s.GetConnection(dirp); + + CompactVoxelSpan ns = field.spans[ni]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns.y); + regs[3] = (uint)ns.reg | ((uint)field.areaTypes[ni] << 16); + + if (ns.GetConnection(dir) != CompactVoxelField.NotConnected) { + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, dir); + int ni2 = (int)field.cells[neighbourCell2].index + ns.GetConnection(dir); + + CompactVoxelSpan ns2 = field.spans[ni2]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns2.y); + regs[2] = (uint)ns2.reg | ((uint)field.areaTypes[ni2] << 16); + } + } + + // Zeroes show up when there are no connections to some spans. E.g. if the current span is on a ledge. + bool noZeros = regs[0] != 0 && regs[1] != 0 && regs[2] != 0 && regs[3] != 0; + + // Check if the vertex is special edge vertex, these vertices will be removed later. + for (int j = 0; j < 4; ++j) { + int a = j; + int b = (j+1) & 0x3; + int c = (j+2) & 0x3; + int d = (j+3) & 0x3; + + // The vertex is a border vertex there are two same exterior cells in a row, + // followed by two interior cells and none of the regions are out of bounds. + bool twoSameExts = (regs[a] & regs[b] & VoxelUtilityBurst.BorderReg) != 0 && regs[a] == regs[b]; + bool twoInts = ((regs[c] | regs[d]) & VoxelUtilityBurst.BorderReg) == 0; + bool intsSameArea = (regs[c]>>16) == (regs[d]>>16); + if (twoSameExts && twoInts && intsSameArea && noZeros) { + isBorderVertex = true; + break; + } + } + } + + return cornerHeight; + } + + static void RemoveRange (NativeList<int> arr, int index, int count) { + for (int i = index; i < arr.Length - count; i++) { + arr[i] = arr[i+count]; + } + arr.Resize(arr.Length - count, NativeArrayOptions.UninitializedMemory); + } + + static void RemoveDegenerateSegments (NativeList<int> simplified) { + // Remove adjacent vertices which are equal on xz-plane, + // or else the triangulator will get confused + for (int i = 0; i < simplified.Length/4; i++) { + int ni = i+1; + if (ni >= (simplified.Length/4)) + ni = 0; + + if (simplified[i*4+0] == simplified[ni*4+0] && + simplified[i*4+2] == simplified[ni*4+2]) { + // Degenerate segment, remove. + RemoveRange(simplified, i, 4); + } + } + } + + int CalcAreaOfPolygon2D (NativeArray<int> verts, int vertexStartIndex, int nverts) { + int area = 0; + + for (int i = 0, j = nverts-1; i < nverts; j = i++) { + int vi = vertexStartIndex + i*4; + int vj = vertexStartIndex + j*4; + area += verts[vi+0] * (verts[vj+2]/field.width) - verts[vj+0] * (verts[vi+2]/field.width); + } + + return (area+1) / 2; + } + + static bool Ileft (NativeArray<int> verts, int a, int b, int c) { + return (verts[b+0] - verts[a+0]) * (verts[c+2] - verts[a+2]) - (verts[c+0] - verts[a+0]) * (verts[b+2] - verts[a+2]) <= 0; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta new file mode 100644 index 0000000..712ca53 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aba1f429a9dee0ef98d35221ff450cda +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs new file mode 100644 index 0000000..b236330 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs @@ -0,0 +1,542 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + using System; + using Pathfinding.Jobs; + using Pathfinding.Util; +#if MODULE_COLLECTIONS_2_1_0_OR_NEWER + using NativeHashMapInt3Int = Unity.Collections.NativeHashMap<Int3, int>; +#else + using NativeHashMapInt3Int = Unity.Collections.NativeParallelHashMap<Int3, int>; +#endif + + /// <summary>VoxelMesh used for recast graphs.</summary> + public struct VoxelMesh : IArenaDisposable { + /// <summary>Vertices of the mesh</summary> + public NativeList<Int3> verts; + + /// <summary> + /// Triangles of the mesh. + /// Each element points to a vertex in the <see cref="verts"/> array + /// </summary> + public NativeList<int> tris; + + /// <summary>Area index for each triangle</summary> + public NativeList<int> areas; + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(verts); + arena.Add(tris); + arena.Add(areas); + } + } + + /// <summary>Builds a polygon mesh from a contour set.</summary> + [BurstCompile] + public struct JobBuildMesh : IJob { + public NativeList<int> contourVertices; + /// <summary>contour set to build a mesh from.</summary> + public NativeList<VoxelContour> contours; + /// <summary>Results will be written to this mesh.</summary> + public VoxelMesh mesh; + public CompactVoxelField field; + + /// <summary> + /// Returns T iff (v_i, v_j) is a proper internal + /// diagonal of P. + /// </summary> + static bool Diagonal (int i, int j, int n, NativeArray<int> verts, NativeArray<int> indices) { + return InCone(i, j, n, verts, indices) && Diagonalie(i, j, n, verts, indices); + } + + static bool InCone (int i, int j, int n, NativeArray<int> verts, NativeArray<int> indices) { + int pi = (indices[i] & 0x0fffffff) * 3; + int pj = (indices[j] & 0x0fffffff) * 3; + int pi1 = (indices[Next(i, n)] & 0x0fffffff) * 3; + int pin1 = (indices[Prev(i, n)] & 0x0fffffff) * 3; + + // If P[i] is a convex vertex [ i+1 left or on (i-1,i) ]. + if (LeftOn(pin1, pi, pi1, verts)) + return Left(pi, pj, pin1, verts) && Left(pj, pi, pi1, verts); + // Assume (i-1,i,i+1) not collinear. + // else P[i] is reflex. + return !(LeftOn(pi, pj, pi1, verts) && LeftOn(pj, pi, pin1, verts)); + } + + /// <summary> + /// Returns true iff c is strictly to the left of the directed + /// line through a to b. + /// </summary> + static bool Left (int a, int b, int c, NativeArray<int> verts) { + return Area2(a, b, c, verts) < 0; + } + + static bool LeftOn (int a, int b, int c, NativeArray<int> verts) { + return Area2(a, b, c, verts) <= 0; + } + + static bool Collinear (int a, int b, int c, NativeArray<int> verts) { + return Area2(a, b, c, verts) == 0; + } + + public static int Area2 (int a, int b, int c, NativeArray<int> verts) { + return (verts[b] - verts[a]) * (verts[c+2] - verts[a+2]) - (verts[c+0] - verts[a+0]) * (verts[b+2] - verts[a+2]); + } + + /// <summary> + /// Returns T iff (v_i, v_j) is a proper internal *or* external + /// diagonal of P, *ignoring edges incident to v_i and v_j*. + /// </summary> + static bool Diagonalie (int i, int j, int n, NativeArray<int> verts, NativeArray<int> indices) { + int d0 = (indices[i] & 0x0fffffff) * 3; + int d1 = (indices[j] & 0x0fffffff) * 3; + + /*int a = (i+1) % indices.Length; + * if (a == j) a = (i-1 + indices.Length) % indices.Length; + * int a_v = (indices[a] & 0x0fffffff) * 4; + * + * if (a != j && Collinear (d0,a_v,d1,verts)) { + * return false; + * }*/ + + // For each edge (k,k+1) of P + for (int k = 0; k < n; k++) { + int k1 = Next(k, n); + // Skip edges incident to i or j + if (!((k == i) || (k1 == i) || (k == j) || (k1 == j))) { + int p0 = (indices[k] & 0x0fffffff) * 3; + int p1 = (indices[k1] & 0x0fffffff) * 3; + + if (Vequal(d0, p0, verts) || Vequal(d1, p0, verts) || Vequal(d0, p1, verts) || Vequal(d1, p1, verts)) + continue; + + if (Intersect(d0, d1, p0, p1, verts)) + return false; + } + } + + + return true; + } + + // Exclusive or: true iff exactly one argument is true. + // The arguments are negated to ensure that they are 0/1 + // values. Then the bitwise Xor operator may apply. + // (This idea is due to Michael Baldwin.) + static bool Xorb (bool x, bool y) { + return !x ^ !y; + } + + // Returns true iff ab properly intersects cd: they share + // a point interior to both segments. The properness of the + // intersection is ensured by using strict leftness. + static bool IntersectProp (int a, int b, int c, int d, NativeArray<int> verts) { + // Eliminate improper cases. + if (Collinear(a, b, c, verts) || Collinear(a, b, d, verts) || + Collinear(c, d, a, verts) || Collinear(c, d, b, verts)) + return false; + + return Xorb(Left(a, b, c, verts), Left(a, b, d, verts)) && Xorb(Left(c, d, a, verts), Left(c, d, b, verts)); + } + + // Returns T iff (a,b,c) are collinear and point c lies + // on the closed segement ab. + static bool Between (int a, int b, int c, NativeArray<int> verts) { + if (!Collinear(a, b, c, verts)) + return false; + // If ab not vertical, check betweenness on x; else on y. + if (verts[a+0] != verts[b+0]) + return ((verts[a+0] <= verts[c+0]) && (verts[c+0] <= verts[b+0])) || ((verts[a+0] >= verts[c+0]) && (verts[c+0] >= verts[b+0])); + else + return ((verts[a+2] <= verts[c+2]) && (verts[c+2] <= verts[b+2])) || ((verts[a+2] >= verts[c+2]) && (verts[c+2] >= verts[b+2])); + } + + // Returns true iff segments ab and cd intersect, properly or improperly. + static bool Intersect (int a, int b, int c, int d, NativeArray<int> verts) { + if (IntersectProp(a, b, c, d, verts)) + return true; + else if (Between(a, b, c, verts) || Between(a, b, d, verts) || + Between(c, d, a, verts) || Between(c, d, b, verts)) + return true; + else + return false; + } + + static bool Vequal (int a, int b, NativeArray<int> verts) { + return verts[a+0] == verts[b+0] && verts[a+2] == verts[b+2]; + } + + /// <summary>(i-1+n) % n assuming 0 <= i < n</summary> + static int Prev (int i, int n) { return i-1 >= 0 ? i-1 : n-1; } + /// <summary>(i+1) % n assuming 0 <= i < n</summary> + static int Next (int i, int n) { return i+1 < n ? i+1 : 0; } + + static int AddVertex (NativeList<Int3> vertices, NativeHashMapInt3Int vertexMap, Int3 vertex) { + if (vertexMap.TryGetValue(vertex, out var index)) { + return index; + } + vertices.AddNoResize(vertex); + vertexMap.Add(vertex, vertices.Length-1); + return vertices.Length-1; + } + + public void Execute () { + // Maximum allowed vertices per polygon. Currently locked to 3. + var nvp = 3; + + int maxVertices = 0; + int maxTris = 0; + int maxVertsPerCont = 0; + + for (int i = 0; i < contours.Length; i++) { + // Skip null contours. + if (contours[i].nverts < 3) continue; + + maxVertices += contours[i].nverts; + maxTris += contours[i].nverts - 2; + maxVertsPerCont = System.Math.Max(maxVertsPerCont, contours[i].nverts); + } + + mesh.verts.Clear(); + if (maxVertices > mesh.verts.Capacity) mesh.verts.SetCapacity(maxVertices); + mesh.tris.ResizeUninitialized(maxTris*nvp); + mesh.areas.ResizeUninitialized(maxTris); + var verts = mesh.verts; + var polys = mesh.tris; + var areas = mesh.areas; + + var indices = new NativeArray<int>(maxVertsPerCont, Allocator.Temp); + var tris = new NativeArray<int>(maxVertsPerCont*3, Allocator.Temp); + var verticesToRemove = new NativeArray<bool>(maxVertices, Allocator.Temp); + var vertexPointers = new NativeHashMapInt3Int(maxVertices, Allocator.Temp); + + int polyIndex = 0; + int areaIndex = 0; + + for (int i = 0; i < contours.Length; i++) { + VoxelContour cont = contours[i]; + + // Skip degenerate contours + if (cont.nverts < 3) { + continue; + } + + for (int j = 0; j < cont.nverts; j++) { + // Convert the z coordinate from the form z*voxelArea.width which is used in other places for performance + contourVertices[cont.vertexStartIndex + j*4+2] /= field.width; + } + + // Copy the vertex positions + for (int j = 0; j < cont.nverts; j++) { + // Try to remove all border vertices + // See https://digestingduck.blogspot.com/2009/08/navmesh-height-accuracy-pt-5.html + var vertexRegion = contourVertices[cont.vertexStartIndex + j*4+3]; + + // Add a new vertex, or reuse an existing one if it has already been added to the mesh + var idx = AddVertex(verts, vertexPointers, new Int3( + contourVertices[cont.vertexStartIndex + j*4], + contourVertices[cont.vertexStartIndex + j*4+1], + contourVertices[cont.vertexStartIndex + j*4+2] + )); + indices[j] = idx; + verticesToRemove[idx] = (vertexRegion & VoxelUtilityBurst.RC_BORDER_VERTEX) != 0; + } + + // Triangulate the contour + int ntris = Triangulate(cont.nverts, verts.AsArray().Reinterpret<int>(12), indices, tris); + + if (ntris < 0) { + // Degenerate triangles. This may lead to a hole in the navmesh. + // We add the triangles that the triangulation generated before it failed. + ntris = -ntris; + } + + // Copy the resulting triangles to the mesh + for (int j = 0; j < ntris*3; polyIndex++, j++) { + polys[polyIndex] = tris[j]; + } + + // Mark all triangles generated by this contour + // as having the area cont.area + for (int j = 0; j < ntris; areaIndex++, j++) { + areas[areaIndex] = cont.area; + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (areaIndex > mesh.areas.Length) throw new System.Exception("Ended up at an unexpected area index"); + if (polyIndex > mesh.tris.Length) throw new System.Exception("Ended up at an unexpected poly index"); +#endif + + // polyIndex might in rare cases not be equal to mesh.tris.Length. + // This can happen if degenerate triangles were generated. + // So we make sure the list is truncated to the right size here. + mesh.tris.ResizeUninitialized(polyIndex); + // Same thing for area index + mesh.areas.ResizeUninitialized(areaIndex); + + RemoveTileBorderVertices(ref mesh, verticesToRemove); + } + + void RemoveTileBorderVertices (ref VoxelMesh mesh, NativeArray<bool> verticesToRemove) { + // Iterate in reverse to avoid having to update the verticesToRemove array as we remove vertices + var vertexScratch = new NativeArray<byte>(mesh.verts.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = mesh.verts.Length - 1; i >= 0; i--) { + if (verticesToRemove[i] && CanRemoveVertex(ref mesh, i, vertexScratch.AsUnsafeSpan())) { + RemoveVertex(ref mesh, i); + } + } + } + + bool CanRemoveVertex (ref VoxelMesh mesh, int vertexToRemove, UnsafeSpan<byte> vertexScratch) { + UnityEngine.Assertions.Assert.IsTrue(vertexScratch.Length >= mesh.verts.Length); + + int remainingEdges = 0; + for (int i = 0; i < mesh.tris.Length; i += 3) { + int touched = 0; + for (int j = 0; j < 3; j++) { + if (mesh.tris[i+j] == vertexToRemove) { + // This vertex is used by a triangle + touched++; + } + } + + if (touched > 0) { + if (touched > 1) throw new Exception("Degenerate triangle. This should have already been removed."); + // If one vertex is removed from a triangle, 1 edge remains + remainingEdges++; + } + } + + if (remainingEdges <= 2) { + // There would be too few edges remaining to create a polygon. + // This can happen for example when a tip of a triangle is marked + // as deletion, but there are no other polys that share the vertex. + // In this case, the vertex should not be removed. + return false; + } + + vertexScratch.FillZeros(); + + for (int i = 0; i < mesh.tris.Length; i += 3) { + for (int a = 0, b = 2; a < 3; b = a++) { + if (mesh.tris[i+a] == vertexToRemove || mesh.tris[i+b] == vertexToRemove) { + // This edge is used by a triangle + int v1 = mesh.tris[i+a]; + int v2 = mesh.tris[i+b]; + + // Update the shared count for the edge. + // We identify the edge by the vertex index which is not the vertex to remove. + vertexScratch[v2 == vertexToRemove ? v1 : v2]++; + } + } + } + + int openEdges = 0; + for (int i = 0; i < vertexScratch.Length; i++) { + if (vertexScratch[i] == 1) openEdges++; + } + + // There should be no more than 2 open edges. + // This catches the case that two non-adjacent polygons + // share the removed vertex. In that case, do not remove the vertex. + return openEdges <= 2; + } + + void RemoveVertex (ref VoxelMesh mesh, int vertexToRemove) { + // Note: Assumes CanRemoveVertex has been called and returned true + + var remainingEdges = new NativeList<int>(16, Allocator.Temp); + var area = -1; + // Find all triangles that use this vertex + for (int i = 0; i < mesh.tris.Length; i += 3) { + int touched = -1; + for (int j = 0; j < 3; j++) { + if (mesh.tris[i+j] == vertexToRemove) { + // This vertex is used by a triangle + touched = j; + break; + } + } + if (touched != -1) { + // Note: Only vertices that are not on an area border will be chosen (see GetCornerHeight), + // so it is safe to assume that all triangles that share this vertex also share an area. + area = mesh.areas[i/3]; + // If one vertex is removed from a triangle, 1 edge remains + remainingEdges.Add(mesh.tris[i+((touched+1) % 3)]); + remainingEdges.Add(mesh.tris[i+((touched+2) % 3)]); + + mesh.tris[i+0] = mesh.tris[mesh.tris.Length-3+0]; + mesh.tris[i+1] = mesh.tris[mesh.tris.Length-3+1]; + mesh.tris[i+2] = mesh.tris[mesh.tris.Length-3+2]; + + mesh.tris.Length -= 3; + mesh.areas.RemoveAtSwapBack(i/3); + i -= 3; + } + } + + UnityEngine.Assertions.Assert.AreNotEqual(-1, area); + + // Build a sorted list of all vertices in the contour for the hole + var sortedVertices = new NativeList<int>(remainingEdges.Length/2 + 1, Allocator.Temp); + sortedVertices.Add(remainingEdges[remainingEdges.Length-2]); + sortedVertices.Add(remainingEdges[remainingEdges.Length-1]); + remainingEdges.Length -= 2; + + while (remainingEdges.Length > 0) { + for (int i = remainingEdges.Length - 2; i >= 0; i -= 2) { + var a = remainingEdges[i]; + var b = remainingEdges[i+1]; + bool added = false; + if (sortedVertices[0] == b) { + sortedVertices.InsertRange(0, 1); + sortedVertices[0] = a; + added = true; + } + if (sortedVertices[sortedVertices.Length-1] == a) { + sortedVertices.AddNoResize(b); + added = true; + } + if (added) { + // Remove the edge and swap with the last one + remainingEdges[i] = remainingEdges[remainingEdges.Length-2]; + remainingEdges[i+1] = remainingEdges[remainingEdges.Length-1]; + remainingEdges.Length -= 2; + } + } + } + + // Remove the vertex + mesh.verts.RemoveAt(vertexToRemove); + + // Patch indices to account for the removed vertex + for (int i = 0; i < mesh.tris.Length; i++) { + if (mesh.tris[i] > vertexToRemove) mesh.tris[i]--; + } + for (int i = 0; i < sortedVertices.Length; i++) { + if (sortedVertices[i] > vertexToRemove) sortedVertices[i]--; + } + + var maxIndices = (sortedVertices.Length - 2) * 3; + var trisBeforeResize = mesh.tris.Length; + mesh.tris.Length += maxIndices; + int newTriCount = Triangulate( + sortedVertices.Length, + mesh.verts.AsArray().Reinterpret<int>(12), + sortedVertices.AsArray(), + // Insert the new triangles at the end of the array + mesh.tris.AsArray().GetSubArray(trisBeforeResize, maxIndices) + ); + + if (newTriCount < 0) { + // Degenerate triangles. This may lead to a hole in the navmesh. + // We add the triangles that the triangulation generated before it failed. + newTriCount = -newTriCount; + } + + // Resize the triangle array to the correct size + mesh.tris.ResizeUninitialized(trisBeforeResize + newTriCount*3); + mesh.areas.AddReplicate(area, newTriCount); + + UnityEngine.Assertions.Assert.AreEqual(mesh.areas.Length, mesh.tris.Length/3); + } + + static int Triangulate (int n, NativeArray<int> verts, NativeArray<int> indices, NativeArray<int> tris) { + int ntris = 0; + var dst = tris; + int dstIndex = 0; + + // The last bit of the index is used to indicate if the vertex can be removed + // in an ear-cutting operation. + const int CanBeRemovedBit = 0x40000000; + // Used to get only the index value, without any flag bits. + const int IndexMask = 0x0fffffff; + + for (int i = 0; i < n; i++) { + int i1 = Next(i, n); + int i2 = Next(i1, n); + if (Diagonal(i, i2, n, verts, indices)) { + indices[i1] |= CanBeRemovedBit; + } + } + + while (n > 3) { + int minLen = int.MaxValue; + int mini = -1; + + for (int q = 0; q < n; q++) { + int q1 = Next(q, n); + if ((indices[q1] & CanBeRemovedBit) != 0) { + int p0 = (indices[q] & IndexMask) * 3; + int p2 = (indices[Next(q1, n)] & IndexMask) * 3; + + int dx = verts[p2+0] - verts[p0+0]; + int dz = verts[p2+2] - verts[p0+2]; + + + //Squared distance + int len = dx*dx + dz*dz; + + if (len < minLen) { + minLen = len; + mini = q; + } + } + } + + if (mini == -1) { + Debug.LogWarning("Degenerate triangles might have been generated.\n" + + "Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case."); + return -ntris; + } + + int i = mini; + int i1 = Next(i, n); + int i2 = Next(i1, n); + + + dst[dstIndex] = indices[i] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[i1] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[i2] & IndexMask; + dstIndex++; + ntris++; + + // Removes P[i1] by copying P[i+1]...P[n-1] left one index. + n--; + for (int k = i1; k < n; k++) { + indices[k] = indices[k+1]; + } + + if (i1 >= n) i1 = 0; + i = Prev(i1, n); + // Update diagonal flags. + if (Diagonal(Prev(i, n), i1, n, verts, indices)) { + indices[i] |= CanBeRemovedBit; + } else { + indices[i] &= IndexMask; + } + if (Diagonal(i, Next(i1, n), n, verts, indices)) { + indices[i1] |= CanBeRemovedBit; + } else { + indices[i1] &= IndexMask; + } + } + + dst[dstIndex] = indices[0] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[1] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[2] & IndexMask; + dstIndex++; + ntris++; + + return ntris; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta new file mode 100644 index 0000000..eb45f18 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 73110b746664b5ec197eda5f732356a5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs new file mode 100644 index 0000000..2576e6e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs @@ -0,0 +1,205 @@ +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization { + /// <summary>Utility for clipping polygons</summary> + internal struct Int3PolygonClipper { + /// <summary>Cache this buffer to avoid unnecessary allocations</summary> + float[] clipPolygonCache; + + /// <summary>Cache this buffer to avoid unnecessary allocations</summary> + int[] clipPolygonIntCache; + + /// <summary>Initialize buffers if they are null</summary> + public void Init () { + if (clipPolygonCache == null) { + clipPolygonCache = new float[7*3]; + clipPolygonIntCache = new int[7*3]; + } + } + + /// <summary> + /// Clips a polygon against an axis aligned half plane. + /// + /// Returns: Number of output vertices + /// + /// The vertices will be scaled and then offset, after that they will be cut using either the + /// x axis, y axis or the z axis as the cutting line. The resulting vertices will be added to the + /// vOut array in their original space (i.e before scaling and offsetting). + /// </summary> + /// <param name="vIn">Input vertices</param> + /// <param name="n">Number of input vertices (may be less than the length of the vIn array)</param> + /// <param name="vOut">Output vertices, needs to be large enough</param> + /// <param name="multi">Scale factor for the input vertices</param> + /// <param name="offset">Offset to move the input vertices with before cutting</param> + /// <param name="axis">Axis to cut along, either x=0, y=1, z=2</param> + public int ClipPolygon (Int3[] vIn, int n, Int3[] vOut, int multi, int offset, int axis) { + Init(); + int[] d = clipPolygonIntCache; + + for (int i = 0; i < n; i++) { + d[i] = multi*vIn[i][axis]+offset; + } + + // Number of resulting vertices + int m = 0; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + bool prev = d[j] >= 0; + bool curr = d[i] >= 0; + + if (prev != curr) { + double s = (double)d[j] / (d[j] - d[i]); + + vOut[m] = vIn[j] + (vIn[i]-vIn[j])*s; + m++; + } + + if (curr) { + vOut[m] = vIn[i]; + m++; + } + } + + return m; + } + } + + /// <summary>Utility for clipping polygons</summary> + internal struct VoxelPolygonClipper { + public unsafe fixed float x[8]; + public unsafe fixed float y[8]; + public unsafe fixed float z[8]; + public int n; + + public UnityEngine.Vector3 this[int i] { + set { + unsafe { + x[i] = value.x; + y[i] = value.y; + z[i] = value.z; + } + } + } + + /// <summary> + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at x = -offset. + /// </summary> + /// <param name="result">Ouput vertices</param> + /// <param name="multi">Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept.</param> + /// <param name="offset">Offset to move the input vertices with before cutting</param> + public void ClipPolygonAlongX ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + float dj = multi*x[(n-1)]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*x[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.x[m] = x[j] + (x[i]-x[j])*s; + result.y[m] = y[j] + (y[i]-y[j])*s; + result.z[m] = z[j] + (z[i]-z[j])*s; + m++; + } + + if (curr) { + result.x[m] = x[i]; + result.y[m] = y[i]; + result.z[m] = z[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + + /// <summary> + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at z = -offset. + /// </summary> + /// <param name="result">Ouput vertices. Only the Y and Z coordinates are calculated. The X coordinates are undefined.</param> + /// <param name="multi">Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept.</param> + /// <param name="offset">Offset to move the input vertices with before cutting</param> + public void ClipPolygonAlongZWithYZ ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + Unity.Burst.CompilerServices.Hint.Assume(n >= 0); + Unity.Burst.CompilerServices.Hint.Assume(n <= 8); + float dj = multi*z[(n-1)]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*z[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.y[m] = y[j] + (y[i]-y[j])*s; + result.z[m] = z[j] + (z[i]-z[j])*s; + m++; + } + + if (curr) { + result.y[m] = y[i]; + result.z[m] = z[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + + /// <summary> + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at z = -offset. + /// </summary> + /// <param name="result">Ouput vertices. Only the Y coordinates are calculated. The X and Z coordinates are undefined.</param> + /// <param name="multi">Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept.</param> + /// <param name="offset">Offset to move the input vertices with before cutting</param> + public void ClipPolygonAlongZWithY ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + Unity.Burst.CompilerServices.Hint.Assume(n >= 3); + Unity.Burst.CompilerServices.Hint.Assume(n <= 8); + float dj = multi*z[n-1]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*z[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.y[m] = y[j] + (y[i]-y[j])*s; + m++; + } + + if (curr) { + result.y[m] = y[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta new file mode 100644 index 0000000..6ab0fa5 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 10347e1eaceee428fa14386ccbaffde5 +timeCreated: 1454161567 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: 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; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta new file mode 100644 index 0000000..7c1c36e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: af78ae0fb20c2907695f4acc47d811a1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs new file mode 100644 index 0000000..0e40a38 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs @@ -0,0 +1,813 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Jobs; +using Unity.Burst; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + [BurstCompile(CompileSynchronously = true)] + public struct JobBuildRegions : IJob { + public CompactVoxelField field; + public NativeList<ushort> distanceField; + public int borderSize; + public int minRegionSize; + public NativeQueue<Int3> srcQue; + public NativeQueue<Int3> dstQue; + public RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode; + public NativeArray<RelevantGraphSurfaceInfo> relevantGraphSurfaces; + + public float cellSize, cellHeight; + public Matrix4x4 graphTransform; + public Bounds graphSpaceBounds; + + void MarkRectWithRegion (int minx, int maxx, int minz, int maxz, ushort region, NativeArray<ushort> srcReg) { + int md = maxz * field.width; + + for (int z = minz*field.width; z < md; z += field.width) { + for (int x = minx; x < maxx; x++) { + CompactVoxelCell c = field.cells[z+x]; + + for (int i = c.index, ni = c.index+c.count; i < ni; i++) { + if (field.areaTypes[i] != CompactVoxelField.UnwalkableArea) { + srcReg[i] = region; + } + } + } + } + } + + public static bool FloodRegion (int x, int z, int i, uint level, ushort r, + CompactVoxelField field, + NativeArray<ushort> distanceField, + NativeArray<ushort> srcReg, + NativeArray<ushort> srcDist, + NativeArray<Int3> stack, + NativeArray<int> flags, + NativeArray<bool> closed) { + int area = field.areaTypes[i]; + + // Flood f mark region. + int stackSize = 1; + + stack[0] = new Int3 { + x = x, + y = i, + z = z, + }; + + srcReg[i] = r; + srcDist[i] = 0; + + int lev = (int)(level >= 2 ? level-2 : 0); + + int count = 0; + + // Store these in local variables (for performance, avoids an extra indirection) + var compactCells = field.cells; + var compactSpans = field.spans; + var areaTypes = field.areaTypes; + + while (stackSize > 0) { + stackSize--; + var c = stack[stackSize]; + //Similar to the Pop operation of an array, but Pop is not implemented in List<> + int ci = c.y; + int cx = c.x; + int cz = c.z; + + CompactVoxelSpan cs = compactSpans[ci]; + + //Debug.DrawRay (ConvertPosition(cx,cz,ci),Vector3.up, Color.cyan); + + // Check if any of the neighbours already have a valid region set. + ushort ar = 0; + + // Loop through four neighbours + // then check one neighbour of the neighbour + // to get the diagonal neighbour + for (int dir = 0; dir < 4; dir++) { + // 8 connected + if (cs.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ax = cx + VoxelUtilityBurst.DX[dir]; + int az = cz + VoxelUtilityBurst.DZ[dir]*field.width; + + int ai = (int)compactCells[ax+az].index + cs.GetConnection(dir); + + if (areaTypes[ai] != area) + continue; + + ushort nr = srcReg[ai]; + + if ((nr & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) // Do not take borders into account. + continue; + + if (nr != 0 && nr != r) { + ar = nr; + // Found a valid region, skip checking the rest + break; + } + + // Rotate dir 90 degrees + int dir2 = (dir+1) & 0x3; + var neighbour2 = compactSpans[ai].GetConnection(dir2); + // Check the diagonal connection + if (neighbour2 != CompactVoxelField.NotConnected) { + int ax2 = ax + VoxelUtilityBurst.DX[dir2]; + int az2 = az + VoxelUtilityBurst.DZ[dir2]*field.width; + + int ai2 = compactCells[ax2+az2].index + neighbour2; + + if (areaTypes[ai2] != area) + continue; + + ushort nr2 = srcReg[ai2]; + + if ((nr2 & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) // Do not take borders into account. + continue; + + if (nr2 != 0 && nr2 != r) { + ar = nr2; + // Found a valid region, skip checking the rest + break; + } + } + } + } + + if (ar != 0) { + srcReg[ci] = 0; + srcDist[ci] = 0xFFFF; + continue; + } + count++; + closed[ci] = true; + + + // Expand neighbours. + for (int dir = 0; dir < 4; ++dir) { + if (cs.GetConnection(dir) == CompactVoxelField.NotConnected) continue; + int ax = cx + VoxelUtilityBurst.DX[dir]; + int az = cz + VoxelUtilityBurst.DZ[dir]*field.width; + int ai = compactCells[ax+az].index + cs.GetConnection(dir); + + if (areaTypes[ai] != area) continue; + if (srcReg[ai] != 0) continue; + + if (distanceField[ai] >= lev && flags[ai] == 0) { + srcReg[ai] = r; + srcDist[ai] = 0; + + stack[stackSize] = new Int3 { + x = ax, + y = ai, + z = az, + }; + stackSize++; + } else { + flags[ai] = r; + srcDist[ai] = 2; + } + } + } + + + return count > 0; + } + + public void Execute () { + srcQue.Clear(); + dstQue.Clear(); + + /*System.Diagnostics.Stopwatch w0 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w1 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w3 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w4 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w5 = new System.Diagnostics.Stopwatch(); + w3.Start();*/ + + int w = field.width; + int d = field.depth; + int wd = w*d; + int spanCount = field.spans.Length; + + int expandIterations = 8; + + var srcReg = new NativeArray<ushort>(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var srcDist = new NativeArray<ushort>(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var closed = new NativeArray<bool>(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var spanFlags = new NativeArray<int>(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var stack = new NativeArray<Int3>(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // The array pool arrays may contain arbitrary data. We need to zero it out. + for (int i = 0; i < spanCount; i++) { + srcReg[i] = 0; + srcDist[i] = 0xFFFF; + closed[i] = false; + spanFlags[i] = 0; + } + + var spanDistances = distanceField; + var areaTypes = field.areaTypes; + var compactCells = field.cells; + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + + ushort regionId = 2; + MarkRectWithRegion(0, borderSize, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(w-borderSize, w, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(0, w, 0, borderSize, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(0, w, d-borderSize, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + + // TODO: Can be optimized + int maxDistance = 0; + for (int i = 0; i < distanceField.Length; i++) { + maxDistance = math.max(distanceField[i], maxDistance); + } + + // A distance is 2 to an adjacent span and 1 for a diagonally adjacent one. + NativeArray<int> sortedSpanCounts = new NativeArray<int>((maxDistance)/2 + 1, Allocator.Temp); + for (int i = 0; i < field.spans.Length; i++) { + // Do not take borders or unwalkable spans into account. + if ((srcReg[i] & BorderReg) == BorderReg || areaTypes[i] == CompactVoxelField.UnwalkableArea) + continue; + + sortedSpanCounts[distanceField[i]/2]++; + } + + var distanceIndexOffsets = new NativeArray<int>(sortedSpanCounts.Length, Allocator.Temp); + for (int i = 1; i < distanceIndexOffsets.Length; i++) { + distanceIndexOffsets[i] = distanceIndexOffsets[i-1] + sortedSpanCounts[i-1]; + } + var totalRelevantSpans = distanceIndexOffsets[distanceIndexOffsets.Length - 1] + sortedSpanCounts[sortedSpanCounts.Length - 1]; + + var bucketSortedSpans = new NativeArray<Int3>(totalRelevantSpans, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // Bucket sort the spans based on distance + for (int z = 0, pz = 0; z < wd; z += w, pz++) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = compactCells[z+x]; + + for (int i = c.index, ni = c.index+c.count; i < ni; i++) { + // Do not take borders or unwalkable spans into account. + if ((srcReg[i] & BorderReg) == BorderReg || areaTypes[i] == CompactVoxelField.UnwalkableArea) + continue; + + int distIndex = distanceField[i] / 2; + bucketSortedSpans[distanceIndexOffsets[distIndex]++] = new Int3(x, i, z); + } + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (distanceIndexOffsets[distanceIndexOffsets.Length - 1] != totalRelevantSpans) throw new System.Exception("Unexpected span count"); +#endif + + // Go through spans in reverse order (i.e largest distances first) + for (int distIndex = sortedSpanCounts.Length - 1; distIndex >= 0; distIndex--) { + var level = (uint)distIndex * 2; + var spansAtLevel = sortedSpanCounts[distIndex]; + for (int i = 0; i < spansAtLevel; i++) { + // Go through the spans stored in bucketSortedSpans for this distance index. + // Note that distanceIndexOffsets[distIndex] will point to the element after the end of the group of spans. + // There is no particular reason for this, the code just turned out to be a bit simpler to implemen that way. + var spanInfo = bucketSortedSpans[distanceIndexOffsets[distIndex] - i - 1]; + int spanIndex = spanInfo.y; + + // This span is adjacent to a region, so we should start the BFS search from it + if (spanFlags[spanIndex] != 0 && srcReg[spanIndex] == 0) { + srcReg[spanIndex] = (ushort)spanFlags[spanIndex]; + srcQue.Enqueue(spanInfo); + closed[spanIndex] = true; + } + } + + // Expand a few iterations out from every known node + for (int expansionIteration = 0; expansionIteration < expandIterations && srcQue.Count > 0; expansionIteration++) { + while (srcQue.Count > 0) { + Int3 spanInfo = srcQue.Dequeue(); + var area = areaTypes[spanInfo.y]; + var span = field.spans[spanInfo.y]; + var region = srcReg[spanInfo.y]; + closed[spanInfo.y] = true; + ushort nextDist = (ushort)(srcDist[spanInfo.y] + 2); + + // Go through the neighbours of the span + for (int dir = 0; dir < 4; dir++) { + var neighbour = span.GetConnection(dir); + if (neighbour == CompactVoxelField.NotConnected) continue; + + int nx = spanInfo.x + VoxelUtilityBurst.DX[dir]; + int nz = spanInfo.z + VoxelUtilityBurst.DZ[dir]*field.width; + + int ni = compactCells[nx+nz].index + neighbour; + + if ((srcReg[ni] & BorderReg) == BorderReg) // Do not take borders into account. + continue; + + // Do not combine different area types + if (area == areaTypes[ni]) { + if (nextDist < srcDist[ni]) { + if (spanDistances[ni] < level) { + srcDist[ni] = nextDist; + spanFlags[ni] = region; + } else if (!closed[ni]) { + srcDist[ni] = nextDist; + if (srcReg[ni] == 0) dstQue.Enqueue(new Int3(nx, ni, nz)); + srcReg[ni] = region; + } + } + } + } + } + Memory.Swap(ref srcQue, ref dstQue); + } + + // Find the first span that has not been seen yet and start a new region that expands from there + var distanceFieldArr = distanceField.AsArray(); + for (int i = 0; i < spansAtLevel; i++) { + var info = bucketSortedSpans[distanceIndexOffsets[distIndex] - i - 1]; + if (srcReg[info.y] == 0) { + if (!FloodRegion(info.x, info.z, info.y, level, regionId, field, distanceFieldArr, srcReg, srcDist, stack, spanFlags, closed)) { + // The starting voxel was already adjacent to an existing region so we skip flooding it. + // It will be visited in the next area expansion. + } else { + regionId++; + } + } + } + } + + var maxRegions = regionId; + + // 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 voxel2worldMatrix = graphTransform * voxelMatrix * Matrix4x4.Translate(new Vector3(0.5f, 0, 0.5f)); + + // Filter out small regions. + FilterSmallRegions(field, srcReg, minRegionSize, maxRegions, this.relevantGraphSurfaces, this.relevantGraphSurfaceMode, voxel2worldMatrix); + + // Write the result out. + for (int i = 0; i < spanCount; i++) { + var span = field.spans[i]; + span.reg = srcReg[i]; + field.spans[i] = span; + } + + // TODO: + // field.maxRegions = maxRegions; + +// #if ASTAR_DEBUGREPLAY +// DebugReplay.BeginGroup("Regions"); +// 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; i < c.index+c.count; i++) { +// CompactVoxelSpan s = field.spans[i]; +// DebugReplay.DrawCube(CompactSpanToVector(x, pz, i), UnityEngine.Vector3.one*cellSize, AstarMath.IntToColor(s.reg, 1.0f)); +// } +// } +// } + +// DebugReplay.EndGroup(); + +// int maxDist = 0; +// for (int i = 0; i < srcDist.Length; i++) if (srcDist[i] != 0xFFFF) maxDist = Mathf.Max(maxDist, srcDist[i]); + +// DebugReplay.BeginGroup("Distances"); +// 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; i < c.index+c.count; i++) { +// CompactVoxelSpan s = field.spans[i]; +// float f = (float)srcDist[i]/maxDist; +// DebugReplay.DrawCube(CompactSpanToVector(x, z/field.width, i), Vector3.one*cellSize, new Color(f, f, f)); +// } +// } +// } + +// DebugReplay.EndGroup(); +// #endif + } + + /// <summary> + /// Find method in the UnionFind data structure. + /// See: https://en.wikipedia.org/wiki/Disjoint-set_data_structure + /// </summary> + static int union_find_find (NativeArray<int> arr, int x) { + if (arr[x] < 0) return x; + return arr[x] = union_find_find(arr, arr[x]); + } + + /// <summary> + /// Join method in the UnionFind data structure. + /// See: https://en.wikipedia.org/wiki/Disjoint-set_data_structure + /// </summary> + static void union_find_union (NativeArray<int> arr, int a, int b) { + a = union_find_find(arr, a); + b = union_find_find(arr, b); + if (a == b) return; + if (arr[a] > arr[b]) { + int tmp = a; + a = b; + b = tmp; + } + arr[a] += arr[b]; + arr[b] = a; + } + + public struct RelevantGraphSurfaceInfo { + public float3 position; + public float range; + } + + /// <summary>Filters out or merges small regions.</summary> + public static void FilterSmallRegions (CompactVoxelField field, NativeArray<ushort> reg, int minRegionSize, int maxRegions, NativeArray<RelevantGraphSurfaceInfo> relevantGraphSurfaces, RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode, float4x4 voxel2worldMatrix) { + // RelevantGraphSurface c = RelevantGraphSurface.Root; + // Need to use ReferenceEquals because it might be called from another thread + bool anySurfaces = relevantGraphSurfaces.Length != 0 && (relevantGraphSurfaceMode != RecastGraph.RelevantGraphSurfaceMode.DoNotRequire); + + // Nothing to do here + if (!anySurfaces && minRegionSize <= 0) { + return; + } + + var counter = new NativeArray<int>(maxRegions, Allocator.Temp); + var bits = new NativeArray<ushort>(maxRegions, Allocator.Temp, NativeArrayOptions.ClearMemory); + for (int i = 0; i < counter.Length; i++) counter[i] = -1; + + int nReg = counter.Length; + + int wd = field.width*field.depth; + + const int RelevantSurfaceSet = 1 << 1; + const int BorderBit = 1 << 0; + + // Mark RelevantGraphSurfaces + + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + // If they can also be adjacent to tile borders, this will also include the BorderBit + int RelevantSurfaceCheck = RelevantSurfaceSet | ((relevantGraphSurfaceMode == RecastGraph.RelevantGraphSurfaceMode.OnlyForCompletelyInsideTile) ? BorderBit : 0x0); + // int RelevantSurfaceCheck = 0; + + if (anySurfaces) { + var world2voxelMatrix = math.inverse(voxel2worldMatrix); + for (int j = 0; j < relevantGraphSurfaces.Length; j++) { + var relevantGraphSurface = relevantGraphSurfaces[j]; + var positionInVoxelSpace = math.transform(world2voxelMatrix, relevantGraphSurface.position); + int3 cellIndex = (int3)math.round(positionInVoxelSpace); + + // Check for out of bounds + if (cellIndex.x >= 0 && cellIndex.z >= 0 && cellIndex.x < field.width && cellIndex.z < field.depth) { + var yScaleFactor = math.length(voxel2worldMatrix.c1.xyz); + int rad = (int)(relevantGraphSurface.range / yScaleFactor); + + CompactVoxelCell cell = field.cells[cellIndex.x+cellIndex.z*field.width]; + for (int i = cell.index; i < cell.index+cell.count; i++) { + CompactVoxelSpan s = field.spans[i]; + if (System.Math.Abs(s.y - cellIndex.y) <= rad && reg[i] != 0) { + bits[union_find_find(counter, reg[i] & ~BorderReg)] |= RelevantSurfaceSet; + } + } + } + } + } + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell cell = field.cells[x+z]; + + for (int i = cell.index; i < cell.index+cell.count; i++) { + CompactVoxelSpan s = field.spans[i]; + + int r = reg[i]; + + // Check if this is an unwalkable span + if ((r & ~BorderReg) == 0) continue; + + if (r >= nReg) { //Probably border + bits[union_find_find(counter, r & ~BorderReg)] |= BorderBit; + continue; + } + + int root = union_find_find(counter, r); + // Count this span + counter[root]--; + + // Iterate through all neighbours of the span. + for (int dir = 0; dir < 4; dir++) { + if (s.GetConnection(dir) == CompactVoxelField.NotConnected) { continue; } + + int nx = x + VoxelUtilityBurst.DX[dir]; + int nz = z + VoxelUtilityBurst.DZ[dir] * field.width; + + int ni = field.cells[nx+nz].index + s.GetConnection(dir); + + int r2 = reg[ni]; + + // Check if the other span belongs to a different region and is walkable + if (r != r2 && (r2 & ~BorderReg) != 0) { + if ((r2 & BorderReg) != 0) { + // If it's a border region we just mark the current region as being adjacent to a border + bits[root] |= BorderBit; + } else { + // Join the adjacent region with this region. + union_find_union(counter, root, r2); + } + //counter[r] = minRegionSize; + } + } + //counter[r]++; + } + } + } + + // Propagate bits to the region group representative using the union find structure + for (int i = 0; i < counter.Length; i++) bits[union_find_find(counter, i)] |= bits[i]; + + for (int i = 0; i < counter.Length; i++) { + int ctr = union_find_find(counter, i); + + // Check if the region is adjacent to border. + // Mark it as being just large enough to always be included in the graph. + if ((bits[ctr] & BorderBit) != 0) counter[ctr] = -minRegionSize-2; + + // Not in any relevant surface + // or it is adjacent to a border (see RelevantSurfaceCheck) + if (anySurfaces && (bits[ctr] & RelevantSurfaceCheck) == 0) counter[ctr] = -1; + } + + for (int i = 0; i < reg.Length; i++) { + int r = reg[i]; + // Ignore border regions + if (r >= nReg) { + continue; + } + + // If the region group is too small then make the span unwalkable + if (counter[union_find_find(counter, r)] >= -minRegionSize-1) { + reg[i] = 0; + } + } + } + } + + static class VoxelUtilityBurst { + /// <summary>All bits in the region which will be interpreted as a tag.</summary> + public const int TagRegMask = TagReg - 1; + + /// <summary> + /// If a cell region has this bit set then + /// The remaining region bits (see <see cref="TagRegMask)"/> will be used for the node's tag. + /// </summary> + public const int TagReg = 1 << 14; + + /// <summary> + /// If heightfield region ID has the following bit set, the region is on border area + /// and excluded from many calculations. + /// </summary> + public const ushort BorderReg = 1 << 15; + + /// <summary> + /// If contour region ID has the following bit set, the vertex will be later + /// removed in order to match the segments and vertices at tile boundaries. + /// </summary> + public const int RC_BORDER_VERTEX = 1 << 16; + + public const int RC_AREA_BORDER = 1 << 17; + + public const int VERTEX_BUCKET_COUNT = 1<<12; + + /// <summary>Tessellate wall edges</summary> + public const int RC_CONTOUR_TESS_WALL_EDGES = 1 << 0; + + /// <summary>Tessellate edges between areas</summary> + public const int RC_CONTOUR_TESS_AREA_EDGES = 1 << 1; + + /// <summary>Tessellate edges at the border of the tile</summary> + public const int RC_CONTOUR_TESS_TILE_EDGES = 1 << 2; + + /// <summary>Mask used with contours to extract region id.</summary> + public const int ContourRegMask = 0xffff; + + public static readonly int[] DX = new int[] { -1, 0, 1, 0 }; + public static readonly int[] DZ = new int[] { 0, 1, 0, -1 }; + + public static void CalculateDistanceField (CompactVoxelField field, NativeArray<ushort> output) { + int wd = field.width*field.depth; + + // Mark boundary cells + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + + int numConnections = 0; + for (int d = 0; d < 4; d++) { + if (s.GetConnection(d) != CompactVoxelField.NotConnected) { + //This function (CalculateDistanceField) is used for both ErodeWalkableArea and by itself. + //The C++ recast source uses different code for those two cases, but I have found it works with one function + //the field.areaTypes[ni] will actually only be one of two cases when used from ErodeWalkableArea + //so it will have the same effect as + // if (area != UnwalkableArea) { + //This line is the one where the differ most + + numConnections++; + } else { + break; + } + } + + // TODO: Check initialization + output[i] = numConnections == 4 ? ushort.MaxValue : (ushort)0; + } + } + } + + // Grassfire transform + // Pass 1 + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + var dist = (int)output[i]; + + if (s.GetConnection(0) != CompactVoxelField.NotConnected) { + // (-1,0) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 0); + + int ni = field.cells[neighbourCell].index+s.GetConnection(0); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(3) != CompactVoxelField.NotConnected) { + // (-1,0) + (0,-1) = (-1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 3); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(3)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + if (s.GetConnection(3) != CompactVoxelField.NotConnected) { + // (0,-1) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 3); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(3)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(2) != CompactVoxelField.NotConnected) { + // (0,-1) + (1,0) = (1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 2); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(2)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + output[i] = (ushort)dist; + } + } + } + + // Pass 2 + + for (int z = wd-field.width; z >= 0; z -= field.width) { + for (int x = field.width-1; x >= 0; x--) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + var dist = (int)output[i]; + + if (s.GetConnection(2) != CompactVoxelField.NotConnected) { + // (-1,0) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 2); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(2)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(1) != CompactVoxelField.NotConnected) { + // (-1,0) + (0,-1) = (-1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 1); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(1)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + if (s.GetConnection(1) != CompactVoxelField.NotConnected) { + // (0,-1) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 1); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(1)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(0) != CompactVoxelField.NotConnected) { + // (0,-1) + (1,0) = (1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 0); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(0)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + output[i] = (ushort)dist; + } + } + } + +// #if ASTAR_DEBUGREPLAY && FALSE +// DebugReplay.BeginGroup("Distance Field"); +// for (int z = wd-field.width; z >= 0; z -= field.width) { +// for (int x = field.width-1; x >= 0; x--) { +// CompactVoxelCell c = field.cells[x+z]; + +// for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { +// DebugReplay.DrawCube(CompactSpanToVector(x, z/field.width, i), Vector3.one*cellSize, new Color((float)output[i]/maxDist, (float)output[i]/maxDist, (float)output[i]/maxDist)); +// } +// } +// } +// DebugReplay.EndGroup(); +// #endif + } + + public static void BoxBlur (CompactVoxelField field, NativeArray<ushort> src, NativeArray<ushort> dst) { + ushort thr = 20; + + int wd = field.width*field.depth; + + for (int z = wd-field.width; z >= 0; z -= field.width) { + for (int x = field.width-1; x >= 0; x--) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + + ushort cd = src[i]; + + if (cd < thr) { + dst[i] = cd; + continue; + } + + int total = (int)cd; + + for (int d = 0; d < 4; d++) { + if (s.GetConnection(d) != CompactVoxelField.NotConnected) { + var neighbourIndex = field.GetNeighbourIndex(cellIndex, d); + int ni = (int)(field.cells[neighbourIndex].index+s.GetConnection(d)); + + total += (int)src[ni]; + + CompactVoxelSpan ns = field.spans[ni]; + + int d2 = (d+1) & 0x3; + + if (ns.GetConnection(d2) != CompactVoxelField.NotConnected) { + var neighbourIndex2 = field.GetNeighbourIndex(neighbourIndex, d2); + + int nni = (int)(field.cells[neighbourIndex2].index+ns.GetConnection(d2)); + total += (int)src[nni]; + } else { + total += cd; + } + } else { + total += cd*2; + } + } + dst[i] = (ushort)((total+5)/9F); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta new file mode 100644 index 0000000..761ad5c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b5b3bda46dccdc886959894545826304 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: |