AStarD::AStar / AStarD::DHeap::Heap

Author: xzripper

AStarD/AStar.d

@@ -0,0 +1,110 @@
+// A* (A-Star) search algorithm for 2D grids by Évan (xzripper): https://github.com/xzripper/AStarD
+
+module AStarD.AStar;
+
+import std.math : abs, pow, sqrt;
+
+import std.algorithm : reverse;
+
+import AStarD.DHeap.Heap;
+
+alias ASPath = int[2][], Grid2d = int[][], Position2D = int[2];
+
+enum Heuristic { MANHATTAN, EUCLIDEAN, OCTILE, CHEBYSHEV }
+
+private static immutable STRAIGHT_DIRECTIONS = [[-1, 0], [1, 0], [0, -1], [0, 1]];
+private static immutable STRAIGHT_DIAGONAL_DIRECTIONS = [[-1, 0], [1, 0], [0, -1], [0, 1], 
+                                                        [-1, -1], [-1, 1], [1, -1], [1, 1]];
+
+private alias Node = float[3];
+
+private float _CalculateHeuristic(
+    Position2D p_Position0,
+    Position2D p_Position1,
+    Heuristic p_Heuristic ) @safe
+{
+    if( p_Heuristic == Heuristic.MANHATTAN ) {
+        return abs( p_Position0[0] - p_Position1[0] ) + abs( p_Position0[1] - p_Position1[1] );
+    } else if( p_Heuristic == Heuristic.EUCLIDEAN ) {
+        return sqrt( cast(float) (pow( p_Position0[0] - p_Position1[0], 2 ) + 
+                                  pow( p_Position0[1] - p_Position1[1], 2 )) );
+    } else if( p_Heuristic == Heuristic.OCTILE ) {
+        float t_DX = abs( p_Position0[0] - p_Position1[0] );
+        float t_DY = abs( p_Position0[1] - p_Position1[1] );
+
+        return (t_DX > t_DY ? t_DX : t_DY) + (sqrt( 2.0f ) - 1) * (t_DY > t_DX ? t_DY : t_DX);
+    } else if( p_Heuristic == Heuristic.CHEBYSHEV ) {
+        float t_MaxOp0 = abs( p_Position0[0] - p_Position1[0] );
+        float t_MaxOp1 = abs( p_Position0[1] - p_Position1[1] );
+
+        return (t_MaxOp0 > t_MaxOp1 ? t_MaxOp0 : t_MaxOp1);
+    } else { assert( 0 ); }
+}
+
+ASPath AStar(
+    Grid2d p_Grid,
+    Position2D p_Start,
+    Position2D p_Target,
+    Heuristic p_Heuristic,
+    bool p_AllowDiagonalMovement ) @safe
+{
+    Heap!Node t_NodesHeap = Heap!Node( HeapType.MIN_HEAP );
+
+    t_NodesHeap.HeapPush( [0, p_Start[0], p_Start[1]] );
+
+    int[2][int[2]] t_CameFrom;
+
+    float[int[2]] t_GScores = [p_Start: 0];
+    float[int[2]] t_FScores = [p_Start: _CalculateHeuristic( p_Start, p_Target, p_Heuristic )];
+
+    while( !t_NodesHeap.Empty() )
+    {
+        float[3] t_HeapPopOut = t_NodesHeap.HeapPop();
+
+        int[2] t_CurrentNodePos = [cast(int) t_HeapPopOut[1], cast(int) t_HeapPopOut[2]];
+
+        if( t_CurrentNodePos == p_Target )
+        {
+            int[2][] t_OutPath;
+
+            while( t_CurrentNodePos in t_CameFrom )
+            {
+                t_OutPath ~= t_CurrentNodePos;
+
+                t_CurrentNodePos = t_CameFrom[t_CurrentNodePos];
+            }
+
+            t_OutPath ~= p_Start;
+
+            return reverse( t_OutPath );
+        }
+
+        foreach( int[2] t_Direction; (p_AllowDiagonalMovement ? STRAIGHT_DIAGONAL_DIRECTIONS : STRAIGHT_DIRECTIONS) )
+        {
+            int[2] t_Neighbour = [t_CurrentNodePos[0] + t_Direction[0], t_CurrentNodePos[1] + t_Direction[1]];
+
+            if( 0 <= t_Neighbour[0] && t_Neighbour[0] < p_Grid.length &&
+                0 <= t_Neighbour[1] && t_Neighbour[1] < p_Grid[0].length )
+            {
+                if( p_Grid[t_Neighbour[0]][t_Neighbour[1]] == 1 ) { continue; }
+
+                float t_TentativeGScore = t_GScores[t_CurrentNodePos] + (p_AllowDiagonalMovement ? 
+                        (t_Direction[0] != 0 && t_Direction[1] != 0 ? sqrt( 2.0f ) : 1.0f) : 1);
+
+                if( t_Neighbour !in t_GScores || t_TentativeGScore < t_GScores[t_Neighbour] )
+                {
+                    t_CameFrom[t_Neighbour] = t_CurrentNodePos;
+
+                    t_GScores[t_Neighbour] = t_TentativeGScore;
+                    t_FScores[t_Neighbour] = t_TentativeGScore + _CalculateHeuristic( t_Neighbour,
+                                                                                      p_Target,
+                                                                                      p_Heuristic );
+
+                    t_NodesHeap.HeapPush( [t_FScores[t_Neighbour], t_Neighbour[0], t_Neighbour[1]] );
+                }
+            }
+        }
+    }
+
+    return [];
+}

AStarD/DHeap/Heap.d

@@ -0,0 +1,142 @@
+/**
+ * This is a heap implementation that can be used as a Min-Heap or Max-Heap.
+ * It keeps the smallest or biggest item at the top depending on the type of heap.
+ * The heap can be built from an already existing array or can be initially empty.
+ * New items can be added, and the data structure will rearrange itself to maintain order.
+ * Popping the top element replaces it with the last and reorders the heap.
+ * It is iterable and optimized to avoid useless allocations of memory.
+ * Uses safe code and minimizes reliance on the garbage collector.
+ *
+ * https://github.com/xzripper/DHeap by Évan
+*/
+
+module DHeap.Heap;
+
+enum HeapType { MAX_HEAP, MIN_HEAP }
+
+struct Heap(T)
+{
+    private T[] m_HeapData;
+
+    private HeapType m_HeapType;
+
+    this( T[] p_HeapInitObjs, HeapType p_HeapType ) @safe
+    {
+        m_HeapData = p_HeapInitObjs;
+        m_HeapType = p_HeapType;
+
+        for(int t_Idx=((cast(int) m_HeapData.length) / 2) - 1; t_Idx >= 0; t_Idx--)
+        {
+            _HeapSiftDown( t_Idx );
+        }
+    }
+
+    this( HeapType p_HeapType ) @safe @nogc
+    {
+        m_HeapType = p_HeapType;
+    }
+
+    void HeapPush( T p_HeapObject ) @safe
+    {
+        m_HeapData ~= p_HeapObject;
+
+        _HeapSiftUp( m_HeapData.length - 1 );
+    }
+
+    T HeapPop() @safe
+    {
+        T t_HeapFront = m_HeapData[0];
+
+        m_HeapData[0] = m_HeapData[$-1];
+        m_HeapData = m_HeapData[0..$-1];
+
+        _HeapSiftDown( 0 );
+
+        return t_HeapFront;
+    }
+
+    private void _HeapSiftUp( size_t p_Idx ) @safe {
+        while( p_Idx > 0 )
+        {
+            size_t t_HeapParent = (p_Idx - 1) / 2;
+
+            if( _Compare( m_HeapData[p_Idx], m_HeapData[t_HeapParent] ) )
+            {
+                _Swap( m_HeapData[p_Idx], m_HeapData[t_HeapParent] );
+
+                p_Idx = t_HeapParent;
+            } else { break; }
+        }
+    }
+
+    private void _HeapSiftDown( size_t p_Idx ) @safe {
+        while( 1 )
+        {
+            size_t t_HeapLeft = 2 * p_Idx + 1;
+            size_t t_HeapRight = 2 * p_Idx + 2;
+
+            size_t t_HeapTarget = p_Idx;
+
+            if( t_HeapLeft < m_HeapData.length && _Compare( m_HeapData[t_HeapLeft], m_HeapData[t_HeapTarget] ) )
+            {
+                t_HeapTarget = t_HeapLeft;
+            }
+
+            if( t_HeapRight < m_HeapData.length && _Compare( m_HeapData[t_HeapRight], m_HeapData[t_HeapTarget] ) )
+            {
+                t_HeapTarget = t_HeapRight;
+            }
+
+            if( t_HeapTarget == p_Idx )
+            {
+                break;
+            }
+
+            _Swap( m_HeapData[p_Idx], m_HeapData[t_HeapTarget] );
+
+            p_Idx = t_HeapTarget;
+        }
+    }
+
+    private void _Swap( ref T p_Object0, ref T p_Object1 ) @safe @nogc
+    {
+        T t_Tmp0 = p_Object0;
+
+        p_Object0 = p_Object1;
+        p_Object1 = t_Tmp0;
+    }
+
+    private bool _Compare( T p_Object0, T p_Object1 ) @safe @nogc
+    {
+        return (m_HeapType == HeapType.MIN_HEAP) ? p_Object0 < p_Object1 : p_Object0 > p_Object1;
+    }
+
+    T HeapFront() const @property @safe @nogc
+    {
+        return m_HeapData[0];
+    }
+
+    bool Empty() const @property @safe @nogc
+    {
+        return m_HeapData.length == 0;
+    }
+
+    size_t Size() const @property @safe @nogc
+    {
+        return m_HeapData.length;
+    }
+
+    int opApply( int delegate(ref T) HEAP_DELEGATE )
+    {
+        foreach( ref t_HeapObject; m_HeapData )
+        {
+            auto t_DelegateOut = HEAP_DELEGATE( t_HeapObject );
+
+            if( t_DelegateOut ) {
+                return t_DelegateOut;
+            }
+        }
+
+        return 0;
+    }
+}