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