src/bcar.cc

   1 #include "bcar.h"
   2
   3 // kinematic constraints
   4 bool BicycleCar::drivable(const BicycleCar &bc) const
   5 {
   6         double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h();
   7         while (a_1 < -M_PI)
   8                 a_1 += 2 * M_PI;
   9         while (a_1 > +M_PI)
  10                 a_1 -= 2 * M_PI;
  11         double h_d = bc.h() - this->h();
  12         while (h_d < -M_PI)
  13                 h_d += 2 * M_PI;
  14         while (h_d > +M_PI)
  15                 h_d -= 2 * M_PI;
  16         double a_2 = 0;
  17         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
  18                 return true;
  19         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
  20                 BicycleCar z(*this); // zone border
  21                 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
  22                 // assert z.h() == bc.h()
  23                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  24                         return true;
  25                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  26                 while (a_2 < -M_PI)
  27                         a_2 += 2 * M_PI;
  28                 while (a_2 > +M_PI)
  29                         a_2 -= 2 * M_PI;
  30                 if (z.h() >= a_2 && a_2 >= this->h())
  31                         return true;
  32         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
  33                 BicycleCar z(*this); // zone border
  34                 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
  35                 // assert z.h() == bc.h()
  36                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  37                         return true;
  38                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  39                 a_2 -= M_PI;
  40                 while (a_2 < -M_PI)
  41                         a_2 += 2 * M_PI;
  42                 while (a_2 > +M_PI)
  43                         a_2 -= 2 * M_PI;
  44                 if (this->h() >= a_2 && a_2 >= z.h())
  45                         return true;
  46         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
  47                 BicycleCar z(*this); // zone border
  48                 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
  49                 // assert z.h() == bc.h()
  50                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  51                         return true;
  52                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  53                 while (a_2 < -M_PI)
  54                         a_2 += 2 * M_PI;
  55                 while (a_2 > +M_PI)
  56                         a_2 -= 2 * M_PI;
  57                 if (this->h() >= a_2 && a_2 >= z.h())
  58                         return true;
  59         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
  60                 BicycleCar z(*this); // zone border
  61                 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
  62                 // assert z.h() == bc.h()
  63                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  64                         return true;
  65                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  66                 a_2 -= M_PI;
  67                 while (a_2 < -M_PI)
  68                         a_2 += 2 * M_PI;
  69                 while (a_2 > +M_PI)
  70                         a_2 -= 2 * M_PI;
  71                 if (z.h() >= a_2 && a_2 >= this->h())
  72                         return true;
  73         } else {
  74                 // Not happenning, as ``-pi <= a <= pi``.
  75         }
  76         return false;
  77 }
  78
  79 double BicycleCar::iradi() const
  80 {
  81         return this->mtr() - this->w() / 2;
  82 }
  83
  84 double BicycleCar::ofradi() const
  85 {
  86         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
  87 }
  88
  89 double BicycleCar::orradi() const
  90 {
  91         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
  92 }
  93
  94 double BicycleCar::perfect_parking_slot_len() const
  95 {
  96         // see Simon R. Blackburn *The Geometry of Perfect Parking*
  97         // see https://www.ma.rhul.ac.uk/SRBparking
  98         double r = this->ctc() / 2;
  99         double l = this->wb();
 100         double k = this->df() - this->wb();
 101         double w = this->w();
 102         return
 103                 this->l()
 104                 + sqrt(
 105                         (r*r - l*l)
 106                         + pow(l + k, 2)
 107                         - pow(sqrt(r*r - l*l) - w, 2)
 108                 )
 109                 - l
 110                 - k
 111         ;
 112 }
 113
 114 void BicycleCar::set_max_steer()
 115 {
 116         this->st(atan(this->wb() / this->mtr()));
 117 }
 118
 119 // car frame
 120 double BicycleCar::lfx() const
 121 {
 122         double lfx = this->x();
 123         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 124         lfx += this->df() * cos(this->h());
 125         lfx += this->sd() * cos(this->h());
 126         return lfx;
 127 }
 128
 129 double BicycleCar::lfy() const
 130 {
 131         double lfy = this->y();
 132         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
 133         lfy += this->df() * sin(this->h());
 134         lfy += this->sd() * sin(this->h());
 135         return lfy;
 136 }
 137
 138 double BicycleCar::lrx() const
 139 {
 140         double lrx = this->x();
 141         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 142         lrx += -this->dr() * cos(this->h());
 143         lrx += -this->sd() * cos(this->h());
 144         return lrx;
 145 }
 146
 147 double BicycleCar::lry() const
 148 {
 149         double lry = this->y();
 150         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
 151         lry += -this->dr() * sin(this->h());
 152         lry += -this->sd() * sin(this->h());
 153         return lry;
 154 }
 155
 156 double BicycleCar::rrx() const
 157 {
 158         double rrx = this->x();
 159         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 160         rrx += -this->dr() * cos(this->h());
 161         rrx += -this->sd() * cos(this->h());
 162         return rrx;
 163 }
 164
 165 double BicycleCar::rry() const
 166 {
 167         double rry = this->y();
 168         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 169         rry += -this->dr() * sin(this->h());
 170         rry += -this->sd() * sin(this->h());
 171         return rry;
 172 }
 173
 174 double BicycleCar::rfx() const
 175 {
 176         double rfx = this->x();
 177         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 178         rfx += this->df() * cos(this->h());
 179         rfx += this->sd() * cos(this->h());
 180         return rfx;
 181 }
 182
 183 double BicycleCar::rfy() const
 184 {
 185         double rfy = this->y();
 186         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
 187         rfy += this->df() * sin(this->h());
 188         rfy += this->sd() * sin(this->h());
 189         return rfy;
 190 }
 191
 192 double BicycleCar::ralx() const
 193 {
 194         double lrx = this->x();
 195         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 196         return lrx;
 197 }
 198 double BicycleCar::raly() const
 199 {
 200         double lry = this->y();
 201         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
 202         return lry;
 203 }
 204
 205 double BicycleCar::rarx() const
 206 {
 207         double rrx = this->x();
 208         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 209         return rrx;
 210 }
 211
 212 double BicycleCar::rary() const
 213 {
 214         double rry = this->y();
 215         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 216         return rry;
 217 }
 218
 219 BicycleCar BicycleCar::ccl() const
 220 {
 221         BicycleCar bc;
 222         bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
 223         bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
 224         bc.h(this->h());
 225         return bc;
 226 }
 227
 228 BicycleCar BicycleCar::ccr() const
 229 {
 230         BicycleCar bc;
 231         bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
 232         bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
 233         bc.h(this->h());
 234         return bc;
 235 }
 236
 237 // moving
 238 void BicycleCar::next()
 239 {
 240         this->x(this->x() + this->sp() * cos(this->h()));
 241         this->y(this->y() + this->sp() * sin(this->h()));
 242         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 243 }
 244
 245 void BicycleCar::rotate(double cx, double cy, double angl)
 246 {
 247         double px = this->x();
 248         double py = this->y();
 249         px -= cx;
 250         py -= cy;
 251         double nx = px * cos(angl) - py * sin(angl);
 252         double ny = px * sin(angl) + py * cos(angl);
 253         this->h(this->h() + angl);
 254         this->x(nx + cx);
 255         this->y(ny + cy);
 256 }
 257
 258 BicycleCar::BicycleCar()
 259 {
 260         // TODO according to mtr_ FIXME
 261         this->mtr_ = sqrt(
 262                         pow(10.82 / 2, 2)
 263                         - pow(this->wb(), 2)
 264                 )
 265                 - this->w() / 2
 266         ;
 267 }
 268
 269 std::tuple<bool, unsigned int, unsigned int> collide(
 270         std::vector<std::tuple<double, double>> &p1,
 271         std::vector<std::tuple<double, double>> &p2
 272 )
 273 {
 274         for (unsigned int i = 0; i < p1.size() - 1; i++) {
 275                 for (unsigned int j = 0; j < p2.size() - 1; j++) {
 276                         auto x = intersect(
 277                                 std::get<0>(p1[i]),
 278                                 std::get<1>(p1[i]),
 279                                 std::get<0>(p1[i + 1]),
 280                                 std::get<1>(p1[i + 1]),
 281                                 std::get<0>(p2[j]),
 282                                 std::get<1>(p2[j]),
 283                                 std::get<0>(p2[j + 1]),
 284                                 std::get<1>(p2[j + 1])
 285                         );
 286                         if (std::get<0>(x))
 287                                 return std::make_tuple(true, i, j);
 288                 }
 289         }
 290         return std::make_tuple(false, 0, 0);
 291 }
 292
 293 bool inside(double x, double y, std::vector<std::tuple<double, double>> &poly)
 294 {
 295         unsigned int i = 0;
 296         unsigned int j = 3;
 297         bool inside = false;
 298         for (i = 0; i < 4; i++) {
 299                 if (
 300                         (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
 301                         && (
 302                                 x < std::get<0>(poly[i])
 303                                 + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
 304                                 * (y - std::get<1>(poly[i]))
 305                                 / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
 306                         )
 307                 )
 308                         inside = !inside;
 309                 j = i;
 310         }
 311         return inside;
 312 }
 313
 314 std::tuple<bool, double, double> intersect(
 315         double x1, double y1,
 316         double x2, double y2,
 317         double x3, double y3,
 318         double x4, double y4
 319 )
 320 {
 321         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 322         if (deno == 0)
 323                 return std::make_tuple(false, 0, 0);
 324         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 325         t /= deno;
 326         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 327         u *= -1;
 328         u /= deno;
 329         if (t < 0 || t > 1 || u < 0 || u > 1)
 330                 return std::make_tuple(false, 0, 0);
 331         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 332 }