src/bcar.cc

   1 #include <cmath>
   2 #include "bcar.h"
   3
   4 // kinematic constraints
   5 bool BicycleCar::drivable(BicycleCar *bc)
   6 {
   7         return true;
   8 }
   9
  10 // car frame
  11 double BicycleCar::lfx()
  12 {
  13         double lfx = this->x();
  14         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
  15         lfx += this->df() * cos(this->h());
  16         lfx += this->sd() * cos(this->h());
  17         return lfx;
  18 }
  19
  20 double BicycleCar::lfy()
  21 {
  22         double lfy = this->y();
  23         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
  24         lfy += this->df() * sin(this->h());
  25         lfy += this->sd() * sin(this->h());
  26         return lfy;
  27 }
  28
  29 double BicycleCar::lrx()
  30 {
  31         double lrx = this->x();
  32         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
  33         lrx += -this->dr() * cos(this->h());
  34         lrx += -this->sd() * cos(this->h());
  35         return lrx;
  36 }
  37
  38 double BicycleCar::lry()
  39 {
  40         double lry = this->y();
  41         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
  42         lry += -this->dr() * sin(this->h());
  43         lry += -this->sd() * sin(this->h());
  44         return lry;
  45 }
  46
  47 double BicycleCar::rrx()
  48 {
  49         double rrx = this->x();
  50         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
  51         rrx += -this->dr() * cos(this->h());
  52         rrx += -this->sd() * cos(this->h());
  53         return rrx;
  54 }
  55
  56 double BicycleCar::rry()
  57 {
  58         double rry = this->y();
  59         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
  60         rry += -this->dr() * sin(this->h());
  61         rry += -this->sd() * sin(this->h());
  62         return rry;
  63 }
  64
  65 double BicycleCar::rfx()
  66 {
  67         double rfx = this->x();
  68         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
  69         rfx += this->df() * cos(this->h());
  70         rfx += this->sd() * cos(this->h());
  71         return rfx;
  72 }
  73
  74 double BicycleCar::rfy()
  75 {
  76         double rfy = this->y();
  77         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
  78         rfy += this->df() * sin(this->h());
  79         rfy += this->sd() * sin(this->h());
  80         return rfy;
  81 }
  82
  83 double BicycleCar::ralx()
  84 {
  85         double lrx = this->x();
  86         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
  87         return lrx;
  88 }
  89 double BicycleCar::raly()
  90 {
  91         double lry = this->y();
  92         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
  93         return lry;
  94 }
  95
  96 double BicycleCar::rarx()
  97 {
  98         double rrx = this->x();
  99         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 100         return rrx;
 101 }
 102
 103 double BicycleCar::rary()
 104 {
 105         double rry = this->y();
 106         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 107         return rry;
 108 }
 109
 110 BicycleCar *BicycleCar::ccl()
 111 {
 112         BicycleCar *bc = new BicycleCar();
 113         bc->x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
 114         bc->y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
 115         bc->h(this->h());
 116         return bc;
 117 }
 118
 119 BicycleCar *BicycleCar::ccr()
 120 {
 121         BicycleCar *bc = new BicycleCar();
 122         bc->x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
 123         bc->y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
 124         bc->h(this->h());
 125         return bc;
 126 }
 127
 128 // moving
 129 void BicycleCar::next()
 130 {
 131         if (this->st() > this->wb() / this->mtr())
 132                 this->st(this->wb() / this->mtr());
 133         if (this->st() < -this->wb() / this->mtr())
 134                 this->st(-this->wb() / this->mtr());
 135         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 136         this->x(this->x() + this->sp() * cos(this->h()));
 137         this->y(this->y() + this->sp() * sin(this->h()));
 138 }
 139
 140 BicycleCar::BicycleCar()
 141 {
 142 }