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 float BicycleCar::dr()
  22 {
  23         return (this->length_ - this->wheel_base_) / 2;
  24 }
  25
  26 float BicycleCar::df()
  27 {
  28         return this->length_ - this->dr();
  29 }
  30
  31 float BicycleCar::lfx()
  32 {
  33
  34         float lfx = this->x();
  35         lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  36         lfx += this->df() * cos(this->h());
  37         lfx += this->safety_dist_ * cos(this->h());
  38         return lfx;
  39 }
  40
  41 float BicycleCar::lrx()
  42 {
  43         float lrx = this->x();
  44         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  45         lrx += -this->dr() * cos(this->h());
  46         lrx += -this->safety_dist_ * cos(this->h());
  47         return lrx;
  48 }
  49
  50 float BicycleCar::rrx()
  51 {
  52         float rrx = this->x();
  53         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  54         rrx += -this->dr() * cos(this->h());
  55         rrx += -this->safety_dist_ * cos(this->h());
  56         return rrx;
  57 }
  58
  59 float BicycleCar::rfx()
  60 {
  61         float rfx = this->x();
  62         rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  63         rfx += this->df() * cos(this->h());
  64         rfx += this->safety_dist_ * cos(this->h());
  65         return rfx;
  66 }
  67
  68 float BicycleCar::lfy()
  69 {
  70         float lfy = this->y();
  71         lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  72         lfy += this->df() * sin(this->h());
  73         lfy += this->safety_dist_ * sin(this->h());
  74         return lfy;
  75 }
  76
  77 float BicycleCar::lry()
  78 {
  79         float lry = this->y();
  80         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  81         lry += -this->dr() * sin(this->h());
  82         lry += -this->safety_dist_ * sin(this->h());
  83         return lry;
  84 }
  85
  86 float BicycleCar::rry()
  87 {
  88         float rry = this->y();
  89         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  90         rry += -this->dr() * sin(this->h());
  91         rry += -this->safety_dist_ * sin(this->h());
  92         return rry;
  93 }
  94
  95 float BicycleCar::rfy()
  96 {
  97         float rfy = this->y();
  98         rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  99         rfy += this->df() * sin(this->h());
 100         rfy += this->safety_dist_ * sin(this->h());
 101         return rfy;
 102 }
 103
 104 float BicycleCar::lwbx()
 105 {
 106         float lrx = this->x();
 107         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
 108         return lrx;
 109 }
 110 float BicycleCar::lwby()
 111 {
 112         float lry = this->y();
 113         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
 114         return lry;
 115 }
 116
 117 float BicycleCar::rwbx()
 118 {
 119         float rrx = this->x();
 120         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
 121         return rrx;
 122 }
 123
 124 float BicycleCar::rwby()
 125 {
 126         float rry = this->y();
 127         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
 128         return rry;
 129 }
 130
 131 RRTNode *BicycleCar::ccl()
 132 {
 133         RRTEdge *l1 = new RRTEdge(
 134                         new RRTNode(this->lwbx(), this->lwby()),
 135                         new RRTNode(this->rwbx(), this->rwby()));
 136         float d = this->width_ * tan(this->alfa());
 137         float x = this->lfx() + d * cos(this->h() + M_PI);
 138         float y = this->lfy() + d * sin(this->h() + M_PI);
 139         RRTEdge *l2 = new RRTEdge(
 140                         new RRTNode(x, y, 0),
 141                         new RRTNode(this->rfx(), this->rfy(), 0));
 142         return l1->intersect(l2, false);
 143 }
 144
 145 RRTNode *BicycleCar::ccr()
 146 {
 147         RRTEdge *l1 = new RRTEdge(
 148                         new RRTNode(this->lwbx(), this->lwby()),
 149                         new RRTNode(this->rwbx(), this->rwby()));
 150         float d = this->width_ * tan(this->alfa());
 151         float x = this->rfx() + d * cos(this->h() - M_PI);
 152         float y = this->rfy() + d * sin(this->h() - M_PI);
 153         RRTEdge *l2 = new RRTEdge(
 154                         new RRTNode(this->lfx(), this->lfy(), 0),
 155                         new RRTNode(x, y, 0));
 156         return l1->intersect(l2, false);
 157 }
 158
 159 float BicycleCar::speed()
 160 {
 161         return this->speed_;
 162 }
 163
 164 float BicycleCar::steer()
 165 {
 166         return this->steer_;
 167 }
 168
 169 bool BicycleCar::speed(float s)
 170 {
 171         this->speed_ = s;
 172         return true;
 173 }
 174
 175 bool BicycleCar::steer(float s)
 176 {
 177         this->steer_ = s;
 178         return true;
 179 }
 180
 181 std::vector<RRTEdge *> BicycleCar::frame()
 182 {
 183         std::vector<RRTEdge *> frame;
 184         frame.push_back(new RRTEdge(
 185                                 new RRTNode(this->lfx(), this->lfy()),
 186                                 new RRTNode(this->lrx(), this->lry())));
 187         frame.push_back(new RRTEdge(
 188                                 new RRTNode(this->lrx(), this->lry()),
 189                                 new RRTNode(this->rrx(), this->rry())));
 190         frame.push_back(new RRTEdge(
 191                                 new RRTNode(this->rrx(), this->rry()),
 192                                 new RRTNode(this->rfx(), this->rfy())));
 193         frame.push_back(new RRTEdge(
 194                                 new RRTNode(this->rfx(), this->rfy()),
 195                                 new RRTNode(this->lfx(), this->rfy())));
 196         return frame;
 197 }
 198
 199 bool BicycleCar::next()
 200 {
 201         if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
 202                 this->steer_ = MAXSTEER(
 203                                 this->wheel_base_,
 204                                 this->turning_radius_);
 205         if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
 206                 this->steer_ = -MAXSTEER(
 207                                 this->wheel_base_,
 208                                 this->turning_radius_);
 209         this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
 210         this->x_ += this->speed_ * cos(this->h_);
 211         this->y_ += this->speed_ * sin(this->h_);
 212         return true;
 213 }
 214
 215 bool BicycleCar::next(float speed, float steer)
 216 {
 217         this->speed_ = speed;
 218         this->steer_ = steer;
 219         return this->next();
 220 }
 221
 222 float BicycleCar::out_radi()
 223 {
 224         return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
 225                         pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
 226 }
 227
 228 float BicycleCar::alfa()
 229 {
 230         return asin((this->length_ + this->wheel_base_) /
 231                         (2 * this->out_radi()));
 232 }
 233
 234 bool BicycleCar::drivable(RRTNode *n)
 235 {
 236         bool drivable = true;
 237         RRTNode *ccl = this->ccl();
 238         RRTNode *ccr = this->ccr();
 239         if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
 240                         pow(this->turning_radius_, 2))
 241                 drivable = false;
 242         if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
 243                         pow(this->turning_radius_, 2))
 244                 drivable = false;
 245         return drivable;
 246 }
 247
 248 BicycleCar *BicycleCar::move(RRTNode *c, float dh)
 249 {
 250         float zx = c->x();
 251         float zy = c->y();
 252         float px = this->x();
 253         float py = this->y();
 254         px -= zx;
 255         py -= zy;
 256         float nx = px * cos(dh) - py * sin(dh);
 257         float ny = px * sin(dh) + py * cos(dh);
 258         px = nx + zx;
 259         py = ny + zy;
 260         float ph = this->h();
 261         ph += dh;
 262         return new BicycleCar(px, py, ph);
 263 }