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 (
 122                 this->ps().heading() + M_PI > this->gc().h()
 123                 && this->ps().heading() < this->gc().h()
 124         )
 125                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 126         else
 127                 return lrs == rrs && (lrs != lfs || lrs != rfs);
 128 }
 129
 130 // find entry
 131 void PSPlanner::fe()
 132 {
 133 }
 134
 135 void PSPlanner::fer()
 136 {
 137         if (this->ps().parallel())
 138                 return this->fer_parallel();
 139         else
 140                 return this->fer_perpendicular();
 141 }
 142
 143 void PSPlanner::fer_parallel()
 144 {
 145         this->cc().st(this->cc().wb() / this->cc().mtr());
 146         if (!this->ps().right())
 147                 this->cc().st(this->cc().st() * -1);
 148         this->cc().sp(0.1);
 149         while (!this->left()) {
 150                 while (!this->collide() && !this->left()) {
 151                         this->cc().next();
 152                 }
 153                 if (this->left() && !this->collide()) {
 154                         break;
 155                 } else {
 156                         this->cc().sp(this->cc().sp() * -1);
 157                         this->cc().next();
 158                         this->cc().st(this->cc().st() * -1);
 159                 }
 160         }
 161 }
 162
 163 void PSPlanner::fer_perpendicular()
 164 {
 165 }
 166
 167 PSPlanner::PSPlanner()
 168 {
 169 }
 170
 171 std::tuple<bool, double, double> intersect(
 172         double x1, double y1,
 173         double x2, double y2,
 174         double x3, double y3,
 175         double x4, double y4
 176 )
 177 {
 178         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 179         if (deno == 0)
 180                 return std::make_tuple(false, 0, 0);
 181         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 182         t /= deno;
 183         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 184         u *= -1;
 185         u /= deno;
 186         if (t < 0 || t > 1 || u < 0 || u > 1)
 187                 return std::make_tuple(false, 0, 0);
 188         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 189 }