Rewrite ext10

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4d34736738fef69270ad1ace1241de456c948e40..1d241c27143573db9116f047b6977e638adfa8db 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,7 @@ link_libraries(jsoncpp_lib)
 add_library(rrts STATIC
        src/rrts.cc
        src/rrtext14.cc
+       src/rrtext10.cc
        src/rrtext2.cc
        src/reeds_shepp.cpp
 )

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 5f5e11bc869651a6ba22b9d14bc23f91e360bd4a..711655fad2dc5652cc7c0f9f79106c7851e57bb5 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -90,22 +90,17 @@ class RRTExt11 : public virtual RRTS {
 };
 
 /*! \brief Different costs extension.
-
-Use different cost for bulding tree data structure and searching in the
-structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and Reza
-Jazar. “Randomized Bidirectional B-Spline Parameterization Motion Planning.”
-IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February
-2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
-
-*/
+ *
+ * Use different cost for bulding tree data structure and searching in the
+ * structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and
+ * Reza Jazar. “Randomized Bidirectional B-Spline Parameterization Motion
+ * Planning.” IEEE Transactions on Intelligent Transportation Systems 17, no. 2
+ * (February 2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
+ */
 class RRTExt10 : public virtual RRTS {
-       public:
-               /*! \brief Reeds and Shepp path length.
-               */
-               double cost_build(RRTNode &f, RRTNode &t);
-               /*! \brief Heuristics distance.
-               */
-               double cost_search(RRTNode &f, RRTNode &t);
+protected:
+       double cost_build(RRTNode const& f, RRTNode const& t) const;
+       double cost_search(RRTNode const& f, RRTNode const& t) const;
 };
 
 /*! \brief Use grid data structure to store RRT nodes.

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index cdb39b808276ceac6363fd8e8a6a1642aeeadcb3..c9b5562a9a82b6cb65bf6a0e23ad8842e2def5e1 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -1,18 +1,21 @@
-#include "rrtext.h"
+#include "rrtext.hh"
 #include "reeds_shepp.h"
 
-double RRTExt10::cost_build(RRTNode &f, RRTNode &t)
+namespace rrts {
+
+double
+RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 {
        double q0[] = {f.x(), f.y(), f.h()};
        double q1[] = {t.x(), t.y(), t.h()};
-       ReedsSheppStateSpace rsss(this->bc.mtr());
+       ReedsSheppStateSpace rsss(this->bc_.mtr());
        return rsss.distance(q0, q1);
 }
 
-double RRTExt10::cost_search(RRTNode &f, RRTNode &t)
+double
+RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-       double cost = 0;
-       cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
-       cost += std::abs(t.h() - f.h());
-       return cost;
+       return f.edist(t) + std::abs(t.h() - f.h());
 }
+
+} // namespace rrts