Rewrite ext2

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

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 447b59d438b82a92ac232d5c9fdcedfc5157ad49..91b657845d8873cb32e925deaee85053e8ae8bde 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -1,21 +1,77 @@
-#include "rrtext.h"
+#include <cassert>
+#include "rrtext.hh"
+#define CUTE_C2_IMPLEMENTATION
+#include "cute_c2.h"
 
-void RRTExt2::init()
+namespace rrts {
+
+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;
+               }
+       }
+       return false;
 }
 
-void RRTExt2::deinit()
+bool
+RRTExt2::collide_steered()
 {
+       unsigned int i = 0;
+       for (auto& n: this->steered_) {
+               if (this->collide(n)) {
+                       break;
+               }
+               i++;
+       }
+       this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+       return this->steered_.size() == 0;
 }
 
-std::tuple<bool, unsigned int, unsigned int>
-RRTExt2::collide_steered_from(RRTNode &f)
+RRTExt2::RRTExt2() : RRTS()
 {
-        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
 {
-        return std::make_tuple(false, 0, 0);
+       return RRTS::json();
 }
+
+void
+RRTExt2::json(Json::Value jvi)
+{
+       RRTS::json(jvi);
+       if (jvi["obst"] == Json::nullValue) {
+               return;
+       }
+       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);
+       }
+}
+
+} // namespace rrts