src/psp.cc

   1 #include <cmath>
   2 #include <list>
   3 #include <queue>
   4 #include "psp.h"
   5
   6 bool PSPlanner::collide()
   7 {
   8         std::vector<std::tuple<double, double>> bc;
   9         bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
  10         bc.push_back(std::make_tuple(this->cc().lrx(), this->cc().lry()));
  11         bc.push_back(std::make_tuple(this->cc().rrx(), this->cc().rry()));
  12         bc.push_back(std::make_tuple(this->cc().rfx(), this->cc().rfy()));
  13         bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
  14         std::vector<std::tuple<double, double>> ps;
  15         ps.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
  16         ps.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
  17         ps.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
  18         ps.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
  19         return std::get<0>(::collide(bc, ps));
  20 }
  21
  22 bool PSPlanner::forward()
  23 {
  24         if (this->ps().parallel())
  25                 return false;
  26         double heading = atan2(
  27                 this->ps().y2() - this->ps().y1(),
  28                 this->ps().x2() - this->ps().x1()
  29         );
  30         while (heading < 0) heading += 2 * M_PI;
  31         double h = this->gc().h();
  32         while (h < 0) h += 2 * M_PI;
  33         if (std::abs(heading - h) < M_PI / 4)
  34                 return true;
  35         return false;
  36 }
  37
  38 void PSPlanner::guess_gc()
  39 {
  40         double x = this->ps().x1();
  41         double y = this->ps().y1();
  42         double h = this->ps().heading();
  43         double dts = + M_PI / 2; // direction to slot
  44         if (this->ps().right())
  45                 dts = - M_PI / 2;
  46         dts *= 0.99; // precision workaround
  47         if (this->ps().parallel()) {
  48                 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
  49                 x += (this->gc().dr() + 0.01) * cos(h);
  50                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
  51                 y += (this->gc().dr() + 0.01) * sin(h);
  52         } else {
  53                 dts = atan2(
  54                         this->ps().y2() - this->ps().y1(),
  55                         this->ps().x2() - this->ps().x1()
  56                 );
  57                 dts *= 1.01; // precision workaround
  58                 if (std::abs(dts - this->ps().heading()) < M_PI / 2) {
  59                         x = this->ps().x4();
  60                         y = this->ps().y4();
  61                         h = dts;
  62                         x += (this->gc().dr() + 0.01) * cos(h);
  63                         y += (this->gc().dr() + 0.01) * sin(h);
  64                         if (this->ps().right())
  65                                 dts -= M_PI / 2;
  66                         else
  67                                 dts += M_PI / 2;
  68                         x += (this->gc().w() / 2 + 0.01) * cos(dts);
  69                         y += (this->gc().w() / 2 + 0.01) * sin(dts);
  70                 } else {
  71                         h = dts + M_PI;
  72                         x += -(this->gc().df() + 0.01) * cos(h);
  73                         y += -(this->gc().df() + 0.01) * sin(h);
  74                         if (this->ps().right())
  75                                 dts += M_PI / 2;
  76                         else
  77                                 dts -= M_PI / 2;
  78                         x += (this->gc().w() / 2 + 0.01) * cos(dts);
  79                         y += (this->gc().w() / 2 + 0.01) * sin(dts);
  80                 }
  81         }
  82         while (h > M_PI)
  83                 h -= 2 * M_PI;
  84         while (h <= -M_PI)
  85                 h += 2 * M_PI;
  86         this->gc().x(x);
  87         this->gc().y(y);
  88         this->gc().h(h);
  89 }
  90
  91 bool PSPlanner::left()
  92 {
  93         double lfx = this->cc().lfx();
  94         double lfy = this->cc().lfy();
  95         double lrx = this->cc().lrx();
  96         double lry = this->cc().lry();
  97         double rrx = this->cc().rrx();
  98         double rry = this->cc().rry();
  99         double rfx = this->cc().rfx();
 100         double rfy = this->cc().rfy();
 101         double lfs = sgn(
 102                 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 103                 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 104         );
 105         double lrs = sgn(
 106                 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 107                 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 108         );
 109         double rrs = sgn(
 110                 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 111                 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 112         );
 113         double rfs = sgn(
 114                 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 115                 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 116         );
 117         if (this->ps().parallel())
 118                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 119         else if (!this->forward())
 120                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 121         else
 122                 return lrs == rrs && (lrs != lfs || lrs != rfs);
 123 }
 124
 125 bool PSPlanner::parked()
 126 {
 127         std::vector<std::tuple<double, double>> slot;
 128         slot.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
 129         slot.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
 130         slot.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
 131         slot.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
 132         return inside(this->gc().lfx(), this->gc().lfy(), slot)
 133                 && inside(this->gc().lrx(), this->gc().lry(), slot)
 134                 && inside(this->gc().rrx(), this->gc().rry(), slot)
 135                 && inside(this->gc().rfx(), this->gc().rfy(), slot);
 136 }
 137
 138 std::vector<BicycleCar> PSPlanner::possible_inits(
 139         unsigned int cnt,
 140         double dist
 141 )
 142 {
 143         std::vector<BicycleCar> pi;
 144         if (!this->ps().parallel()) {
 145                 this->cc().sp(this->cc().sp() * -dist);
 146                 this->cc().st(this->cc().st() * -1);
 147         } else {
 148                 this->cc().sp(this->cc().sp() * dist);
 149                 this->cc().st(this->cc().st() * 1);
 150         }
 151         BicycleCar orig_cc(this->cc());
 152         for (unsigned int i = 0; i < cnt; i++) {
 153                 this->cc().next();
 154                 pi.push_back(BicycleCar(this->cc()));
 155         }
 156         this->cc() = BicycleCar(orig_cc);
 157         return pi;
 158 }
 159
 160 // find entry
 161 void PSPlanner::fe()
 162 {
 163         if (this->ps().parallel())
 164                 return this->fe_parallel();
 165         else
 166                 return this->fe_perpendicular();
 167 }
 168
 169 void PSPlanner::fe_parallel()
 170 {
 171         // angle for distance from "entry" corner
 172         double dist_angl = this->ps().heading() + M_PI;
 173         dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
 174         // set bicycle car `bci` basic dimensions and heading
 175         BicycleCar bci = BicycleCar(this->gc());
 176         BicycleCar bco = BicycleCar(this->gc());
 177         bci.h(this->ps().heading());
 178         // move 0.01 from the "entry" corner
 179         bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
 180         bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
 181         // align with parking "top" of slot (move backward)
 182         dist_angl = bci.h() + M_PI;
 183         bci.x(bci.x() + bci.df() * cos(dist_angl));
 184         bci.y(bci.y() + bci.df() * sin(dist_angl));
 185         // align with "entry" to pakring slot (move outside)
 186         dist_angl = this->ps().heading();
 187         dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
 188         bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
 189         bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
 190         // set default speed, steer
 191         bci.st(bci.wb() / bci.mtr());
 192         if (!this->ps().right())
 193                 bci.st(bci.st() * -1);
 194         bci.sp(-0.01);
 195         // BFS - init all starts
 196         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
 197         double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
 198         if (this->ps().right())
 199                 dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
 200         else
 201                 dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
 202         double DIST_ANGL = dist_angl;
 203         std::queue<BicycleCar, std::list<BicycleCar>> q;
 204         while (
 205                 (
 206                         this->ps().right()
 207                         && dist_angl < DIST_ANGL + 3 * M_PI / 4
 208                 )
 209                 || (
 210                         !this->ps().right()
 211                         && dist_angl > DIST_ANGL - 3 * M_PI / 4
 212                 )
 213         ) {
 214                 this->cc() = BicycleCar(bci);
 215                 if (this->ps().right()) {
 216                         this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
 217                         this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
 218                 } else {
 219                         this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
 220                         this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
 221                 }
 222                 this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
 223                 if (!this->collide()) {
 224                         q.push(BicycleCar(this->cc()));
 225                 }
 226                 dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
 227         }
 228         // BFS - find entry current car `cc` and corresponding goal car `gc`
 229         unsigned int iter_cntr = 0;
 230         while (!q.empty() && iter_cntr < 9) {
 231                 this->cc() = BicycleCar(q.front());
 232                 q.pop();
 233                 while (
 234                         !this->collide()
 235                         && (std::abs(
 236                                 this->cc().h() - this->ps().heading()
 237                         ) > M_PI / 32)
 238                         && (std::abs(
 239                                 this->cc().h() - this->ps().heading()
 240                         ) < M_PI / 2)
 241                 )
 242                         this->cc().next();
 243                 this->cc().sp(this->cc().sp() * -1);
 244                 this->cc().next();
 245                 this->gc() = BicycleCar(this->cc());
 246                 if (this->parked())
 247                         goto successfinish;
 248                 this->cc().st(this->cc().st() * -1);
 249                 q.push(BicycleCar(this->cc()));
 250                 if (sgn(this->cc().st()) == sgn(q.front().st()))
 251                         iter_cntr++;
 252         }
 253         // fallback to fer
 254         this->gc() = BicycleCar(bco);
 255 successfinish:
 256         return this->fer_parallel();
 257 }
 258
 259 void PSPlanner::fe_perpendicular()
 260 {
 261         // TODO Try multiple angles when going from parking slot.
 262         //
 263         //      Do not use just the maximum steer angle. Test angles
 264         //      until the whole current car `cc` is out of the parking
 265         //      slot `ps`.
 266         //
 267         //      Another approach could be testing angles from the
 268         //      beginning of the escape parkig slot maneuver.
 269         if (this->forward())
 270                 this->cc().sp(-0.01);
 271         else
 272                 this->cc().sp(0.01);
 273         while (!this->left())
 274                 this->cc().next();
 275         return;
 276 }
 277
 278 void PSPlanner::fer()
 279 {
 280         if (this->ps().parallel())
 281                 return this->fer_parallel();
 282         else
 283                 return this->fer_perpendicular();
 284 }
 285
 286 void PSPlanner::fer_parallel()
 287 {
 288         this->cc().st(this->cc().wb() / this->cc().mtr());
 289         if (!this->ps().right())
 290                 this->cc().st(this->cc().st() * -1);
 291         this->cc().sp(0.01);
 292         while (!this->left()) {
 293                 while (!this->collide() && !this->left())
 294                         this->cc().next();
 295                 if (this->left() && !this->collide()) {
 296                         break;
 297                 } else {
 298                         this->cc().sp(this->cc().sp() * -1);
 299                         this->cc().next();
 300                         this->cc().st(this->cc().st() * -1);
 301                 }
 302         }
 303 }
 304
 305 void PSPlanner::fer_perpendicular()
 306 {
 307         bool delta_use[] = {true, true, true};
 308         double cc_h = this->cc().h();
 309         double x;
 310         double y;
 311         // check inner radius
 312         if (this->forward()) {
 313                 x = this->ps().x1();
 314                 y = this->ps().y1();
 315         } else {
 316                 x = this->ps().x4();
 317                 y = this->ps().y4();
 318         }
 319         double x1;
 320         double y1;
 321         if (this->ps().right()) {
 322                 x1 = this->cc().ccr().x();
 323                 y1 = this->cc().ccr().y();
 324         } else {
 325                 x1 = this->cc().ccl().x();
 326                 y1 = this->cc().ccl().y();
 327         }
 328         double IR = this->cc().iradi();
 329         double a = 1;
 330         double b;
 331         if (this->forward())
 332                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 333         else
 334                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 335         double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 336         double D = pow(b, 2) - 4 * a * c;
 337         double delta;
 338         delta = -b - sqrt(D);
 339         delta /= 2 * a;
 340         double delta_1 = delta;
 341         if (D < 0)
 342                 delta_use[0] = false;
 343         // check outer radius
 344         if (this->forward()) {
 345                 x = this->ps().x4();
 346                 y = this->ps().y4();
 347         } else {
 348                 x = this->ps().x1();
 349                 y = this->ps().y1();
 350         }
 351         IR = this->cc().ofradi();
 352         a = 1;
 353         if (this->forward())
 354                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 355         else
 356                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 357         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 358         D = pow(b, 2) - 4 * a * c;
 359         if (this->forward()) {
 360                 delta = -b + sqrt(D);
 361                 delta /= 2 * a;
 362         }
 363         double delta_2 = delta;
 364         if (D < 0)
 365                 delta_use[1] = false;
 366         delta = -b - sqrt(D);
 367         delta /= 2 * a;
 368         double delta_3 = delta;
 369         if (D < 0)
 370                 delta_use[2] = false;
 371         if (delta_use[0] && delta_use[1] && delta_use[22])
 372                 delta = std::max(delta_1, std::max(delta_2, delta_3));
 373         else if (delta_use[0] && delta_use[1])
 374                 delta = std::max(delta_1, delta_2);
 375         else if (delta_use[0] && delta_use[2])
 376                 delta = std::max(delta_1, delta_3);
 377         else if (delta_use[1] && delta_use[2])
 378                 delta = std::max(delta_2, delta_3);
 379         else if (delta_use[0])
 380                 delta = delta_1;
 381         else if (delta_use[1])
 382                 delta = delta_2;
 383         else if (delta_use[2])
 384                 delta = delta_3;
 385         else
 386                 return;
 387         // current car `cc` can get out of slot with max steer
 388         this->cc().x(this->cc().x() + delta * cos(cc_h));
 389         this->cc().y(this->cc().y() + delta * sin(cc_h));
 390         this->cc().h(cc_h);
 391         // get current car `cc` out of slot
 392         if (this->forward())
 393                 this->cc().sp(-0.01);
 394         else
 395                 this->cc().sp(0.01);
 396         this->cc().st(this->cc().wb() / this->cc().mtr());
 397         if (this->ps().right())
 398                 this->cc().st(this->cc().st() * -1);
 399         while (!this->left()) {
 400                 while (!this->collide() && !this->left())
 401                         this->cc().next();
 402                 if (this->left() && !this->collide()) {
 403                         break;
 404                 } else {
 405                         this->cc().sp(this->cc().sp() * -1);
 406                         this->cc().next();
 407                         this->cc().st(this->cc().st() * -1);
 408                 }
 409         }
 410 }
 411
 412 PSPlanner::PSPlanner()
 413 {
 414 }