Search Unity

Priority queue/min heap implementation with native containers?

Discussion in 'Data Oriented Technology Stack' started by snacktime, Oct 14, 2018.

  1. snacktime

    snacktime

    Joined:
    Apr 15, 2013
    Posts:
    2,312
    Sitting down to write one thought I'd check if anyone had done this yet. Was going to just take the fastest existing one which seems to be this and basically port it.
     
  2. tertle

    tertle

    Joined:
    Jan 25, 2011
    Posts:
    1,558
    I didn't write one with native containers, but instead made a native container version of a min heap.

    It's based off: https://github.com/roy-t/AStar/blob/master/Roy-T.AStar/MinHeap.cs
    Which is a very stripped down optimized version for pathfinding but it should be a decent enough starting point.

    Code (CSharp):
    1. using System;
    2. using Unity.Collections;
    3. using Unity.Collections.LowLevel.Unsafe;
    4. using Unity.Mathematics;
    5.  
    6. namespace BovineLabs.Systems.Pathfinding
    7. {
    8.     [NativeContainerSupportsDeallocateOnJobCompletion]
    9.     [NativeContainerSupportsMinMaxWriteRestriction]
    10.     [NativeContainer]
    11.     public unsafe struct NativeMinHeap : IDisposable
    12.     {
    13.         [NativeDisableUnsafePtrRestriction] private void* m_Buffer;
    14.         private int m_capacity;
    15. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    16.         private AtomicSafetyHandle m_Safety;
    17.         [NativeSetClassTypeToNullOnSchedule] private DisposeSentinel m_DisposeSentinel;
    18. #endif
    19.         private Allocator m_AllocatorLabel;
    20.  
    21.         private int m_head;
    22.         private int m_length;
    23.         private int m_MinIndex;
    24.         private int m_MaxIndex;
    25.  
    26.         public NativeMinHeap(int capacity, Allocator allocator/*, NativeArrayOptions options = NativeArrayOptions.ClearMemory*/)
    27.         {
    28.             Allocate(capacity, allocator, out this);
    29.             /*if ((options & NativeArrayOptions.ClearMemory) != NativeArrayOptions.ClearMemory)
    30.                 return;
    31.             UnsafeUtility.MemClear(m_Buffer, (long) m_capacity * UnsafeUtility.SizeOf<MinHeapNode>());*/
    32.         }
    33.  
    34.         private static void Allocate(int capacity, Allocator allocator, out NativeMinHeap nativeMinHeap)
    35.         {
    36.             var size = (long) UnsafeUtility.SizeOf<MinHeapNode>() * capacity;
    37.             if (allocator <= Allocator.None)
    38.                 throw new ArgumentException("Allocator must be Temp, TempJob or Persistent", nameof (allocator));
    39.             if (capacity < 0)
    40.                 throw new ArgumentOutOfRangeException(nameof (capacity), "Length must be >= 0");
    41.             if (size > int.MaxValue)
    42.                 throw new ArgumentOutOfRangeException(nameof (capacity),
    43.                     $"Length * sizeof(T) cannot exceed {(object) int.MaxValue} bytes");
    44.  
    45.             nativeMinHeap.m_Buffer = UnsafeUtility.Malloc(size, UnsafeUtility.AlignOf<MinHeapNode>(), allocator);
    46.             nativeMinHeap.m_capacity = capacity;
    47.             nativeMinHeap.m_AllocatorLabel = allocator;
    48.             nativeMinHeap.m_MinIndex = 0;
    49.             nativeMinHeap.m_MaxIndex = capacity - 1;
    50.             nativeMinHeap.m_head = -1;
    51.             nativeMinHeap.m_length = 0;
    52.  
    53. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    54.             DisposeSentinel.Create(out nativeMinHeap.m_Safety, out nativeMinHeap.m_DisposeSentinel, 1, allocator);
    55. #endif
    56.  
    57.  
    58.         }
    59.  
    60.         public bool HasNext()
    61.         {
    62. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    63.             AtomicSafetyHandle.CheckReadAndThrow(m_Safety);
    64. #endif
    65.             return m_head >= 0;
    66.         }
    67.  
    68.         public void Push(MinHeapNode node)
    69.         {
    70. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    71.             if (m_length == m_capacity)
    72.                 throw new IndexOutOfRangeException($"Capacity Reached");
    73.             AtomicSafetyHandle.CheckReadAndThrow(m_Safety);
    74. #endif
    75.  
    76.             if (m_head < 0)
    77.             {
    78.                 m_head = m_length;
    79.             }
    80.             else if (node.ExpectedCost < this[m_head].ExpectedCost)
    81.             {
    82.                 node.Next = m_head;
    83.                 m_head = m_length;
    84.             }
    85.             else
    86.             {
    87.                 var currentPtr = m_head;
    88.                 var current = this[currentPtr];
    89.  
    90.                 while (current.Next >= 0 && this[current.Next].ExpectedCost <= node.ExpectedCost)
    91.                 {
    92.                     currentPtr = current.Next;
    93.                     current = this[current.Next];
    94.                 }
    95.  
    96.                 node.Next = current.Next;
    97.                 current.Next = m_length;
    98.  
    99.                 UnsafeUtility.WriteArrayElement(m_Buffer, currentPtr, current);
    100.             }
    101.  
    102.             UnsafeUtility.WriteArrayElement(m_Buffer, m_length, node);
    103.             m_length += 1;
    104.         }
    105.  
    106.         public int Pop()
    107.         {
    108. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    109.             AtomicSafetyHandle.CheckWriteAndThrow(m_Safety);
    110. #endif
    111.             var result = m_head;
    112.             m_head = this[m_head].Next;
    113.             return result;
    114.         }
    115.  
    116.         public MinHeapNode this[int index]
    117.         {
    118.             get
    119.             {
    120. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    121.                 if (index < m_MinIndex || index > m_MaxIndex)
    122.                     FailOutOfRangeError(index);
    123.                 AtomicSafetyHandle.CheckReadAndThrow(m_Safety);
    124. #endif
    125.    
    126.                 return UnsafeUtility.ReadArrayElement<MinHeapNode>(m_Buffer, index);
    127.             }
    128.         }
    129.  
    130.         public void Clear()
    131.         {
    132.             m_head = -1;
    133.             m_length = 0;
    134.         }
    135.  
    136.         public void Dispose()
    137.         {
    138.             if (!UnsafeUtility.IsValidAllocator(m_AllocatorLabel))
    139.                 throw new InvalidOperationException("The NativeArray can not be Disposed because it was not allocated with a valid allocator.");
    140. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    141.             DisposeSentinel.Dispose(ref m_Safety, ref m_DisposeSentinel);
    142. #endif
    143.             UnsafeUtility.Free(m_Buffer, m_AllocatorLabel);
    144.             m_Buffer = null;
    145.             m_capacity = 0;
    146.         }
    147.  
    148. #if ENABLE_UNITY_COLLECTIONS_CHECKS
    149.         private void FailOutOfRangeError(int index)
    150.         {
    151.             if (index < m_capacity && (this.m_MinIndex != 0 || this.m_MaxIndex != m_capacity - 1))
    152.                 throw new IndexOutOfRangeException(
    153.                     $"Index {(object) index} is out of restricted IJobParallelFor range [{(object) this.m_MinIndex}...{(object) this.m_MaxIndex}] in ReadWriteBuffer.\nReadWriteBuffers are restricted to only read & write the element at the job index. You can use double buffering strategies to avoid race conditions due to reading & writing in parallel to the same elements from a job.");
    154.             throw new IndexOutOfRangeException(
    155.                 $"Index {(object) index} is out of range of '{(object) m_capacity}' Length.");
    156.         }
    157. #endif
    158.     }
    159.  
    160.     public struct MinHeapNode
    161.     {
    162.         public MinHeapNode(int2 position, float expectedCost)
    163.         {
    164.             Position = position;
    165.             ExpectedCost = expectedCost;
    166.             Next = -1;
    167.         }
    168.  
    169.         public int2 Position { get; } // TODO to position
    170.         public float ExpectedCost { get; }
    171.         public int Next { get; set; }
    172.     }
    173. }
    Public methods
    Code (CSharp):
    1. bool HasNext();
    2. void Push(MinHeapNode node);
    3. int Pop();
    4. MinHeapNode this[int index];
    5. void Clear();
    6. void Dispose();
    As you'd expect, it works fine in jobs but no Concurrent version (I didn't need it as each pathfinder is it's own job in my system. Also I'd probably need to use a lock to implement it)
     
    Last edited: Oct 15, 2018
  3. snacktime

    snacktime

    Joined:
    Apr 15, 2013
    Posts:
    2,312
    Thanks for sharing that! Ya this approach is better for a native container implementation then the one I linked. The other looked to be faster in theory, but in practice requires significantly more array writes plus a secondary index to support updating arbitrary nodes.
     
  4. snacktime

    snacktime

    Joined:
    Apr 15, 2013
    Posts:
    2,312
    Ah I thought that looked familiar my use case is pathfinding also. I have an older version of his stuff where I fixed up the allocations, but I was taking a new look at what to use while wanting to move it all to jobs and native containers.
     
  5. tertle

    tertle

    Joined:
    Jan 25, 2011
    Posts:
    1,558
  6. tertle

    tertle

    Joined:
    Jan 25, 2011
    Posts:
    1,558
    Oh yeah, I forgot I had added a bunch more capabilities. I haven't worked on this much recently as I've had another contract, but someone might find it useful.

    Here's my most recent path job, which adds support for grids with different heights (climbing etc)

    Code (CSharp):
    1.         [BurstCompile]
    2.         private struct FindPathJob : IJob
    3.         {
    4.             private const float Rad2Deg = 57.29578f;
    5.        
    6.             public PathRequest Request;
    7.             public NavigationCapabilities Capabilities;
    8.             public int DimX;
    9.             public int DimY;
    10.             public int Iterations;
    11.        
    12.             [ReadOnly] public NativeArray<Neighbour> Neighbours;
    13.             [ReadOnly] public BufferArray<Cell> Grid;
    14.        
    15.             public DynamicBuffer<float3> Waypoints;
    16.             public NativeArray<float> CostSoFar;
    17.             public NativeArray<int2> CameFrom;
    18.             public NativeMinHeap OpenSet;
    19.  
    20.             public float3 Position;
    21.  
    22.             private int2 _start;
    23.             private int2 _goal;
    24.  
    25.             public void Execute()
    26.             {
    27.                 Waypoints.Clear();
    28.  
    29.                 _start = Position.xz.FloorToInt();
    30.                 _goal = Request.To.FloorToInt();
    31.  
    32.                 // Special case when the start is the same point as the goal
    33.                 if (_start.Equals(_goal))
    34.                 {
    35.                     // We just set the destination as the goal, but need to get the correct height
    36.                     var index = GetIndex(_goal);
    37.                     var cell = Grid[0][index];
    38.                     var point = new float3(Request.To.x, cell.Height, Request.To.y);
    39.                     Waypoints.Add(point);
    40.                     return;
    41.                 }
    42.  
    43.                 if (ProcessPath())
    44.                 {
    45.                     ReconstructPath();
    46.                     //CleanupEnds();
    47.                 }
    48.             }
    49.  
    50.             private bool ProcessPath()
    51.             {
    52.                 var grid = Grid[0];
    53.            
    54.                 // Push the start to the set
    55.                 var head = new MinHeapNode(_start, H(_start, _goal));
    56.                 OpenSet.Push(head);
    57.  
    58.                 // While we still have potential nodes to explore
    59.                 while (OpenSet.HasNext())
    60.                 {
    61.                     var currentIndex = OpenSet.Pop();
    62.                     var current = OpenSet[currentIndex];
    63.                
    64.                     // Found our goal
    65.                     if (current.Position.Equals(_goal))
    66.                         return true;
    67.                
    68.                     // Path might still be obtainable but we've run out of allowed iterations
    69.                     if (Iterations == 0)
    70.                     {
    71.                         if (Request.NavigateToBestIfIncomplete)
    72.                         {
    73.                             // Return the best result we've found so far
    74.                             // Need to update goal so we can reconstruct the shorter path
    75.                             _goal = current.Position;
    76.                             return true;
    77.                         }
    78.  
    79.                         return false;
    80.                     }
    81.                
    82.                     Iterations--;
    83.  
    84.                     // Found our goal
    85.                     if (current.Position.Equals(_goal))
    86.                         return true;
    87.  
    88.                     var initialCost = CostSoFar[GetIndex(current.Position)];
    89.  
    90.                     var fromIndex = GetIndex(current.Position);
    91.                
    92.                     // Loop our potential cells - generally neighbours but could include portals
    93.                     for (var i = 0; i < Neighbours.Length; i++)
    94.                     {
    95.                         Neighbour neigbour = Neighbours[i];
    96.                         var position = current.Position + neigbour.Offset;
    97.  
    98.                         // Make sure the node isn't outside our grid
    99.                         if (position.x < 0 || position.x >= DimX || position.y < 0 || position.y >= DimY)
    100.                             continue;
    101.  
    102.                         var index = GetIndex(position);
    103.                    
    104.                         // Get the cost of going to this cell
    105.                         var cellCost = GetCellCost(fromIndex, index, neigbour, true, grid);
    106.                    
    107.                         // Infinity means the cell is unwalkable, skip it
    108.                         if (float.IsInfinity(cellCost))
    109.                             continue;
    110.  
    111.                         var newCost = initialCost + neigbour.Distance * cellCost;
    112.                         var oldCost = CostSoFar[index];
    113.  
    114.                         // If we've explored this cell before and it was a better path, ignore this route
    115.                         if (!(oldCost <= 0) && !(newCost < oldCost))
    116.                             continue;
    117.  
    118.                         // Update the costing and best path
    119.                         CostSoFar[index] = newCost;
    120.                         CameFrom[index] = current.Position;
    121.  
    122.                         // Push the node onto our heap
    123.                         var expectedCost = newCost + H(position, _goal);
    124.                         OpenSet.Push(new MinHeapNode(position, expectedCost));
    125.                     }
    126.                 }
    127.  
    128.                 if (Request.NavigateToNearestIfBlocked)
    129.                 {
    130.                     // TODO
    131.                 }
    132.  
    133.                 //All routes have been explored without finding a route to destination
    134.                 return false;
    135.             }
    136.  
    137.             private float GetCellCost(int fromIndex, int toIndex, Neighbour neighbour, bool areNeighbours, DynamicBuffer<Cell> grid)
    138.             {
    139.                 var target = grid[toIndex];
    140.                 if (target.Blocked)
    141.                     return float.PositiveInfinity;
    142.  
    143.                 // If we're not neighbours, then we're a portal and can just go straight there
    144.                 if (!areNeighbours)
    145.                     return 1;
    146.  
    147.                 var from = grid[fromIndex];
    148.  
    149.                 var heightDiff = target.Height - @from.Height;
    150.                 var absDiff = math.abs(heightDiff);
    151.  
    152.                 // Should precompute this
    153.                 var dropHeight = 0;
    154.                 var climbHeight = 0;
    155.  
    156.                 if (heightDiff > 0)
    157.                     climbHeight = absDiff;
    158.                 else
    159.                     dropHeight = absDiff;
    160.  
    161.                 var slope = math.atan(absDiff / neighbour.Distance) * Rad2Deg;
    162.                 // End precompute
    163.  
    164.                 if ((Capabilities.MaxClimbHeight < climbHeight || Capabilities.MaxDropHeight < dropHeight) &&
    165.                     Capabilities.MaxSlopeAngle < slope)
    166.                     return float.PositiveInfinity;
    167.  
    168.                 return 1;
    169.  
    170.             }
    171.  
    172.             private static float H(int2 p0, int2 p1)
    173.             {
    174.                 var dx = p0.x - p1.x;
    175.                 var dy = p0.y - p1.y;
    176.                 var sqr = dx * dx + dy * dy;
    177.                 return math.sqrt(sqr);
    178.             }
    179.  
    180.             private void ReconstructPath()
    181.             {
    182.                 var grid = Grid[0];
    183.            
    184.                 // Check if our actual goal is closer to the goal node or the next node
    185.                 var end = GetPosition(_goal, grid);
    186.                 var current = CameFrom[GetIndex(_goal)];
    187.                 var currentNode = GetPosition(current, grid);
    188.            
    189.                 var to = new float3(Request.To.x, currentNode.y, Request.To.y);
    190.                 Waypoints.Add(to);
    191.  
    192.                 if (math.lengthsq(end.xz - to.xz) < math.length(currentNode.xz - to.xz))
    193.                     Waypoints.Add(end);
    194.  
    195.                 while (!current.Equals(_start))
    196.                 {
    197.                     Waypoints.Add(GetPosition(current, grid));
    198.                     current = CameFrom[GetIndex(current)];
    199.                 }
    200.            
    201.                 // Check if our actual start closer to the current node or previous
    202.                 var from = Position.xz;
    203.                 var start = GetPosition(current, grid);
    204.                 var next = Waypoints[Waypoints.Length - 1];
    205.  
    206.                 if (math.lengthsq(start.xz - from) < math.length(next.xz - from))
    207.                     Waypoints.Add(start);
    208.  
    209.                 Waypoints.Reverse();
    210.             }
    211.  
    212.             private float3 GetPosition(int2 point, DynamicBuffer<Cell> grid)
    213.             {
    214.                 var index = GetIndex(point);
    215.                 var cell = grid[index];
    216.                 var fPoint = point + new float2(0.5f, 0.5f);
    217.                 return new float3(fPoint.x, cell.Height, fPoint.y);
    218.             }
    219.  
    220.             private int GetIndex(int2 i)
    221.             {
    222.                 return i.y * DimX + i.x;
    223.             }
    224.         }
    Couple of the components

    Code (CSharp):
    1.     [Serializable]
    2.     public struct NavigationCapabilities : IComponentData
    3.     {
    4.         public float MaxSlopeAngle;
    5.         public float MaxClimbHeight;
    6.         public float MaxDropHeight;
    7.     }
    8.  
    9.     public struct PathRequest : IComponentData
    10.     {
    11.         public float2 To;
    12.         public Bool NavigateToNearestIfBlocked;
    13.         public Bool NavigateToBestIfIncomplete;
    14.     }
     
  7. snacktime

    snacktime

    Joined:
    Apr 15, 2013
    Posts:
    2,312
    One issue here in some cases is when positions go negative. This is a helper class for managing that which applies offsets and can also convert position back to index.

    Code (csharp):
    1.  
    2. public struct GridMeta
    3.     {
    4.         public int Width;
    5.         public int2 Center;
    6.         public int OffsetX;
    7.         public int OffsetY;
    8.         public int LowerBoundsX;
    9.         public int LowerBoundsY;
    10.         public int UpperBoundsX;
    11.         public int UpperBoundsY;
    12.  
    13.         public GridMeta(int width, int2 center)
    14.         {
    15.             Width = width;
    16.             Center = center;
    17.             int half = width / 2;
    18.  
    19.             UpperBoundsX = center.x + half - 1;
    20.             LowerBoundsX = center.x - half;
    21.             if (LowerBoundsX < 0)
    22.             {
    23.                 OffsetX = math.abs(LowerBoundsX);
    24.             }
    25.             else if (LowerBoundsX > 0)
    26.             {
    27.                 OffsetX = -math.abs(LowerBoundsX);
    28.             }
    29.             else
    30.             {
    31.                 OffsetX = 0;
    32.             }
    33.  
    34.  
    35.             UpperBoundsY = center.y + half - 1;
    36.             LowerBoundsY = center.y - half;
    37.  
    38.             if (LowerBoundsY < 0)
    39.             {
    40.                 OffsetY = math.abs(LowerBoundsY);
    41.             }
    42.             else if (LowerBoundsY > 0)
    43.             {
    44.                 OffsetY = -math.abs(LowerBoundsY);
    45.             }
    46.             else
    47.             {
    48.                 OffsetY = 0;
    49.             }
    50.         }
    51.  
    52.         [MethodImpl(MethodImplOptions.AggressiveInlining)]
    53.         public int PositionToIndex(int x, int y)
    54.         {
    55.             return Width * (x + OffsetX) + (y + OffsetY);
    56.         }
    57.  
    58.         [MethodImpl(MethodImplOptions.AggressiveInlining)]
    59.         public int2 IndexToPosition(int index)
    60.         {
    61.             int y = index % Width;
    62.             int x = index / Width;
    63.             return new int2(x - OffsetX, y - OffsetY);
    64.         }
    65.  
    66.     }
    67.  
     
  8. tertle

    tertle

    Joined:
    Jan 25, 2011
    Posts:
    1,558
    Actually, if you look at my original post I have an Offset field for handling negatives but my project now avoids negative space (for convenience and performance) which removed the need for it.
     
  9. snacktime

    snacktime

    Joined:
    Apr 15, 2013
    Posts:
    2,312
    Something I remembered while trying to void clearing those costs which is fairly expensive. Use a versioned value, then increment that version on every pathfind.