3 #define CUTE_C2_IMPLEMENTATION
9 RRTExt2::collide(RRTNode const& n)
11 this->c2x_bc_.p.x = n.x();
12 this->c2x_bc_.p.y = n.y();
13 this->c2x_bc_.r.c = cos(n.h());
14 this->c2x_bc_.r.s = sin(n.h());
15 for (auto& o: this->c2_obstacles_) {
16 if (c2PolytoPoly(&this->c2_bc_, &this->c2x_bc_, &o, NULL)) {
24 RRTExt2::collide_steered()
27 for (auto& n: this->steered_) {
28 if (this->collide(n)) {
33 this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
34 return this->steered_.size() == 0;
37 RRTExt2::RRTExt2() : RRTS()
39 this->c2_bc_.count = 4;
40 this->c2_bc_.verts[0].x = this->bc_.lfx();
41 this->c2_bc_.verts[0].y = this->bc_.lfy();
42 this->c2_bc_.verts[1].x = this->bc_.lrx();
43 this->c2_bc_.verts[1].y = this->bc_.lry();
44 this->c2_bc_.verts[2].x = this->bc_.rrx();
45 this->c2_bc_.verts[2].y = this->bc_.rry();
46 this->c2_bc_.verts[3].x = this->bc_.rfx();
47 this->c2_bc_.verts[3].y = this->bc_.rfy();
57 RRTExt2::json(Json::Value jvi)
60 if (jvi["obst"] == Json::nullValue) {
63 for (auto& o: jvi["obst"]) {
64 assert(o.size() < C2_MAX_POLYGON_VERTS);
66 c2tmp.count = o.size();
69 c2tmp.verts[i].x = c[0].asDouble();
70 c2tmp.verts[i].y = c[1].asDouble();
73 this->c2_obstacles_.push_back(c2tmp);