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 rfx = this->cc().rfx();
  97         double rfy = this->cc().rfy();
  98         double ccx = this->cc().x();
  99         double ccy = this->cc().y();
 100         double lfs = sgn(
 101                 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 102                 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 103         );
 104         double rfs = sgn(
 105                 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 106                 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 107         );
 108         double ccs = sgn(
 109                 (ccx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 110                 - (ccy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 111         );
 112         return lfs == rfs && lfs != ccs;
 113 }
 114
 115 // find entry
 116 void PSPlanner::fe()
 117 {
 118 }
 119
 120 void PSPlanner::fer()
 121 {
 122         if (this->ps().parallel())
 123                 return this->fer_parallel();
 124         else
 125                 return this->fer_perpendicular();
 126 }
 127
 128 void PSPlanner::fer_parallel()
 129 {
 130         this->cc().st(this->cc().wb() / this->cc().mtr());
 131         if (!this->ps().right())
 132                 this->cc().st(this->cc().st() * -1);
 133         this->cc().sp(0.1);
 134         while (!this->left()) {
 135                 while (!this->collide() && !this->left()) {
 136                         this->cc().next();
 137                 }
 138                 if (this->left() && !this->collide()) {
 139                         break;
 140                 } else {
 141                         this->cc().sp(this->cc().sp() * -1);
 142                         this->cc().next();
 143                         this->cc().st(this->cc().st() * -1);
 144                 }
 145         }
 146 }
 147
 148 void PSPlanner::fer_perpendicular()
 149 {
 150 }
 151
 152 PSPlanner::PSPlanner()
 153 {
 154 }
 155
 156 std::tuple<bool, double, double> intersect(
 157         double x1, double y1,
 158         double x2, double y2,
 159         double x3, double y3,
 160         double x4, double y4
 161 )
 162 {
 163         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 164         if (deno == 0)
 165                 return std::make_tuple(false, 0, 0);
 166         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 167         t /= deno;
 168         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 169         u *= -1;
 170         u /= deno;
 171         if (t < 0 || t > 1 || u < 0 || u > 1)
 172                 return std::make_tuple(false, 0, 0);
 173         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 174 }