Rewrite ext2

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ec47914bf6a0bb0473d62987617eb62559a0f7bc..d9903e077dac0e6c1fed1c46a03c7cc9797bca0c 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ link_libraries(jsoncpp_lib)
 
 add_library(rrts STATIC
        src/rrts.cc
+       src/rrtext2.cc
        src/reeds_shepp.cpp
 )
 target_include_directories(rrts PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl)

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 8184e6a5c6c525bd10c42b4f6bd8201f6377e3f3..6d8679aa77f3ea42d1df4101fc8ac6804c183c5a 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -1,7 +1,7 @@
-#ifndef RRTEXT_H
-#define RRTEXT_H
+#ifndef RRTS_RRTEXT_H
+#define RRTS_RRTEXT_H
 
-#include "rrts.h"
+#include "rrts.hh"
 
 // ext2
 #include "cute_c2.h"
@@ -324,29 +324,16 @@ class RRTExt3 : public virtual RRTS {
 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 */
 class RRTExt2 : public virtual RRTS {
-       private:
-               c2Poly c2_bc_;
-               c2x c2x_bc_;
-               std::vector<c2Poly> c2_obstacles_;
-       public:
-               void init();
-               void deinit();
-
-               // Collide RRT procedures
-               std::tuple<bool, unsigned int, unsigned int>
-               collide_steered_from(RRTNode &f);
-               std::tuple<bool, unsigned int, unsigned int>
-               collide_tmp_steered_from(RRTNode &f);
-
-               std::tuple<bool, unsigned int, unsigned int>
-               collide_two_nodes(RRTNode &f, RRTNode &t);
-
-               // getters, setters
-               c2Poly &c2_bc() { return this->c2_bc_; }
-               c2x &c2x_bc() { return this->c2x_bc_; }
-               std::vector<c2Poly> &c2_obstacles() {
-                       return this->c2_obstacles_;
-               };
+private:
+       c2Poly c2_bc_;
+       c2x c2x_bc_;
+       std::vector<c2Poly> c2_obstacles_;
+       bool collide(RRTNode const& n);
+       bool collide_steered();
+public:
+       RRTExt2();
+       Json::Value json() const;
+       void json(Json::Value jvi);
 };
 
 /*! \brief Different costs extension.
@@ -364,4 +351,5 @@ class RRTExt1 : public virtual RRTS {
                double cost_search(RRTNode &f, RRTNode &t);
 };
 
-#endif /* RRTEXT_H */
+} // namespace rrts
+#endif /* RRTS_RRTEXT_H */

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 77ac2e5d097fe86d534cdd27646f2faa10a1c4d9..91b657845d8873cb32e925deaee85053e8ae8bde 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -1,137 +1,77 @@
-#include "rrtext.h"
+#include <cassert>
+#include "rrtext.hh"
 #define CUTE_C2_IMPLEMENTATION
 #include "cute_c2.h"
 
-void RRTExt2::init()
-{
-       BicycleCar bc;
-       this->c2_bc().count = 4;
-       this->c2_bc().verts[0].x = bc.lfx();
-       this->c2_bc().verts[0].y = bc.lfy();
-       this->c2_bc().verts[1].x = bc.lrx();
-       this->c2_bc().verts[1].y = bc.lry();
-       this->c2_bc().verts[2].x = bc.rrx();
-       this->c2_bc().verts[2].y = bc.rry();
-       this->c2_bc().verts[3].x = bc.rfx();
-       this->c2_bc().verts[3].y = bc.rfy();
+namespace rrts {
 
-       for (auto o: this->obstacles()) {
-               c2Poly c2tmp;
-               c2tmp.count = (o.poly().size() < C2_MAX_POLYGON_VERTS)
-                       ? o.poly().size()
-                       : C2_MAX_POLYGON_VERTS
-               ;
-               int i = 0;
-               for (auto c: o.poly()) {
-                       if (i < C2_MAX_POLYGON_VERTS) {
-                               c2tmp.verts[i].x = std::get<0>(c);
-                               c2tmp.verts[i].y = std::get<1>(c);
-                       }
-                       i++;
+bool
+RRTExt2::collide(RRTNode const& n)
+{
+       this->c2x_bc_.p.x = n.x();
+       this->c2x_bc_.p.y = n.y();
+       this->c2x_bc_.r.c = cos(n.h());
+       this->c2x_bc_.r.s = sin(n.h());
+       for (auto& o: this->c2_obstacles_) {
+               if (c2PolytoPoly(&this->c2_bc_, &this->c2x_bc_, &o, NULL)) {
+                       return true;
                }
-               this->c2_obstacles().push_back(c2tmp);
        }
+       return false;
 }
 
-void RRTExt2::deinit()
-{
-       this->c2_obstacles().clear();
-       RRTS::deinit();
-}
-
-std::tuple<bool, unsigned int, unsigned int>
-RRTExt2::collide_steered_from(RRTNode &f)
+bool
+RRTExt2::collide_steered()
 {
-       this->c2x_bc().p.x = f.x();
-       this->c2x_bc().p.y = f.y();
-       this->c2x_bc().r.c = cos(f.h());
-       this->c2x_bc().r.s = sin(f.h());
-       for (auto &o: this->c2_obstacles()) {
-               if (c2PolytoPoly(
-                       &this->c2_bc(), &this->c2x_bc(),
-                       &o, NULL
-               )) {
-                       return std::make_tuple(true, 0, 0);
-               }
-       }
        unsigned int i = 0;
-       for (auto &n: this->steered()) {
-               this->c2x_bc().p.x = n.x();
-               this->c2x_bc().p.y = n.y();
-               this->c2x_bc().r.c = cos(n.h());
-               this->c2x_bc().r.s = sin(n.h());
-               for (auto &o: this->c2_obstacles()) {
-                       if (c2PolytoPoly(
-                               &this->c2_bc(), &this->c2x_bc(),
-                               &o, NULL
-                       )) {
-                               return std::make_tuple(true, i, 0);
-                       }
+       for (auto& n: this->steered_) {
+               if (this->collide(n)) {
+                       break;
                }
                i++;
        }
-       return std::make_tuple(false, 0, 0);
+       this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+       return this->steered_.size() == 0;
 }
-std::tuple<bool, unsigned int, unsigned int>
-RRTExt2::collide_tmp_steered_from(RRTNode &f)
+
+RRTExt2::RRTExt2() : RRTS()
 {
-       this->c2x_bc().p.x = f.x();
-       this->c2x_bc().p.y = f.y();
-       this->c2x_bc().r.c = cos(f.h());
-       this->c2x_bc().r.s = sin(f.h());
-       for (auto &o: this->c2_obstacles()) {
-               if (c2PolytoPoly(
-                       &this->c2_bc(), &this->c2x_bc(),
-                       &o, NULL
-               )) {
-                       return std::make_tuple(true, 0, 0);
-               }
-       }
-       unsigned int i = 0;
-       for (auto n: this->tmp_steered_) {
-               this->c2x_bc().p.x = n.x();
-               this->c2x_bc().p.y = n.y();
-               this->c2x_bc().r.c = cos(n.h());
-               this->c2x_bc().r.s = sin(n.h());
-               for (auto &o: this->c2_obstacles()) {
-                       if (c2PolytoPoly(
-                               &this->c2_bc(), &this->c2x_bc(),
-                               &o, NULL
-                       )) {
-                               return std::make_tuple(true, i, 0);
-                       }
-               }
-               i++;
-       }
-       return std::make_tuple(false, 0, 0);
+       this->c2_bc_.count = 4;
+       this->c2_bc_.verts[0].x = this->bc_.lfx();
+       this->c2_bc_.verts[0].y = this->bc_.lfy();
+       this->c2_bc_.verts[1].x = this->bc_.lrx();
+       this->c2_bc_.verts[1].y = this->bc_.lry();
+       this->c2_bc_.verts[2].x = this->bc_.rrx();
+       this->c2_bc_.verts[2].y = this->bc_.rry();
+       this->c2_bc_.verts[3].x = this->bc_.rfx();
+       this->c2_bc_.verts[3].y = this->bc_.rfy();
 }
 
-std::tuple<bool, unsigned int, unsigned int>
-RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t)
+Json::Value
+RRTExt2::json() const
 {
-       this->c2x_bc().p.x = f.x();
-       this->c2x_bc().p.y = f.y();
-       this->c2x_bc().r.c = cos(f.h());
-       this->c2x_bc().r.s = sin(f.h());
-       for (auto &o: this->c2_obstacles()) {
-               if (c2PolytoPoly(
-                       &this->c2_bc(), &this->c2x_bc(),
-                       &o, NULL
-               )) {
-                       return std::make_tuple(true, 1, 0);
-               }
+       return RRTS::json();
+}
+
+void
+RRTExt2::json(Json::Value jvi)
+{
+       RRTS::json(jvi);
+       if (jvi["obst"] == Json::nullValue) {
+               return;
        }
-       this->c2x_bc().p.x = t.x();
-       this->c2x_bc().p.y = t.y();
-       this->c2x_bc().r.c = cos(t.h());
-       this->c2x_bc().r.s = sin(t.h());
-       for (auto &o: this->c2_obstacles()) {
-               if (c2PolytoPoly(
-                       &this->c2_bc(), &this->c2x_bc(),
-                       &o, NULL
-               )) {
-                       return std::make_tuple(true, 2, 0);
+       for (auto& o: jvi["obst"]) {
+               assert(o.size() < C2_MAX_POLYGON_VERTS);
+               c2Poly c2tmp;
+               c2tmp.count = o.size();
+               unsigned int i = 0;
+               for (auto& c: o) {
+                       c2tmp.verts[i].x = c[0].asDouble();
+                       c2tmp.verts[i].y = c[1].asDouble();
+                       i++;
                }
+               this->c2_obstacles_.push_back(c2tmp);
        }
-       return std::make_tuple(false, 0, 0);
 }
+
+} // namespace rrts