Merge branch 'fix-steer'

[hubacji1/iamcar2.git] / rrts / incl / rrts.hh

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 97fe2d028cfb5cb4ad7e981ab94de4c15329f9a3..cccdc44ecb2e97582438aa2d99237a79dcb834fb 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;
@@ -52,9 +53,15 @@ public:
        RRTNode* p() const;
 
        /*! Set parent node. */
+       void p(RRTNode& p, bool connecting_goal);
        void p(RRTNode& p);
 
-       /*! Get number of backward-forward direction changes. */
+       /*! Get number of backward-forward direction changes.
+        *
+        * The parent node may be cusp to this node, i.e. parent may have sp ==
+        * 0 or sgn(sp) differs from sgn(this->sp). In such a situation, the
+        * number of this->cusp_cnt is incremented by one.
+        */
        unsigned int cusp_cnt() const;
 
        /*! Set number of backward-forward direction changes. */
@@ -72,6 +79,15 @@ 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);
+
+       /*! Return true if p would be cusp if set as parent of this. */
+       bool would_be_cusp_if_parent(RRTNode const& p) const;
+
        bool operator==(RRTNode const& n);
 };