1 files changed, 295 insertions, 0 deletions
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;
+		}
+	}
+}