Dynamic connectivity

Author: Avanatiker

common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt

@@ -47,6 +47,7 @@ class DStarLite(
     var start: FastVector,
     val goal: FastVector,
     val heuristic: (FastVector, FastVector) -> Double,
+    private val connectivity: GraphUtil.Connectivity = GraphUtil.Connectivity.N26,
 ) {
     // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present
     private val gMap = mutableMapOf<FastVector, Double>()
@@ -150,7 +151,7 @@ class DStarLite(
      */
     fun invalidate(u: FastVector, prune: Boolean = false) {
         val modified = mutableSetOf(u)
-        (GraphUtil.n26(u).keys + u).forEach { v ->
+        (GraphUtil.neighborhood(u, connectivity).keys + u).forEach { v ->
             val current = graph.neighbors(v)
             val updated = graph.nodeInitializer(v)
             val removed = current.filter { w -> w !in updated }

common/src/main/kotlin/com/lambda/util/GraphUtil.kt

@@ -66,6 +66,7 @@ object GraphUtil {
         }
     }
 
+    fun neighborhood(o: FastVector, conn: Connectivity) = neighborhood(o, conn.minDistSq, conn.maxDistSq)
     fun n6(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 1)
     fun n18(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 2)
     fun n26(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 3)
@@ -94,4 +95,10 @@ object GraphUtil {
 
     private const val COST_SQRT_2 = 1.4142135623730951
     private const val COST_SQRT_3 = 1.7320508075688772
+
+    enum class Connectivity(val minDistSq: Int, val maxDistSq: Int) {
+        N6(minDistSq = 1, maxDistSq = 1),
+        N18(minDistSq = 1, maxDistSq = 2),
+        N26(minDistSq = 1, maxDistSq = 3);
+    }
 }