src/rrtext2.cc

   1 #include "rrtext.h"
   2 #define CUTE_C2_IMPLEMENTATION
   3 #include "cute_c2.h"
   4
   5 void RRTExt2::init()
   6 {
   7         BicycleCar bc;
   8         this->c2_bc().count = 4;
   9         this->c2_bc().verts[0].x = bc.lfx();
  10         this->c2_bc().verts[0].y = bc.lfy();
  11         this->c2_bc().verts[1].x = bc.lrx();
  12         this->c2_bc().verts[1].y = bc.lry();
  13         this->c2_bc().verts[2].x = bc.rrx();
  14         this->c2_bc().verts[2].y = bc.rry();
  15         this->c2_bc().verts[3].x = bc.rfx();
  16         this->c2_bc().verts[3].y = bc.rfy();
  17
  18         for (auto o: this->obstacles()) {
  19                 c2Poly c2tmp;
  20                 c2tmp.count = (o.poly().size() < C2_MAX_POLYGON_VERTS)
  21                         ? o.poly().size()
  22                         : C2_MAX_POLYGON_VERTS
  23                 ;
  24                 int i = 0;
  25                 for (auto c: o.poly()) {
  26                         if (i < C2_MAX_POLYGON_VERTS) {
  27                                 c2tmp.verts[i].x = std::get<0>(c);
  28                                 c2tmp.verts[i].y = std::get<1>(c);
  29                         }
  30                         i++;
  31                 }
  32                 this->c2_obstacles().push_back(c2tmp);
  33         }
  34 }
  35
  36 void RRTExt2::deinit()
  37 {
  38         this->c2_obstacles().clear();
  39         RRTS::deinit();
  40 }
  41
  42 std::tuple<bool, unsigned int, unsigned int>
  43 RRTExt2::collide_steered_from(RRTNode &f)
  44 {
  45         this->c2x_bc().p.x = f.x();
  46         this->c2x_bc().p.y = f.y();
  47         this->c2x_bc().r.c = cos(f.h());
  48         this->c2x_bc().r.s = sin(f.h());
  49         for (auto &o: this->c2_obstacles()) {
  50                 if (c2PolytoPoly(
  51                         &this->c2_bc(), &this->c2x_bc(),
  52                         &o, NULL
  53                 )) {
  54                         return std::make_tuple(true, 0, 0);
  55                 }
  56         }
  57         unsigned int i = 0;
  58         for (auto &n: this->steered()) {
  59                 this->c2x_bc().p.x = n.x();
  60                 this->c2x_bc().p.y = n.y();
  61                 this->c2x_bc().r.c = cos(n.h());
  62                 this->c2x_bc().r.s = sin(n.h());
  63                 for (auto &o: this->c2_obstacles()) {
  64                         if (c2PolytoPoly(
  65                                 &this->c2_bc(), &this->c2x_bc(),
  66                                 &o, NULL
  67                         )) {
  68                                 return std::make_tuple(true, i, 0);
  69                         }
  70                 }
  71                 i++;
  72         }
  73         return std::make_tuple(false, 0, 0);
  74 }
  75 std::tuple<bool, unsigned int, unsigned int>
  76 RRTExt2::collide_tmp_steered_from(RRTNode &f)
  77 {
  78         this->c2x_bc().p.x = f.x();
  79         this->c2x_bc().p.y = f.y();
  80         this->c2x_bc().r.c = cos(f.h());
  81         this->c2x_bc().r.s = sin(f.h());
  82         for (auto &o: this->c2_obstacles()) {
  83                 if (c2PolytoPoly(
  84                         &this->c2_bc(), &this->c2x_bc(),
  85                         &o, NULL
  86                 )) {
  87                         return std::make_tuple(true, 0, 0);
  88                 }
  89         }
  90         unsigned int i = 0;
  91         for (auto n: this->tmp_steered_) {
  92                 this->c2x_bc().p.x = n.x();
  93                 this->c2x_bc().p.y = n.y();
  94                 this->c2x_bc().r.c = cos(n.h());
  95                 this->c2x_bc().r.s = sin(n.h());
  96                 for (auto &o: this->c2_obstacles()) {
  97                         if (c2PolytoPoly(
  98                                 &this->c2_bc(), &this->c2x_bc(),
  99                                 &o, NULL
 100                         )) {
 101                                 return std::make_tuple(true, i, 0);
 102                         }
 103                 }
 104                 i++;
 105         }
 106         return std::make_tuple(false, 0, 0);
 107 }
 108
 109 std::tuple<bool, unsigned int, unsigned int>
 110 RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t)
 111 {
 112         this->c2x_bc().p.x = f.x();
 113         this->c2x_bc().p.y = f.y();
 114         this->c2x_bc().r.c = cos(f.h());
 115         this->c2x_bc().r.s = sin(f.h());
 116         for (auto &o: this->c2_obstacles()) {
 117                 if (c2PolytoPoly(
 118                         &this->c2_bc(), &this->c2x_bc(),
 119                         &o, NULL
 120                 )) {
 121                         return std::make_tuple(true, 1, 0);
 122                 }
 123         }
 124         this->c2x_bc().p.x = t.x();
 125         this->c2x_bc().p.y = t.y();
 126         this->c2x_bc().r.c = cos(t.h());
 127         this->c2x_bc().r.s = sin(t.h());
 128         for (auto &o: this->c2_obstacles()) {
 129                 if (c2PolytoPoly(
 130                         &this->c2_bc(), &this->c2x_bc(),
 131                         &o, NULL
 132                 )) {
 133                         return std::make_tuple(true, 2, 0);
 134                 }
 135         }
 136         return std::make_tuple(false, 0, 0);
 137 }