Implement circle-line intersection function

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e50ea821eb29ba3484e063e3cbc1e3f632fb3435..667803b74923c401fe1e79ac164ef25f6b280c6b 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -10,6 +10,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ## Unreleased
 ### Added
 - Rotation of BicycleCar around the point.
+- Circle-line intersection.
 
 ### Changed
 - When set up BicycleCar heading, set the interval to `[-pi, +pi]`.

diff --git a/src/bcar.cc b/src/bcar.cc
index 39ec31dfdd34b66d3c294a108c6b758cfbfcd2c1..68554e660de7e5e4a5b4b17a5d1cb47ec958af5c 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -338,5 +338,20 @@ intersect(
     double x1, double y1,
     double x2, double y2
 ) {
-    return std::make_tuple(false, 0, 0, 0, 0);
+    x2 -= cx;
+    x1 -= cx;
+    y2 -= cy;
+    y1 -= cy;
+    double dx = x2 - x1;
+    double dy = y2 - y1;
+    double dr = sqrt(dx*dx + dy*dy);
+    double D = x1*y2 - x2*y1;
+    if (r*r * dr*dr - D*D < 0)
+        return std::make_tuple(false, 0, 0, 0, 0);
+    // intersection coordinates
+    double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+    double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+    double iy1 = (-D*dx + std::abs(dy) * sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+    double iy2 = (-D*dx - std::abs(dy) * sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+    return std::make_tuple(true, ix1, iy1, ix2, iy2);
 }