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 }
 162
 163 bool PSPlanner::forward()
 164 {
 165         double heading = this->ps().heading();
 166         while (heading < 0) heading += 2 * M_PI;
 167         if (!this->ps().parallel())
 168                 heading -= M_PI / 2;
 169         double h = this->gc().h();
 170         while (h < 0) h += 2 * M_PI;
 171         if (-0.00001 < heading - h && heading - h < 0.00001)
 172                 return true;
 173         else
 174                 return false;
 175 }
 176
 177 PSPlanner::PSPlanner()
 178 {
 179 }
 180
 181 std::tuple<bool, double, double> intersect(
 182         double x1, double y1,
 183         double x2, double y2,
 184         double x3, double y3,
 185         double x4, double y4
 186 )
 187 {
 188         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 189         if (deno == 0)
 190                 return std::make_tuple(false, 0, 0);
 191         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 192         t /= deno;
 193         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 194         u *= -1;
 195         u /= deno;
 196         if (t < 0 || t > 1 || u < 0 || u > 1)
 197                 return std::make_tuple(false, 0, 0);
 198         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 199 }