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