Reformat IS_NEAR, add IS_NEAR for goal

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9600bb42db98097971ff5a8c969787cb37d4d0b5..8a44e6e572cbfff7a61fc79b92b579226c1e1cff 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -471,7 +471,7 @@ bool RRTBase::goal_found(
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode* ))
 {
-        if (IS_NEAR(node, this->goal_)) {
+        if (GOAL_IS_NEAR(node, this->goal_)) {
                 if (this->goal_found_) {
                         if (node->ccost() + this->cost(node, this->goal_) <
                                         this->goal_->ccost()) {
@@ -529,7 +529,7 @@ bool RRTBase::goal_found(
         RRTNode *goal
 )
 {
-        if (IS_NEAR(node, goal)) {
+        if (GOAL_IS_NEAR(node, goal)) {
                 if (this->goal_found_) {
                         if (
                                 goal->ccost() != -1

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 4ea4adfcfb57032d2a0852d29b50c8fb7247735f..4a501bfa8b804869c73192110fc3a3bdb8b435e1 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,11 +20,21 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <vector>
 
-#define IS_NEAR(a, b) ({ __typeof__ (a) _a = (a); \
-                __typeof__ (b) _b = (b); \
-                pow(pow(_b->x() - _a->x(), 2) + \
-                                pow(_b->y() - _a->y(), 2), 0.5) < 0.2 && \
-                std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; })
+#define IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.2 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
+
+#define GOAL_IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.05 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
 
 template <typename T> int sgn(T val) {
             return (T(0) < val) - (val < T(0));