search for shortest path algorithm

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

@@ -276,7 +276,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
 public fun allFinished(paths: List<List<DubinsTangent>>,
                        finalObstacle: DubinsObstacle): Boolean {
     for (path in paths) {
-        if (path[-1].endObstacle != finalObstacle) {
+        if (path.last().endObstacle != finalObstacle) {
             return false
         }
     }
@@ -296,8 +296,8 @@ public fun pathLength(path: List<DubinsTangent>): Double {
     return tangentsLength + arcsLength
 }
 
-public fun shortestPath(path: List<List<DubinsTangent>>): List<List<DubinsTangent>> {
+public fun shortestPath(path: List<List<DubinsTangent>>): List<DubinsTangent> {
-    return path.sortedBy { pathLength(it) }
+    return path.sortedBy { pathLength(it) }[0]
 }
 
 public typealias Path = List<DubinsTangent>
@@ -309,7 +309,7 @@ public fun findAllPaths(
     finalDirection: DoubleVector2D,
     finalRadius: Double,
     obstacles: List<DubinsObstacle>
-) {
+): List<MutableList<DubinsTangent>> {
     val initialCircles = constructTangentCircles(
         startingPoint,
         startingDirection,
@@ -318,15 +318,15 @@ public fun findAllPaths(
         finalPoint,
         finalDirection,
         finalRadius)
-    var outputTangents = mutableMapOf<PathTypes, List<Path>>()
+    var outputTangents = mutableMapOf<PathTypes, MutableList<MutableList<DubinsTangent>>>()
     for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
         for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
             val finalCircle = finalCircles[j]!!
             val finalObstacle = DubinsObstacle(listOf(finalCircle))
             outputTangents[listOf(i,
                            DubinsPath.SimpleType.S,
-                           j)] = listOf(
+                           j)] = mutableListOf(
-                                    listOf(DubinsTangent(
+                                    mutableListOf(DubinsTangent(
                                         initialCircles[i]!!,
                                         initialCircles[i]!!,
                                         DubinsObstacle(listOf(initialCircles[i]!!)),
@@ -338,13 +338,13 @@ public fun findAllPaths(
             while (!allFinished(outputTangents[listOf(i,
                     DubinsPath.SimpleType.S,
                     j)]!!, finalObstacle)) {
-                var newOutputTangents = listOf<Path>()
+                var newOutputTangents = mutableListOf<MutableList<DubinsTangent>>()
                 for (line in outputTangents[listOf(i,
                     DubinsPath.SimpleType.S,
                     j)]!!) {
-                    var currentCircle = line[-1].endCircle
+                    var currentCircle = line.last().endCircle
-                    var currentDirection = line[-1].route[-1]
+                    var currentDirection = line.last().route.last()
-                    var currentObstacle = line[-1].endObstacle
+                    var currentObstacle = line.last().endObstacle
                     var nextObstacle = DubinsObstacle(listOf())
                     if (currentObstacle != finalObstacle) {
                         var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf(
@@ -362,20 +362,7 @@ public fun findAllPaths(
                             nextObstacle = finalObstacle
                         }
                         var nextTangents = outerTangents(currentObstacle, nextObstacle)
-//                        for (pathType in listOf(
+
-//                            listOf(DubinsPath.SimpleType.L,
-//                                DubinsPath.SimpleType.S,
-//                                DubinsPath.SimpleType.L),
-//                            listOf(DubinsPath.SimpleType.L,
-//                                DubinsPath.SimpleType.S,
-//                                DubinsPath.SimpleType.R),
-//                            listOf(DubinsPath.SimpleType.R,
-//                                DubinsPath.SimpleType.S,
-//                                DubinsPath.SimpleType.L),
-//                            listOf(DubinsPath.SimpleType.R,
-//                                DubinsPath.SimpleType.S,
-//                                DubinsPath.SimpleType.R)
-//                        )) {
                         for (pathType in nextTangents.keys) {
                             for (obstacle in obstacles) {
                                 // in Python code here try/except was used, but seems unneeded
@@ -396,12 +383,117 @@ public fun findAllPaths(
                                 nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
                                         as MutableMap<DubinsPath.Type, DubinsTangent>
                         }
-                        TODO("rewrite fragment from Python")
+                        val tangentsAlong = mutableListOf<DubinsTangent>()
+                        for (tangent in nextTangents.values) {
+                            if (tangent.startCircle == line.last().endCircle) {
+                                val lengthMaxPossible = arcLength(
+                                    tangent.startCircle,
+                                    line.last().lineSegment.end,
+                                    tangent.startObstacle.nextTangent(
+                                        tangent.startCircle,
+                                        DubinsPath.toType(listOf(currentDirection, DubinsPath.SimpleType.S, currentDirection)),
+                                    ).lineSegment.begin,
+                                    currentDirection
+                                    )
+                                val lengthCalculated = arcLength(
+                                    tangent.startCircle,
+                                    line.last().lineSegment.end,
+                                    tangent.lineSegment.begin,
+                                    currentDirection)
+                                if (lengthCalculated > lengthMaxPossible) {
+                                    val tangentsAlong = tangentsAlongTheObstacle(
+                                        currentCircle,
+                                        DubinsPath.toType(listOf(
+                                            currentDirection,
+                                            DubinsPath.SimpleType.S,
+                                            currentDirection)),
+                                        tangent.startCircle,
+                                        currentObstacle
+                                    )
+                                }
+                                else {
+                                    val tangentsAlong = mutableListOf<DubinsTangent>()
+                                }
+                            }
+                            else {
+                                val tangentsAlong = tangentsAlongTheObstacle(
+                                    currentCircle,
+                                    DubinsPath.toType(listOf(
+                                        currentDirection,
+                                        DubinsPath.SimpleType.S,
+                                        currentDirection)),
+                                    tangent.startCircle,
+                                    currentObstacle
+                                )
+                            }
+                            newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList())
+                        }
+                        outputTangents[listOf(
+                            i,
+                            DubinsPath.SimpleType.S,
+                            j
+                        )] = newOutputTangents
+                    }
+                    else {
+                        // minor changes from Python code
+                        newOutputTangents.add(line)
+                        outputTangents[listOf(
+                            i,
+                            DubinsPath.SimpleType.S,
+                            j
+                        )] = newOutputTangents
                     }
                 }
             }
+            for (lineId in outputTangents[listOf(
+                i,
+                DubinsPath.SimpleType.S,
+                j
+            )]!!.indices) {
+                val lastDirection = outputTangents[listOf(
+                    i,
+                    DubinsPath.SimpleType.S,
+                    j
+                )]!![lineId].last().route[2]
+                outputTangents[listOf(
+                    i,
+                    DubinsPath.SimpleType.S,
+                    j
+                )]!![lineId].add(DubinsTangent(
+                    finalCircles[j]!!,
+                    finalCircles[j]!!,
+                    DubinsObstacle(
+                        listOf(finalCircles[j]!!)
+                    ),
+                    DubinsObstacle(
+                        listOf(finalCircles[j]!!)
+                    ),
+                    LineSegment2D(finalPoint, finalPoint),
+                    listOf(
+                        lastDirection,
+                        DubinsPath.SimpleType.S,
+                        j
+                    )
+                ))
             }
         }
+    }
+    return outputTangents[listOf(
+        DubinsPath.SimpleType.L,
+        DubinsPath.SimpleType.S,
+        DubinsPath.SimpleType.L
+        )]!! + outputTangents[listOf(
+        DubinsPath.SimpleType.L,
+        DubinsPath.SimpleType.S,
+        DubinsPath.SimpleType.R
+        )]!! + outputTangents[listOf(
+        DubinsPath.SimpleType.R,
+        DubinsPath.SimpleType.S,
+        DubinsPath.SimpleType.L
+        )]!! + outputTangents[listOf(
+        DubinsPath.SimpleType.R,
+        DubinsPath.SimpleType.S,
+        DubinsPath.SimpleType.L)]!!
 }
 
 

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

@@ -39,14 +39,14 @@ public fun Circle2D.tangentsToCircle(
             } else {
                 r1.absoluteValue + r2.absoluteValue
             }
-            if (distance <= r) TODO("Intersecting circles are not supported yet")
+            if (distance * distance >= r * r) {
                 val l = sqrt(distance * distance - r * r)
                 angle2 = if (r1.absoluteValue > r2.absoluteValue) {
                     angle1 + r1.sign * atan2(r.absoluteValue, l)
                 } else {
                     angle1 - r2.sign * atan2(r.absoluteValue, l)
                 }
-            val w = vector(-cos(angle2), sin(angle2))
+                val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
                 put(
                     route,
                     LineSegment(
@@ -54,7 +54,61 @@ public fun Circle2D.tangentsToCircle(
                         other.center + w * r2
                     )
                 )
-
+            } else {
+                throw Exception("Circles should not intersect")
+            }
+        }
+    }
+}
+
+public fun dubinsTangentsToCircles(
+    firstCircle: Circle2D,
+    secondCircle: Circle2D,
+    firstObstacle: DubinsObstacle,
+    secondObstacle: DubinsObstacle
+): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
+    val line = LineSegment(firstCircle.center, secondCircle.center)
+    val distance = line.begin.distanceTo(line.end)
+    val angle1 = atan2(secondCircle.center.x - firstCircle.center.x,
+        secondCircle.center.y - firstCircle.center.y)
+    var r: Double
+    var angle2: Double
+    val routes = mapOf(
+        DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
+        DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
+        DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
+        DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
+    )
+    return buildMap {
+        for ((route, r1r2) in routes) {
+            val r1 = r1r2.first
+            val r2 = r1r2.second
+            r = if (r1.sign == r2.sign) {
+                r1.absoluteValue - r2.absoluteValue
+            } else {
+                r1.absoluteValue + r2.absoluteValue
+            }
+            if (distance * distance >= r * r) {
+                val l = sqrt(distance * distance - r * r)
+                angle2 = if (r1.absoluteValue > r2.absoluteValue) {
+                    angle1 + r1.sign * atan2(r.absoluteValue, l)
+                } else {
+                    angle1 - r2.sign * atan2(r.absoluteValue, l)
+                }
+                val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
+                put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
+                    secondCircle,
+                    firstObstacle,
+                    secondObstacle,
+                    LineSegment2D(
+                        firstCircle.center + w * r1,
+                        secondCircle.center + w * r2
+                    ),
+                    DubinsPath.toSimpleTypes(route))
+                )
+            } else {
+                throw Exception("Circles should not intersect")
+            }
         }
     }
 }

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt

@@ -0,0 +1,39 @@
+/*
+ * Copyright 2018-2023 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory
+
+import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.DoubleVector2D
+import space.kscience.kmath.geometry.Euclidean2DSpace.vector
+import kotlin.test.Test
+import kotlin.test.assertTrue
+
+class DubinsTest {
+    @Test
+    fun firstPath() {
+        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(7.0, 1.0), 5.0))))
+
+        val outputTangents = findAllPaths(
+            startPoint,
+            startDirection,
+            startRadius,
+            finalPoint,
+            finalDirection,
+            finalRadius,
+            obstacles)
+        val length = pathLength(shortestPath(outputTangents))
+        TODO("fix negative indices in boundaryTangents and accomplish test")
+        assertTrue(false)
+    }
+}