From 0b278a5676a56da22fb2febb44ec541d3e210634 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 30 Jun 2020 13:29:42 +0200
Subject: [PATCH] Implement circle-line intersection function

---
 CHANGELOG.md |  1 +
 src/bcar.cc  | 17 ++++++++++++++++-
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e50ea82..667803b 100644
--- 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 39ec31d..68554e6 100644
--- 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);
 }
-- 
2.39.2