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         if (0 <= a_1 && a_1 <= M_PI/2) { // left front
  13         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
  14         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
  15         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
  16         } else {
  17                 // Not happenning, as ``-pi <= a <= pi``.
  18         }
  19         return false;
  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         this->x(this->x() + this->sp() * cos(this->h()));
 184         this->y(this->y() + this->sp() * sin(this->h()));
 185         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 186 }
 187
 188 void BicycleCar::rotate(double cx, double cy, double angl)
 189 {
 190         double px = this->x();
 191         double py = this->y();
 192         px -= cx;
 193         py -= cy;
 194         double nx = px * cos(angl) - py * sin(angl);
 195         double ny = px * sin(angl) + py * cos(angl);
 196         this->h(this->h() + angl);
 197         this->x(nx + cx);
 198         this->y(ny + cy);
 199 }
 200
 201 BicycleCar::BicycleCar()
 202 {
 203         // TODO according to mtr_ FIXME
 204         this->mtr_ = sqrt(
 205                         pow(10.82 / 2, 2)
 206                         - pow(this->wb(), 2)
 207                 )
 208                 - this->w() / 2
 209         ;
 210 }
 211
 212 std::tuple<bool, unsigned int, unsigned int> collide(
 213         std::vector<std::tuple<double, double>> &p1,
 214         std::vector<std::tuple<double, double>> &p2
 215 )
 216 {
 217         for (unsigned int i = 0; i < p1.size() - 1; i++) {
 218                 for (unsigned int j = 0; j < p2.size() - 1; j++) {
 219                         auto x = intersect(
 220                                 std::get<0>(p1[i]),
 221                                 std::get<1>(p1[i]),
 222                                 std::get<0>(p1[i + 1]),
 223                                 std::get<1>(p1[i + 1]),
 224                                 std::get<0>(p2[j]),
 225                                 std::get<1>(p2[j]),
 226                                 std::get<0>(p2[j + 1]),
 227                                 std::get<1>(p2[j + 1])
 228                         );
 229                         if (std::get<0>(x))
 230                                 return std::make_tuple(true, i, j);
 231                 }
 232         }
 233         return std::make_tuple(false, 0, 0);
 234 }
 235
 236 bool inside(double x, double y, std::vector<std::tuple<double, double>> &poly)
 237 {
 238         unsigned int i = 0;
 239         unsigned int j = 3;
 240         bool inside = false;
 241         for (i = 0; i < 4; i++) {
 242                 if (
 243                         (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
 244                         && (
 245                                 x < std::get<0>(poly[i])
 246                                 + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
 247                                 * (y - std::get<1>(poly[i]))
 248                                 / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
 249                         )
 250                 )
 251                         inside = !inside;
 252                 j = i;
 253         }
 254         return inside;
 255 }
 256
 257 std::tuple<bool, double, double> intersect(
 258         double x1, double y1,
 259         double x2, double y2,
 260         double x3, double y3,
 261         double x4, double y4
 262 )
 263 {
 264         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 265         if (deno == 0)
 266                 return std::make_tuple(false, 0, 0);
 267         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 268         t /= deno;
 269         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 270         u *= -1;
 271         u /= deno;
 272         if (t < 0 || t > 1 || u < 0 || u > 1)
 273                 return std::make_tuple(false, 0, 0);
 274         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 275 }