Seems to be working... finally

demo/trajectory-playground/src/jvmMain/kotlin/Main.kt

@@ -57,6 +57,10 @@ fun FeatureGroup<XY>.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) ->
     polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray)
 }
 
+fun FeatureGroup<XY>.pose(pose2D: Pose2D) = with(Euclidean2DSpace){
+    line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY() )
+}
+
 @Composable
 @Preview
 fun closePoints() {
@@ -120,10 +124,14 @@ fun doubleObstacle() {
         )
 
         obstacles.forEach { obstacle(it)  }
+        val enter = Pose2D(-5, -1, Angle.pi / 4)
+        val exit = Pose2D(20, 4, Angle.pi * 3 / 4)
+        pose(enter)
+        pose(exit)
 
         Obstacles.avoidObstacles(
-            Pose2D(-5, -1, Angle.pi / 4),
+            enter,
-            Pose2D(20, 4, Angle.pi * 3 / 4),
+            exit,
             0.5,
             *obstacles
         ).forEach {
@@ -137,7 +145,9 @@ fun doubleObstacle() {
 @Preview
 fun playground() {
     val examples = listOf(
-        "Close starting points"
+        "Close starting points",
+        "Single obstacle",
+        "Two obstacles",
     )
 
     var currentExample by remember { mutableStateOf(examples.first()) }
@@ -153,6 +163,8 @@ fun playground() {
     }) {
         when (currentExample) {
             examples[0] -> closePoints()
+            examples[1] -> singleObstacle()
+            examples[2] -> doubleObstacle()
         }
     }
 }

trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt

@@ -2,6 +2,7 @@ package space.kscience.kmath.geometry
 
 import space.kscience.kmath.operations.DoubleField.pow
 import space.kscience.trajectory.*
+import kotlin.math.abs
 import kotlin.math.sign
 
 public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
@@ -71,3 +72,14 @@ public fun Circle2D.tangent(bearing: Angle, direction: Trajectory2D.Direction):
     Pose2D(coordinates, tangentAngle)
 }
 
+
+public fun CircleTrajectory2D.containsPoint(point: DoubleVector2D): Boolean = with(Euclidean2DSpace) {
+    val radiusVector = point - center
+    if (abs(norm(radiusVector) - circle.radius) > 1e-4 * circle.radius) error("Wrong radius")
+    val radiusVectorBearing = radiusVector.bearing
+    val offset = (radiusVectorBearing - arcStart).normalized()
+    when {
+        arcAngle >= Angle.zero -> offset < arcAngle
+        else -> arcAngle < offset - Angle.piTimes2
+    }
+}

trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt

@@ -101,7 +101,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         obstacleIndex: Int,
         obstacleDirection: Trajectory2D.Direction,
         arc: CircleTrajectory2D
-    ): ObstacleTangent? = with(Euclidean2DSpace) {
+    ): ObstacleTangent = with(Euclidean2DSpace) {
         val obstacle = obstacles[obstacleIndex]
         for (circleIndex in obstacle.arcs.indices) {
             val obstacleArc = obstacle.arcs[circleIndex]
@@ -118,7 +118,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                 )
             }
         }
-        return null
+        error("Tangent from obstacle $obstacleIndex to circle ${arc.circle} not found")
     }
 
 
@@ -206,7 +206,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                 connection.obstacleIndex,
                 connection.direction,
                 endArc
-            ) ?: return emptySet()
+            ) ?: error("No tangents between obstacle and endpoint")
 
             if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
                 TangentPath(tangents + tangentToEnd)

trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt

@@ -1,8 +1,6 @@
 package space.kscience.trajectory
 
-import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.*
-import space.kscience.kmath.geometry.DoubleVector2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.trajectory.DubinsPath.Type
 import kotlin.math.*
 
@@ -66,19 +64,8 @@ internal fun tangentsBetweenCircles(
 internal fun tangentsBetweenArcs(
     first: CircleTrajectory2D,
     second: CircleTrajectory2D,
-): Map<Type, StraightTrajectory2D> {
+): Map<Type, StraightTrajectory2D> = tangentsBetweenCircles(first.circle, second.circle).filterValues {
-
+    first.containsPoint(it.begin) && second.containsPoint(it.end)
-    fun CircleTrajectory2D.containsPoint(point: DoubleVector2D): Boolean = with(Euclidean2DSpace){
-        val radiusVectorBearing = (point - center).bearing
-        return when(direction){
-            Trajectory2D.L -> radiusVectorBearing in arcEnd..arcStart
-            Trajectory2D.R -> radiusVectorBearing in arcStart..arcEnd
-        }
-    }
-
-    return tangentsBetweenCircles(first.circle, second.circle).filterValues {
-        first.containsPoint(it.begin) && second.containsPoint(it.end)
-    }
 }
 
 /**
@@ -86,7 +73,7 @@ internal fun tangentsBetweenArcs(
  */
 public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: Int): CompositeTrajectory2D {
     require(fromIndex in arcs.indices) { "$fromIndex is not in ${arcs.indices}" }
-    val startCircle = arcs[fromIndex]
+    val startCircle = arcs[fromIndex].circle
     val segments = buildList {
         val reserve = mutableListOf<Trajectory2D>()
 
@@ -96,7 +83,7 @@ public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex:
         }
 
         var i = 0
-        while (sourceSegments[i] !== startCircle) {
+        while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) {
             //put all segments before target circle on the reserve
             reserve.add(sourceSegments[i])
             i++
@@ -124,7 +111,7 @@ public fun Obstacle.circumvention(
     toIndex: Int,
 ): CompositeTrajectory2D {
     require(toIndex in arcs.indices) { "$toIndex is not in ${arcs.indices}" }
-    val toCircle = arcs[toIndex]
+    val toCircle = arcs[toIndex].circle
     val fullCircumvention = circumvention(direction, fromIndex).segments
     return CompositeTrajectory2D(
         buildList {
@@ -133,7 +120,7 @@ public fun Obstacle.circumvention(
                 val segment = fullCircumvention[i]
                 add(segment)
                 i++
-            } while (segment !== toCircle)
+            } while ((segment as? CircleTrajectory2D)?.circle !== toCircle)
         }
     )
 }

trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt

@@ -11,6 +11,8 @@ import space.kscience.trajectory.Trajectory2D
 import kotlin.math.PI
 import kotlin.test.Test
 import kotlin.test.assertEquals
+import kotlin.test.assertFalse
+import kotlin.test.assertTrue
 
 class ArcTests {
 
@@ -34,9 +36,26 @@ class ArcTests {
         val arc = CircleTrajectory2D(
             circle,
             Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
-            Pose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
+            Pose2D(x = 1.0, y = -1.0, bearing = (PI * 3 / 2).radians)
         )
         assertEquals(Trajectory2D.R, arc.direction)
         assertEquals(PI / 2, arc.length, 1e-4)
     }
+
+    @Test
+    fun arcContains() = with(Euclidean2DSpace) {
+        val circle = circle(0, 0, 1.0)
+
+        val arc1 = CircleTrajectory2D(circle, Angle.pi / 4, Angle.piDiv2)
+        assertTrue { arc1.containsPoint(vector(1, 0)) }
+        assertFalse { arc1.containsPoint(vector(0, 1)) }
+        assertFalse { arc1.containsPoint(vector(-1, 0)) }
+
+        val arc2 = CircleTrajectory2D(circle, Angle.pi / 4, -Angle.piDiv2 * 3)
+        assertEquals(Trajectory2D.L, arc2.direction)
+        assertFalse { arc2.containsPoint(vector(1, 0)) }
+        assertTrue { arc2.containsPoint(vector(0, 1)) }
+        assertTrue { arc2.containsPoint(vector(-1, 0)) }
+
+    }
 }

trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt

@@ -36,7 +36,7 @@ class ObstacleTest {
         )
         assertTrue { outputTangents.isNotEmpty() }
         val length = outputTangents.minOf { it.length }
-        assertEquals(27.2113183, length, 1e-6)
+        assertEquals(25.0, length, 2.0)
     }
 
     @Test
@@ -59,7 +59,7 @@ class ObstacleTest {
         )
         assertTrue { paths.isNotEmpty() }
         val length = paths.minOf { it.length }
-        assertEquals(28.9678224, length, 1e-6)
+        assertEquals(28.0, length, 2.0)
     }
 
     @Test
@@ -94,7 +94,7 @@ class ObstacleTest {
             obstacle
         )
         assertTrue { paths.isNotEmpty() }
-        assertEquals(18.37, paths.minOf { it.length }, 1e-2)
+        assertEquals(12.0, paths.minOf { it.length }, 2.0)
     }
 
     @Test