From 3d2f293da0c8309ce8d7bea15df80485aaa28101 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 15 Jul 2021 10:42:10 +0200
Subject: [PATCH] Rewrite ext10

---
 CMakeLists.txt  |  1 +
 incl/rrtext.hh  | 25 ++++++++++---------------
 src/rrtext10.cc | 19 +++++++++++--------
 3 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4d34736..1d241c2 100644
--- 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 5f5e11b..711655f 100644
--- 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 cdb39b8..c9b5562 100644
--- 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
-- 
2.39.2