Fix A* node ordering (#249)

cda-tum · Feb 26, 2023 · e30f48e · e30f48e
1 parent 699d294
commit e30f48e
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/include/heuristic/HeuristicMapper.hpp b/include/heuristic/HeuristicMapper.hpp
@@ -339,8 +339,10 @@ inline bool operator<(const HeuristicMapper::Node& x,
 
 inline bool operator>(const HeuristicMapper::Node& x,
                       const HeuristicMapper::Node& y) {
-  const auto xcost = static_cast<double>(x.costFixed) + x.lookaheadPenalty;
-  const auto ycost = static_cast<double>(y.costFixed) + y.lookaheadPenalty;
+  const auto xcost =
+      static_cast<double>(x.costFixed) + x.lookaheadPenalty + x.costHeur;
+  const auto ycost =
+      static_cast<double>(y.costFixed) + y.lookaheadPenalty + y.costHeur;
   if (std::abs(xcost - ycost) > 1e-6) {
     return xcost > ycost;
   }