Add and use #cusp

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

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 69f9175b204752a28b95ad13d49ecad320518f07..3cee13bbf0219aeae311a8e07929b20dafefb606 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 /*! \brief RRT* extensions.
  *
  * The extensions are used to implement or change the default behavior of the
@@ -104,6 +110,7 @@ private:
                RRTNode* node = nullptr;
                unsigned int i = 0;
                bool v = false;
+               double d = 0.0;
                bool vi();
                DijkstraNode(RRTNode* n);
        };
@@ -111,11 +118,16 @@ private:
        public:
                int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
        };
+       class DijkstraNodeBackwardComparator {
+       public:
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+       };
        std::vector<RRTNode*> opath_;
        double ogoal_cc_ = 0.0;
        double otime_ = 0.0;
        std::vector<DijkstraNode> dn_;
-       void pick_interesting();
+       void interesting_forward();
+       void interesting_backward();
        void dijkstra_forward();
        void dijkstra_backward();
        void compute_path();
@@ -149,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