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