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