perception/obstacle.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "obstacle.h"
  20
  21 float CircleObstacle::r()
  22 {
  23         return this->h();
  24 }
  25
  26 bool CircleObstacle::collide(RRTNode *n)
  27 {
  28         float xx = n->x() - this->x();
  29         xx *= xx;
  30         float yy = n->y() - this->y();
  31         yy *= yy;
  32         float rr = this->r() * this->r();
  33         if (xx + yy <= rr) {
  34                 return true;
  35         }
  36         return false;
  37 }
  38
  39 bool CircleObstacle::collide(RRTEdge *e)
  40 {
  41         std::vector<RRTEdge *> edges;
  42         edges.push_back(e);
  43         return this->collide(edges);
  44 }
  45
  46 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
  47 {
  48         std::vector<RRTEdge *> bedges;
  49         float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
  50         float angl = 2 * M_PI / 4;
  51         float x1;
  52         float y1;
  53         float x2;
  54         float y2;
  55         int i;
  56         for (i = 0; i < 4; i++) {
  57                 x1 = radi * cos(i * angl);
  58                 y1 = radi * sin(i * angl);
  59                 x2 = radi * cos((i + 1) * angl);
  60                 y2 = radi * sin((i + 1) * angl);
  61                 x1 += this->x();
  62                 y1 += this->y();
  63                 x2 += this->x();
  64                 y2 += this->y();
  65                 bedges.push_back(new RRTEdge(
  66                                         new RRTNode(x1, y1, 0),
  67                                         new RRTNode(x2, y2, 0)));
  68         }
  69         for (auto &be: bedges) {
  70                 for (auto &e: edges) {
  71                         if (SegmentObstacle(
  72                                                 be->init(),
  73                                                 be->goal())
  74                                         .collide(e)) {
  75                                 for (auto e: bedges) {
  76                                         delete e->init();
  77                                         delete e->goal();
  78                                         delete e;
  79                                 }
  80                                 return true;
  81                         }
  82                 }
  83         }
  84         for (auto e: bedges) {
  85                 delete e->init();
  86                 delete e->goal();
  87                 delete e;
  88         }
  89         return false;
  90 }
  91
  92 float CircleObstacle::dist_to(RRTNode *n)
  93 {
  94         float xx = n->x() - this->x();
  95         xx *= xx;
  96         float yy = n->y() - this->y();
  97         yy *= yy;
  98         return (float) (sqrt(xx + yy) - this->r());
  99 }
 100
 101 void PolygonObstacle::add_bnode(RRTNode *n)
 102 {
 103         this->bnodes_.push_back(n);
 104 }
 105
 106 bool PolygonObstacle::collide(RRTNode *n)
 107 {
 108         // From substack/point-in-polygon, see
 109         // https://github.com/substack/point-in-polygon/blob/master/index.js
 110         bool inside = false;
 111         unsigned int i;
 112         int j;
 113         for (i = 0, j = this->bnodes_.size() - 1;
 114                         i < this->bnodes_.size();
 115                         j = i++) {
 116                 float xi = this->bnodes_[i]->x();
 117                 float yi = this->bnodes_[i]->y();
 118                 float xj = this->bnodes_[j]->x();
 119                 float yj = this->bnodes_[j]->y();
 120                 bool intersect = ((yi > n->y()) != (yj > n->y())) &&
 121                         (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
 122                 if (intersect)
 123                         inside = !inside;
 124         }
 125         return inside;
 126 }
 127
 128 bool PolygonObstacle::collide(RRTEdge *e)
 129 {
 130         for (auto &f: this->frame()) {
 131                 if (SegmentObstacle(f->init(), f->goal()).collide(e))
 132                         return true;
 133         }
 134         return false;
 135 }
 136
 137 bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
 138 {
 139         for (auto &e: edges) {
 140                 if (this->collide(e))
 141                         return true;
 142         }
 143         return false;
 144 }
 145
 146 bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
 147 {
 148         for (auto &e: edges) {
 149                 if (this->collide(e))
 150                         return true;
 151         }
 152         return false;
 153 }
 154
 155 float PolygonObstacle::dist_to(RRTNode *n)
 156 {
 157         return 0;
 158 }
 159
 160 std::vector<RRTEdge *> PolygonObstacle::frame()
 161 {
 162         std::vector<RRTEdge *> frame;
 163         unsigned int i;
 164         int j;
 165         for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
 166                 frame.push_back(new RRTEdge(
 167                         new RRTNode(
 168                                 this->bnodes_[j]->x(),
 169                                 this->bnodes_[j]->y(),
 170                                 this->bnodes_[j]->h()
 171                         ),
 172                         new RRTNode(
 173                                 this->bnodes_[i]->x(),
 174                                 this->bnodes_[i]->y(),
 175                                 this->bnodes_[i]->h()
 176                         )
 177                 ));
 178         }
 179         return frame;
 180 }
 181
 182 std::vector<RRTNode *> &PolygonObstacle::bnodes()
 183 {
 184         return this->bnodes_;
 185 }
 186
 187 bool SegmentObstacle::collide(RRTNode *n)
 188 {
 189         return false;
 190 }
 191
 192 bool SegmentObstacle::collide(RRTEdge *e)
 193 {
 194         if (this->intersect(e, true))
 195                 return true;
 196         return false;
 197 }
 198
 199 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
 200 {
 201         for (auto &e: edges) {
 202                 if (this->collide(e)) {
 203                         return true;
 204                 }
 205         }
 206         return false;
 207 }
 208
 209 float SegmentObstacle::dist_to(RRTNode *n)
 210 {
 211         // see https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
 212         float x1 = this->init()->x();
 213         float y1 = this->init()->y();
 214         float x2 = this->goal()->x();
 215         float y2 = this->goal()->y();
 216         float x0 = n->x();
 217         float y0 = n->y();
 218         float nume = (y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1;
 219         nume = std::abs(nume);
 220         float deno = sqrt(pow(y2 - y1, 2) + pow(x2 - x1, 2));
 221         return nume / deno;
 222 }