From 2bce369c5dcdae8b4ba16f498fd19d31807b29df Mon Sep 17 00:00:00 2001
From: Artyom Degtyarev <artdegt98@mail.ru>
Date: Thu, 9 Mar 2023 16:03:48 +0300
Subject: [PATCH] search for shortest path algorithm

---
 .../kmath/trajectory/DubinsObstacle.kt        | 38 ++++++++++++-------
 .../kscience/kmath/trajectory/DubinsTest.kt   | 37 +++++++++++++++++-
 2 files changed, 60 insertions(+), 15 deletions(-)

diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt
index d3ab13f26..6b247e55b 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt
@@ -111,13 +111,24 @@ public class DubinsObstacle(
         else {
             for (i in this.circles.indices) {
                 if (this.circles[i] == circle) {
-                    return DubinsTangent(this.circles[i],
-                                         this.circles[i-1],
-                                         this,
-                                         this,
-                                         LineSegment2D(this.tangents[i-1].lineSegment.end,
-                                                        this.tangents[i-1].lineSegment.begin),
-                                         DubinsPath.toSimpleTypes(route))
+                    if (i > 0) {
+                        return DubinsTangent(this.circles[i],
+                            this.circles[i-1],
+                            this,
+                            this,
+                            LineSegment2D(this.tangents[i-1].lineSegment.end,
+                                this.tangents[i-1].lineSegment.begin),
+                            DubinsPath.toSimpleTypes(route))
+                    }
+                    else {
+                        return DubinsTangent(this.circles[0],
+                            this.circles.last(),
+                            this,
+                            this,
+                            LineSegment2D(this.tangents.last().lineSegment.end,
+                                this.tangents.last().lineSegment.begin),
+                            DubinsPath.toSimpleTypes(route))
+                    }
                 }
             }
         }
@@ -156,7 +167,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
     val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0)
     val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) +
             (this.begin.y - this.end.y) * (this.end.y - circle.center.y))
-    val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) -
+    val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y).pow(2.0) -
             circle.radius.pow(2.0)
     val d = b.pow(2.0) - 4 * a * c
     if (d < 1e-6) {
@@ -289,7 +300,7 @@ public fun allFinished(paths: List<List<DubinsTangent>>,
 public fun pathLength(path: List<DubinsTangent>): Double {
     val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)}
     val arcsLength = buildList<Double>{
-        for (i in 1..path.size) {
+        for (i in 1..path.lastIndex) {
             add(arcLength(path[i].startCircle,
                 path[i-1].lineSegment.end,
                 path[i].lineSegment.begin,
@@ -366,13 +377,14 @@ public fun findAllPaths(
                         }
                         var nextTangents = outerTangents(currentObstacle, nextObstacle)
 
-                        for (pathType in nextTangents.keys) {
+                        for (pathType in DubinsPath.Type.values()) {
                             for (obstacle in obstacles) {
                                 // in Python code here try/except was used, but seems unneeded
-                                if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
-                                    nextTangents.remove(pathType)
+                                if (nextTangents.containsKey(pathType)) {
+                                    if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
+                                        nextTangents.remove(pathType)
+                                    }
                                 }
-
                             }
                         }
                         nextTangents = if (nextObstacle == finalObstacle) {
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt
index a0697c59c..16d77c84d 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt
@@ -33,8 +33,41 @@ class DubinsTest {
             finalRadius,
             obstacles)
         val length = pathLength(shortestPath(outputTangents))
-        TODO("fix negative indices in boundaryTangents and accomplish test")
-        assertTrue(false)
+        println(length)
+    }
+
+    @Test
+    fun secondPath() {
+        val startPoint = vector(-5.0, -1.0)
+        val startDirection = vector(1.0, 1.0)
+        val startRadius = 0.5
+        val finalPoint = vector(20.0, 4.0)
+        val finalDirection = vector(1.0, -1.0)
+        val finalRadius = 0.5
+
+        val obstacles = listOf(
+            DubinsObstacle(listOf(
+                Circle2D(vector(1.0, 6.5), 0.5),
+                Circle2D(vector(2.0, 1.0), 0.5),
+                Circle2D(vector(6.0, 0.0), 0.5),
+                Circle2D(vector(5.0, 5.0), 0.5)
+            )), DubinsObstacle(listOf(
+                Circle2D(vector(10.0, 1.0), 0.5),
+                Circle2D(vector(16.0, 0.0), 0.5),
+                Circle2D(vector(14.0, 6.0), 0.5),
+                Circle2D(vector(9.0, 4.0), 0.5)
+            ))
+        )
+        val outputTangents = findAllPaths(
+            startPoint,
+            startDirection,
+            startRadius,
+            finalPoint,
+            finalDirection,
+            finalRadius,
+            obstacles)
+        val length = pathLength(shortestPath(outputTangents))
+        println(length)
     }
     @Test
     fun outerTangentsTest1() {