Reformat IS_NEAR, add IS_NEAR for goal

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5768286c8f3c28c665980fdea2c623aed1727da2..8a44e6e572cbfff7a61fc79b92b579226c1e1cff 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -91,10 +91,13 @@ RRTBase::~RRTBase()
         delete this->goal_;
 }
 
-RRTBase::RRTBase():
-        root_(new RRTNode()),
-        goal_(new RRTNode()),
-        gen_(std::random_device{}())
+RRTBase::RRTBase()
+        : root_(new RRTNode())
+        , goal_(new RRTNode())
+        , gen_(std::random_device{}())
+        , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
+        , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
@@ -102,10 +105,13 @@ RRTBase::RRTBase():
         this->add_ixy(this->root_);
 }
 
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
-        root_(init),
-        goal_(goal),
-        gen_(std::random_device{}())
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
+        : root_(init)
+        , goal_(goal)
+        , gen_(std::random_device{}())
+        , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
+        , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
@@ -465,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()) {
@@ -523,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
@@ -984,25 +990,19 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::setSamplingInfo(SamplingInfo si)
+{
+        this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
+        this->ndy_ = std::normal_distribution<float>(si.y0, si.y);
+        this->ndh_ = std::normal_distribution<float>(si.h0, si.h);
+}
+
 RRTNode *RRTBase::sample()
 {
-        if (this->useSamplingInfo_ && this->nodes().size() % 2 == 0) {
-                float x = static_cast<float>(rand());
-                x /= static_cast<float>(RAND_MAX / this->samplingInfo_.x);
-                x -= this->samplingInfo_.x / 2;
-                x += this->samplingInfo_.x0;
-                float y = static_cast<float>(rand());
-                y /= static_cast<float>(RAND_MAX / this->samplingInfo_.y);
-                y -= this->samplingInfo_.y / 2;
-                y += this->samplingInfo_.y0;
-                float h = static_cast<float>(rand());
-                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
-                h -= this->samplingInfo_.h / 2;
-                h += this->samplingInfo_.h0;
-                return new RRTNode(x, y, h);
-        } else {
-                return sa1();
-        }
+        float x = this->ndx_(this->gen_);
+        float y = this->ndy_(this->gen_);
+        float h = this->ndh_(this->gen_);
+        return new RRTNode(x, y, h);
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)