src/bcar.cc

   1 #include <cmath>
   2 #include "bcar.h"
   3
   4 // kinematic constraints
   5 bool BicycleCar::drivable(BicycleCar *bc)
   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()
  23 {
  24         return 0;
  25 }
  26
  27 double BicycleCar::ofradi()
  28 {
  29         return 0;
  30 }
  31
  32 double BicycleCar::orradi()
  33 {
  34         return 0;
  35 }
  36
  37 // car frame
  38 double BicycleCar::lfx()
  39 {
  40         double lfx = this->x();
  41         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
  42         lfx += this->df() * cos(this->h());
  43         lfx += this->sd() * cos(this->h());
  44         return lfx;
  45 }
  46
  47 double BicycleCar::lfy()
  48 {
  49         double lfy = this->y();
  50         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
  51         lfy += this->df() * sin(this->h());
  52         lfy += this->sd() * sin(this->h());
  53         return lfy;
  54 }
  55
  56 double BicycleCar::lrx()
  57 {
  58         double lrx = this->x();
  59         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
  60         lrx += -this->dr() * cos(this->h());
  61         lrx += -this->sd() * cos(this->h());
  62         return lrx;
  63 }
  64
  65 double BicycleCar::lry()
  66 {
  67         double lry = this->y();
  68         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
  69         lry += -this->dr() * sin(this->h());
  70         lry += -this->sd() * sin(this->h());
  71         return lry;
  72 }
  73
  74 double BicycleCar::rrx()
  75 {
  76         double rrx = this->x();
  77         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
  78         rrx += -this->dr() * cos(this->h());
  79         rrx += -this->sd() * cos(this->h());
  80         return rrx;
  81 }
  82
  83 double BicycleCar::rry()
  84 {
  85         double rry = this->y();
  86         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
  87         rry += -this->dr() * sin(this->h());
  88         rry += -this->sd() * sin(this->h());
  89         return rry;
  90 }
  91
  92 double BicycleCar::rfx()
  93 {
  94         double rfx = this->x();
  95         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
  96         rfx += this->df() * cos(this->h());
  97         rfx += this->sd() * cos(this->h());
  98         return rfx;
  99 }
 100
 101 double BicycleCar::rfy()
 102 {
 103         double rfy = this->y();
 104         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
 105         rfy += this->df() * sin(this->h());
 106         rfy += this->sd() * sin(this->h());
 107         return rfy;
 108 }
 109
 110 double BicycleCar::ralx()
 111 {
 112         double lrx = this->x();
 113         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 114         return lrx;
 115 }
 116 double BicycleCar::raly()
 117 {
 118         double lry = this->y();
 119         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
 120         return lry;
 121 }
 122
 123 double BicycleCar::rarx()
 124 {
 125         double rrx = this->x();
 126         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 127         return rrx;
 128 }
 129
 130 double BicycleCar::rary()
 131 {
 132         double rry = this->y();
 133         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 134         return rry;
 135 }
 136
 137 BicycleCar *BicycleCar::ccl()
 138 {
 139         BicycleCar *bc = new BicycleCar();
 140         bc->x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
 141         bc->y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
 142         bc->h(this->h());
 143         return bc;
 144 }
 145
 146 BicycleCar *BicycleCar::ccr()
 147 {
 148         BicycleCar *bc = new BicycleCar();
 149         bc->x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
 150         bc->y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
 151         bc->h(this->h());
 152         return bc;
 153 }
 154
 155 // moving
 156 void BicycleCar::next()
 157 {
 158         if (this->st() > this->wb() / this->mtr())
 159                 this->st(this->wb() / this->mtr());
 160         if (this->st() < -this->wb() / this->mtr())
 161                 this->st(-this->wb() / this->mtr());
 162         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 163         this->x(this->x() + this->sp() * cos(this->h()));
 164         this->y(this->y() + this->sp() * sin(this->h()));
 165 }
 166
 167 BicycleCar::BicycleCar()
 168 {
 169 }