[WIP] A lot of bugfixes

demo/trajectory-playground/build.gradle.kts

@@ -0,0 +1,30 @@
+plugins {
+    kotlin("multiplatform")
+    id("org.jetbrains.compose")
+}
+
+val ktorVersion: String by rootProject.extra
+
+kotlin {
+    jvm()
+    jvmToolchain(11)
+    sourceSets {
+        val jvmMain by getting {
+            dependencies {
+                implementation(projects.mapsKtScheme)
+                implementation(projects.trajectoryKt)
+                implementation(compose.desktop.currentOs)
+                implementation(spclibs.logback.classic)
+            }
+        }
+        val jvmTest by getting
+    }
+}
+
+compose {
+    desktop {
+        application {
+            mainClass = "MainKt"
+        }
+    }
+}

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

@@ -0,0 +1,166 @@
+import androidx.compose.desktop.ui.tooling.preview.Preview
+import androidx.compose.foundation.layout.Column
+import androidx.compose.material.Button
+import androidx.compose.material.MaterialTheme
+import androidx.compose.material.Scaffold
+import androidx.compose.material.Text
+import androidx.compose.runtime.*
+import androidx.compose.ui.graphics.Color
+import androidx.compose.ui.window.Window
+import androidx.compose.ui.window.application
+import center.sciprog.maps.features.*
+import center.sciprog.maps.scheme.SchemeView
+import center.sciprog.maps.scheme.XY
+import space.kscience.kmath.geometry.Angle
+import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.DoubleVector2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.trajectory.*
+import kotlin.random.Random
+
+private fun DoubleVector2D.toXY() = XY(x.toFloat(), y.toFloat())
+
+fun FeatureGroup<XY>.trajectory(
+    trajectory: Trajectory2D,
+    colorPicker: (Trajectory2D) -> Color = { Color.Blue },
+): FeatureRef<XY, FeatureGroup<XY>> = group {
+    when (trajectory) {
+        is StraightTrajectory2D -> line(
+            aCoordinates = trajectory.begin.toXY(),
+            bCoordinates = trajectory.end.toXY(),
+        ).color(colorPicker(trajectory))
+
+        is CircleTrajectory2D -> with(Euclidean2DSpace) {
+            val topLeft = trajectory.circle.center + vector(-trajectory.circle.radius, trajectory.circle.radius)
+            val bottomRight = trajectory.circle.center + vector(trajectory.circle.radius, -trajectory.circle.radius)
+
+            val rectangle = Rectangle(
+                topLeft.toXY(),
+                bottomRight.toXY()
+            )
+
+            arc(
+                oval = rectangle,
+                startAngle = trajectory.arcStart - Angle.piDiv2,
+                arcLength = trajectory.arcAngle,
+            ).color(colorPicker(trajectory))
+        }
+
+        is CompositeTrajectory2D -> trajectory.segments.forEach {
+            trajectory(it, colorPicker)
+        }
+    }
+}
+
+fun FeatureGroup<XY>.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) -> Color = { Color.Red }) {
+    trajectory(obstacle.circumvention, colorPicker)
+    polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray)
+}
+
+@Composable
+@Preview
+fun closePoints() {
+    SchemeView {
+
+        val obstacle = Obstacle(
+            Circle2D(Euclidean2DSpace.vector(0.0, 0.0), 1.0),
+            Circle2D(Euclidean2DSpace.vector(0.0, 1.0), 1.0),
+            Circle2D(Euclidean2DSpace.vector(1.0, 1.0), 1.0),
+            Circle2D(Euclidean2DSpace.vector(1.0, 0.0), 1.0)
+        )
+
+        val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
+            Pose2D(-1, -1, Angle.pi),
+            Pose2D(-1, -1, Angle.piDiv2),
+            1.0,
+            obstacle
+        )
+
+        obstacle(obstacle)
+
+        trajectory(paths.first()) { Color.Green }
+        trajectory(paths.last()) { Color.Magenta }
+
+    }
+}
+
+@Composable
+@Preview
+fun singleObstacle() {
+    SchemeView {
+        val obstacle = Obstacle(Circle2D(Euclidean2DSpace.vector(7.0, 1.0), 5.0))
+        obstacle(obstacle)
+        Obstacles.avoidObstacles(
+            Pose2D(-5, -1, Angle.pi / 4),
+            Pose2D(20, 4, Angle.pi * 3 / 4),
+            0.5,
+            obstacle
+        ).forEach {
+            trajectory(it).color(Color(Random.nextInt()))
+        }
+    }
+}
+
+@Composable
+@Preview
+fun doubleObstacle() {
+    SchemeView {
+        val obstacles = arrayOf(
+            Obstacle(
+                Circle2D(Euclidean2DSpace.vector(1.0, 6.5), 0.5),
+                Circle2D(Euclidean2DSpace.vector(2.0, 1.0), 0.5),
+                Circle2D(Euclidean2DSpace.vector(6.0, 0.0), 0.5),
+                Circle2D(Euclidean2DSpace.vector(5.0, 5.0), 0.5)
+            ), Obstacle(
+                Circle2D(Euclidean2DSpace.vector(10.0, 1.0), 0.5),
+                Circle2D(Euclidean2DSpace.vector(16.0, 0.0), 0.5),
+                Circle2D(Euclidean2DSpace.vector(14.0, 6.0), 0.5),
+                Circle2D(Euclidean2DSpace.vector(9.0, 4.0), 0.5)
+            )
+        )
+
+        obstacles.forEach { obstacle(it)  }
+
+        Obstacles.avoidObstacles(
+            Pose2D(-5, -1, Angle.pi / 4),
+            Pose2D(20, 4, Angle.pi * 3 / 4),
+            0.5,
+            *obstacles
+        ).forEach {
+            trajectory(it).color(Color(Random.nextInt()))
+        }
+    }
+}
+
+
+@Composable
+@Preview
+fun playground() {
+    val examples = listOf(
+        "Close starting points"
+    )
+
+    var currentExample by remember { mutableStateOf(examples.first()) }
+
+    Scaffold(floatingActionButton = {
+        Column {
+            examples.forEach {
+                Button(onClick = { currentExample = it }) {
+                    Text(it)
+                }
+            }
+        }
+    }) {
+        when (currentExample) {
+            examples[0] -> closePoints()
+        }
+    }
+}
+
+fun main() = application {
+    Window(title = "Trajectory-playground", onCloseRequest = ::exitApplication) {
+        MaterialTheme {
+            playground()
+        }
+    }
+}

maps-kt-scheme/src/commonMain/kotlin/center/sciprog/maps/scheme/schemeFeatures.kt

@@ -63,7 +63,7 @@ public fun FeatureGroup<XY>.arc(
     arcLength: Angle,
     id: String? = null,
 ): FeatureRef<XY, ArcFeature<XY>> = arc(
-    oval = XYCoordinateSpace.Rectangle(center.toCoordinates(), radius, radius),
+    oval = XYCoordinateSpace.Rectangle(center.toCoordinates(), 2*radius, 2*radius),
     startAngle = startAngle,
     arcLength = arcLength,
     id = id

settings.gradle.kts

@@ -54,6 +54,7 @@ include(
     ":maps-kt-scheme",
     ":demo:maps",
     ":demo:scheme",
-    ":demo:polygon-editor"
+    ":demo:polygon-editor",
+    ":demo:trajectory-playground"
 )
 

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

@@ -1,8 +1,7 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.kmath.operations.DoubleField.pow
-import space.kscience.trajectory.Pose2D
+import space.kscience.trajectory.*
-import space.kscience.trajectory.Trajectory2D
 import kotlin.math.sign
 
 public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
@@ -49,6 +48,15 @@ public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSe
     }
 }
 
+public fun Euclidean2DSpace.intersectsTrajectory(segment: LineSegment2D, trajectory: Trajectory2D): Boolean =
+    when (trajectory) {
+        is CircleTrajectory2D -> intersects(segment, trajectory.circle)
+        is StraightTrajectory2D -> intersects(segment, trajectory)
+        is CompositeTrajectory2D -> trajectory.segments.any { trajectorySegment ->
+            intersectsTrajectory(segment, trajectorySegment)
+        }
+    }
+
 /**
  * Compute tangent pose to a circle
  *

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

@@ -1,6 +1,7 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.kmath.misc.zipWithNextCircular
+import space.kscience.trajectory.Trajectory2D
 
 public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Double> = object : Polygon<Double> {
     override val points: List<Vector2D<Double>> get() = points
@@ -8,3 +9,9 @@ public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Doubl
 
 public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, segment: LineSegment2D): Boolean =
     polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, segment) }
+
+public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, circle: Circle2D): Boolean =
+    polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, circle) }
+
+public fun Euclidean2DSpace.intersectsTrajectory(polygon: Polygon<Double>, trajectory: Trajectory2D): Boolean =
+    polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { edge -> intersectsTrajectory(edge, trajectory) }

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

@@ -5,24 +5,27 @@
 
 package space.kscience.trajectory
 
-import space.kscience.kmath.geometry.Angle
+import space.kscience.kmath.geometry.*
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.Vector2D
 import space.kscience.kmath.misc.zipWithNextCircular
 
 
 public interface Obstacle {
-    public val circles: List<Circle2D>
+    public val arcs: List<CircleTrajectory2D>
     public val center: Vector2D<Double>
 
+    /**
+     * A closed right-handed circuit minimal path circumvention of an obstacle.
+     */
     public val circumvention: CompositeTrajectory2D
 
+    public val polygon: Polygon<Double>
+
+
     /**
      * Check if obstacle has intersection with given [Trajectory2D]
      */
     public fun intersects(trajectory: Trajectory2D): Boolean =
-        Euclidean2DSpace.trajectoryIntersects(circumvention, trajectory)
+        Euclidean2DSpace.intersectsTrajectory(polygon, trajectory)
 
 
     public companion object {
@@ -30,58 +33,20 @@ public interface Obstacle {
     }
 }
 
-private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
+private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) : Obstacle {
+    override val arcs: List<CircleTrajectory2D> by lazy {
+        circumvention.segments.filterIsInstance<CircleTrajectory2D>()
+    }
+
     override val center: Vector2D<Double> by lazy {
         Euclidean2DSpace.vector(
-            circles.sumOf { it.center.x } / circles.size,
+            arcs.sumOf { it.center.x } / arcs.size,
-            circles.sumOf { it.center.y } / circles.size
+            arcs.sumOf { it.center.y } / arcs.size
         )
     }
 
-    override val circumvention: CompositeTrajectory2D by lazy {
+    override val polygon: Polygon<Double> by lazy {
-        with(Euclidean2DSpace) {
+        Euclidean2DSpace.polygon(arcs.map { it.circle.center })
-            /**
-             * A closed right-handed circuit minimal path circumvention of an obstacle.
-             * @return null if number of distinct circles in the obstacle is less than
-             */
-            require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
-
-            if (circles.size == 1) {
-                // a circumvention consisting of a single circle, starting on top
-//                val circle = circles.first()
-//                val top = vector(circle.center.x + circle.radius, circle.center.y)
-//                val start = DubinsPose2D(
-//                    top,
-//                    Angle.piDiv2
-//                )
-                return@lazy CompositeTrajectory2D(
-                    CircleTrajectory2D(circles.first(), Angle.zero, Angle.zero)
-                )
-            }
-
-            //TODO use convex hull
-            //distinct and sorted in right-handed direction
-            val circles = circles.distinct().sortedBy {
-                (it.center - center).bearing
-            }
-
-            val tangents = circles.zipWithNextCircular { a: Circle2D, b: Circle2D ->
-                tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR]
-                    ?: error("Can't find right handed circumvention")
-            }
-
-            val trajectory: List<Trajectory2D> = buildList {
-                for (i in 0 until tangents.lastIndex) {
-                    add(tangents[i])
-                    add(CircleTrajectory2D(circles[i + 1], tangents[i].endPose, tangents[i + 1].beginPose))
-                }
-                add(tangents.last())
-                add(CircleTrajectory2D(circles[0], tangents.last().endPose, tangents.first().beginPose))
-            }
-
-            return@lazy CompositeTrajectory2D(trajectory)
-
-        }
     }
 
     override fun equals(other: Any?): Boolean {
@@ -90,24 +55,66 @@ private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
 
         other as ObstacleImpl
 
-        return circles == other.circles
+        return arcs == other.arcs
     }
 
     override fun hashCode(): Int {
-        return circles.hashCode()
+        return arcs.hashCode()
     }
 
     override fun toString(): String {
-        return "Obstacle(circles=$circles)"
+        return "Obstacle(circles=$arcs)"
     }
-
-
 }
 
-public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles))
+public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace) {
+    val center = vector(
+        circles.sumOf { it.center.x },
+        circles.sumOf { it.center.y }
+    )/ circles.size
+
+
+    require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
+
+    if (circles.size == 1) {
+        return ObstacleImpl(
+            CompositeTrajectory2D(
+                CircleTrajectory2D(circles.first(), Angle.zero, Angle.piTimes2)
+            )
+        )
+    }
+
+    //TODO use convex hull
+    //distinct and sorted in right-handed direction
+    val convex = circles.distinct().sortedBy {
+        (it.center - center).bearing
+    }
+
+    val tangents = convex.zipWithNextCircular { a: Circle2D, b: Circle2D ->
+        tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR]
+            ?: error("Can't find right handed circumvention")
+    }
+
+    val trajectory: List<Trajectory2D> = buildList {
+        for (i in 0 until tangents.lastIndex) {
+            add(tangents[i])
+            add(CircleTrajectory2D(convex[i + 1], tangents[i].endPose, tangents[i + 1].beginPose, Trajectory2D.R))
+        }
+        add(tangents.last())
+        add(CircleTrajectory2D(convex[0], tangents.last().endPose, tangents.first().beginPose, Trajectory2D.R))
+    }
+
+    val circumvention = CompositeTrajectory2D(trajectory)
+
+
+    return ObstacleImpl(circumvention)
+}
+
+
+public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
 
 public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
-    ObstacleImpl(points.map { Circle2D(it, radius) })
+    Obstacle(points.map { Circle2D(it, radius) })
 
 
 

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

@@ -1,389 +0,0 @@
-package space.kscience.trajectory
-
-
-//
-//
-//private class LR<T>(val l: T, val r: T) {
-//    operator fun get(direction: Trajectory2D.Direction) = when (direction) {
-//        Trajectory2D.L -> l
-//        Trajectory2D.R -> r
-//    }
-//}
-//
-//private class TangentPath(val tangents: List<ObstacleTangent>) {
-//    fun last() = tangents.last()
-//}
-//
-//private fun TangentPath(vararg tangents: ObstacleTangent) = TangentPath(listOf(*tangents))
-//
-//private fun Circle2D.isInside(other: Circle2D): Boolean {
-//    return center.distanceTo(other.center) + radius <= other.radius
-//}
-//
-//
-//internal class ObstacleShell(
-//    nodes: List<Circle2D>,
-//) : Obstacle {
-//    override val circles: List<Circle2D>
-//    override val center: Vector2D<Double>
-//    private val shell: List<LineSegment2D>
-//    private val shellDirection: Trajectory2D.Direction
-//
-//    init {
-//        this.center = Euclidean2DSpace.vector(
-//            nodes.sumOf { it.center.x } / nodes.size,
-//            nodes.sumOf { it.center.y } / nodes.size
-//        )
-//
-////        this.circles = nodes.filter { node ->
-////            //filter nodes inside other nodes
-////            nodes.none{ node !== it  && node.isInside(it) }
-////        }
-//
-//        this.circles = nodes.distinct()
-//
-//        if (nodes.size < 2) {
-//            shell = emptyList()
-//            shellDirection = Trajectory2D.R
-//        } else {
-//
-//            //ignore cases when one circle is inside another one
-//            val lslTangents = circles.zipWithNextCircular { a, b ->
-//                tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL] ?: error("Intersecting circles")
-//            }
-//
-//            val rsrTangents = circles.zipWithNextCircular { a, b ->
-//                tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] ?: error("Intersecting circles")
-//            }
-//
-//
-//            val lslToCenter = lslTangents.sumOf { it.begin.distanceTo(center) } +
-//                    lslTangents.sumOf { it.end.distanceTo(center) }
-//            val rsrToCenter = rsrTangents.sumOf { it.begin.distanceTo(center) } +
-//                    rsrTangents.sumOf { it.end.distanceTo(center) }
-//
-//            if (rsrToCenter >= lslToCenter) {
-//                this.shell = rsrTangents
-//                this.shellDirection = Trajectory2D.R
-//            } else {
-//                this.shell = lslTangents
-//                this.shellDirection = Trajectory2D.L
-//            }
-//        }
-//    }
-//
-//    constructor(obstacle: Obstacle) : this(obstacle.circles)
-//
-//    /**
-//     * Check if segment has any intersections with this obstacle
-//     */
-//    override fun intersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) {
-//        shell.any { tangent -> intersects(segment, tangent) }
-//                || circles.any { circle -> intersects(segment, circle) }
-//    }
-//
-//    internal fun innerIntersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) {
-//        intersects(polygon(circles.map { it.center }), segment)
-//    }
-//
-//    override fun intersects(circle: Circle2D): Boolean = with(Euclidean2DSpace) {
-//        shell.any { tangent -> intersects(tangent, circle) }
-//                || circles.any { c2 -> intersectsOrInside(circle, c2) }
-//    }
-//
-//    /**
-//     * Tangent to next obstacle node in given direction
-//     */
-//    fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent {
-//        if (circleIndex == -1) error("Circle does not belong to this tangent")
-//
-//        val nextCircleIndex = if (direction == this.shellDirection) {
-//            if (circleIndex == circles.lastIndex) 0 else circleIndex + 1
-//        } else {
-//            if (circleIndex == 0) circles.lastIndex else circleIndex - 1
-//        }
-//
-//        return ObstacleTangent(
-//            LineSegment(
-//                shell[nextCircleIndex].end,
-//                shell[nextCircleIndex].begin
-//            ),
-//            ObstacleConnection(this, circleIndex, direction),
-//            ObstacleConnection(this, nextCircleIndex, direction),
-//        )
-//    }
-//
-//    /**
-//     * All tangents in given direction
-//     */
-//    internal fun tangentsAlong(
-//        initialCircleIndex: Int,
-//        finalCircleIndex: Int,
-//        direction: Trajectory2D.Direction,
-//    ): List<ObstacleTangent> {
-//        return buildList {
-//            var currentIndex = initialCircleIndex
-//            do {
-//                val tangent = nextTangent(currentIndex, direction)
-//                add(tangent)
-//                currentIndex = tangent.endNode.nodeIndex
-//            } while (currentIndex != finalCircleIndex)
-//        }
-//    }
-//
-//    override fun equals(other: Any?): Boolean {
-//        if (other == null || other !is ObstacleShell) return false
-//        return circles == other.circles
-//    }
-//
-//    override fun hashCode(): Int {
-//        return circles.hashCode()
-//    }
-//}
-//
-//internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleShell(listOf(*circles))
-//
-//
-//private fun arcLength(
-//    circle: Circle2D,
-//    point1: DoubleVector2D,
-//    point2: DoubleVector2D,
-//    direction: Trajectory2D.Direction,
-//): Double {
-//    val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
-//    val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
-//    var angle = 0.0
-//    when (direction) {
-//        Trajectory2D.L -> {
-//            angle = if (phi2 >= phi1) {
-//                phi2 - phi1
-//            } else {
-//                2 * PI + phi2 - phi1
-//            }
-//        }
-//
-//        Trajectory2D.R -> {
-//            angle = if (phi2 >= phi1) {
-//                2 * PI - (phi2 - phi1)
-//            } else {
-//                -(phi2 - phi1)
-//            }
-//        }
-//    }
-//    return circle.radius * angle
-//}
-//
-//private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
-//    return Pair(
-//        r * Euclidean2DSpace.vector(v.y / Euclidean2DSpace.norm(v), -v.x / Euclidean2DSpace.norm(v)),
-//        r * Euclidean2DSpace.vector(-v.y / Euclidean2DSpace.norm(v), v.x / Euclidean2DSpace.norm(v))
-//    )
-//}
-//
-//
-//private fun constructTangentCircles(
-//    point: DoubleVector2D,
-//    direction: DoubleVector2D,
-//    r: Double,
-//): LR<Circle2D> {
-//    val center1 = point + normalVectors(direction, r).first
-//    val center2 = point + normalVectors(direction, r).second
-//    val p1 = center1 - point
-//    return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
-//        LR(
-//            Circle2D(center1, r),
-//            Circle2D(center2, r)
-//        )
-//    } else {
-//        LR(
-//            Circle2D(center2, r),
-//            Circle2D(center1, r)
-//        )
-//    }
-//}
-//
-//private fun sortedObstacles(
-//    currentObstacle: Obstacle,
-//    obstacles: List<Obstacle>,
-//): List<Obstacle> {
-//    return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) }
-//}
-//
-///**
-// * Check if all proposed paths have ended at [finalObstacle]
-// */
-//private fun allFinished(
-//    paths: List<TangentPath>,
-//    finalObstacle: Obstacle,
-//): Boolean = paths.all { it.last().endNode.obstacle === finalObstacle }
-//
-//private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
-//
-//
-//private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
-//    buildList {
-//        tangents.zipWithNext().forEach { (left, right: ObstacleTangent) ->
-//            add(left.lineSegment.toTrajectory())
-//            add(
-//                CircleTrajectory2D.of(
-//                    right.startCircle.center,
-//                    left.lineSegment.end,
-//                    right.lineSegment.begin,
-//                    right.startDirection
-//                )
-//            )
-//        }
-//
-//        add(tangents.last().lineSegment.toTrajectory())
-//    }
-//)
-//
-//internal fun findAllPaths(
-//    start: DubinsPose2D,
-//    startingRadius: Double,
-//    finish: DubinsPose2D,
-//    finalRadius: Double,
-//    obstacles: List<ObstacleShell>,
-//): List<CompositeTrajectory2D> {
-//    fun DubinsPose2D.direction() = Euclidean2DSpace.vector(cos(bearing), sin(bearing))
-//
-//    // two circles for the initial point
-//    val initialCircles = constructTangentCircles(
-//        start,
-//        start.direction(),
-//        startingRadius
-//    )
-//
-//    //two circles for the final point
-//    val finalCircles = constructTangentCircles(
-//        finish,
-//        finish.direction(),
-//        finalRadius
-//    )
-//
-//    //all valid trajectories
-//    val trajectories = mutableListOf<CompositeTrajectory2D>()
-//
-//    for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
-//        for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
-//            //Using obstacle to minimize code bloat
-//            val initialObstacle = ObstacleShell(initialCircles[i])
-//            val finalObstacle = ObstacleShell(finalCircles[j])
-//
-//            var currentPaths: List<TangentPath> = listOf(
-//                TangentPath(
-//                    //We need only the direction of the final segment from this
-//                    ObstacleTangent(
-//                        LineSegment(start, start),
-//                        ObstacleConnection(initialObstacle, 0, i),
-//                        ObstacleConnection(initialObstacle, 0, i),
-//                    )
-//                )
-//            )
-//            while (!allFinished(currentPaths, finalObstacle)) {
-//                // paths after next obstacle iteration
-//                val newPaths = mutableListOf<TangentPath>()
-//                // for each path propagate it one obstacle further
-//                for (tangentPath: TangentPath in currentPaths) {
-//                    val currentNode = tangentPath.last().endNode
-//                    val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
-//                    val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle)
-//
-//                    // If path is finished, ignore it
-//                    // TODO avoid returning to ignored obstacle on the next cycle
-//                    if (currentObstacle == finalObstacle) {
-//                        newPaths.add(tangentPath)
-//                    } else {
-//                        val tangentToFinal: ObstacleTangent =
-//                            outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
-//                                currentDirection,
-//                                Trajectory2D.S,
-//                                j
-//                            )] ?: break
-//
-//                        // searching for the nearest obstacle that intersects with the direct path
-//                        val nextObstacle = obstacles.filter { obstacle ->
-//                            obstacle.intersects(tangentToFinal)
-//                        }.minByOrNull { currentObstacle.center.distanceTo(it.center) } ?: finalObstacle
-//
-//                        //TODO add break check for end of path
-//
-//                        // All valid tangents from current obstacle to the next one
-//                        val nextTangents: Collection<ObstacleTangent> = outerTangents(
-//                            currentObstacle,
-//                            nextObstacle
-//                        ).filter { (key, tangent) ->
-////                            obstacles.none { obstacle ->
-////                                obstacle === currentObstacle
-////                                        || obstacle === nextObstacle
-////                                        || obstacle.innerIntersects(tangent)
-////                            } && // does not intersect other obstacles
-//                                    key.first == currentDirection && // initial direction is the same as end of previous segment direction
-//                                    (nextObstacle != finalObstacle || key.third == j) // if it is the last, it should be the same as the one we are searching for
-//                        }.values
-//
-//                        for (tangent in nextTangents) {
-//                            val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) {
-//                                //if the previous segment last circle is the same as first circle of the next segment
-//
-//                                //If obstacle consists of single circle, do not walk around
-//                                if (currentObstacle.circles.size < 2) {
-//                                    emptyList()
-//                                } else {
-//                                    val lengthMaxPossible = arcLength(
-//                                        tangent.startCircle,
-//                                        tangentPath.last().lineSegment.end,
-//                                        currentObstacle.nextTangent(
-//                                            tangent.beginNode.nodeIndex,
-//                                            currentDirection
-//                                        ).lineSegment.begin,
-//                                        currentDirection
-//                                    )
-//
-//                                    val lengthCalculated = arcLength(
-//                                        tangent.startCircle,
-//                                        tangentPath.last().lineSegment.end,
-//                                        tangent.lineSegment.begin,
-//                                        currentDirection
-//                                    )
-//                                    // ensure that path does not go inside the obstacle
-//                                    if (lengthCalculated > lengthMaxPossible) {
-//                                        currentObstacle.tangentsAlong(
-//                                            currentNode.nodeIndex,
-//                                            tangent.beginNode.nodeIndex,
-//                                            currentDirection,
-//                                        )
-//                                    } else {
-//                                        emptyList()
-//                                    }
-//                                }
-//                            } else {
-//                                currentObstacle.tangentsAlong(
-//                                    currentNode.nodeIndex,
-//                                    tangent.beginNode.nodeIndex,
-//                                    currentDirection,
-//                                )
-//                            }
-//                            newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
-//                        }
-//                    }
-//                }
-//                currentPaths = newPaths
-//            }
-//
-//            trajectories += currentPaths.map { tangentPath ->
-////                val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
-//                val end = Obstacle(finalCircles[j])
-//                TangentPath(
-//                    tangentPath.tangents +
-//                            ObstacleTangent(
-//                                LineSegment(finish, finish),
-//                                ObstacleConnection(end, 0, j),
-//                                ObstacleConnection(end, 0, j)
-//                            )
-//                )
-//            }.map { it.toTrajectory() }
-//        }
-//    }
-//    return trajectories
-//}

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

@@ -1,11 +1,8 @@
 package space.kscience.trajectory
 
 import space.kscience.kmath.geometry.*
-import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import kotlin.collections.component1
 import kotlin.collections.component2
-import kotlin.math.PI
-import kotlin.math.atan2
 
 
 public class Obstacles(public val obstacles: List<Obstacle>) {
@@ -16,7 +13,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         val direction: Trajectory2D.Direction,
     ) {
         val obstacle: Obstacle get() = obstacles[obstacleIndex]
-        val circle: Circle2D get() = obstacle.circles[nodeIndex]
+        val circle: Circle2D get() = obstacle.arcs[nodeIndex].circle
     }
 
     private inner class ObstacleTangent(
@@ -47,18 +44,16 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
     ): Map<DubinsPath.Type, ObstacleTangent> = with(Euclidean2DSpace) {
         val first = obstacles[firstIndex]
         val second = obstacles[secondIndex]
-        val firstPolygon = polygon(first.circles.map { it.center })
-        val secondPolygon = polygon(second.circles.map { it.center })
         buildMap {
-            for (firstCircleIndex in first.circles.indices) {
+            for (firstCircleIndex in first.arcs.indices) {
-                val firstCircle = first.circles[firstCircleIndex]
+                val firstCircle = first.arcs[firstCircleIndex]
-                for (secondCircleIndex in second.circles.indices) {
+                for (secondCircleIndex in second.arcs.indices) {
-                    val secondCircle = second.circles[secondCircleIndex]
+                    val secondCircle = second.arcs[secondCircleIndex]
-                    for ((pathType, segment) in tangentsBetweenCircles(
+                    for ((pathType, segment) in tangentsBetweenArcs(
                         firstCircle,
                         secondCircle
                     )) {
-                        if (!intersects(firstPolygon, segment) && !intersects(secondPolygon, segment)) {
+                        if (!first.intersects(segment) && !second.intersects(segment)) {
                             put(
                                 pathType,
                                 ObstacleTangent(
@@ -75,21 +70,19 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
     }
 
 
-    private fun tangentsFromCircle(
+    private fun tangentsFromArc(
-        circle: Circle2D,
+        arc: CircleTrajectory2D,
-        direction: Trajectory2D.Direction,
         obstacleIndex: Int,
     ): Map<DubinsPath.Type, ObstacleTangent> = with(Euclidean2DSpace) {
         val obstacle = obstacles[obstacleIndex]
-        val polygon = polygon(obstacle.circles.map { it.center })
         buildMap {
-            for (circleIndex in obstacle.circles.indices) {
+            for (circleIndex in obstacle.arcs.indices) {
-                val obstacleCircle = obstacle.circles[circleIndex]
+                val obstacleArc = obstacle.arcs[circleIndex]
-                for ((pathType, segment) in tangentsBetweenCircles(
+                for ((pathType, segment) in tangentsBetweenArcs(
-                    circle,
+                    arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
-                    obstacleCircle
+                    obstacleArc
                 )) {
-                    if (pathType.first == direction && !intersects(polygon, segment)) {
+                    if (pathType.first == arc.direction && !intersects(obstacle.polygon, segment)) {
                         put(
                             pathType,
                             ObstacleTangent(
@@ -104,21 +97,19 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         }
     }
 
-    private fun tangentToCircle(
+    private fun tangentToArc(
         obstacleIndex: Int,
         obstacleDirection: Trajectory2D.Direction,
-        circle: Circle2D,
+        arc: CircleTrajectory2D
-        direction: Trajectory2D.Direction,
     ): ObstacleTangent? = with(Euclidean2DSpace) {
         val obstacle = obstacles[obstacleIndex]
-        val polygon = polygon(obstacle.circles.map { it.center })
+        for (circleIndex in obstacle.arcs.indices) {
-        for (circleIndex in obstacle.circles.indices) {
+            val obstacleArc = obstacle.arcs[circleIndex]
-            val obstacleCircle = obstacle.circles[circleIndex]
+            tangentsBetweenArcs(
-            tangentsBetweenCircles(
+                obstacleArc,
-                obstacleCircle,
+                arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
-                circle
+            )[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
-            ).get(DubinsPath.Type(obstacleDirection, Trajectory2D.S, direction))?.takeIf {
+                !obstacle.intersects(it)
-                !intersects(polygon, it)
             }?.let {
                 return ObstacleTangent(
                     it,
@@ -163,11 +154,13 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
             first.circle,
             tangent1.tangentTrajectory.endPose,
             first.endPose,
+            tangent1.to.direction
         )
         circumvention[circumvention.lastIndex] = CircleTrajectory2D(
             last.circle,
             last.beginPose,
-            tangent2.tangentTrajectory.beginPose
+            tangent2.tangentTrajectory.beginPose,
+            tangent2.from.direction
         )
         return CompositeTrajectory2D(circumvention)
     }
@@ -186,19 +179,18 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         )
     }
 
-    public fun allTrajectoriesAvoiding(
+    private fun avoiding(
-        startCircle: Circle2D,
+        dubinsPath: CompositeTrajectory2D,
-        startDirection: Trajectory2D.Direction,
+    ): Collection<Trajectory2D> = with(Euclidean2DSpace) {
-        endCircle: Circle2D,
-        endDirection: Trajectory2D.Direction,
-    ): Collection<Trajectory2D> {
-        val directTangent: StraightTrajectory2D = tangentsBetweenCircles(startCircle, endCircle).get(
-            DubinsPath.Type(startDirection, Trajectory2D.S, endDirection)
-        ) ?: return emptySet()
-
         //fast return if no obstacles intersect direct path
-        if (obstacles.none { it.intersects(directTangent) }) return listOf(directTangent)
+        if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
 
+        val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
+        val endArc = dubinsPath.segments.last() as CircleTrajectory2D
+
+        /**
+         * Continue current tangent to final point or to the next obstacle
+         */
         /**
          * Continue current tangent to final point or to the next obstacle
          */
@@ -207,14 +199,13 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
             require(connection != null)
 
             //indices of obstacles that are not on previous path
-            val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }
+            val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }.toSet()
 
             //a tangent to end point, null if tangent could not be constructed
-            val tangentToEnd: ObstacleTangent = tangentToCircle(
+            val tangentToEnd: ObstacleTangent = tangentToArc(
                 connection.obstacleIndex,
                 connection.direction,
-                endCircle,
+                endArc
-                endDirection
             ) ?: return emptySet()
 
             if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
@@ -239,125 +230,113 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
 
         //find nearest obstacle that has valid tangents to
         val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
-            compareByDescending<Int> { obstacles[it].intersects(directTangent) } //take intersecting obstacles
+            compareByDescending<Int> { obstacles[it].intersects(dubinsPath) } //take intersecting obstacles
-                .thenBy { startCircle.center.distanceTo(obstacles[it].center) } //then nearest
+                .thenBy { beginArc.circle.center.distanceTo(obstacles[it].center) } //then nearest
-        ).firstNotNullOf { obstacleIndex ->
+        ).firstNotNullOfOrNull { obstacleIndex ->
-            tangentsFromCircle(startCircle, startDirection, obstacleIndex).values
+            tangentsFromArc(beginArc, obstacleIndex).values
                 .filter { it.isValid }.takeIf { it.isNotEmpty() }
-        }
+        }?: return emptySet()
 
         var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
 
         while (!paths.all { it.isFinished }) {
             paths = paths.flatMap { it.nextSteps() }
         }
-        return paths.map { it.toTrajectory() }
+        return paths.map {
+            CompositeTrajectory2D(
+                //arc from starting point
+                CircleTrajectory2D(
+                    beginArc.circle,
+                    beginArc.beginPose,
+                    it.tangents.first().tangentTrajectory.beginPose,
+                    beginArc.direction
+                ),
+                it.toTrajectory(),
+                //arc to the end point
+                CircleTrajectory2D(
+                    endArc.circle,
+                    it.tangents.last().tangentTrajectory.endPose,
+                    endArc.endPose,
+                    endArc.direction
+                ),
+            )
+        }
+    }
+
+    public fun allTrajectories(
+        start: Pose2D,
+        finish: Pose2D,
+        radius: Double,
+    ): List<Trajectory2D> {
+
+        val dubinsPaths: List<CompositeTrajectory2D> = DubinsPath.all(start, finish, radius)
+
+        return dubinsPaths.flatMap {
+            avoiding(it)
+        }
     }
 
 
     public companion object {
-        private data class LR<T>(val l: T, val r: T) {
+//        private data class LR<T>(val l: T, val r: T) {
-            operator fun get(direction: Trajectory2D.Direction) = when (direction) {
+//            operator fun get(direction: Trajectory2D.Direction) = when (direction) {
-                Trajectory2D.L -> l
+//                Trajectory2D.L -> l
-                Trajectory2D.R -> r
+//                Trajectory2D.R -> r
-            }
+//            }
-        }
+//        }
-
+//
-        private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> =
+//        private fun constructTangentCircles(
-            with(Euclidean2DSpace) {
+//            pose: Pose2D,
-                Pair(
+//            r: Double,
-                    r * vector(v.y / norm(v), -v.x / norm(v)),
+//        ): LR<Circle2D> = with(Euclidean2DSpace) {
-                    r * vector(-v.y / norm(v), v.x / norm(v))
+//            val center1 = pose + vector(r*sin(pose.bearing + Angle.piDiv2), r*cos(pose.bearing + Angle.piDiv2))
-                )
+//            val center2 = pose + vector(r*sin(pose.bearing - Angle.piDiv2), r*cos(pose.bearing - Angle.piDiv2))
-            }
+//            LR(
-
+//                Circle2D(center2, r),
-        private fun constructTangentCircles(
+//                Circle2D(center1, r)
-            pose: Pose2D,
+//            )
-            r: Double,
+//        }
-        ): LR<Circle2D> = with(Euclidean2DSpace) {
-            val direction = Pose2D.bearingToVector(pose.bearing)
-            //TODO optimize to use bearing
-            val center1 = pose + normalVectors(direction, r).first
-            val center2 = pose + normalVectors(direction, r).second
-            val p1 = center1 - pose
-            return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
-                LR(
-                    Circle2D(center1, r),
-                    Circle2D(center2, r)
-                )
-            } else {
-                LR(
-                    Circle2D(center2, r),
-                    Circle2D(center1, r)
-                )
-            }
-        }
 
         public fun avoidObstacles(
             start: Pose2D,
             finish: Pose2D,
-            startingRadius: Double,
+            radius: Double,
             obstacleList: List<Obstacle>,
-            finalRadius: Double = startingRadius,
         ): List<Trajectory2D> {
             val obstacles = Obstacles(obstacleList)
-            val initialCircles = constructTangentCircles(
+            return obstacles.allTrajectories(start, finish, radius)
-                start,
-                startingRadius
-            )
-
-            //two circles for the final point
-            val finalCircles = constructTangentCircles(
-                finish,
-                finalRadius
-            )
-            val lr = listOf(Trajectory2D.L, Trajectory2D.R)
-            return buildList {
-                lr.forEach { beginDirection ->
-                    lr.forEach { endDirection ->
-                        addAll(
-                            obstacles.allTrajectoriesAvoiding(
-                                initialCircles[beginDirection],
-                                beginDirection,
-                                finalCircles[endDirection],
-                                endDirection
-                            )
-                        )
-                    }
-                }
-            }
         }
 
         public fun avoidObstacles(
             start: Pose2D,
             finish: Pose2D,
-            trajectoryRadius: Double,
+            radius: Double,
             vararg obstacles: Obstacle,
-        ): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
+        ): List<Trajectory2D> = avoidObstacles(start, finish, radius, obstacles.toList())
 
         public fun avoidPolygons(
             start: Pose2D,
             finish: Pose2D,
-            trajectoryRadius: Double,
+            radius: Double,
             vararg polygons: Polygon<Double>,
         ): List<Trajectory2D> {
             val obstacles: List<Obstacle> = polygons.map { polygon ->
-                Obstacle(polygon.points, trajectoryRadius)
+                Obstacle(polygon.points, radius)
             }
-            return avoidObstacles(start, finish, trajectoryRadius, obstacles)
+            return avoidObstacles(start, finish, radius, obstacles)
         }
 
 
         public fun avoidPolygons(
             start: Pose2D,
             finish: Pose2D,
-            trajectoryRadius: Double,
+            radius: Double,
             polygons: Collection<Polygon<Double>>,
         ): List<Trajectory2D> {
             val obstacles: List<Obstacle> = polygons.map { polygon ->
-                Obstacle(polygon.points, trajectoryRadius)
+                Obstacle(polygon.points, radius)
             }
-            return avoidObstacles(start, finish, trajectoryRadius, obstacles)
+            return avoidObstacles(start, finish, radius, obstacles)
         }
     }
 

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

@@ -89,6 +89,8 @@ public data class CircleTrajectory2D(
         circle.radius * kotlin.math.abs(arcAngle.radians)
     }
 
+    val center: Vector2D<Double> get() = circle.center
+
 
     override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, arcEnd, -arcAngle)
 
@@ -101,25 +103,6 @@ public fun CircleTrajectory2D(
     end: DoubleVector2D,
     direction: Trajectory2D.Direction,
 ): CircleTrajectory2D = with(Euclidean2DSpace) {
-//    fun calculatePose(
-//        vector: DoubleVector2D,
-//        theta: Angle,
-//        direction: Trajectory2D.Direction,
-//    ): DubinsPose2D = DubinsPose2D(
-//        vector,
-//        when (direction) {
-//            Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
-//            Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
-//        }
-//    )
-//
-//    val s1 = StraightTrajectory2D(center, start)
-//    val s2 = StraightTrajectory2D(center, end)
-//    val pose1 = calculatePose(start, s1.bearing, direction)
-//    val pose2 = calculatePose(end, s2.bearing, direction)
-//    val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
-//    if (trajectory.direction != direction) error("Trajectory direction mismatch")
-//    return trajectory
     val startVector = start - center
     val endVector = end - center
     val startRadius = norm(startVector)
@@ -147,6 +130,36 @@ public fun CircleTrajectory2D(
     )
 }
 
+public fun CircleTrajectory2D(
+    circle: Circle2D,
+    start: DoubleVector2D,
+    end: DoubleVector2D,
+    direction: Trajectory2D.Direction,
+): CircleTrajectory2D = with(Euclidean2DSpace) {
+    val startVector = start - circle.center
+    val endVector = end - circle.center
+    val startBearing = startVector.bearing
+    val endBearing = endVector.bearing
+    CircleTrajectory2D(
+        circle,
+        startBearing,
+        when (direction) {
+            Trajectory2D.L -> if (endBearing >= startBearing) {
+                endBearing - startBearing - Angle.piTimes2
+            } else {
+                endBearing - startBearing
+            }
+
+            Trajectory2D.R -> if (endBearing >= startBearing) {
+                endBearing - startBearing
+            } else {
+                endBearing + Angle.piTimes2 - startBearing
+            }
+        }
+    )
+}
+
+@Deprecated("Use angle notation instead")
 public fun CircleTrajectory2D(
     circle: Circle2D,
     beginPose: Pose2D,
@@ -172,18 +185,18 @@ public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Tr
 public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
     CompositeTrajectory2D(segments.toList())
 
-public fun Euclidean2DSpace.trajectoryIntersects(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
+public fun Euclidean2DSpace.intersectsTrajectory(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
     is CircleTrajectory2D -> when (b) {
         is CircleTrajectory2D -> intersectsOrInside(a.circle, b.circle)
         is StraightTrajectory2D -> intersects(a.circle, b)
-        is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) }
+        is CompositeTrajectory2D -> b.segments.any { intersectsTrajectory(it, a) }
     }
 
     is StraightTrajectory2D -> when (b) {
         is CircleTrajectory2D -> intersects(a, b.circle)
         is StraightTrajectory2D -> intersects(a, b)
-        is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) }
+        is CompositeTrajectory2D -> b.segments.any { intersectsTrajectory(it, a) }
     }
 
-    is CompositeTrajectory2D -> a.segments.any { trajectoryIntersects(it, b) }
+    is CompositeTrajectory2D -> a.segments.any { intersectsTrajectory(it, b) }
 }

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

@@ -1,6 +1,7 @@
 package space.kscience.trajectory
 
 import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.DoubleVector2D
 import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.trajectory.DubinsPath.Type
 import kotlin.math.*
@@ -62,12 +63,30 @@ internal fun tangentsBetweenCircles(
     }
 }
 
+internal fun tangentsBetweenArcs(
+    first: CircleTrajectory2D,
+    second: CircleTrajectory2D,
+): Map<Type, StraightTrajectory2D> {
+
+    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)
+    }
+}
+
 /**
  * Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex]
  */
 public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: Int): CompositeTrajectory2D {
-    require(fromIndex in circles.indices) { "$fromIndex is not in ${circles.indices}" }
+    require(fromIndex in arcs.indices) { "$fromIndex is not in ${arcs.indices}" }
-    val startCircle = circles[fromIndex]
+    val startCircle = arcs[fromIndex]
     val segments = buildList {
         val reserve = mutableListOf<Trajectory2D>()
 
@@ -77,7 +96,7 @@ public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex:
         }
 
         var i = 0
-        while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) {
+        while (sourceSegments[i] !== startCircle) {
             //put all segments before target circle on the reserve
             reserve.add(sourceSegments[i])
             i++
@@ -104,8 +123,8 @@ public fun Obstacle.circumvention(
     fromIndex: Int,
     toIndex: Int,
 ): CompositeTrajectory2D {
-    require(toIndex in circles.indices) { "$toIndex is not in ${circles.indices}" }
+    require(toIndex in arcs.indices) { "$toIndex is not in ${arcs.indices}" }
-    val toCircle = circles[toIndex]
+    val toCircle = arcs[toIndex]
     val fullCircumvention = circumvention(direction, fromIndex).segments
     return CompositeTrajectory2D(
         buildList {
@@ -114,7 +133,7 @@ public fun Obstacle.circumvention(
                 val segment = fullCircumvention[i]
                 add(segment)
                 i++
-            } while ((segment as? CircleTrajectory2D)?.circle != toCircle)
+            } while (segment !== toCircle)
         }
     )
 }

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

@@ -5,6 +5,7 @@
 
 package space.kscience.trajectory
 
+import space.kscience.kmath.geometry.Angle
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace.vector
 import space.kscience.kmath.geometry.degrees
@@ -27,16 +28,10 @@ class ObstacleTest {
 
     @Test
     fun singeObstacle() {
-        val startPoint = vector(-5.0, -1.0)
+        val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles(
-        val startDirection = vector(1.0, 1.0)
+            Pose2D(-5,-1, Angle.pi/4),
-        val startRadius = 0.5
+            Pose2D(20,4, Angle.pi*3/4),
-        val finalPoint = vector(20.0, 4.0)
+            0.5,
-        val finalDirection = vector(1.0, -1.0)
-
-        val outputTangents = Obstacles.avoidObstacles(
-            Pose2D(startPoint, startDirection),
-            Pose2D(finalPoint, finalDirection),
-            startRadius,
             Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
         )
         assertTrue { outputTangents.isNotEmpty() }
@@ -46,16 +41,10 @@ class ObstacleTest {
 
     @Test
     fun twoObstacles() {
-        val startPoint = vector(-5.0, -1.0)
-        val startDirection = vector(1.0, 1.0)
-        val radius = 0.5
-        val finalPoint = vector(20.0, 4.0)
-        val finalDirection = vector(1.0, -1.0)
-
         val paths = Obstacles.avoidObstacles(
-            Pose2D(startPoint, startDirection),
+            Pose2D(-5,-1, Angle.pi/4),
-            Pose2D(finalPoint, finalDirection),
+            Pose2D(20,4, Angle.pi*3/4),
-            radius,
+            0.5,
             Obstacle(
                 Circle2D(vector(1.0, 6.5), 0.5),
                 Circle2D(vector(2.0, 1.0), 0.5),
@@ -74,7 +63,7 @@ class ObstacleTest {
     }
 
     @Test
-    fun circumvention(){
+    fun circumvention() {
         val obstacle = Obstacle(
             Circle2D(vector(0.0, 0.0), 1.0),
             Circle2D(vector(0.0, 1.0), 1.0),
@@ -86,32 +75,26 @@ class ObstacleTest {
 
         assertEquals(4, circumvention.segments.count { it is CircleTrajectory2D })
 
-        assertEquals(4 + 2* PI, circumvention.length, 1e-4)
+        assertEquals(4 + 2 * PI, circumvention.length, 1e-4)
     }
 
     @Test
     fun closePoints() {
-        val startPoint = vector(-1.0, -1.0)
+        val obstacle = Obstacle(
-        val startDirection = vector(0.0, 1.0)
-        val startRadius = 1.0
-        val finalPoint = vector(-1, -1)
-        val finalDirection = vector(1.0, 0)
-
-        val paths = Obstacles.avoidObstacles(
-            Pose2D(startPoint, startDirection),
-            Pose2D(finalPoint, finalDirection),
-            startRadius,
-            Obstacle(
             Circle2D(vector(0.0, 0.0), 1.0),
             Circle2D(vector(0.0, 1.0), 1.0),
             Circle2D(vector(1.0, 1.0), 1.0),
             Circle2D(vector(1.0, 0.0), 1.0)
         )
+
+        val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
+            Pose2D(-0.9, -0.9, Angle.pi),
+            Pose2D(-0.9, -0.9, Angle.piDiv2),
+            1.0,
+            obstacle
         )
         assertTrue { paths.isNotEmpty() }
-        val length = paths.minOf { it.length }
+        assertEquals(18.37, paths.minOf { it.length }, 1e-2)
-        println(length)
-        //assertEquals(28.9678224, length, 1e-6)
     }
 
     @Test