From 9c306d1ac761584f613d5db591def9fd9e7f22b2 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 21 Oct 2019 15:26:30 +0200
Subject: [PATCH] Add ext6 (RS for build and search)

---
 CMakeLists.txt |  1 +
 README.md      |  1 +
 api/rrtext.h   | 12 ++++++++++++
 src/rrtext6.cc | 18 ++++++++++++++++++
 4 files changed, 32 insertions(+)
 create mode 100644 src/rrtext6.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 048d82a..4690652 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/api)
 
 add_library(rrts SHARED
         src/rrts.cc
+        src/rrtext6.cc
         src/rrtext5.cc
         src/rrtext4.cc
         src/rrtext3.cc
diff --git a/README.md b/README.md
index 0a2b57f..4ff9eb6 100644
--- a/README.md
+++ b/README.md
@@ -30,6 +30,7 @@ and upgrades to RRT, *extensions* are declared in `rrtext.h` and implemented in
 
 ## Implemented extensions
 There is a list of implemented extensions and what they include:
+- `rrtext6.cc`: Reeds and Shepp for both -- building and search costs,
 - `rrtext5.cc`: different cost for building (Reeds and Shepp) and searching
   (Euclidean distance),
 - `rrtext4.cc`: store RRT nodes to grid,
diff --git a/api/rrtext.h b/api/rrtext.h
index 172b220..5c9721a 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -6,6 +6,18 @@
 // ext2
 #include "cute_c2.h"
 
+/*! \brief Reeds and Shepp cost for building and search.
+*/
+class RRTExt6 : public virtual RRTS {
+        public:
+                /*! \brief Reeds and Shepp path length.
+                */
+                double cost_build(RRTNode &f, RRTNode &t);
+                /*! \brief Reeds and Shepp path length.
+                */
+                double cost_search(RRTNode &f, RRTNode &t);
+};
+
 /*! \brief Different costs extension.
 
 Use different cost for bulding tree data structure and searching in the
diff --git a/src/rrtext6.cc b/src/rrtext6.cc
new file mode 100644
index 0000000..02d9de9
--- /dev/null
+++ b/src/rrtext6.cc
@@ -0,0 +1,18 @@
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+double RRTExt6::cost_build(RRTNode &f, RRTNode &t)
+{
+        double q0[] = {f.x(), f.y(), f.h()};
+        double q1[] = {t.x(), t.y(), t.h()};
+        ReedsSheppStateSpace rsss(f.mtr());
+        return rsss.distance(q0, q1);
+}
+
+double RRTExt6::cost_search(RRTNode &f, RRTNode &t)
+{
+        double q0[] = {f.x(), f.y(), f.h()};
+        double q1[] = {t.x(), t.y(), t.h()};
+        ReedsSheppStateSpace rsss(f.mtr());
+        return rsss.distance(q0, q1);
+}
-- 
2.39.2