X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/blobdiff_plain/2b8e6939f840d42853267e1455a4d2cf755dd780..c9ef2911f543824cd39424b400fe19e5ce271d19:/src/rrtext2.cc

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 77ac2e5..91b6578 100644
--- 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