Store info about if parent is cusp in node

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 97fe2d028cfb5cb4ad7e981ab94de4c15329f9a3..dc146c488bd0b8cc731f9fd7f3a8f7044b7bfe63 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -38,6 +38,7 @@ private:
        RRTNode* _p = nullptr;
        unsigned int _cusp_cnt = 0;
        int _segment_type = 0;  // 0 ~ straight, 1 ~ left, -1 right
+       bool _p_is_cusp = false;
 public:
        /*! Get cost to parent. */
        double c() const;
@@ -72,6 +73,12 @@ public:
        /*! Set Reeds & Shepp segment type. */
        void st(int st);
 
+       /*! Return true if the parent is cusp node and false otherwise. */
+       bool p_is_cusp(void) const;
+
+       /*! Set if the parent node is cusp (direction changed from parent). */
+       void p_is_cusp(bool isit);
+
        bool operator==(RRTNode const& n);
 };
 

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 8c200281786ea074f328c53bbac6352bf2645a37..ae27cfa49aeb4cf8e24da544651fe8c2617239c9 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -89,6 +89,18 @@ 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::operator==(RRTNode const& n)
 {