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         // see http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
  42         // Find the closest point on seq.
  43         float seg_v[] = {
  44                 e->goal()->x() - e->init()->x(),
  45                 e->goal()->y() - e->init()->y()
  46         };
  47         float pt_v[] = {
  48                 this->x() - e->init()->x(),
  49                 this->y() - e->init()->y()
  50         };
  51         float seg_vl = sqrt(pow(seg_v[0], 2) + pow(seg_v[1], 2));
  52         // seg_vl must be > 0 otherwise it is invalid segment length.
  53         if (seg_vl <= 0)
  54                 return false;
  55         float seg_v_unit[] = {seg_v[0] / seg_vl, seg_v[1] / seg_vl};
  56         float proj = pt_v[0]*seg_v_unit[0] + pt_v[1]*seg_v_unit[1];
  57         float closest[] = {0, 0};
  58         if (proj <= 0) {
  59                 closest[0] = e->init()->x();
  60                 closest[1] = e->init()->y();
  61         } else if (proj >= seg_vl) {
  62                 closest[0] = e->goal()->x();
  63                 closest[1] = e->goal()->y();
  64         } else {
  65                 float proj_v[] = {seg_v_unit[0] * proj, seg_v_unit[1] * proj};
  66                 closest[0] = proj_v[0] + e->init()->x();
  67                 closest[1] = proj_v[1] + e->init()->y();
  68         }
  69         // Find the segment circle.
  70         float dist_v[] = {this->x() - closest[0], this->y() - closest[1]};
  71         float dist = sqrt(pow(dist_v[0], 2) + pow(dist_v[1], 2));
  72         if (dist <= this->r())
  73                 return true;
  74         return false;
  75         // Offset computation.
  76         // float offset[] = {
  77         //        dist_v[0] / dist * (BCAR_TURNING_RADIUS - dist),
  78         //        dist_v[1] / dist * (BCAR_TURNING_RADIUS - dist)
  79         // };
  80 }
  81
  82 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
  83 {
  84         std::vector<RRTEdge *> bedges;
  85         float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
  86         float angl = 2 * M_PI / 4;
  87         float x1;
  88         float y1;
  89         float x2;
  90         float y2;
  91         int i;
  92         for (i = 0; i < 4; i++) {
  93                 x1 = radi * cos(i * angl);
  94                 y1 = radi * sin(i * angl);
  95                 x2 = radi * cos((i + 1) * angl);
  96                 y2 = radi * sin((i + 1) * angl);
  97                 x1 += this->x();
  98                 y1 += this->y();
  99                 x2 += this->x();
 100                 y2 += this->y();
 101                 bedges.push_back(new RRTEdge(
 102                                         new RRTNode(x1, y1, 0),
 103                                         new RRTNode(x2, y2, 0)));
 104         }
 105         for (auto &be: bedges) {
 106                 for (auto &e: edges) {
 107                         if (SegmentObstacle(
 108                                                 be->init(),
 109                                                 be->goal())
 110                                         .collide(e)) {
 111                                 for (auto e: bedges) {
 112                                         delete e->init();
 113                                         delete e->goal();
 114                                         delete e;
 115                                 }
 116                                 return true;
 117                         }
 118                 }
 119         }
 120         for (auto e: bedges) {
 121                 delete e->init();
 122                 delete e->goal();
 123                 delete e;
 124         }
 125         return false;
 126 }
 127
 128 float CircleObstacle::dist_to(RRTNode *n)
 129 {
 130         float xx = n->x() - this->x();
 131         xx *= xx;
 132         float yy = n->y() - this->y();
 133         yy *= yy;
 134         return (float) (sqrt(xx + yy) - this->r());
 135 }
 136
 137 void PolygonObstacle::add_bnode(RRTNode *n)
 138 {
 139         this->bnodes_.push_back(n);
 140 }
 141
 142 bool PolygonObstacle::collide(RRTNode *n)
 143 {
 144         // From substack/point-in-polygon, see
 145         // https://github.com/substack/point-in-polygon/blob/master/index.js
 146         bool inside = false;
 147         unsigned int i;
 148         int j;
 149         for (i = 0, j = this->bnodes_.size() - 1;
 150                         i < this->bnodes_.size();
 151                         j = i++) {
 152                 float xi = this->bnodes_[i]->x();
 153                 float yi = this->bnodes_[i]->y();
 154                 float xj = this->bnodes_[j]->x();
 155                 float yj = this->bnodes_[j]->y();
 156                 bool intersect = ((yi > n->y()) != (yj > n->y())) &&
 157                         (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
 158                 if (intersect)
 159                         inside = !inside;
 160         }
 161         return inside;
 162 }
 163
 164 bool PolygonObstacle::collide(RRTEdge *e)
 165 {
 166         for (auto &f: this->frame()) {
 167                 if (SegmentObstacle(f->init(), f->goal()).collide(e))
 168                         return true;
 169         }
 170         return false;
 171 }
 172
 173 bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
 174 {
 175         for (auto &e: edges) {
 176                 if (this->collide(e))
 177                         return true;
 178         }
 179         return false;
 180 }
 181
 182 bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
 183 {
 184         for (auto &e: edges) {
 185                 if (this->collide(e))
 186                         return true;
 187         }
 188         return false;
 189 }
 190
 191 float PolygonObstacle::dist_to(RRTNode *n)
 192 {
 193         return 0;
 194 }
 195
 196 std::vector<RRTEdge *> PolygonObstacle::frame()
 197 {
 198         std::vector<RRTEdge *> frame;
 199         unsigned int i;
 200         int j;
 201         for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
 202                 frame.push_back(new RRTEdge(
 203                         new RRTNode(
 204                                 this->bnodes_[j]->x(),
 205                                 this->bnodes_[j]->y(),
 206                                 this->bnodes_[j]->h()
 207                         ),
 208                         new RRTNode(
 209                                 this->bnodes_[i]->x(),
 210                                 this->bnodes_[i]->y(),
 211                                 this->bnodes_[i]->h()
 212                         )
 213                 ));
 214         }
 215         return frame;
 216 }
 217
 218 std::vector<RRTNode *> &PolygonObstacle::bnodes()
 219 {
 220         return this->bnodes_;
 221 }
 222
 223 bool SegmentObstacle::collide(RRTNode *n)
 224 {
 225         return false;
 226 }
 227
 228 bool SegmentObstacle::collide(RRTEdge *e)
 229 {
 230         if (this->intersect(e, true))
 231                 return true;
 232         return false;
 233 }
 234
 235 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
 236 {
 237         for (auto &e: edges) {
 238                 if (this->collide(e)) {
 239                         return true;
 240                 }
 241         }
 242         return false;
 243 }
 244
 245 float SegmentObstacle::dist_to(RRTNode *n)
 246 {
 247         // see https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
 248         float x1 = this->init()->x();
 249         float y1 = this->init()->y();
 250         float x2 = this->goal()->x();
 251         float y2 = this->goal()->y();
 252         float x0 = n->x();
 253         float y0 = n->y();
 254         float nume = (y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1;
 255         nume = std::abs(nume);
 256         float deno = sqrt(pow(y2 - y1, 2) + pow(x2 - x1, 2));
 257         return nume / deno;
 258 }