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 bool SegmentObstacle::collide(RRTNode *n)
  93 {
  94         return false;
  95 }
  96
  97 bool SegmentObstacle::collide(RRTEdge *e)
  98 {
  99         // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 100         float x1 = this->init()->x();
 101         float y1 = this->init()->y();
 102         float x2 = this->goal()->x();
 103         float y2 = this->goal()->y();
 104         float x3 = e->init()->x();
 105         float y3 = e->init()->y();
 106         float x4 = e->goal()->x();
 107         float y4 = e->goal()->y();
 108         float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 109         if (deno == 0) {
 110                 return false; // parallel
 111         }
 112         //return true; // colliding lines, not line segments
 113         //float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
 114         //        (x1 - x2) * (x3 * y4 - y3 * x4);
 115         //px /= deno;
 116         //float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
 117         //        (y1 - y2) * (x3 * y4 - y3 * x4);
 118         //py /= deno;
 119         float s = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 120         s /= deno;
 121         if (s < 0 || s > 1) {
 122                 return false;
 123         }
 124         float t = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 125         t *= -1;
 126         t /= deno;
 127         if (t < 0 || t > 1) {
 128                 return false;
 129         }
 130         return true;
 131 }
 132
 133 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
 134 {
 135         for (auto &e: edges) {
 136                 if (this->collide(e)) {
 137                         return true;
 138                 }
 139         }
 140         return false;
 141 }