src/pslot.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cassert>
   8 #include <cmath>
   9 #include "pslot.hh"
  10
  11 namespace bcar {
  12
  13 ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
  14                 _border{p,
  15                         Point(p.x() + W * cos(h - M_PI / 2.0),
  16                                 p.y() + W * sin(h - M_PI / 2.0)),
  17                         Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
  18                                 p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
  19                         Point(p.x() + L * cos(h), p.y() + L * sin(h))},
  20                 _entry(_border[0], _border[3]),
  21                 _rear(_border[0], _border[1]),
  22                 _curb(_border[1], _border[2]),
  23                 _front(_border[2], _border[3])
  24 {
  25 }
  26
  27 ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
  28                 double rfx, double rfy, double lfx, double lfy) :
  29                         _border{Point(lrx, lry), Point(rrx, rry),
  30                                 Point(rfx, rfy), Point(lfx, lfy)},
  31                         _entry(_border[0], _border[3]),
  32                         _rear(_border[0], _border[1]),
  33                         _curb(_border[1], _border[2]),
  34                         _front(_border[2], _border[3])
  35 {
  36 }
  37
  38 double
  39 ParkingSlot::len() const
  40 {
  41         return this->_entry.len();
  42 }
  43
  44 double
  45 ParkingSlot::w() const
  46 {
  47         return this->_rear.len();
  48 }
  49
  50 double
  51 ParkingSlot::lfx() const
  52 {
  53         return this->_border[3].x();
  54 }
  55
  56 double
  57 ParkingSlot::lfy() const
  58 {
  59         return this->_border[3].y();
  60 }
  61
  62 double
  63 ParkingSlot::lrx() const
  64 {
  65         return this->_border[0].x();
  66 }
  67
  68 double
  69 ParkingSlot::lry() const
  70 {
  71         return this->_border[0].y();
  72 }
  73
  74 double
  75 ParkingSlot::rrx() const
  76 {
  77         return this->_border[1].x();
  78 }
  79
  80 double
  81 ParkingSlot::rry() const
  82 {
  83         return this->_border[1].y();
  84 }
  85
  86 double
  87 ParkingSlot::rfx() const
  88 {
  89         return this->_border[2].x();
  90 }
  91
  92 double
  93 ParkingSlot::rfy() const
  94 {
  95         return this->_border[2].y();
  96 }
  97
  98 double
  99 ParkingSlot::h() const
 100 {
 101         return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
 102 }
 103
 104 Point
 105 ParkingSlot::lf() const
 106 {
 107         return Point(this->lfx(), this->lfy());
 108 }
 109
 110 Point
 111 ParkingSlot::lr() const
 112 {
 113         return Point(this->lrx(), this->lry());
 114 }
 115
 116 Point
 117 ParkingSlot::rr() const
 118 {
 119         return Point(this->rrx(), this->rry());
 120 }
 121
 122 Point
 123 ParkingSlot::rf() const
 124 {
 125         return Point(this->rfx(), this->rfy());
 126 }
 127
 128 Line
 129 ParkingSlot::entry() const
 130 {
 131         return this->_entry;
 132 }
 133
 134 Line
 135 ParkingSlot::rear() const
 136 {
 137         return this->_rear;
 138 }
 139
 140 Line
 141 ParkingSlot::curb() const
 142 {
 143         return this->_curb;
 144 }
 145
 146 Line
 147 ParkingSlot::front() const
 148 {
 149         return this->_front;
 150 }
 151
 152 void
 153 ParkingSlot::set_parking_speed(double s)
 154 {
 155         this->_parking_speed = s;
 156 }
 157
 158 unsigned int
 159 ParkingSlot::get_max_cusp() const
 160 {
 161         return this->_max_cusp;
 162 }
 163
 164 void
 165 ParkingSlot::set_max_cusp(unsigned int m)
 166 {
 167         this->_max_cusp = m;
 168 }
 169
 170 void
 171 ParkingSlot::set_delta_angle_to_slot(double d)
 172 {
 173         this->_delta_angle_to_slot = d;
 174 }
 175
 176 bool
 177 ParkingSlot::parallel() const
 178 {
 179         return this->_entry.len() > this->_rear.len();
 180 }
 181
 182 bool
 183 ParkingSlot::right() const
 184 {
 185         return this->_border[1].on_right_side_of(this->_entry);
 186 }
 187
 188 void
 189 ParkingSlot::swap_side()
 190 {
 191         this->_border[1].rotate(this->_border[0], M_PI);
 192         this->_border[2].rotate(this->_border[3], M_PI);
 193         this->_entry = Line(this->_border[0], this->_border[3]);
 194         this->_rear = Line(this->_border[0], this->_border[1]);
 195         this->_curb = Line(this->_border[1], this->_border[2]);
 196         this->_front = Line(this->_border[2], this->_border[3]);
 197 }
 198
 199 bool
 200 ParkingSlot::parked(BicycleCar const& c) const
 201 {
 202         auto b_len = sizeof(this->_border) / sizeof(this->_border[0]);
 203         std::vector<Point> b(this->_border, this->_border + b_len);
 204         return c.lf().inside_of(b) && c.lr().inside_of(b)
 205                 && c.rr().inside_of(b) && c.rf().inside_of(b);
 206 }
 207
 208 bool
 209 ParkingSlot::collide(BicycleCar const& c) const
 210 {
 211         return c.left().intersects_with(this->_rear)
 212                 || c.left().intersects_with(this->_curb)
 213                 || c.left().intersects_with(this->_front)
 214                 || c.rear().intersects_with(this->_rear)
 215                 || c.rear().intersects_with(this->_curb)
 216                 || c.rear().intersects_with(this->_front)
 217                 || c.right().intersects_with(this->_rear)
 218                 || c.right().intersects_with(this->_curb)
 219                 || c.right().intersects_with(this->_front)
 220                 || c.front().intersects_with(this->_rear)
 221                 || c.front().intersects_with(this->_curb)
 222                 || c.front().intersects_with(this->_front);
 223 }
 224
 225 void
 226 ParkingSlot::set_to_start(BicycleCar& c)
 227 {
 228         c.h(this->h());
 229         double clen = -this->_offset + this->len() - c.df();
 230         double cw = c.w() / 2.0;
 231         c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
 232         c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
 233         c.set_max_steer();
 234         assert(this->_parking_speed < 0.0);
 235         c.sp(this->_parking_speed);
 236 }
 237
 238 std::vector<BicycleCar>
 239 ParkingSlot::drive_in_slot(BicycleCar c)
 240 {
 241         assert(this->parallel());
 242         assert(this->right());
 243         assert(c.len() < this->len());
 244         assert(c.w() < this->w());
 245         std::vector<BicycleCar> path;
 246         path.reserve(this->_max_cusp + 2);
 247         path.push_back(c);
 248         unsigned int cusp = 0;
 249         while (cusp < this->_max_cusp + 1) {
 250                 if (this->parked(c)) {
 251                         if (cusp < this->_max_cusp) {
 252                                 this->_max_cusp = cusp;
 253                         }
 254                         path.push_back(c);
 255                         return path;
 256                 }
 257                 double sx = c.x() + 10.0 * cos(this->h());
 258                 double sy = c.y() + 10.0 * sin(this->h());
 259                 double cx = c.x() + 10.0 * cos(c.h());
 260                 double cy = c.y() + 10.0 * sin(c.h());
 261                 if (Point(cx, cy).on_right_side_of(
 262                                 Line(Point(c.x(), c.y()), Point(sx, sy)))) {
 263                         return std::vector<BicycleCar>();
 264                 }
 265                 c.next();
 266                 if (this->collide(c)) {
 267                         c.sp(c.sp() * -1.0);
 268                         c.next();
 269                         path.push_back(c);
 270                         c.st(c.st() * -1.0);
 271                         cusp += 1;
 272                 }
 273         }
 274         return std::vector<BicycleCar>();
 275 }
 276
 277 std::vector<BicycleCar>
 278 ParkingSlot::drive_of_slot(BicycleCar c)
 279 {
 280         assert(this->parallel());
 281         assert(this->right());
 282         assert(c.len() < this->len());
 283         assert(c.w() < this->w());
 284         assert(this->parked(c));
 285         std::vector<BicycleCar> path;
 286         path.reserve(this->_max_cusp + 2);
 287         path.push_back(c);
 288         unsigned int cusp = 0;
 289         auto b_len = sizeof(this->_border) / sizeof(this->_border[0]);
 290         std::vector<Point> b(this->_border, this->_border + b_len);
 291         while (cusp < this->_max_cusp + 1) {
 292                 if (!c.lf().inside_of(b) && !c.rf().inside_of(b)) {
 293                         if (cusp < this->_max_cusp) {
 294                                 this->_max_cusp = cusp;
 295                         }
 296                         path.push_back(c);
 297                         return path;
 298                 }
 299                 c.next();
 300                 if (this->collide(c)) {
 301                         c.sp(c.sp() * -1.0);
 302                         c.next();
 303                         path.push_back(c);
 304                         c.st(c.st() * -1.0);
 305                         cusp += 1;
 306                 }
 307         }
 308         return std::vector<BicycleCar>();
 309 }
 310
 311 std::vector<Pose>
 312 ParkingSlot::steer_in_slot(BicycleCar c)
 313 {
 314         std::vector<Pose> path;
 315         while (!this->parked(c)) {
 316                 path.push_back(c);
 317                 c.next();
 318                 if (this->collide(c)) {
 319                         c.sp(c.sp() * -1.0);
 320                         c.next();
 321                         c.st(c.st() * -1.0);
 322                 }
 323         }
 324         return path;
 325 }
 326
 327 PoseRange
 328 ParkingSlot::fe(BicycleCar c)
 329 {
 330         if (!this->parallel()) {
 331                 double gd = 0.0;
 332                 double dd = 0.0;
 333                 double radi = 0.0;
 334                 if (this->_parking_speed < 0) {
 335                         gd = c.df();
 336                         c.h(this->_rear.h() + M_PI);
 337                         c.sp(1.0);
 338                         radi = c.iradi();
 339                 } else {
 340                         gd = c.dr();
 341                         c.h(this->_rear.h());
 342                         c.sp(-1.0);
 343                         radi = c.ofradi();
 344                 }
 345                 c.x(this->_entry.m().x() + gd * cos(this->_rear.h()));
 346                 c.y(this->_entry.m().y() + gd * sin(this->_rear.h()));
 347                 Point cc(0.0, 0.0);
 348                 if (this->right()) {
 349                         cc = c.ccl();
 350                 } else {
 351                         cc = c.ccr();
 352                 }
 353                 this->_rear.intersects_with(cc, radi);
 354                 dd = std::min(this->_border[0].edist(this->_rear.i1()),
 355                         this->_border[0].edist(this->_rear.i2()));
 356                 c.st(0.0);
 357                 c.sp(c.sp() * dd);
 358                 c.next();
 359                 c.sp(this->_parking_speed);
 360                 return PoseRange(c.x(), c.y(), c.h(), c.h());
 361         }
 362         bool swapped = false;
 363         if (!this->right()) {
 364                 this->swap_side();
 365                 swapped = true;
 366         }
 367         this->set_to_start(c);
 368         auto const rc = c.rf();
 369         this->_curb.intersects_with(rc, c.len());
 370         double max_to_slot;
 371         auto const& rr = c.rr();
 372         auto const& i1 = this->_curb.i1();
 373         auto const& i2 = this->_curb.i2();
 374         if (rr.edist(i1) < rr.edist(i2)) {
 375                 max_to_slot = rr.min_angle_between(rc, i1);
 376         } else {
 377                 max_to_slot = rr.min_angle_between(rc, i2);
 378         }
 379         std::vector<BicycleCar> starts;
 380         double a_to_slot = 0.0;
 381         while (a_to_slot < max_to_slot) {
 382                 a_to_slot += this->_delta_angle_to_slot;
 383                 c.rotate(rc, this->_delta_angle_to_slot);
 384                 starts.push_back(c);
 385         }
 386         for (auto s: starts) {
 387                 auto r = this->drive_in_slot(s);
 388                 if (r.size() > 0) {
 389                         this->_ispaths.push_back(r);
 390                 }
 391         }
 392         if (this->_ispaths.size() == 0) {
 393                 return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
 394         }
 395         if (this->_ispaths.size() == 1) {
 396                 auto f = this->_ispaths.front().front();
 397                 return PoseRange(f, f);
 398         }
 399         auto& c1 = this->_ispaths.front().front();
 400         auto& c2 = this->_ispaths.back().front();
 401         PoseRange p(c1, c2);
 402         if (swapped) {
 403                 this->swap_side();
 404                 p.reflect(this->_entry);
 405         }
 406         return p;
 407 }
 408
 409 PoseRange
 410 ParkingSlot::recompute_entry(PoseRange p)
 411 {
 412         p.rotate(Point(0.0, 0.0), this->h());
 413         p.translate(this->_border[0]);
 414         if (!this->right()) {
 415                 p.reflect(this->_entry);
 416         }
 417         return p;
 418 }
 419
 420 void
 421 ParkingSlot::gen_gnuplot_to(std::ostream& out)
 422 {
 423         this->rear().gen_gnuplot_to(out);
 424         this->curb().gen_gnuplot_to(out);
 425         this->front().gen_gnuplot_to(out);
 426 }
 427
 428 std::ostream&
 429 operator<<(std::ostream& o, ParkingSlot const& s)
 430 {
 431         o << "[";
 432         o << s._border[0] << ",";
 433         o << s._border[1] << ",";
 434         o << s._border[2] << ",";
 435         o << s._border[3];
 436         o << "]";
 437         return o;
 438 }
 439
 440 } // namespace bcar