Set is cusp and cusp count when setting parent

[hubacji1/iamcar2.git] / rrts / src / rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 7cd4684deadee7ab413b97553309b178becd3678..da4ae36f6fcbb6a78164113a2f0f82f0ac27a7c8 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -60,20 +60,22 @@ RRTNode::p(RRTNode& p)
        if (this != &p) {
                this->_p = &p;
        }
+       this->p_is_cusp(this->would_be_cusp_if_parent(p));
+       this->cusp_cnt(p);
 }
 
 unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
 {
-       return this->_cusp;
+       return this->_cusp_cnt;
 }
 
 void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::cusp_cnt(RRTNode const& p)
 {
-       this->_cusp = p.cusp();
-       if (this->sp() != p.sp() || this->sp() == 0.0) {
-               this->_cusp++;
+       this->_cusp_cnt = p.cusp_cnt();
+       if (this->_p_is_cusp) {
+               this->_cusp_cnt++;
        }
 }
 
@@ -89,6 +91,43 @@ RRTNode::st(int st)
        this->_segment_type = st;
 }
 
+bool
+RRTNode::p_is_cusp(void) const
+{
+       return this->_p_is_cusp;
+}
+
+void
+RRTNode::p_is_cusp(bool isit)
+{
+       this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+       if (p.sp() == 0) {
+               assert(this->sp() != 0);
+               if (p.p()) {
+                       if (sgn(p.p()->sp()) != sgn(this->sp())) {
+                               return true;
+                       } else {
+                               return false;
+                       }
+               } else {
+                       return true; // only root has no parent and it is cusp
+               }
+       } else {
+               if (this->sp() == 0) {
+                       return false; // this is cusp, not the parent
+               } else if (sgn(p.sp()) != sgn(this->sp())) {
+                       return true;
+               } else {
+                       return false;
+               }
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -140,7 +179,6 @@ RRTS::join_steered(RRTNode* f)
                RRTNode* t = &this->_nodes.back();
                t->p(*f);
                t->c(this->cost_build(*f, *t));
-               t->cusp(*f);
                this->_steered.erase(this->_steered.begin());
                f = t;
        }
@@ -151,6 +189,12 @@ RRTS::connect()
 {
        RRTNode* f = this->_nn;
        RRTNode* t = &this->_steered.front();
+       // Require the steer method to return first node equal to nn:
+       assert(std::abs(t->x() - f->x()) < 1e-3);
+       assert(std::abs(t->y() - f->x()) < 1e-3);
+       assert(std::abs(t->h() - f->x()) < 1e-3);
+       this->_steered.erase(this->_steered.begin());
+       t = &this->_steered.front();
 #if USE_RRTS
        double cost = f->cc() + this->cost_build(*f, *t);
        for (auto n: this->nv_) {
@@ -172,7 +216,6 @@ RRTS::connect()
        t = &this->_nodes.back();
        t->p(*f);
        t->c(this->cost_build(*f, *t));
-       t->cusp(*f);
        this->_steered.erase(this->_steered.begin());
        return true;
 }