Adjust a* heuristics based on grid connectivity

Author: mlsdpk

figures/img0.png

@@ -37,6 +37,8 @@ class ASTAR : public BFS {
   std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>,
                       MinimumDistanceASTAR>
       frontier_;
+
+  bool use_manhattan_heuristics_{true};
 };
 
 }  // namespace graph_based

include/States/Algorithms/GraphBased/ASTAR/ASTAR.h

@@ -6,10 +6,23 @@ namespace path_finding_visualizer {
 namespace graph_based {
 namespace utils {
 
-inline double L1_Distance(const std::shared_ptr<Node> &n1,
-                          const std::shared_ptr<Node> &n2) {
-  return fabs(n1->getPos().x - n2->getPos().x) +
-         fabs(n1->getPos().y - n2->getPos().y);
+inline double distanceCost(const std::shared_ptr<Node> &n1,
+                           const std::shared_ptr<Node> &n2) {
+  return std::sqrt(
+      (n1->getPos().x - n2->getPos().x) * (n1->getPos().x - n2->getPos().x) +
+      (n1->getPos().y - n2->getPos().y) * (n1->getPos().y - n2->getPos().y));
+}
+
+inline double costToGoHeuristics(const std::shared_ptr<Node> &n1,
+                                 const std::shared_ptr<Node> &n2,
+                                 bool use_manhattan = false) {
+  if (use_manhattan)
+    return fabs(n1->getPos().x - n2->getPos().x) +
+           fabs(n1->getPos().y - n2->getPos().y);
+
+  return std::sqrt(
+      (n1->getPos().x - n2->getPos().x) * (n1->getPos().x - n2->getPos().x) +
+      (n1->getPos().y - n2->getPos().y) * (n1->getPos().y - n2->getPos().y));
 }
 
 inline void addNeighbours(std::vector<std::shared_ptr<Node>> &nodes,

include/States/Algorithms/GraphBased/Utils.h

@@ -17,8 +17,12 @@ void ASTAR::initAlgorithm() {
   while (!frontier_.empty()) {
     frontier_.pop();
   }
+
+  use_manhattan_heuristics_ = (grid_connectivity_ == 0) ? true : false;
+
   nodeStart_->setGDistance(0.0);
-  nodeStart_->setFDistance(utils::L1_Distance(nodeStart_, nodeEnd_));
+  nodeStart_->setFDistance(utils::costToGoHeuristics(
+      nodeStart_, nodeEnd_, use_manhattan_heuristics_));
   frontier_.push(nodeStart_);
 }
 
@@ -65,15 +69,18 @@ void ASTAR::solveConcurrently(
           }
 
           double dist = nodeCurrent->getGDistance() +
-                        utils::L1_Distance(nodeCurrent, nodeNeighbour);
+                        utils::costToGoHeuristics(nodeCurrent, nodeNeighbour,
+                                                  use_manhattan_heuristics_);
 
           if (dist < nodeNeighbour->getGDistance()) {
             nodeNeighbour->setParentNode(nodeCurrent);
             nodeNeighbour->setGDistance(dist);
 
             // f = g + h
-            double f_dist = nodeCurrent->getGDistance() +
-                            utils::L1_Distance(nodeNeighbour, s_nodeEnd);
+            double f_dist =
+                nodeCurrent->getGDistance() +
+                utils::costToGoHeuristics(nodeNeighbour, s_nodeEnd,
+                                          use_manhattan_heuristics_);
             nodeNeighbour->setFDistance(f_dist);
             nodeNeighbour->setFrontier(true);
             frontier_.push(nodeNeighbour);

src/States/Algorithms/GraphBased/ASTAR/ASTAR.cpp

@@ -63,7 +63,7 @@ void DIJKSTRA::solveConcurrently(
           }
 
           double dist = nodeCurrent->getGDistance() +
-                        utils::L1_Distance(nodeCurrent, nodeNeighbour);
+                        utils::distanceCost(nodeCurrent, nodeNeighbour);
 
           if (dist < nodeNeighbour->getGDistance()) {
             nodeNeighbour->setParentNode(nodeCurrent);