src/psp.cc

   1 #include "psp.h"
   2
   3 bool PSPlanner::collide()
   4 {
   5         return false;
   6 }
   7
   8 // find entry
   9 void PSPlanner::fe()
  10 {
  11 }
  12
  13 void PSPlanner::fer()
  14 {
  15 }
  16
  17 PSPlanner::PSPlanner()
  18 {
  19 }
  20
  21 std::tuple<bool, double, double> intersect(
  22         double x1, double y1,
  23         double x2, double y2,
  24         double x3, double y3,
  25         double x4, double y4
  26 )
  27 {
  28         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
  29         if (deno == 0)
  30                 return std::make_tuple(false, 0, 0);
  31         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
  32         t /= deno;
  33         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
  34         u *= -1;
  35         u /= deno;
  36         if (t < 0 || t > 1 || u < 0 || u > 1)
  37                 return std::make_tuple(false, 0, 0);
  38         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
  39 }