From 565feef44efefe42b7d19ccd1d72f17960168c14 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 26 Sep 2019 16:36:06 +0200
Subject: [PATCH] Implement ext2 init

---
 api/rrtext.h   |  9 +++++++++
 src/rrtext2.cc | 29 +++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/api/rrtext.h b/api/rrtext.h
index a071a13..b94d3e8 100644
--- 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 447b59d..3b890e8 100644
--- 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()
-- 
2.39.2