Fix computation of cusp cnt

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index dc146c488bd0b8cc731f9fd7f3a8f7044b7bfe63..8503e0c79ea0e61ee2e1e6bd5538f5990d602352 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -55,7 +55,12 @@ public:
        /*! Set parent node. */
        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. */

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index ae27cfa49aeb4cf8e24da544651fe8c2617239c9..3d84e6225c0d10ed6ccf99da5ef512be8f122a3b 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -72,7 +72,7 @@ void
 RRTNode::cusp_cnt(RRTNode const& p)
 {
        this->_cusp_cnt = p.cusp_cnt();
-       if (this->sp() != p.sp() || this->sp() == 0.0) {
+       if (this->_p_is_cusp) {
                this->_cusp_cnt++;
        }
 }