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