Add circle line segment intersection function

diff --git a/src/psp.cc b/src/psp.cc
index edec2aa16e4934bc9e7119b736e5bfbcdd74076b..dc81f8401a2ff3af3246bc1757dbec107511417d 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -55,6 +55,33 @@ 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));