Modified heuristic function

vortexntnu · Mar 3, 2024 · 11d3d0b · 11d3d0b
1 parent 02da37c
commit 11d3d0b
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/path_planner/D_star_lite/D_star_lite/dsl.py b/path_planner/D_star_lite/D_star_lite/dsl.py
@@ -116,7 +116,11 @@ def c(self, node1: Node, node2: Node):
 
     def h(self, s: Node):
         # Can be modified
-        return 1
+        #return 1
+        #return max(abs(self.start.x - s.x), abs(self.start.y - s.y)) # Manhattan distance
+        dx = abs(self.start.x - s.x)
+        dy = abs(self.start.y - s.y)
+        return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy) # Chebyshev distance
 
     def calculate_key(self, s: Node):
         return (min(self.g[s.x][s.y], self.rhs[s.x][s.y]) + self.h(s) + self.km, min(self.g[s.x][s.y], self.rhs[s.x][s.y]))