Implement naive steer

diff --git a/api/rrts.h b/api/rrts.h
index 910b3e41de87b06904aa8c5b95b886e69ff9f7c9..b18858643c9806b0ea71e35f1b2fcbd605aee6d2 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -41,6 +41,7 @@ class RRTS {
                 std::vector<RRTNode> goals_;
                 std::vector<RRTNode> nodes_;
                 std::vector<RRTNode> samples_;
+                std::vector<RRTNode> steered_;
 
                 // RRT procedures
                 bool collide();
@@ -59,7 +60,7 @@ class RRTS {
         public:
                 /*! \brief Return path found by RRT*.
                 */
-                std::vector<std::reference_wrapper<RRTNode>> path();
+                std::vector<RRTNode *> path();
                 /*! \brief Run next RRT* iteration.
                 */
                 bool next();
@@ -68,6 +69,7 @@ class RRTS {
                 std::vector<RRTNode> &goals() { return this->goals_; }
                 std::vector<RRTNode> &nodes() { return this->nodes_; }
                 std::vector<RRTNode> &samples() { return this->samples_; }
+                std::vector<RRTNode> &steered() { return this->steered_; }
 
                 RRTS();
 };

diff --git a/src/rrts.cc b/src/rrts.cc
index c038dd17c9846c9b4dc3ff4f2896c5470336b385..4c5c004e3d0ff3ca3206cc3864b6764e33c5b5de 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -58,13 +58,14 @@ std::vector<RRTNode *> RRTS::nv(RRTNode &t)
         return nv;
 }
 
-bool RRTS::steer(
-        RRTNode &f,
-        RRTNode &t
-)
+void RRTS::steer(RRTNode &f, RRTNode &t)
 {
-        bool next = false;
-        return next;
+        double angl = atan2(t.y() - f.y(), t.x() - f.x());
+        this->steered().clear();
+        this->steered().push_back(RRTNode());
+        this->steered().back().x(f.x() + ETA * cos(angl));
+        this->steered().back().y(f.y() + ETA * sin(angl));
+        this->steered().back().h(angl);
 }
 
 // RRT* procedures