Implement ext2 init

[hubacji1/rrts.git] / src / rrtext2.cc

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()