search for shortest path algorithm

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

@@ -136,7 +136,8 @@ public class DubinsObstacle(
     error("next tangent not found")
     }
 
-    public fun equals(other: DubinsObstacle): Boolean {
+    override fun equals(other: Any?): Boolean {
+        if (other == null || other !is DubinsObstacle) return false
         return this.circles == other.circles
     }
 }
@@ -258,7 +259,7 @@ public fun constructTangentCircles(point: DoubleVector2D,
     val center2 = point + normalVectors(direction, r).second
     val p1 = center1 - point
     val p2 = center2 - point
-    return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) {
+    return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI/2, -3*PI/2)) {
         mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r),
             DubinsPath.SimpleType.R to Circle2D(center2, r))
     }
@@ -270,7 +271,7 @@ public fun constructTangentCircles(point: DoubleVector2D,
 
 public fun sortedObstacles(currentObstacle: DubinsObstacle,
                            obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
-    return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed()
+    return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed()
 }
 
 public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
@@ -389,13 +390,13 @@ public fun findAllPaths(
                         }
                         nextTangents = if (nextObstacle == finalObstacle) {
                             nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
-                                    (DubinsPath.toSimpleTypes(it.key)[0] == j)}
+                                    (DubinsPath.toSimpleTypes(it.key)[2] == j)}
                                     as MutableMap<DubinsPath.Type, DubinsTangent>
                         } else {
                             nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
                                     as MutableMap<DubinsPath.Type, DubinsTangent>
                         }
-                        val tangentsAlong = mutableListOf<DubinsTangent>()
+                        var tangentsAlong = mutableListOf<DubinsTangent>()
                         for (tangent in nextTangents.values) {
                             if (tangent.startCircle == line.last().endCircle) {
                                 val lengthMaxPossible = arcLength(
@@ -413,7 +414,7 @@ public fun findAllPaths(
                                     tangent.lineSegment.begin,
                                     currentDirection)
                                 if (lengthCalculated > lengthMaxPossible) {
-                                    val tangentsAlong = tangentsAlongTheObstacle(
+                                    tangentsAlong = tangentsAlongTheObstacle(
                                         currentCircle,
                                         DubinsPath.toType(listOf(
                                             currentDirection,
@@ -424,11 +425,11 @@ public fun findAllPaths(
                                     )
                                 }
                                 else {
-                                    val tangentsAlong = mutableListOf<DubinsTangent>()
+                                    tangentsAlong = mutableListOf<DubinsTangent>()
                                 }
                             }
                             else {
-                                val tangentsAlong = tangentsAlongTheObstacle(
+                                tangentsAlong = tangentsAlongTheObstacle(
                                     currentCircle,
                                     DubinsPath.toType(listOf(
                                         currentDirection,
@@ -456,6 +457,11 @@ public fun findAllPaths(
                         )] = newOutputTangents
                     }
                 }
+                outputTangents[listOf(
+                    i,
+                    DubinsPath.SimpleType.S,
+                    j
+                )] = newOutputTangents
             }
             for (lineId in outputTangents[listOf(
                 i,
@@ -505,7 +511,7 @@ public fun findAllPaths(
         )]!! + outputTangents[listOf(
         DubinsPath.SimpleType.R,
         DubinsPath.SimpleType.S,
-        DubinsPath.SimpleType.L)]!!
+        DubinsPath.SimpleType.R)]!!
 }
 
 

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

@@ -7,6 +7,8 @@ package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.DoubleVector2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.Euclidean2DSpace.minus
 import space.kscience.kmath.geometry.Euclidean2DSpace.vector
 import kotlin.test.Test
 import kotlin.test.assertTrue
@@ -68,6 +70,18 @@ class DubinsTest {
             obstacles)
         val length = pathLength(shortestPath(outputTangents))
         println(length)
+        for (path in outputTangents) {
+            println(pathLength(path))
+            println(path.size)
+            for (tangent in path) {
+//                println(tangent.route)
+//                println(tangent.startCircle)
+//                println(tangent.endCircle)
+//                println(Euclidean2DSpace.norm(tangent.lineSegment.end - tangent.lineSegment.begin))
+            }
+            println()
+            println()
+        }
     }
     @Test
     fun outerTangentsTest1() {
@@ -78,7 +92,6 @@ class DubinsTest {
             Circle2D(vector(5.0, 5.0), 1.0)
         )
         println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2)))
-        assertTrue(false)
     }
     @Test
     fun outerTangentsTest2() {
@@ -100,6 +113,28 @@ class DubinsTest {
                 }
             }
         }
-        assertTrue(false)
+    }
+    @Test
+    fun tangentsTest() {
+        val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
+        val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
+        val obstacle1 = DubinsObstacle(listOf(circle1))
+        val obstacle2 = DubinsObstacle(listOf(circle2))
+        val tangent = dubinsTangentsToCircles(circle1, circle2, obstacle1, obstacle2)
+        println(tangent)
+    }
+    @Test
+    fun equalCircles() {
+        val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
+        val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
+        println(circle1 == circle2)
+    }
+    @Test
+    fun equalObstacles() {
+        val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
+        val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
+        val obstacle1 = DubinsObstacle(listOf(circle1))
+        val obstacle2 = DubinsObstacle(listOf(circle2))
+        println(obstacle1 == obstacle2)
     }
 }