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 void
 159 ParkingSlot::set_max_cusp(unsigned int m)
 160 {
 161         this->max_cusp_ = m;
 162 }
 163
 164 void
 165 ParkingSlot::set_delta_angle_to_slot(double d)
 166 {
 167         this->delta_angle_to_slot_ = d;
 168 }
 169
 170 bool
 171 ParkingSlot::parallel() const
 172 {
 173         return this->entry_.len() > this->rear_.len();
 174 }
 175
 176 bool
 177 ParkingSlot::right() const
 178 {
 179         return this->border_[1].on_right_side_of(this->entry_);
 180 }
 181
 182 void
 183 ParkingSlot::swap_side()
 184 {
 185         this->border_[1].rotate(this->border_[0], M_PI);
 186         this->border_[2].rotate(this->border_[3], M_PI);
 187         this->entry_ = Line(this->border_[0], this->border_[3]);
 188         this->rear_ = Line(this->border_[0], this->border_[1]);
 189         this->curb_ = Line(this->border_[1], this->border_[2]);
 190         this->front_ = Line(this->border_[2], this->border_[3]);
 191 }
 192
 193 bool
 194 ParkingSlot::parked(BicycleCar const& c) const
 195 {
 196         auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
 197         std::vector<Point> b(this->border_, this->border_ + b_len);
 198         return c.lf().inside_of(b) && c.lr().inside_of(b)
 199                 && c.rr().inside_of(b) && c.rf().inside_of(b);
 200 }
 201
 202 bool
 203 ParkingSlot::collide(BicycleCar const& c) const
 204 {
 205         return c.left().intersects_with(this->rear_)
 206                 || c.left().intersects_with(this->curb_)
 207                 || c.left().intersects_with(this->front_)
 208                 || c.rear().intersects_with(this->rear_)
 209                 || c.rear().intersects_with(this->curb_)
 210                 || c.rear().intersects_with(this->front_)
 211                 || c.right().intersects_with(this->rear_)
 212                 || c.right().intersects_with(this->curb_)
 213                 || c.right().intersects_with(this->front_)
 214                 || c.front().intersects_with(this->rear_)
 215                 || c.front().intersects_with(this->curb_)
 216                 || c.front().intersects_with(this->front_);
 217 }
 218
 219 std::vector<BicycleCar>
 220 ParkingSlot::drive_in_slot(BicycleCar c)
 221 {
 222         assert(this->parallel());
 223         assert(this->right());
 224         assert(c.len() < this->len());
 225         assert(c.w() < this->w());
 226         std::vector<BicycleCar> path;
 227         path.reserve(this->max_cusp_ + 2);
 228         path.push_back(c);
 229         unsigned int cusp = 0;
 230         while (cusp < this->max_cusp_ + 1) {
 231                 if (this->parked(c)) {
 232                         if (cusp < this->max_cusp_) {
 233                                 this->max_cusp_ = cusp;
 234                         }
 235                         path.push_back(c);
 236                         return path;
 237                 }
 238                 double sx = c.x() + 10.0 * cos(this->h());
 239                 double sy = c.y() + 10.0 * sin(this->h());
 240                 double cx = c.x() + 10.0 * cos(c.h());
 241                 double cy = c.y() + 10.0 * sin(c.h());
 242                 if (Point(cx, cy).on_right_side_of(
 243                                 Line(Point(c.x(), c.y()), Point(sx, sy)))) {
 244                         return std::vector<BicycleCar>();
 245                 }
 246                 c.next();
 247                 if (this->collide(c)) {
 248                         c.sp(c.sp() * -1.0);
 249                         c.next();
 250                         path.push_back(c);
 251                         c.st(c.st() * -1.0);
 252                         cusp += 1;
 253                 }
 254         }
 255         return std::vector<BicycleCar>();
 256 }
 257
 258 std::vector<BicycleCar>
 259 ParkingSlot::drive_of_slot(BicycleCar c)
 260 {
 261         assert(this->parallel());
 262         assert(this->right());
 263         assert(c.len() < this->len());
 264         assert(c.w() < this->w());
 265         assert(this->parked(c));
 266         std::vector<BicycleCar> path;
 267         path.reserve(this->max_cusp_ + 2);
 268         path.push_back(c);
 269         unsigned int cusp = 0;
 270         auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
 271         std::vector<Point> b(this->border_, this->border_ + b_len);
 272         while (cusp < this->max_cusp_ + 1) {
 273                 if (!c.lf().inside_of(b) && !c.rf().inside_of(b)) {
 274                         if (cusp < this->max_cusp_) {
 275                                 this->max_cusp_ = cusp;
 276                         }
 277                         path.push_back(c);
 278                         return path;
 279                 }
 280                 c.next();
 281                 if (this->collide(c)) {
 282                         c.sp(c.sp() * -1.0);
 283                         c.next();
 284                         path.push_back(c);
 285                         c.st(c.st() * -1.0);
 286                         cusp += 1;
 287                 }
 288         }
 289         return std::vector<BicycleCar>();
 290 }
 291
 292 std::vector<Pose>
 293 ParkingSlot::steer_in_slot(BicycleCar c)
 294 {
 295         std::vector<Pose> path;
 296         while (!this->parked(c)) {
 297                 path.push_back(c);
 298                 c.next();
 299                 if (this->collide(c)) {
 300                         c.sp(c.sp() * -1.0);
 301                         c.next();
 302                         c.st(c.st() * -1.0);
 303                 }
 304         }
 305         return path;
 306 }
 307
 308 PoseRange
 309 ParkingSlot::fe(BicycleCar c)
 310 {
 311         if (!this->parallel()) {
 312                 double gd = 0.0;
 313                 double dd = 0.0;
 314                 double radi = 0.0;
 315                 if (this->parking_speed_ < 0) {
 316                         gd = c.df();
 317                         c.h(this->rear_.h() + M_PI);
 318                         c.sp(1.0);
 319                         radi = c.iradi();
 320                 } else {
 321                         gd = c.dr();
 322                         c.h(this->rear_.h());
 323                         c.sp(-1.0);
 324                         radi = c.ofradi();
 325                 }
 326                 c.x(this->entry_.m().x() + gd * cos(this->rear_.h()));
 327                 c.y(this->entry_.m().y() + gd * sin(this->rear_.h()));
 328                 Point cc(0.0, 0.0);
 329                 if (this->right()) {
 330                         cc = c.ccl();
 331                 } else {
 332                         cc = c.ccr();
 333                 }
 334                 this->rear_.intersects_with(cc, radi);
 335                 dd = std::min(this->border_[0].edist(this->rear_.i1()),
 336                         this->border_[0].edist(this->rear_.i2()));
 337                 c.st(0.0);
 338                 c.sp(c.sp() * dd);
 339                 c.next();
 340                 c.sp(this->parking_speed_);
 341                 return PoseRange(c.x(), c.y(), c.h(), c.h());
 342         }
 343         bool swapped = false;
 344         if (!this->right()) {
 345                 this->swap_side();
 346                 swapped = true;
 347         }
 348         c.h(this->h());
 349         double clen = -this->offset_ + this->len() - c.df();
 350         double cw = c.w() / 2.0;
 351         c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
 352         c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
 353         c.set_max_steer();
 354         assert(this->parking_speed_ < 0.0);
 355         c.sp(this->parking_speed_);
 356         auto const rc = c.rf();
 357         this->curb_.intersects_with(rc, c.len());
 358         double max_to_slot;
 359         auto const& rr = c.rr();
 360         auto const& i1 = this->curb_.i1();
 361         auto const& i2 = this->curb_.i2();
 362         if (rr.edist(i1) < rr.edist(i2)) {
 363                 max_to_slot = rr.min_angle_between(rc, i1);
 364         } else {
 365                 max_to_slot = rr.min_angle_between(rc, i2);
 366         }
 367         std::vector<BicycleCar> starts;
 368         double a_to_slot = 0.0;
 369         while (a_to_slot < max_to_slot) {
 370                 a_to_slot += this->delta_angle_to_slot_;
 371                 c.rotate(rc, this->delta_angle_to_slot_);
 372                 starts.push_back(c);
 373         }
 374         std::vector<std::vector<BicycleCar>> entries;
 375         for (auto s: starts) {
 376                 auto r = this->drive_in_slot(s);
 377                 if (r.size() > 0) {
 378                         entries.push_back(r);
 379                 }
 380         }
 381         if (entries.size() == 0) {
 382                 return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
 383         }
 384         if (entries.size() == 1) {
 385                 auto f = entries.front().front();
 386                 return PoseRange(f, f);
 387         }
 388         auto& c1 = entries.front().front();
 389         auto& c2 = entries.back().front();
 390         PoseRange p(c1, c2);
 391         if (swapped) {
 392                 this->swap_side();
 393                 p.reflect(this->entry_);
 394         }
 395         return p;
 396 }
 397
 398 PoseRange
 399 ParkingSlot::recompute_entry(PoseRange p)
 400 {
 401         p.rotate(Point(0.0, 0.0), this->h());
 402         p.translate(this->border_[0]);
 403         if (!this->right()) {
 404                 p.reflect(this->entry_);
 405         }
 406         return p;
 407 }
 408
 409 std::ostream&
 410 operator<<(std::ostream& o, ParkingSlot const& s)
 411 {
 412         o << "[";
 413         o << s.border_[0] << ",";
 414         o << s.border_[1] << ",";
 415         o << s.border_[2] << ",";
 416         o << s.border_[3];
 417         o << "]";
 418         return o;
 419 }
 420
 421 } // namespace bcar