Fix segment-circle intersection

trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt

@@ -1,9 +1,10 @@
 package space.kscience.kmath.geometry
 
-import space.kscience.kmath.operations.DoubleField.pow
 import space.kscience.trajectory.*
 import kotlin.math.abs
+import kotlin.math.pow
 import kotlin.math.sign
+import kotlin.math.sqrt
 
 public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
     Circle2D(vector(x, y), radius = radius.toDouble())
@@ -23,17 +24,25 @@ public fun Euclidean2DSpace.intersectsOrInside(circle1: Circle2D, circle2: Circl
  * https://mathworld.wolfram.com/Circle-LineIntersection.html
  */
 public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D): Boolean {
-    val begin = segment.begin
+    val direction = segment.end - segment.begin
-    val end = segment.end
+    val radiusVector = segment.begin - circle.center
-    val d = begin.distanceTo(end)
-    val det = (begin.x - circle.center.x) * (end.y - circle.center.y) -
-            (end.x - circle.center.x) * (begin.y - circle.center.y)
 
-    val incidence = circle.radius.pow(2) * d.pow(2) - det.pow(2)
+    val a = direction dot direction
+    val b = 2 * (radiusVector dot direction)
+    val c = (radiusVector dot radiusVector) - circle.radius.pow(2)
 
-    return incidence >= 0
+    val discriminantSquared = b * b - 4 * a * c
+    if (discriminantSquared < 0) return false
+
+    val discriminant = sqrt(discriminantSquared)
+
+    val t1 = (-b - discriminant) / (2 * a) // first intersection point in relative coordinates
+    val t2 = (-b + discriminant) / (2 * a) //second intersection point in relative coordinates
+
+    return t1.sign != t2.sign || (t1-1.0).sign != (t2-1).sign
 }
 
+
 public fun Euclidean2DSpace.intersects(circle: Circle2D, segment: LineSegment2D): Boolean =
     intersects(segment, circle)
 

trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt

@@ -149,7 +149,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         //cutting first and last arcs to accommodate connection points
         val first = circumvention.first() as CircleTrajectory2D
         val last = circumvention.last() as CircleTrajectory2D
-        //arc between end of the nangent and end of previous arc (begin of the the next one)
+        //arc between end of the tangent and end of previous arc (begin of the next one)
         circumvention[0] = CircleTrajectory2D(
             first.circle,
             tangent1.tangentTrajectory.endPose,
@@ -182,7 +182,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
     private fun avoiding(
         dubinsPath: CompositeTrajectory2D,
     ): Collection<Trajectory2D> = with(Euclidean2DSpace) {
-        //fast return if no obstacles intersect direct path
+        //fast return if no obstacles intersect the direct path
         if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
 
         val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
@@ -208,7 +208,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                 endArc
             ) ?: return emptySet()
 
-            if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
+            // if no intersections, finish
+            if (obstacles.indices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
                 TangentPath(tangents + tangentToEnd)
             )
 

trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt

@@ -89,13 +89,14 @@ class ObstacleTest {
         )
 
         val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
-            Pose2D(-0.9, -0.9, Angle.pi),
+            Pose2D(-1, -1, Angle.pi),
-            Pose2D(-0.9, -0.9, Angle.piDiv2),
+            Pose2D(-1, -1, Angle.piDiv2),
             1.0,
             obstacle
         )
         assertTrue { paths.isNotEmpty() }
-        assertEquals(12.0, paths.minOf { it.length }, 2.0)
+        assertEquals(9.5, paths.minOf { it.length }, 1.0)
+        assertEquals(12.5, paths.maxOf { it.length }, 1.0)
     }
 
     @Test