kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt

@@ -111,6 +111,7 @@ public class DubinsObstacle(
         else {
             for (i in this.circles.indices) {
                 if (this.circles[i] == circle) {
+                    if (i > 0) {
                         return DubinsTangent(this.circles[i],
                             this.circles[i-1],
                             this,
@@ -119,6 +120,16 @@ public class DubinsObstacle(
                                 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.containsKey(pathType)) {
                                     if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
                                         nextTangents.remove(pathType)
                                     }
-
+                                }
                             }
                         }
                         nextTangents = if (nextObstacle == finalObstacle) {

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")
+        println(length)
-        assertTrue(false)
+    }
+
+    @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() {