Implement ext2 init

diff --git a/api/rrtext.h b/api/rrtext.h
index a071a133599e8462e947d7c6fd8e9fe12a9aece4..b94d3e84b1c5c4365bb669a5c7b491773c52aa39 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -11,6 +11,9 @@
 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 */
 class RRTExt2 : public RRTS {
+        private:
+                c2Poly c2_bc_;
+                std::vector<c2Poly> c2_obstacles_;
         public:
                 void init();
                 void deinit();
@@ -21,6 +24,12 @@ class RRTExt2 : public RRTS {
 
                 std::tuple<bool, unsigned int, unsigned int>
                 collide_two_nodes(RRTNode &f, RRTNode &t);
+
+                // getters, setters
+                c2Poly &c2_bc() { return this->c2_bc_; }
+                std::vector<c2Poly> &c2_obstacles() {
+                        return this->c2_obstacles_;
+                };
 };
 
 /*! \brief Different costs extension.

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 447b59d438b82a92ac232d5c9fdcedfc5157ad49..3b890e8e20df78672b830f61c39877e18b312357 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -1,7 +1,36 @@
 #include "rrtext.h"
+#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();
+
+        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++;
+                }
+                this->c2_obstacles().push_back(c2tmp);
+        }
 }
 
 void RRTExt2::deinit()