src/pslot.cc

   1 #include <cassert>
   2 #include <cmath>
   3 #include "pslot.hh"
   4
   5 namespace bcar {
   6
   7 ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
   8                 border_({p,
   9                         Point(p.x() + W * cos(h - M_PI / 2.0),
  10                                 p.y() + W * sin(h - M_PI / 2.0)),
  11                         Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
  12                                 p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
  13                         Point(p.x() + L * cos(h), p.y() + L * sin(h))}),
  14                 entry_(border_[0], border_[3]),
  15                 rear_(border_[0], border_[1]),
  16                 curb_(border_[1], border_[2]),
  17                 front_(border_[2], border_[3])
  18 {
  19 }
  20
  21 ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
  22                 double rfx, double rfy, double lfx, double lfy) :
  23                         border_({Point(lrx, lry), Point(rrx, rry),
  24                                 Point(rfx, rfy), Point(lfx, lfy)}),
  25                         entry_(border_[0], border_[3]),
  26                         rear_(border_[0], border_[1]),
  27                         curb_(border_[1], border_[2]),
  28                         front_(border_[2], border_[3])
  29 {
  30 }
  31
  32 double
  33 ParkingSlot::len() const
  34 {
  35         return this->entry_.len();
  36 }
  37
  38 double
  39 ParkingSlot::w() const
  40 {
  41         return this->rear_.len();
  42 }
  43
  44 double
  45 ParkingSlot::lfx() const
  46 {
  47         return this->border_[3].x();
  48 }
  49
  50 double
  51 ParkingSlot::lfy() const
  52 {
  53         return this->border_[3].y();
  54 }
  55
  56 double
  57 ParkingSlot::lrx() const
  58 {
  59         return this->border_[0].x();
  60 }
  61
  62 double
  63 ParkingSlot::lry() const
  64 {
  65         return this->border_[0].y();
  66 }
  67
  68 double
  69 ParkingSlot::rrx() const
  70 {
  71         return this->border_[1].x();
  72 }
  73
  74 double
  75 ParkingSlot::rry() const
  76 {
  77         return this->border_[1].y();
  78 }
  79
  80 double
  81 ParkingSlot::rfx() const
  82 {
  83         return this->border_[2].x();
  84 }
  85
  86 double
  87 ParkingSlot::rfy() const
  88 {
  89         return this->border_[2].y();
  90 }
  91
  92 double
  93 ParkingSlot::h() const
  94 {
  95         return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
  96 }
  97
  98 bool
  99 ParkingSlot::parallel() const
 100 {
 101         return this->entry_.len() > this->rear_.len();
 102 }
 103
 104 bool
 105 ParkingSlot::right() const
 106 {
 107         return this->border_[1].on_right_side_of(this->entry_);
 108 }
 109
 110 void
 111 ParkingSlot::swap_side()
 112 {
 113         this->border_[1].rotate(this->border_[0], M_PI);
 114         this->border_[2].rotate(this->border_[3], M_PI);
 115 }
 116
 117 bool
 118 ParkingSlot::parked(BicycleCar const& c) const
 119 {
 120         auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
 121         std::vector<Point> b(this->border_, this->border_ + b_len);
 122         return c.lf().inside_of(b) && c.lr().inside_of(b)
 123                 && c.rr().inside_of(b) && c.rf().inside_of(b);
 124 }
 125
 126 bool
 127 ParkingSlot::collide(BicycleCar const& c) const
 128 {
 129         return c.left().intersects_with(this->rear_)
 130                 && c.left().intersects_with(this->curb_)
 131                 && c.left().intersects_with(this->front_)
 132                 && c.rear().intersects_with(this->rear_)
 133                 && c.rear().intersects_with(this->curb_)
 134                 && c.rear().intersects_with(this->front_)
 135                 && c.right().intersects_with(this->rear_)
 136                 && c.right().intersects_with(this->curb_)
 137                 && c.right().intersects_with(this->front_)
 138                 && c.front().intersects_with(this->rear_)
 139                 && c.front().intersects_with(this->curb_)
 140                 && c.front().intersects_with(this->front_);
 141 }
 142
 143 std::vector<BicycleCar>
 144 ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
 145 {
 146         assert(this->parallel());
 147         assert(this->right());
 148         assert(c.len() < this->len());
 149         assert(c.w() < this->w());
 150         std::vector<BicycleCar> path;
 151         path.reserve(max + 2);
 152         path.push_back(c);
 153         unsigned int cusp = 0;
 154         while (cusp < max + 1) {
 155                 if (this->parked(c)) {
 156                         if (cusp < max) {
 157                                 max = cusp;
 158                         }
 159                         path.push_back(c);
 160                         return path;
 161                 }
 162                 if (c.h() < this->h()) {
 163                         return std::vector<BicycleCar>();
 164                 }
 165                 c.next();
 166                 if (this->collide(c)) {
 167                         c.sp(c.sp() * -1.0);
 168                         c.next();
 169                         path.push_back(c);
 170                         c.st(c.st() * -1.0);
 171                         cusp += 1;
 172                 }
 173         }
 174         return std::vector<BicycleCar>();
 175 }
 176
 177 std::vector<Pose>
 178 ParkingSlot::steer_in_slot(BicycleCar c)
 179 {
 180         std::vector<Pose> path;
 181         while (!this->parked(c)) {
 182                 path.push_back(c);
 183                 c.next();
 184                 if (this->collide(c)) {
 185                         c.sp(c.sp() * -1.0);
 186                         c.next();
 187                         c.st(c.st() * -1.0);
 188                 }
 189         }
 190         path.push_back(c);
 191         return path;
 192 }
 193
 194 PoseRange
 195 ParkingSlot::fe(BicycleCar c, unsigned int& max)
 196 {
 197         assert(this->parallel());
 198         assert(this->right());
 199         c.h(this->h());
 200         double clen = this->offset_ + this->len() - c.df();
 201         double cw = this->offset_ + c.w() / 2.0;
 202         c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
 203         c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
 204         c.set_max_steer();
 205         c.sp(-0.01);
 206         auto const& b3 = this->border_[3];
 207         this->curb_.intersects_with(b3, c.len());
 208         double max_to_slot;
 209         auto const& rr = c.rr();
 210         auto const& i1 = this->curb_.i1();
 211         auto const& i2 = this->curb_.i2();
 212         if (rr.edist(i1) < rr.edist(i2)) {
 213                 max_to_slot = rr.min_angle_between(b3, i1);
 214         } else {
 215                 max_to_slot = rr.min_angle_between(b3, i2);
 216         }
 217         std::vector<BicycleCar> starts;
 218         double a_to_slot = 0.0;
 219         while (a_to_slot < max_to_slot) {
 220                 a_to_slot += 0.001;
 221                 c.rotate(b3, 0.001);
 222                 starts.push_back(c);
 223         }
 224         std::vector<std::vector<BicycleCar>> entries;
 225         for (auto s: starts) {
 226                 auto r = this->drive_in_slot(s, max);
 227                 if (r.size() > 0) {
 228                         entries.push_back(r);
 229                 }
 230         }
 231         if (entries.size() == 0) {
 232                 return PoseRange();
 233         }
 234         auto& c1 = entries.front().front();
 235         auto& c2 = entries.back().front();
 236         double b = std::min(c1.h(), c2.h());
 237         double e = std::max(c1.h(), c2.h());
 238         clen = c.len();
 239         Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h()));
 240         Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h()));
 241         Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h()));
 242         Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h()));
 243         Line li1(b1, e1);
 244         Line li2(b2, e2);
 245         li1.intersects_with(li2);
 246         PoseRange pr;
 247         pr.x(li1.i1().x());
 248         pr.y(li1.i1().y());
 249         pr.b(b);
 250         pr.e(e);
 251         return pr;
 252 }
 253
 254 std::ostream&
 255 operator<<(std::ostream& o, ParkingSlot const& s)
 256 {
 257         o << "[";
 258         o << s.border_[0] << ",";
 259         o << s.border_[1] << ",";
 260         o << s.border_[2] << ",";
 261         o << s.border_[3];
 262         o << "]";
 263         return o;
 264 }
 265
 266 } // namespace bcar