src/psp.cc

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