src/rrtext2.cc

   1 #include <cassert>
   2 #include "rrtext.hh"
   3 #define CUTE_C2_IMPLEMENTATION
   4 #include "cute_c2.h"
   5
   6 namespace rrts {
   7
   8 bool
   9 RRTExt2::collide(RRTNode const& n)
  10 {
  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)) {
  17                         return true;
  18                 }
  19         }
  20         return false;
  21 }
  22
  23 bool
  24 RRTExt2::collide_steered()
  25 {
  26         unsigned int i = 0;
  27         for (auto& n: this->steered_) {
  28                 if (this->collide(n)) {
  29                         break;
  30                 }
  31                 i++;
  32         }
  33         this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
  34         return this->steered_.size() == 0;
  35 }
  36
  37 RRTExt2::RRTExt2() : RRTS()
  38 {
  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();
  48 }
  49
  50 Json::Value
  51 RRTExt2::json() const
  52 {
  53         return RRTS::json();
  54 }
  55
  56 void
  57 RRTExt2::json(Json::Value jvi)
  58 {
  59         RRTS::json(jvi);
  60         if (jvi["obst"] == Json::nullValue) {
  61                 return;
  62         }
  63         for (auto& o: jvi["obst"]) {
  64                 assert(o.size() < C2_MAX_POLYGON_VERTS);
  65                 c2Poly c2tmp;
  66                 c2tmp.count = o.size();
  67                 unsigned int i = 0;
  68                 for (auto& c: o) {
  69                         c2tmp.verts[i].x = c[0].asDouble();
  70                         c2tmp.verts[i].y = c[1].asDouble();
  71                         i++;
  72                 }
  73                 this->c2_obstacles_.push_back(c2tmp);
  74         }
  75 }
  76
  77 } // namespace rrts