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