Add circle line segment intersection function

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index d9635df388eacfc13d49586500c8e2e892b21c18..dc81f8401a2ff3af3246bc1757dbec107511417d 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -55,6 +55,38 @@ void PSPlanner::gc_to_4()
         this->gc().h(angl_slot);
 }
 
+std::tuple<double, double, double, double> circle_line_intersection(
+        double cx, double cy, double r,
+        double x1, double y1,
+        double x2, double y2
+)
+{
+        double t = (y2 - y1) / (x2 - x1);
+        //double a = 1 + pow(t, 2);
+        //double b = - 2 * cx - 2 * pow(t, 2) * x1 + 2 * t * y1 - 2 * t * cy;
+        //double c = pow(cx, 2) + pow(t, 2) * pow(x1, 2) - 2 * t * y1 * x1
+        //        + pow(y1, 2) + 2 * t * cy * x1 - 2 * y1 * cy + pow(cy, 2)
+        //        - pow(r, 2);
+        double a = 1 + pow(t, 2);
+        double b = - 2 * cx + 2 * t * (-t * x1 + y1) - 2 * cy * t;
+        double c = pow(cx, 2) + pow(cy, 2) - pow(r, 2);
+        c += pow(-t * x1 + y1, 2);
+        c += 2 * cy * t * x1 - 2 * cy * y1;
+        double D = pow(b, 2) - 4 * a * c;
+        if (D < 0)
+                return std::make_tuple(cx, cy, cx, cy);
+        double res_x1 = (-b + sqrt(D)) / (2 * a);
+        double res_y1 = t * (res_x1 - x1) + y1;
+        double res_x2 = (-b - sqrt(D)) / (2 * a);
+        double res_y2 = t * (res_x2 - x1) + y1;
+        return std::make_tuple(res_x1, res_y1, res_x2, res_y2);
+}
+
+double edist(double x1, double y1, double x2, double y2)
+{
+        return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
+}
+
 void PSPlanner::guess_gc()
 {
         double x = this->ps().x1();
@@ -70,12 +102,14 @@ void PSPlanner::guess_gc()
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {
-                dts = atan2(
-                        this->ps().y2() - this->ps().y1(),
-                        this->ps().x2() - this->ps().x1()
-                );
-                dts *= 1.01; // precision workaround
-                if (std::abs(dts - this->ps().heading()) < M_PI / 2) {
+                if (std::abs(
+                        atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        )
+                        - this->ps().heading()
+                ) < M_PI / 2) {
+                        // forward parking
                         x = this->ps().x4();
                         y = this->ps().y4();
                         h = dts;
@@ -88,6 +122,12 @@ void PSPlanner::guess_gc()
                         x += (this->gc().w() / 2 + 0.01) * cos(dts);
                         y += (this->gc().w() / 2 + 0.01) * sin(dts);
                 } else {
+                        dts = atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        );
+                        dts *= 1.01; // precision workaround
+                        // backward parking
                         h = dts + M_PI;
                         x += -(this->gc().df() + 0.01) * cos(h);
                         y += -(this->gc().df() + 0.01) * sin(h);