From 79d655a6503a5a6dcb2d761c59f751f8734d16c6 Mon Sep 17 00:00:00 2001 From: Rafael Rey Date: Tue, 22 Jun 2021 11:37:08 +0200 Subject: [PATCH] Fixed precision error --- src/Planners/AStarGenerator.cpp | 2 +- src/Planners/LazyThetaStarGenerator.cpp | 2 +- src/Planners/ThetaStarGenerator.cpp | 2 +- src/utils/geometry_utils.cpp | 6 +++--- src/utils/heuristic.cpp | 6 +++--- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Planners/AStarGenerator.cpp b/src/Planners/AStarGenerator.cpp index c7401f7..08d85f3 100644 --- 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 index e5b7092..82d3809 100644 --- 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 index a6e1587..e0fd759 100644 --- 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 index 4fb229e..e688a56 100644 --- 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(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(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 index d86513c..e062049 100644 --- 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(10 * (delta.x + delta.y + delta.z)); + return static_cast(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(10 * sqrt(pow(delta.x, 2) + pow(delta.y, 2) + pow(delta.z, 2))); + return static_cast(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); } }