Add and use #cusp

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

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 0e12a5c4dc52ce82295ddcdea3a312ac68525b41..603c0a8cc58a7168a8479fc14b04387977226619 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 /*! \brief RRT* structure and default procedures.
  *
  * \file
@@ -30,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;
@@ -46,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);
 };
 
@@ -70,6 +83,10 @@ protected:
        double cost_ = 0.0;
        double eta_ = 0.5;
        double time_ = 0.0;
+       double last_goal_cc_ = 0.0;
+       std::vector<RRTNode> last_path_;
+       void recompute_cc(RRTNode* g);
+       void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;
        void join_steered(RRTNode* f);