src/psp.cc

   1 #include "psp.h"
   2
   3 bool PSPlanner::collide()
   4 {
   5         if(std::get<0>(intersect(
   6                 this->cc().lfx(), this->cc().lfy(),
   7                 this->cc().lrx(), this->cc().lry(),
   8                 this->ps().x1(), this->ps().y1(),
   9                 this->ps().x2(), this->ps().y2()
  10         )))
  11                 return true;
  12         if(std::get<0>(intersect(
  13                 this->cc().rfx(), this->cc().rfy(),
  14                 this->cc().rrx(), this->cc().rry(),
  15                 this->ps().x1(), this->ps().y1(),
  16                 this->ps().x2(), this->ps().y2()
  17         )))
  18                 return true;
  19         if(std::get<0>(intersect(
  20                 this->cc().lfx(), this->cc().lfy(),
  21                 this->cc().rfx(), this->cc().rfy(),
  22                 this->ps().x1(), this->ps().y1(),
  23                 this->ps().x2(), this->ps().y2()
  24         )))
  25                 return true;
  26         if(std::get<0>(intersect(
  27                 this->cc().lrx(), this->cc().lry(),
  28                 this->cc().rrx(), this->cc().rry(),
  29                 this->ps().x1(), this->ps().y1(),
  30                 this->ps().x2(), this->ps().y2()
  31         )))
  32                 return true;
  33         if(std::get<0>(intersect(
  34                 this->cc().lfx(), this->cc().lfy(),
  35                 this->cc().lrx(), this->cc().lry(),
  36                 this->ps().x2(), this->ps().y2(),
  37                 this->ps().x3(), this->ps().y3()
  38         )))
  39                 return true;
  40         if(std::get<0>(intersect(
  41                 this->cc().rfx(), this->cc().rfy(),
  42                 this->cc().rrx(), this->cc().rry(),
  43                 this->ps().x2(), this->ps().y2(),
  44                 this->ps().x3(), this->ps().y3()
  45         )))
  46                 return true;
  47         if(std::get<0>(intersect(
  48                 this->cc().lfx(), this->cc().lfy(),
  49                 this->cc().rfx(), this->cc().rfy(),
  50                 this->ps().x2(), this->ps().y2(),
  51                 this->ps().x3(), this->ps().y3()
  52         )))
  53                 return true;
  54         if(std::get<0>(intersect(
  55                 this->cc().lrx(), this->cc().lry(),
  56                 this->cc().rrx(), this->cc().rry(),
  57                 this->ps().x2(), this->ps().y2(),
  58                 this->ps().x3(), this->ps().y3()
  59         )))
  60                 return true;
  61         if(std::get<0>(intersect(
  62                 this->cc().lfx(), this->cc().lfy(),
  63                 this->cc().lrx(), this->cc().lry(),
  64                 this->ps().x3(), this->ps().y3(),
  65                 this->ps().x4(), this->ps().y4()
  66         )))
  67                 return true;
  68         if(std::get<0>(intersect(
  69                 this->cc().rfx(), this->cc().rfy(),
  70                 this->cc().rrx(), this->cc().rry(),
  71                 this->ps().x3(), this->ps().y3(),
  72                 this->ps().x4(), this->ps().y4()
  73         )))
  74                 return true;
  75         if(std::get<0>(intersect(
  76                 this->cc().lfx(), this->cc().lfy(),
  77                 this->cc().rfx(), this->cc().rfy(),
  78                 this->ps().x3(), this->ps().y3(),
  79                 this->ps().x4(), this->ps().y4()
  80         )))
  81                 return true;
  82         if(std::get<0>(intersect(
  83                 this->cc().lrx(), this->cc().lry(),
  84                 this->cc().rrx(), this->cc().rry(),
  85                 this->ps().x3(), this->ps().y3(),
  86                 this->ps().x4(), this->ps().y4()
  87         )))
  88                 return true;
  89         return false;
  90 }
  91
  92 bool PSPlanner::left()
  93 {
  94         double lfx = this->cc().lfx();
  95         double lfy = this->cc().lfy();
  96         double lrx = this->cc().lrx();
  97         double lry = this->cc().lry();
  98         double rrx = this->cc().rrx();
  99         double rry = this->cc().rry();
 100         double rfx = this->cc().rfx();
 101         double rfy = this->cc().rfy();
 102         double lfs = sgn(
 103                 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 104                 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 105         );
 106         double lrs = sgn(
 107                 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 108                 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 109         );
 110         double rrs = sgn(
 111                 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 112                 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 113         );
 114         double rfs = sgn(
 115                 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 116                 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 117         );
 118         return lfs == rfs && (lfs != lrs || lfs != rrs);
 119 }
 120
 121 // find entry
 122 void PSPlanner::fe()
 123 {
 124 }
 125
 126 void PSPlanner::fer()
 127 {
 128         if (this->ps().parallel())
 129                 return this->fer_parallel();
 130         else
 131                 return this->fer_perpendicular();
 132 }
 133
 134 void PSPlanner::fer_parallel()
 135 {
 136         this->cc().st(this->cc().wb() / this->cc().mtr());
 137         if (!this->ps().right())
 138                 this->cc().st(this->cc().st() * -1);
 139         this->cc().sp(0.1);
 140         while (!this->left()) {
 141                 while (!this->collide() && !this->left()) {
 142                         this->cc().next();
 143                 }
 144                 if (this->left() && !this->collide()) {
 145                         break;
 146                 } else {
 147                         this->cc().sp(this->cc().sp() * -1);
 148                         this->cc().next();
 149                         this->cc().st(this->cc().st() * -1);
 150                 }
 151         }
 152 }
 153
 154 void PSPlanner::fer_perpendicular()
 155 {
 156 }
 157
 158 PSPlanner::PSPlanner()
 159 {
 160 }
 161
 162 std::tuple<bool, double, double> intersect(
 163         double x1, double y1,
 164         double x2, double y2,
 165         double x3, double y3,
 166         double x4, double y4
 167 )
 168 {
 169         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 170         if (deno == 0)
 171                 return std::make_tuple(false, 0, 0);
 172         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 173         t /= deno;
 174         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 175         u *= -1;
 176         u /= deno;
 177         if (t < 0 || t > 1 || u < 0 || u > 1)
 178                 return std::make_tuple(false, 0, 0);
 179         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 180 }