src/psp.cc

   1 #include <cmath>
   2 #include "psp.h"
   3
   4 bool PSPlanner::collide()
   5 {
   6         if(std::get<0>(intersect(
   7                 this->cc().lfx(), this->cc().lfy(),
   8                 this->cc().lrx(), this->cc().lry(),
   9                 this->ps().x1(), this->ps().y1(),
  10                 this->ps().x2(), this->ps().y2()
  11         )))
  12                 return true;
  13         if(std::get<0>(intersect(
  14                 this->cc().rfx(), this->cc().rfy(),
  15                 this->cc().rrx(), this->cc().rry(),
  16                 this->ps().x1(), this->ps().y1(),
  17                 this->ps().x2(), this->ps().y2()
  18         )))
  19                 return true;
  20         if(std::get<0>(intersect(
  21                 this->cc().lfx(), this->cc().lfy(),
  22                 this->cc().rfx(), this->cc().rfy(),
  23                 this->ps().x1(), this->ps().y1(),
  24                 this->ps().x2(), this->ps().y2()
  25         )))
  26                 return true;
  27         if(std::get<0>(intersect(
  28                 this->cc().lrx(), this->cc().lry(),
  29                 this->cc().rrx(), this->cc().rry(),
  30                 this->ps().x1(), this->ps().y1(),
  31                 this->ps().x2(), this->ps().y2()
  32         )))
  33                 return true;
  34         if(std::get<0>(intersect(
  35                 this->cc().lfx(), this->cc().lfy(),
  36                 this->cc().lrx(), this->cc().lry(),
  37                 this->ps().x2(), this->ps().y2(),
  38                 this->ps().x3(), this->ps().y3()
  39         )))
  40                 return true;
  41         if(std::get<0>(intersect(
  42                 this->cc().rfx(), this->cc().rfy(),
  43                 this->cc().rrx(), this->cc().rry(),
  44                 this->ps().x2(), this->ps().y2(),
  45                 this->ps().x3(), this->ps().y3()
  46         )))
  47                 return true;
  48         if(std::get<0>(intersect(
  49                 this->cc().lfx(), this->cc().lfy(),
  50                 this->cc().rfx(), this->cc().rfy(),
  51                 this->ps().x2(), this->ps().y2(),
  52                 this->ps().x3(), this->ps().y3()
  53         )))
  54                 return true;
  55         if(std::get<0>(intersect(
  56                 this->cc().lrx(), this->cc().lry(),
  57                 this->cc().rrx(), this->cc().rry(),
  58                 this->ps().x2(), this->ps().y2(),
  59                 this->ps().x3(), this->ps().y3()
  60         )))
  61                 return true;
  62         if(std::get<0>(intersect(
  63                 this->cc().lfx(), this->cc().lfy(),
  64                 this->cc().lrx(), this->cc().lry(),
  65                 this->ps().x3(), this->ps().y3(),
  66                 this->ps().x4(), this->ps().y4()
  67         )))
  68                 return true;
  69         if(std::get<0>(intersect(
  70                 this->cc().rfx(), this->cc().rfy(),
  71                 this->cc().rrx(), this->cc().rry(),
  72                 this->ps().x3(), this->ps().y3(),
  73                 this->ps().x4(), this->ps().y4()
  74         )))
  75                 return true;
  76         if(std::get<0>(intersect(
  77                 this->cc().lfx(), this->cc().lfy(),
  78                 this->cc().rfx(), this->cc().rfy(),
  79                 this->ps().x3(), this->ps().y3(),
  80                 this->ps().x4(), this->ps().y4()
  81         )))
  82                 return true;
  83         if(std::get<0>(intersect(
  84                 this->cc().lrx(), this->cc().lry(),
  85                 this->cc().rrx(), this->cc().rry(),
  86                 this->ps().x3(), this->ps().y3(),
  87                 this->ps().x4(), this->ps().y4()
  88         )))
  89                 return true;
  90         return false;
  91 }
  92
  93 bool PSPlanner::left()
  94 {
  95         double lfx = this->cc().lfx();
  96         double lfy = this->cc().lfy();
  97         double lrx = this->cc().lrx();
  98         double lry = this->cc().lry();
  99         double rrx = this->cc().rrx();
 100         double rry = this->cc().rry();
 101         double rfx = this->cc().rfx();
 102         double rfy = this->cc().rfy();
 103         double lfs = sgn(
 104                 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 105                 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 106         );
 107         double lrs = sgn(
 108                 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 109                 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 110         );
 111         double rrs = sgn(
 112                 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 113                 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 114         );
 115         double rfs = sgn(
 116                 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 117                 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 118         );
 119         if (this->ps().parallel())
 120                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 121         else if (!this->forward())
 122                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 123         else
 124                 return lrs == rrs && (lrs != lfs || lrs != rfs);
 125 }
 126
 127 // find entry
 128 void PSPlanner::fe()
 129 {
 130 }
 131
 132 void PSPlanner::fer()
 133 {
 134         if (this->ps().parallel())
 135                 return this->fer_parallel();
 136         else
 137                 return this->fer_perpendicular();
 138 }
 139
 140 void PSPlanner::fer_parallel()
 141 {
 142         this->cc().st(this->cc().wb() / this->cc().mtr());
 143         if (!this->ps().right())
 144                 this->cc().st(this->cc().st() * -1);
 145         this->cc().sp(0.1);
 146         while (!this->left()) {
 147                 while (!this->collide() && !this->left())
 148                         this->cc().next();
 149                 if (this->left() && !this->collide()) {
 150                         break;
 151                 } else {
 152                         this->cc().sp(this->cc().sp() * -1);
 153                         this->cc().next();
 154                         this->cc().st(this->cc().st() * -1);
 155                 }
 156         }
 157 }
 158
 159 void PSPlanner::fer_perpendicular()
 160 {
 161         double cc_h = this->cc().h();
 162         double x;
 163         double y;
 164         // check inner radius
 165         if (this->forward()) {
 166                 x = this->ps().x1();
 167                 y = this->ps().y1();
 168         } else {
 169                 x = this->ps().x4();
 170                 y = this->ps().y4();
 171         }
 172         double x1;
 173         double y1;
 174         if (this->ps().right()) {
 175                 x1 = this->cc().ccr().x();
 176                 y1 = this->cc().ccr().y();
 177         } else {
 178                 x1 = this->cc().ccl().x();
 179                 y1 = this->cc().ccl().y();
 180         }
 181         double IR = this->cc().iradi();
 182         double a = 1;
 183         double b;
 184         if (this->forward())
 185                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 186         else
 187                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 188         double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 189         double D = D = pow(b, 2) - 4 * a * c;
 190         double delta;
 191         delta = -b - sqrt(D);
 192         delta /= 2 * a;
 193         double delta_1 = delta;
 194         // check outer radius
 195         if (this->forward()) {
 196                 x = this->ps().x4();
 197                 y = this->ps().y4();
 198         } else {
 199                 x = this->ps().x1();
 200                 y = this->ps().y1();
 201         }
 202         IR = this->cc().ofradi();
 203         a = 1;
 204         if (this->forward())
 205                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 206         else
 207                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 208         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 209         D = pow(b, 2) - 4 * a * c;
 210         if (this->forward()) {
 211                 delta = -b + sqrt(D);
 212                 delta /= 2 * a;
 213         }
 214         double delta_2 = delta;
 215         delta = -b - sqrt(D);
 216         delta /= 2 * a;
 217         double delta_3 = delta;
 218         delta = std::max(delta_1, std::max(delta_2, delta_3));
 219         // current car `cc` can get out of slot with max steer
 220         this->cc().x(this->cc().x() + delta * cos(cc_h));
 221         this->cc().y(this->cc().y() + delta * sin(cc_h));
 222         this->cc().h(cc_h);
 223         // get current car `cc` out of slot
 224         if (this->forward())
 225                 this->cc().sp(-0.1);
 226         else
 227                 this->cc().sp(0.1);
 228         this->cc().st(this->cc().wb() / this->cc().mtr());
 229         if (this->ps().right())
 230                 this->cc().st(this->cc().st() * -1);
 231         while (!this->left()) {
 232                 while (!this->collide() && !this->left())
 233                         this->cc().next();
 234                 if (this->left() && !this->collide()) {
 235                         break;
 236                 } else {
 237                         this->cc().sp(this->cc().sp() * -1);
 238                         this->cc().next();
 239                         this->cc().st(this->cc().st() * -1);
 240                 }
 241         }
 242 }
 243
 244 bool PSPlanner::forward()
 245 {
 246         double heading = this->ps().heading();
 247         while (heading < 0) heading += 2 * M_PI;
 248         if (!this->ps().parallel())
 249                 heading -= M_PI / 2;
 250         double h = this->gc().h();
 251         while (h < 0) h += 2 * M_PI;
 252         if (-0.00001 < heading - h && heading - h < 0.00001)
 253                 return true;
 254         else
 255                 return false;
 256 }
 257
 258 PSPlanner::PSPlanner()
 259 {
 260 }
 261
 262 std::tuple<bool, double, double> intersect(
 263         double x1, double y1,
 264         double x2, double y2,
 265         double x3, double y3,
 266         double x4, double y4
 267 )
 268 {
 269         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 270         if (deno == 0)
 271                 return std::make_tuple(false, 0, 0);
 272         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 273         t /= deno;
 274         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 275         u *= -1;
 276         u /= deno;
 277         if (t < 0 || t > 1 || u < 0 || u > 1)
 278                 return std::make_tuple(false, 0, 0);
 279         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 280 }