src/bcar.cc

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