vehicle_platform/bcar.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "bcar.h"
  20
  21 // getter
  22 float BicycleCar::length()
  23 {
  24         return this->length_;
  25 }
  26
  27 float BicycleCar::wheelbase()
  28 {
  29         return this->wheel_base_;
  30 }
  31
  32 float BicycleCar::width()
  33 {
  34         return this->width_;
  35 }
  36
  37 float BicycleCar::dr()
  38 {
  39         return (this->length_ - this->wheel_base_) / 2;
  40 }
  41
  42 float BicycleCar::df()
  43 {
  44         return this->length_ - this->dr();
  45 }
  46
  47 float BicycleCar::lfx()
  48 {
  49
  50         float lfx = this->x();
  51         lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  52         lfx += this->df() * cos(this->h());
  53         lfx += this->safety_dist_ * cos(this->h());
  54         return lfx;
  55 }
  56
  57 float BicycleCar::lrx()
  58 {
  59         float lrx = this->x();
  60         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  61         lrx += -this->dr() * cos(this->h());
  62         lrx += -this->safety_dist_ * cos(this->h());
  63         return lrx;
  64 }
  65
  66 float BicycleCar::rrx()
  67 {
  68         float rrx = this->x();
  69         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  70         rrx += -this->dr() * cos(this->h());
  71         rrx += -this->safety_dist_ * cos(this->h());
  72         return rrx;
  73 }
  74
  75 float BicycleCar::rfx()
  76 {
  77         float rfx = this->x();
  78         rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  79         rfx += this->df() * cos(this->h());
  80         rfx += this->safety_dist_ * cos(this->h());
  81         return rfx;
  82 }
  83
  84 float BicycleCar::lfy()
  85 {
  86         float lfy = this->y();
  87         lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  88         lfy += this->df() * sin(this->h());
  89         lfy += this->safety_dist_ * sin(this->h());
  90         return lfy;
  91 }
  92
  93 float BicycleCar::lry()
  94 {
  95         float lry = this->y();
  96         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  97         lry += -this->dr() * sin(this->h());
  98         lry += -this->safety_dist_ * sin(this->h());
  99         return lry;
 100 }
 101
 102 float BicycleCar::rry()
 103 {
 104         float rry = this->y();
 105         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
 106         rry += -this->dr() * sin(this->h());
 107         rry += -this->safety_dist_ * sin(this->h());
 108         return rry;
 109 }
 110
 111 float BicycleCar::rfy()
 112 {
 113         float rfy = this->y();
 114         rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
 115         rfy += this->df() * sin(this->h());
 116         rfy += this->safety_dist_ * sin(this->h());
 117         return rfy;
 118 }
 119
 120 float BicycleCar::lwbx()
 121 {
 122         float lrx = this->x();
 123         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
 124         return lrx;
 125 }
 126 float BicycleCar::lwby()
 127 {
 128         float lry = this->y();
 129         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
 130         return lry;
 131 }
 132
 133 float BicycleCar::rwbx()
 134 {
 135         float rrx = this->x();
 136         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
 137         return rrx;
 138 }
 139
 140 float BicycleCar::rwby()
 141 {
 142         float rry = this->y();
 143         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
 144         return rry;
 145 }
 146
 147 RRTNode *BicycleCar::ccl()
 148 {
 149         RRTEdge *l1 = new RRTEdge(
 150                         new RRTNode(this->lwbx(), this->lwby()),
 151                         new RRTNode(this->rwbx(), this->rwby()));
 152         float d = this->width_ * tan(this->alfa());
 153         float x = this->lfx() + d * cos(this->h() + M_PI);
 154         float y = this->lfy() + d * sin(this->h() + M_PI);
 155         RRTEdge *l2 = new RRTEdge(
 156                         new RRTNode(x, y, 0),
 157                         new RRTNode(this->rfx(), this->rfy(), 0));
 158         return l1->intersect(l2, false);
 159 }
 160
 161 RRTNode *BicycleCar::ccr()
 162 {
 163         RRTEdge *l1 = new RRTEdge(
 164                         new RRTNode(this->lwbx(), this->lwby()),
 165                         new RRTNode(this->rwbx(), this->rwby()));
 166         float d = this->width_ * tan(this->alfa());
 167         float x = this->rfx() + d * cos(this->h() - M_PI);
 168         float y = this->rfy() + d * sin(this->h() - M_PI);
 169         RRTEdge *l2 = new RRTEdge(
 170                         new RRTNode(this->lfx(), this->lfy(), 0),
 171                         new RRTNode(x, y, 0));
 172         return l1->intersect(l2, false);
 173 }
 174
 175 float BicycleCar::speed()
 176 {
 177         return this->speed_;
 178 }
 179
 180 float BicycleCar::steer()
 181 {
 182         return this->steer_;
 183 }
 184
 185 BicycleCar *BicycleCar::bcparent() const
 186 {
 187         return this->bcparent_;
 188 }
 189
 190 // setter
 191 bool BicycleCar::speed(float s)
 192 {
 193         this->speed_ = s;
 194         return true;
 195 }
 196
 197 bool BicycleCar::steer(float s)
 198 {
 199         this->steer_ = s;
 200         return true;
 201 }
 202
 203 void BicycleCar::bcparent(BicycleCar *bcp)
 204 {
 205         this->bcparent_ = bcp;
 206 }
 207
 208 // other
 209 std::vector<RRTEdge *> BicycleCar::frame()
 210 {
 211         std::vector<RRTEdge *> frame;
 212         frame.push_back(new RRTEdge(
 213                                 new RRTNode(this->lfx(), this->lfy()),
 214                                 new RRTNode(this->lrx(), this->lry())));
 215         frame.push_back(new RRTEdge(
 216                                 new RRTNode(this->lrx(), this->lry()),
 217                                 new RRTNode(this->rrx(), this->rry())));
 218         frame.push_back(new RRTEdge(
 219                                 new RRTNode(this->rrx(), this->rry()),
 220                                 new RRTNode(this->rfx(), this->rfy())));
 221         frame.push_back(new RRTEdge(
 222                                 new RRTNode(this->rfx(), this->rfy()),
 223                                 new RRTNode(this->lfx(), this->lfy())));
 224         return frame;
 225 }
 226
 227 bool BicycleCar::next()
 228 {
 229         if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
 230                 this->steer_ = MAXSTEER(
 231                                 this->wheel_base_,
 232                                 this->turning_radius_);
 233         if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
 234                 this->steer_ = -MAXSTEER(
 235                                 this->wheel_base_,
 236                                 this->turning_radius_);
 237         this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
 238         this->x_ += this->speed_ * cos(this->h_);
 239         this->y_ += this->speed_ * sin(this->h_);
 240         return true;
 241 }
 242
 243 bool BicycleCar::next(float speed, float steer)
 244 {
 245         this->speed_ = speed;
 246         this->steer_ = steer;
 247         return this->next();
 248 }
 249
 250 float BicycleCar::diag_radi()
 251 {
 252         float xx = pow((this->length_ + this->wheel_base_) / 2, 2);
 253         float yy = pow(this->width_ / 2, 2);
 254         return pow(xx + yy, 0.5);
 255 }
 256
 257 float BicycleCar::diag_rradi()
 258 {
 259         float xx = pow(this->dr(), 2);
 260         float yy = pow(this->width_ / 2, 2);
 261         return pow(xx + yy, 0.5);
 262 }
 263
 264 float BicycleCar::out_radi()
 265 {
 266         return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
 267                         pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
 268 }
 269
 270 float BicycleCar::alfa()
 271 {
 272         return asin((this->length_ + this->wheel_base_) /
 273                         (2 * this->out_radi()));
 274 }
 275
 276 bool BicycleCar::drivable(RRTNode *n)
 277 {
 278         bool drivable = true;
 279         RRTNode *ccl = this->ccl();
 280         RRTNode *ccr = this->ccr();
 281         if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
 282                         pow(this->turning_radius_, 2))
 283                 drivable = false;
 284         if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
 285                         pow(this->turning_radius_, 2))
 286                 drivable = false;
 287         return drivable;
 288 }
 289
 290 BicycleCar *BicycleCar::move(RRTNode *c, float dh)
 291 {
 292         float zx = c->x();
 293         float zy = c->y();
 294         float px = this->x();
 295         float py = this->y();
 296         px -= zx;
 297         py -= zy;
 298         float nx = px * cos(dh) - py * sin(dh);
 299         float ny = px * sin(dh) + py * cos(dh);
 300         px = nx + zx;
 301         py = ny + zy;
 302         float ph = this->h();
 303         ph += dh;
 304         return new BicycleCar(px, py, ph);
 305 }