Fixed precision error

robotics-upo · Jun 22, 2021 · 79d655a · 79d655a
1 parent 0c48db4
commit 79d655a
Show file tree

Hide file tree

Showing 5 changed files with 9 additions and 9 deletions.
diff --git a/src/Planners/AStarGenerator.cpp b/src/Planners/AStarGenerator.cpp
@@ -102,7 +102,7 @@ PathData AStarGenerator::findPath(const Vec3i &source_, const Vec3i &target_)
             if ( discrete_world_.isOccupied(newCoordinates) || 
                  discrete_world_.isInClosedList(newCoordinates) ) 
                 continue;
-            unsigned int totalCost = current->G + ( i < 6 ? 10 : ( i < 18 ? 14 : 17) ); //This is more efficient
+            unsigned int totalCost = current->G + (i < 6 ? 100 : (i < 18 ? 141 : 173)); //This is more efficient
 
             Node *successor = discrete_world_.getNodePtr(newCoordinates);
 

diff --git a/src/Planners/LazyThetaStarGenerator.cpp b/src/Planners/LazyThetaStarGenerator.cpp
@@ -111,7 +111,7 @@ namespace Planners
                 if (discrete_world_.isOccupied(newCoordinates) ||
                     discrete_world_.isInClosedList(newCoordinates))
                     continue;
-                unsigned int totalCost = current->G + (i < 6 ? 10 : (i < 18 ? 14 : 17)); //This is more efficient
+                unsigned int totalCost = current->G + (i < 6 ? 100 : (i < 18 ? 141 : 173)); //This is more efficient
 
                 Node *successor = discrete_world_.getNodePtr(newCoordinates);
 

diff --git a/src/Planners/ThetaStarGenerator.cpp b/src/Planners/ThetaStarGenerator.cpp
@@ -89,7 +89,7 @@ namespace Planners
                 if (discrete_world_.isOccupied(newCoordinates) ||
                     discrete_world_.isInClosedList(newCoordinates))
                     continue;
-                unsigned int totalCost = current->G + (i < 6 ? 10 : (i < 18 ? 14 : 17));
+                unsigned int totalCost = current->G + (i < 6 ? 100 : (i < 18 ? 141 : 173)); //This is more efficient
 
                 Node *successor = discrete_world_.getNodePtr(newCoordinates);
 

diff --git a/src/utils/geometry_utils.cpp b/src/utils/geometry_utils.cpp
@@ -20,9 +20,9 @@ namespace Planners
             }
             unsigned int distanceBetween2Nodes(const Node &n1, const Node &n2)
             {
-                return static_cast<unsigned int>(10 * sqrt(pow(n1.coordinates.x - n2.coordinates.x, 2) +
-                                                           pow(n1.coordinates.y - n2.coordinates.y, 2) +
-                                                           pow(n1.coordinates.z - n2.coordinates.z, 2)));
+                return static_cast<unsigned int>(100 * sqrt(pow(n1.coordinates.x - n2.coordinates.x, 2) +
+                                                            pow(n1.coordinates.y - n2.coordinates.y, 2) +
+                                                            pow(n1.coordinates.z - n2.coordinates.z, 2)));
             }
             unsigned int distanceBetween2Nodes(const Node *n1, const Node *n2)
             {

diff --git a/src/utils/heuristic.cpp b/src/utils/heuristic.cpp
@@ -12,13 +12,13 @@ namespace Planners
     unsigned int Heuristic::manhattan(Vec3i source_, Vec3i target_)
     {
         auto delta = std::move(getDelta(source_, target_));
-        return static_cast<unsigned int>(10 * (delta.x + delta.y + delta.z));
+        return static_cast<unsigned int>(100 * (delta.x + delta.y + delta.z));
     }
 
     unsigned int Heuristic::euclidean(Vec3i source_, Vec3i target_)
     {
         auto delta = std::move(getDelta(source_, target_));
-        return static_cast<unsigned int>(10 * sqrt(pow(delta.x, 2) + pow(delta.y, 2) + pow(delta.z, 2)));
+        return static_cast<unsigned int>(100 * sqrt(pow(delta.x, 2) + pow(delta.y, 2) + pow(delta.z, 2)));
     }
     unsigned int Heuristic::dikjstra(Vec3i source_, Vec3i target_)
     {
@@ -28,7 +28,7 @@ namespace Planners
     unsigned int Heuristic::octagonal(Vec3i source_, Vec3i target_)
     {
         auto delta = std::move(getDelta(source_, target_));
-        return 10 * (delta.x + delta.y + delta.z) + (-6) * std::min(delta.x, delta.y);
+        return 100 * (delta.x + delta.y + delta.z) + (-6) * std::min(delta.x, delta.y);
     }
 
 }