From dde5b2c9f770a0a47c55a81d8d438f7d8360f13f Mon Sep 17 00:00:00 2001
From: Artyom Degtyarev <artdegt98@mail.ru>
Date: Fri, 28 Apr 2023 13:07:18 +0300
Subject: [PATCH] intersection of lineSegment and Circle fixed

---
 .../kscience/trajectory/obstacleInternal.kt     | 11 ++++++++---
 .../space/kscience/trajectory/ObstacleTest.kt   | 17 +++++++++++++++++
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt
index 7b16679..58002ec 100644
--- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt
+++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt
@@ -242,12 +242,17 @@ private fun LineSegment2D.intersectsCircle(circle: Circle2D): Boolean {
             (begin.y - end.y) * (end.y - circle.center.y))
     val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
             circle.radius.pow(2.0)
-    val d = b.pow(2.0) - 4 * a * c
+
+    val aNormalized = a / (a * a + b * b + c * c)
+    val bNormalized = b / (a * a + b * b + c * c)
+    val cNormalized = c / (a * a + b * b + c * c)
+
+    val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
     if (d < 1e-6) {
         return false
     } else {
-        val t1 = (-b - d.pow(0.5)) * 0.5 / a
-        val t2 = (-b + d.pow(0.5)) * 0.5 / a
+        val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
+        val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
         if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
             return true
         }
diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
index c53d821..37514ed 100644
--- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
+++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
@@ -99,6 +99,23 @@ class ObstacleTest {
         )
         val length = paths.minOf { it.length }
     }
+    @Test
+    fun fromMapLess() {
+
+        val paths = Obstacle.avoidObstacles(
+            DubinsPose2D(x = 48.4149535516561, y = 299.50862534208703, bearing = 3.401475378237137.degrees),
+            DubinsPose2D(x = 45.66638489126448, y = 283.00541087567504, bearing = 325.32183928982727.degrees),
+            0.5,
+            Obstacle(
+                Circle2D(vector(x=44.60882236175772, y=289.52640759535935), radius=0.5),
+                Circle2D(vector(x=45.558751549431164, y=289.71165594902174), radius=0.5),
+                Circle2D(vector(x=46.590308440141426, y=289.3897500160981), radius=0.5),
+                Circle2D(vector(x=46.242119397653354, y=287.94964842121634), radius=0.5),
+                Circle2D(vector(x=44.92318047505464, y=288.0132403305273), radius=0.5)
+            )
+        )
+        val length = paths.minOf { it.length }
+    }
 
     @Test
     fun equalObstacles() {