Add and use #cusp

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 02cb9c6c3046534ea619178a3cafefbd80b3dbec..3cee13bbf0219aeae311a8e07929b20dafefb606 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -161,7 +161,8 @@ class RRTExt11 : public virtual RRTS {
 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
  *
  * Use Reeds & Shepp path length for building tree data structure and Euclidean
- * distance plus (abs) heading difference for searching it.
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
  *
  * \ingroup ext-cost
  * \see https://doi.org/10.1109/TITS.2015.2477355

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 7b7f4fc4c7500cc70fda0254565b7026bdb23be9..603c0a8cc58a7168a8479fc14b04387977226619 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -36,6 +36,7 @@ private:
        double c_ = 0.0;
        double cc_ = 0.0;
        RRTNode* p_ = nullptr;
+       unsigned int cusp_ = 0;
 public:
        /*! Get cost to parent. */
        double c() const;
@@ -52,6 +53,12 @@ public:
        /*! Set parent node. */
        void p(RRTNode& p);
 
+       /*! Get number of backward-forward direction changes. */
+       unsigned int cusp() const;
+
+       /*! Set number of backward-forward direction changes. */
+       void cusp(RRTNode const& p);
+
        bool operator==(RRTNode const& n);
 };
 

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 63e2ade6596f449b21d72c932bc359d9e988e8bf..83fb6ed639681802058da73ff7c6d5a5961eb34d 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -21,7 +21,7 @@ RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 double
 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-       return f.edist(t) + std::abs(t.h() - f.h());
+       return f.edist(t) + std::abs(t.h() - f.h()) + f.cusp() * 0.1;
 }
 
 } // namespace rrts

diff --git a/src/rrts.cc b/src/rrts.cc
index 23f60a0eebaa7860ce9cea77d5bc8a25324139f7..68b4f6e32171f554e026f3bb171a820f970ff07f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,6 +59,21 @@ RRTNode::p(RRTNode& p)
        }
 }
 
+unsigned int
+RRTNode::cusp() const
+{
+       return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+       this->cusp_ = p.cusp();
+       if (this->sp() != p.sp() || this->sp() == 0.0) {
+               this->cusp_++;
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -108,6 +123,7 @@ 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;
        }
@@ -143,6 +159,7 @@ 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;
 }