src/bcar.cc

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