From 877d75ac68bb487854ed5c9d02b892b2e0e854ef Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 6 Jan 2021 14:51:37 +0100
Subject: [PATCH] Add different steer extensions

---
 CMakeLists.txt  |  1 +
 README.md       |  3 +++
 api/rrtce.h     | 20 ++++++++++++++++++++
 api/rrtext.h    |  9 +++++++++
 src/rrtext12.cc | 19 +++++++++++++++++++
 5 files changed, 52 insertions(+)
 create mode 100644 src/rrtext12.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0c4ec8..934f97e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/api)
 
 add_library(rrts SHARED
         src/rrts.cc
+        src/rrtext12.cc
         src/rrtext11.cc
         src/rrtext10.cc
         src/rrtext9.cc
diff --git a/README.md b/README.md
index 9cd6e0f..d33050f 100644
--- a/README.md
+++ b/README.md
@@ -47,6 +47,7 @@ number accomply to file `src/rrtextN.cc` where `N` is:
 9.  "nn" 3D grid for nodes storage,
 10. "cost" RS-H -- Reeds & Shepp (build), B-Spline paper (search)
 11. "goal zone" gz -- Use drivable of libbcar to check if goal found.
+12. "steer" -- Use random control input for `steer1`, use R&S for `steer2`.
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 [K-d tree]: https://en.wikipedia.org/wiki/K-d_tree
@@ -93,6 +94,8 @@ number accomply to class `RRTCEn` where `n` is:
 32. cute, gz, Dijkstra, 3D tree, RS-M
 33. cute, gz, Dijkstra, 3D tree, RS-H
 
+34. cute, gz, Dijkstra, 3D tree, RS-H, diff. steer
+
 # Contribute
 Use [OneFlow][3] branching model and keep the [changelog][4].
 
diff --git a/api/rrtce.h b/api/rrtce.h
index 5579314..76dd645 100644
--- a/api/rrtce.h
+++ b/api/rrtce.h
@@ -11,6 +11,26 @@ Compound extensions have no implementation.
 #include "rrts.h"
 #include "rrtext.h"
 
+class RRTCE34
+        : public RRTExt2
+        , public RRTExt11
+        , public RRTExt3
+        , public RRTExt8
+        , public RRTExt10
+        , public RRTExt12
+{
+        public:
+                void init()
+                {
+                        RRTExt2::init();
+                        RRTExt8::init();
+                }
+                void deinit()
+                {
+                        RRTExt2::deinit();
+                        RRTExt8::deinit();
+                }
+};
 class RRTCE33
         : public RRTExt2
         , public RRTExt11
diff --git a/api/rrtext.h b/api/rrtext.h
index 43212e5..5768778 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -16,6 +16,15 @@
 // ext9
 #define GRID_MAX_HI 60
 
+/*! \brief Different `steer` procedures.
+
+Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
+*/
+class RRTExt12 : public virtual RRTS {
+        protected:
+                void steer1(RRTNode &f, RRTNode &t);
+};
+
 /*! \brief Goal Zone.
 
 */
diff --git a/src/rrtext12.cc b/src/rrtext12.cc
new file mode 100644
index 0000000..561b234
--- /dev/null
+++ b/src/rrtext12.cc
@@ -0,0 +1,19 @@
+#include "rrtext.h"
+#include <iostream>
+
+void RRTExt12::steer1(RRTNode &f, RRTNode &t)
+{
+        // assume that `t` is circle uniformly sampled
+        auto bc = BicycleCar();
+        bc.x(f.x());
+        bc.y(f.y());
+        bc.h(f.h());
+        bc.set_max_steer();
+        auto max_steer = bc.st();
+        // map t.h() in -PI...+PI to -max_steer...+max_steer
+        bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
+        bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
+        this->steered().clear();
+        bc.next();
+        this->steered().push_back(RRTNode(bc));
+}
-- 
2.39.2