[WIP] full rewrite of obstacle avoidance

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

@@ -0,0 +1,49 @@
+package space.kscience.kmath.geometry
+
+import space.kscience.kmath.operations.DoubleField.pow
+import kotlin.math.sign
+
+public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
+    Circle2D(vector(x, y), radius = radius.toDouble())
+
+public fun Euclidean2DSpace.segment(begin: DoubleVector2D, end: DoubleVector2D): LineSegment2D =
+    LineSegment(begin, end)
+
+public fun Euclidean2DSpace.segment(x1: Number, y1: Number, x2: Number, y2: Number): LineSegment2D =
+    LineSegment(vector(x1, y1), vector(x2, y2))
+
+public fun Euclidean2DSpace.intersectsOrInside(circle1: Circle2D, circle2: Circle2D): Boolean {
+    val distance = norm(circle2.center - circle1.center)
+    return distance <= circle1.radius + circle2.radius
+}
+
+/**
+ * https://mathworld.wolfram.com/Circle-LineIntersection.html
+ */
+public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D): Boolean {
+    val begin = segment.begin
+    val end = segment.end
+    val d = begin.distanceTo(end)
+    val det = (begin.x - circle.center.x) * (end.y - circle.center.y) -
+            (end.x - circle.center.x) * (begin.y - circle.center.y)
+
+    val incidence = circle.radius.pow(2) * d.pow(2) - det.pow(2)
+
+    return incidence >= 0
+}
+
+public fun Euclidean2DSpace.intersects(circle: Circle2D, segment: LineSegment2D): Boolean =
+    intersects(segment, circle)
+
+public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSegment2D): Boolean {
+    infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
+    infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
+
+    return with(Euclidean2DSpace) {
+        (segment2.begin - segment1.begin) crossSign (segment2.end - segment1.begin) !=
+                (segment2.begin - segment1.end) crossSign (segment2.end - segment1.end) &&
+                (segment1.begin - segment2.begin) crossSign (segment1.end - segment2.begin) !=
+                (segment1.begin - segment2.end) crossSign (segment1.end - segment2.end)
+    }
+}
+

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

@@ -0,0 +1,10 @@
+package space.kscience.kmath.geometry
+
+import space.kscience.kmath.misc.zipWithNextCircular
+
+public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Double> = object : Polygon<Double> {
+    override val points: List<Vector2D<Double>> get() = points
+}
+
+public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, segment: LineSegment2D): Boolean =
+    polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, segment) }

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

@@ -74,6 +74,8 @@ public object DubinsPath {
 
         override fun toString(): String = "${first}${second}${third}"
 
+        public val last: Direction get() = third
+
         public companion object {
             public val RLR: Type = Type(R, L, R)
             public val LRL: Type = Type(L, R, L)

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

@@ -24,6 +24,11 @@ public interface DubinsPose2D : DoubleVector2D {
     public val coordinates: DoubleVector2D
     public val bearing: Angle
 
+    /**
+     * Reverse the direction of this pose to the opposite, keeping other parameters the same
+     */
+    public fun reversed(): DubinsPose2D
+
     public companion object {
         public fun bearingToVector(bearing: Angle): Vector2D<Double> =
             Euclidean2DSpace.vector(cos(bearing), sin(bearing))
@@ -35,12 +40,15 @@ public interface DubinsPose2D : DoubleVector2D {
     }
 }
 
+
 @Serializable
 public class PhaseVector2D(
     override val coordinates: DoubleVector2D,
     public val velocity: DoubleVector2D,
 ) : DubinsPose2D, DoubleVector2D by coordinates {
     override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
+
+    override fun reversed(): DubinsPose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
 }
 
 @Serializable
@@ -50,7 +58,9 @@ private class DubinsPose2DImpl(
     override val bearing: Angle,
 ) : DubinsPose2D, DoubleVector2D by coordinates {
 
-    override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
+    override fun reversed(): DubinsPose2D = DubinsPose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
+
+    override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
 }
 
 public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
@@ -69,7 +79,8 @@ public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
     }
 }
 
-public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, bearing)
+public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D =
+    DubinsPose2DImpl(coordinate, bearing)
 
 public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
     DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))

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

@@ -5,96 +5,109 @@
 
 package space.kscience.trajectory
 
+import space.kscience.kmath.geometry.Angle
 import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.LineSegment2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.Polygon
 import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.misc.zipWithNextCircular
 
 
 public interface Obstacle {
     public val circles: List<Circle2D>
     public val center: Vector2D<Double>
 
-    public fun intersects(segment: LineSegment2D): Boolean
+    public val circumvention: CompositeTrajectory2D
+
+    /**
+     * Check if obstacle has intersection with given [Trajectory2D]
+     */
+    public fun intersects(trajectory: Trajectory2D): Boolean =
+        Euclidean2DSpace.trajectoryIntersects(circumvention, trajectory)
 
-    public fun intersects(circle: Circle2D): Boolean
 
     public companion object {
-        public fun avoidObstacles(
-            start: DubinsPose2D,
-            finish: DubinsPose2D,
-            trajectoryRadius: Double,
-            vararg obstacles: Obstacle,
-        ): List<CompositeTrajectory2D> {
-            val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
-                ObstacleShell(polygon.circles)
-            }
-            return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
-        }
 
-        public fun avoidPolygons(
-            start: DubinsPose2D,
-            finish: DubinsPose2D,
-            trajectoryRadius: Double,
-            vararg obstacles: Polygon<Double>,
-        ): List<CompositeTrajectory2D> {
-            val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
-                ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
-            }
-            return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
-        }
-
-        public fun avoidObstacles(
-            start: DubinsPose2D,
-            finish: DubinsPose2D,
-            trajectoryRadius: Double,
-            obstacles: Collection<Obstacle>,
-        ): List<CompositeTrajectory2D> {
-            val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
-                ObstacleShell(polygon.circles)
-            }
-            return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
-        }
-
-        public fun avoidPolygons(
-            start: DubinsPose2D,
-            finish: DubinsPose2D,
-            trajectoryRadius: Double,
-            obstacles: Collection<Polygon<Double>>,
-        ): List<CompositeTrajectory2D> {
-            val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
-                ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
-            }
-            return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
-        }
     }
 }
 
-public fun Obstacle.intersectsTrajectory(trajectory: Trajectory2D): Boolean = when (trajectory) {
+private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
-    is CircleTrajectory2D -> intersects(trajectory.circle)
+    override val center: Vector2D<Double> by lazy {
-    is StraightTrajectory2D -> intersects(trajectory)
+        Euclidean2DSpace.vector(
-    is CompositeTrajectory2D -> trajectory.segments.any { intersectsTrajectory(it) }
+            circles.sumOf { it.center.x } / circles.size,
+            circles.sumOf { it.center.y } / circles.size
+        )
+    }
+
+    override val circumvention: CompositeTrajectory2D by lazy {
+        with(Euclidean2DSpace) {
+            /**
+             * 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 startEnd = DubinsPose2D(
+                    top,
+                    Angle.piDiv2
+                )
+                return@lazy CompositeTrajectory2D(
+                    CircleTrajectory2D(circle, startEnd, startEnd)
+                )
+            }
+
+            //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 {
+        if (this === other) return true
+        if (other == null || this::class != other::class) return false
+
+        other as ObstacleImpl
+
+        return circles == other.circles
+    }
+
+    override fun hashCode(): Int {
+        return circles.hashCode()
+    }
+
+    override fun toString(): String {
+        return "Obstacle(circles=$circles)"
+    }
+
+
 }
 
-public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles))
+public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles))
 
 public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
-    ObstacleShell(points.map { Circle2D(it, radius) })
+    ObstacleImpl(points.map { Circle2D(it, radius) })
-
-//public fun Trajectory2D.intersects(
-//    polygon: Polygon<Double>,
-//    radius: Double,
-//): Boolean {
-//    val obstacle = Obstacle(polygon.points.map { point -> Circle2D(point, radius) })
-//    return when (this) {
-//        is CircleTrajectory2D -> {
-//            val nearestCircle = obstacle.circles.minBy { it.center.distanceTo(circle.center) }
-//
-//        }
-//        is StraightTrajectory2D -> obstacle.intersects(this)
-//        is CompositeTrajectory2D -> segments.any { it.intersects(polygon, radius) }
-//    }
-//}
 
 
 

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

@@ -0,0 +1,389 @@
+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

@@ -0,0 +1,363 @@
+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>) {
+
+    private inner class ObstacleConnection(
+        val obstacleIndex: Int,
+        val nodeIndex: Int,
+        val direction: Trajectory2D.Direction,
+    ) {
+        val obstacle: Obstacle get() = obstacles[obstacleIndex]
+        val circle: Circle2D get() = obstacle.circles[nodeIndex]
+    }
+
+    private inner class ObstacleTangent(
+        val tangentTrajectory: StraightTrajectory2D,
+        val from: ObstacleConnection?,
+        val to: ObstacleConnection?,
+    ) {
+        /**
+         * If false this tangent intersects another obstacle
+         */
+        val isValid by lazy {
+            obstacles.indices.none {
+                it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory)
+            }
+        }
+    }
+
+
+    /**
+     * All tangents between two obstacles
+     *
+     * In general generates 4 paths.
+     *  TODO check intersections.
+     */
+    private fun tangentsBetween(
+        firstIndex: Int,
+        secondIndex: Int,
+    ): 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) {
+                val firstCircle = first.circles[firstCircleIndex]
+                for (secondCircleIndex in second.circles.indices) {
+                    val secondCircle = second.circles[secondCircleIndex]
+                    for ((pathType, segment) in tangentsBetweenCircles(
+                        firstCircle,
+                        secondCircle
+                    )) {
+                        if (!intersects(firstPolygon, segment) && !intersects(secondPolygon, segment)) {
+                            put(
+                                pathType,
+                                ObstacleTangent(
+                                    segment,
+                                    ObstacleConnection(firstIndex, firstCircleIndex, pathType.first),
+                                    ObstacleConnection(secondIndex, secondCircleIndex, pathType.third)
+                                )
+                            )
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+
+    private fun tangentsFromCircle(
+        circle: Circle2D,
+        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) {
+                val obstacleCircle = obstacle.circles[circleIndex]
+                for ((pathType, segment) in tangentsBetweenCircles(
+                    circle,
+                    obstacleCircle
+                )) {
+                    if (pathType.first == direction && !intersects(polygon, segment)) {
+                        put(
+                            pathType,
+                            ObstacleTangent(
+                                segment,
+                                null,
+                                ObstacleConnection(obstacleIndex, circleIndex, pathType.third)
+                            )
+                        )
+                    }
+                }
+            }
+        }
+    }
+
+    private fun tangentToCircle(
+        obstacleIndex: Int,
+        obstacleDirection: Trajectory2D.Direction,
+        circle: Circle2D,
+        direction: Trajectory2D.Direction,
+    ): ObstacleTangent? = with(Euclidean2DSpace) {
+        val obstacle = obstacles[obstacleIndex]
+        val polygon = polygon(obstacle.circles.map { it.center })
+        for (circleIndex in obstacle.circles.indices) {
+            val obstacleCircle = obstacle.circles[circleIndex]
+            tangentsBetweenCircles(
+                obstacleCircle,
+                circle
+            ).get(DubinsPath.Type(obstacleDirection, Trajectory2D.S, direction))?.takeIf {
+                !intersects(polygon, it)
+            }?.let {
+                return ObstacleTangent(
+                    it,
+                    ObstacleConnection(obstacleIndex, circleIndex, obstacleDirection),
+                    null,
+                )
+            }
+        }
+        return null
+    }
+
+
+    private val tangentsCache = hashMapOf<Pair<Int, Int>, Map<DubinsPath.Type, ObstacleTangent>>()
+
+    private fun getAllTangents(i: Int, j: Int): Map<DubinsPath.Type, ObstacleTangent> =
+        tangentsCache.getOrPut(i to j) {
+            tangentsBetween(i, j)
+        }
+
+
+    /**
+     * Circumvention trajectory alongside obstacle. Replacing first and last arcs with appropriate cuts
+     */
+    private fun trajectoryBetween(tangent1: ObstacleTangent, tangent2: ObstacleTangent): CompositeTrajectory2D {
+        require(tangent1.to != null)
+        require(tangent2.from != null)
+
+        require(tangent1.to.obstacleIndex == tangent2.from.obstacleIndex)
+        require(tangent1.to.direction == tangent2.from.direction)
+
+        val circumvention = tangent1.to.obstacle.circumvention(
+            tangent1.to.direction,
+            tangent1.to.nodeIndex,
+            tangent2.from.nodeIndex
+        ).segments.toMutableList()
+
+        //cutting first and last arcs to accommodate connection points
+        val first = circumvention.first() as CircleTrajectory2D
+        val last = circumvention.last() as CircleTrajectory2D
+        circumvention[0] = CircleTrajectory2D(
+            first.circle,
+            tangent1.tangentTrajectory.endPose,
+            first.end
+        )
+        circumvention[circumvention.lastIndex] = CircleTrajectory2D(
+            last.circle,
+            last.begin,
+            tangent2.tangentTrajectory.beginPose
+        )
+        return CompositeTrajectory2D(circumvention)
+    }
+
+    private inner class TangentPath(val tangents: List<ObstacleTangent>) {
+        val isFinished get() = tangents.last().to == null
+
+        fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
+            buildList<Trajectory2D> {
+                add(tangents.first().tangentTrajectory)
+                tangents.zipWithNext().forEach { (l, r) ->
+                    addAll(trajectoryBetween(l, r).segments)
+                    add(r.tangentTrajectory)
+                }
+            }
+        )
+    }
+
+    public fun allTrajectoriesAvoiding(
+        startCircle: Circle2D,
+        startDirection: Trajectory2D.Direction,
+        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)
+
+        /**
+         * Continue current tangent to final point or to the next obstacle
+         */
+        fun TangentPath.nextSteps(): Collection<TangentPath> {
+            val connection = tangents.last().to
+            require(connection != null)
+
+            //indices of obstacles that are not on previous path
+            val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }
+
+            //a tangent to end point, null if tangent could not be constructed
+            val tangentToEnd: ObstacleTangent = tangentToCircle(
+                connection.obstacleIndex,
+                connection.direction,
+                endCircle,
+                endDirection
+            ) ?: return emptySet()
+
+            if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
+                TangentPath(tangents + tangentToEnd)
+            )
+
+            // tangents to other obstacles
+            return remainingObstacleIndices.sortedWith(
+                compareByDescending<Int> { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles
+                    .thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest
+            ).firstNotNullOf { nextObstacleIndex ->
+                //all tangents from cache
+                getAllTangents(connection.obstacleIndex, nextObstacleIndex).filter {
+                    //filtered by direction
+                    it.key.first == connection.direction
+                }.values.takeIf { it.isNotEmpty() } // skip if empty
+            }.map {
+                TangentPath(tangents + it)
+            }
+        }
+
+
+        //find nearest obstacle that has valid tangents to
+        val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
+            compareByDescending<Int> { obstacles[it].intersects(directTangent) } //take intersecting obstacles
+                .thenBy { startCircle.center.distanceTo(obstacles[it].center) } //then nearest
+        ).firstNotNullOf { obstacleIndex ->
+            tangentsFromCircle(startCircle, startDirection, obstacleIndex).values
+                .filter { it.isValid }.takeIf { it.isNotEmpty() }
+        }
+
+        var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
+
+        while (!paths.all { it.isFinished }) {
+            paths = paths.flatMap { it.nextSteps() }
+        }
+        return paths.map { it.toTrajectory() }
+    }
+
+
+    public companion object {
+        private data class LR<T>(val l: T, val r: T) {
+            operator fun get(direction: Trajectory2D.Direction) = when (direction) {
+                Trajectory2D.L -> l
+                Trajectory2D.R -> r
+            }
+        }
+
+        private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> =
+            with(Euclidean2DSpace) {
+                Pair(
+                    r * vector(v.y / norm(v), -v.x / norm(v)),
+                    r * vector(-v.y / norm(v), v.x / norm(v))
+                )
+            }
+
+        private fun constructTangentCircles(
+            pose: DubinsPose2D,
+            r: Double,
+        ): LR<Circle2D> = with(Euclidean2DSpace) {
+            val direction = DubinsPose2D.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: DubinsPose2D,
+            finish: DubinsPose2D,
+            startingRadius: Double,
+            obstacleList: List<Obstacle>,
+            finalRadius: Double = startingRadius,
+        ): List<Trajectory2D> {
+            val obstacles = Obstacles(obstacleList)
+            val initialCircles = constructTangentCircles(
+                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: DubinsPose2D,
+            finish: DubinsPose2D,
+            trajectoryRadius: Double,
+            vararg obstacles: Obstacle,
+        ): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
+
+        public fun avoidPolygons(
+            start: DubinsPose2D,
+            finish: DubinsPose2D,
+            trajectoryRadius: Double,
+            vararg polygons: Polygon<Double>,
+        ): List<Trajectory2D> {
+            val obstacles: List<Obstacle> = polygons.map { polygon ->
+                Obstacle(polygon.points, trajectoryRadius)
+            }
+            return avoidObstacles(start, finish, trajectoryRadius, obstacles)
+        }
+
+
+        public fun avoidPolygons(
+            start: DubinsPose2D,
+            finish: DubinsPose2D,
+            trajectoryRadius: Double,
+            polygons: Collection<Polygon<Double>>,
+        ): List<Trajectory2D> {
+            val obstacles: List<Obstacle> = polygons.map { polygon ->
+                Obstacle(polygon.points, trajectoryRadius)
+            }
+            return avoidObstacles(start, finish, trajectoryRadius, obstacles)
+        }
+    }
+
+}

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

@@ -11,16 +11,24 @@ import kotlinx.serialization.Serializable
 import kotlinx.serialization.UseSerializers
 import space.kscience.kmath.geometry.*
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
+import space.kscience.kmath.geometry.Euclidean2DSpace.minus
 import kotlin.math.atan2
 
 @Serializable
 public sealed interface Trajectory2D {
     public val length: Double
 
+    public val beginPose: DubinsPose2D
+    public val endPose: DubinsPose2D
+
+    /**
+     * Produce a trajectory with reversed order of points
+     */
+    public fun reversed(): Trajectory2D
 
     public sealed interface Type
 
-    public sealed interface Direction: Type
+    public sealed interface Direction : Type
 
     public object R : Direction {
         override fun toString(): String = "R"
@@ -35,6 +43,9 @@ public sealed interface Trajectory2D {
     }
 }
 
+
+public val DoubleVector2D.bearing: Angle get() = (atan2(x, y).radians).normalized()
+
 /**
  * Straight path segment. The order of start and end defines the direction
  */
@@ -47,49 +58,60 @@ public data class StraightTrajectory2D(
 
     override val length: Double get() = begin.distanceTo(end)
 
-    public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized()
+    public val bearing: Angle get() = (end - begin).bearing
+
+    override val beginPose: DubinsPose2D get() = DubinsPose2D(begin, bearing)
+    override val endPose: DubinsPose2D get() = DubinsPose2D(end, bearing)
+
+    override fun reversed(): StraightTrajectory2D = StraightTrajectory2D(end, begin)
 }
 
+public fun StraightTrajectory2D(segment: LineSegment2D): StraightTrajectory2D =
+    StraightTrajectory2D(segment.begin, segment.end)
+
 /**
  * An arc segment
  */
 @Serializable
 @SerialName("arc")
-public data class CircleTrajectory2D(
+public data class CircleTrajectory2D (
     public val circle: Circle2D,
-    public val start: DubinsPose2D,
+    public val begin: DubinsPose2D,
     public val end: DubinsPose2D,
 ) : Trajectory2D {
 
+    override val beginPose: DubinsPose2D get() = begin
+    override val endPose: DubinsPose2D get() = end
+
     /**
      * Arc length in radians
      */
-    val arcLength: Angle
+    val arcAngle: Angle
         get() = if (direction == Trajectory2D.L) {
-            start.bearing - end.bearing
+            begin.bearing - end.bearing
         } else {
-            end.bearing - start.bearing
+            end.bearing - begin.bearing
         }.normalized()
 
 
     override val length: Double by lazy {
-        circle.radius * arcLength.radians
+        circle.radius * arcAngle.radians
     }
 
     public val direction: Trajectory2D.Direction by lazy {
-        if (start.y < circle.center.y) {
+        when {
-            if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
+            begin.y < circle.center.y -> if (begin.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
-        } else if (start.y > circle.center.y) {
+            begin.y > circle.center.y -> if (begin.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
-            if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
+            else -> if (begin.bearing == Angle.zero) {
+                if (begin.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
             } else {
-            if (start.bearing == Angle.zero) {
+                if (begin.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
-                if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
-            } else {
-                if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
             }
         }
     }
 
+    override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, end.reversed(), begin.reversed())
+
     public companion object {
         public fun of(
             center: DoubleVector2D,
@@ -124,8 +146,28 @@ public data class CircleTrajectory2D(
 @SerialName("composite")
 public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
     override val length: Double get() = segments.sumOf { it.length }
+
+    override val beginPose: DubinsPose2D get() = segments.first().beginPose
+    override val endPose: DubinsPose2D get() = segments.last().endPose
+
+    override fun reversed(): CompositeTrajectory2D = CompositeTrajectory2D(segments.map { it.reversed() }.reversed())
 }
 
 public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
     CompositeTrajectory2D(segments.toList())
 
+public fun Euclidean2DSpace.trajectoryIntersects(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 StraightTrajectory2D -> when (b) {
+        is CircleTrajectory2D -> intersects(a, b.circle)
+        is StraightTrajectory2D -> intersects(a, b)
+        is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) }
+    }
+
+    is CompositeTrajectory2D -> a.segments.any { trajectoryIntersects(it, b) }
+}

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

@@ -0,0 +1,120 @@
+package space.kscience.trajectory
+
+import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.trajectory.DubinsPath.Type
+import kotlin.math.*
+
+
+/**
+ * Create inner and outer tangents between two circles.
+ * This method returns a map of segments using [DubinsPath] connection type notation.
+ */
+internal fun tangentsBetweenCircles(
+    first: Circle2D,
+    second: Circle2D,
+): Map<Type, StraightTrajectory2D> = with(Euclidean2DSpace) {
+    // Distance between centers
+    val distanceBetweenCenters: Double = first.center.distanceTo(second.center)
+
+    // return empty map if one circle is inside another
+    val minRadius = min(first.radius, second.radius)
+    val maxRadius = max(first.radius, second.radius)
+
+    val listOfTangents = when {
+        // one circle inside another, no tangents
+        distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap()
+        // circles intersect, only outer tangents
+        distanceBetweenCenters - minRadius <= maxRadius -> listOf(Type.RSR, Type.LSL)
+        // no intersections all tangents
+        else -> listOf(Type.RSR, Type.RSL, Type.LSR, Type.LSL)
+    }
+
+    val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y)
+
+    return listOfTangents.associateWith { route ->
+        val r1 = when (route.first) {
+            Trajectory2D.L -> -first.radius
+            Trajectory2D.R -> first.radius
+        }
+        val r2 = when (route.third) {
+            Trajectory2D.L -> -second.radius
+            Trajectory2D.R -> second.radius
+        }
+        val r = if (r1.sign == r2.sign) {
+            r1.absoluteValue - r2.absoluteValue
+        } else {
+            r1.absoluteValue + r2.absoluteValue
+        }
+
+        val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r)
+        val 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))
+
+        StraightTrajectory2D(
+            first.center + w * r1,
+            second.center + w * r2
+        )
+    }
+}
+
+/**
+ * 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}" }
+    val startCircle = circles[fromIndex]
+    val segments = buildList {
+        val reserve = mutableListOf<Trajectory2D>()
+
+        val sourceSegments = when (direction) {
+            Trajectory2D.L -> circumvention.reversed().segments
+            Trajectory2D.R -> circumvention.segments
+        }
+
+        var i = 0
+        while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) {
+            //put all segments before target circle on the reserve
+            reserve.add(sourceSegments[i])
+            i++
+        }
+
+        while (i < sourceSegments.size) {
+            // put required segments on result list
+            add(sourceSegments[i])
+            i++
+        }
+        //add remaining segments from reserve
+        addAll(reserve)
+        check(i == sourceSegments.size)
+    }
+    return CompositeTrajectory2D(segments)
+}
+
+/**
+ * Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex]
+ * and ending (including) at obstacle node with given [toIndex]
+ */
+public fun Obstacle.circumvention(
+    direction: Trajectory2D.Direction,
+    fromIndex: Int,
+    toIndex: Int,
+): CompositeTrajectory2D {
+    require(toIndex in circles.indices) { "$toIndex is not in ${circles.indices}" }
+    val toCircle = circles[toIndex]
+    val fullCircumvention = circumvention(direction, fromIndex).segments
+    return CompositeTrajectory2D(
+        buildList {
+            var i = 0
+            do {
+                val segment = fullCircumvention[i]
+                add(segment)
+                i++
+            } while ((segment as? CircleTrajectory2D)?.circle != toCircle)
+        }
+    )
+}

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

@@ -1,536 +0,0 @@
-package space.kscience.trajectory
-
-import space.kscience.kmath.geometry.*
-import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
-import space.kscience.kmath.geometry.Euclidean2DSpace.minus
-import space.kscience.kmath.geometry.Euclidean2DSpace.plus
-import space.kscience.kmath.geometry.Euclidean2DSpace.times
-import space.kscience.kmath.misc.zipWithNextCircular
-import space.kscience.kmath.operations.DoubleField.pow
-import kotlin.math.*
-
-
-internal data class ObstacleConnection(
-    val obstacle: Obstacle,
-    val nodeIndex: Int,
-    val direction: Trajectory2D.Direction,
-) {
-    val circle get() = obstacle.circles[nodeIndex]
-}
-
-internal data class ObstacleTangent(
-    val lineSegment: LineSegment2D,
-    val beginNode: ObstacleConnection,
-    val endNode: ObstacleConnection,
-) : LineSegment2D by lineSegment {
-    val startCircle get() = beginNode.circle
-    val startDirection get() = beginNode.direction
-    val endCircle get() = endNode.circle
-    val endDirection get() = endNode.direction
-}
-
-
-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))
-
-/**
- * Create inner and outer tangents between two circles.
- * This method returns a map of segments using [DubinsPath] connection type notation.
- */
-internal fun tangentsBetweenCircles(
-    first: Circle2D,
-    second: Circle2D,
-): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
-    // Distance between centers
-    val distanceBetweenCenters: Double = first.center.distanceTo(second.center)
-
-    // return empty map if one circle is inside another
-    val minRadius = min(first.radius, second.radius)
-    val maxRadius = max(first.radius, second.radius)
-
-    val listOfTangents = when {
-        // one circle inside another, no tangents
-        distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap()
-        // circles intersect, only outer tangents
-        distanceBetweenCenters - minRadius <= maxRadius -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.LSL)
-        // no intersections all tangents
-        else -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.RSL, DubinsPath.Type.LSR, DubinsPath.Type.LSL)
-    }
-
-    val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y)
-
-    return listOfTangents.associateWith { route ->
-        val r1 = when (route.first) {
-            Trajectory2D.L -> -first.radius
-            Trajectory2D.R -> first.radius
-        }
-        val r2 = when (route.third) {
-            Trajectory2D.L -> -second.radius
-            Trajectory2D.R -> second.radius
-        }
-        val r = if (r1.sign == r2.sign) {
-            r1.absoluteValue - r2.absoluteValue
-        } else {
-            r1.absoluteValue + r2.absoluteValue
-        }
-
-        val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r)
-        val 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))
-
-        LineSegment(
-            first.center + w * r1,
-            second.center + w * r2
-        )
-    }
-}
-
-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 =
-        shell.any { tangent -> segment.intersectsSegment(tangent) }
-                || circles.any { circle -> segment.intersectsCircle(circle) }
-
-    override fun intersects(circle: Circle2D): Boolean =
-        shell.any { tangent -> tangent.intersectsCircle(circle) }
-
-    /**
-     * 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 LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean {
-    infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
-    infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
-
-    return with(Euclidean2DSpace) {
-        (other.begin - begin) crossSign (other.end - begin) !=
-                (other.begin - end) crossSign (other.end - end) &&
-                (begin - other.begin) crossSign (end - other.begin) !=
-                (begin - other.end) crossSign (end - other.end)
-    }
-}
-
-private fun LineSegment2D.intersectsCircle(circle: Circle2D): Boolean {
-    val a = (begin.x - end.x).pow(2.0) + (begin.y - end.y).pow(2.0)
-    val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
-            (begin.y - end.y) * (end.y - circle.center.y))
-    val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
-            circle.radius.pow(2.0)
-
-    val aNormalized = a / (a * a + b * b + c * c)
-    val bNormalized = b / (a * a + b * b + c * c)
-    val cNormalized = c / (a * a + b * b + c * c)
-
-    val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
-    if (d < 1e-6) {
-        return false
-    } else {
-        val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
-        val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
-        if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
-            return true
-        }
-    }
-    return false
-}
-
-
-/**
- * All tangents between two obstacles
- *
- * In general generates 4 paths.
- *  TODO check intersections.
- */
-private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, ObstacleTangent> = buildMap {
-
-    for (firstCircleIndex in first.circles.indices) {
-        for (secondCircleIndex in second.circles.indices) {
-            for ((pathType, segment) in tangentsBetweenCircles(
-                first.circles[firstCircleIndex],
-                second.circles[secondCircleIndex]
-            )) {
-                val tangent = ObstacleTangent(
-                    segment,
-                    ObstacleConnection(first, firstCircleIndex, pathType.first),
-                    ObstacleConnection(second, secondCircleIndex, pathType.third)
-                )
-
-                if (!(first.intersects(tangent)) && !(second.intersects(tangent))) {
-                    put(
-                        pathType,
-                        tangent
-                    )
-                }
-            }
-        }
-    }
-}
-
-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 {
-    for (path in paths) {
-        if (path.last().endNode.obstacle !== finalObstacle) {
-            return false
-        }
-    }
-    return true
-}
-
-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 = sortedObstacles(currentObstacle, obstacles).find { obstacle ->
-                            obstacle.intersects(tangentToFinal)
-                        } ?: 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.intersects(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/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt

@@ -3,21 +3,19 @@
  * 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.trajectory.segments
+package space.kscience.kmath.geometry
 
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.circumference
-import space.kscience.kmath.geometry.degrees
 import space.kscience.trajectory.CircleTrajectory2D
+import space.kscience.trajectory.DubinsPose2D
 import space.kscience.trajectory.Trajectory2D
+import kotlin.math.PI
 import kotlin.test.Test
 import kotlin.test.assertEquals
 
 class ArcTests {
 
     @Test
-    fun arcTest() = with(Euclidean2DSpace){
+    fun arc() = with(Euclidean2DSpace) {
         val circle = Circle2D(vector(0.0, 0.0), 2.0)
         val arc = CircleTrajectory2D.of(
             circle.center,
@@ -26,7 +24,19 @@ class ArcTests {
             Trajectory2D.R
         )
         assertEquals(circle.circumference / 4, arc.length, 1.0)
-        assertEquals(0.0, arc.start.bearing.degrees)
+        assertEquals(0.0, arc.begin.bearing.degrees)
         assertEquals(90.0, arc.end.bearing.degrees)
     }
+
+    @Test
+    fun quarter() = with(Euclidean2DSpace) {
+        val circle = circle(1, 0, 1)
+        val arc = CircleTrajectory2D(
+            circle,
+            DubinsPose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
+            DubinsPose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
+        )
+        assertEquals(Trajectory2D.R, arc.direction)
+        assertEquals(PI / 2, arc.length, 1e-4)
+    }
 }

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

@@ -0,0 +1,135 @@
+/*
+ * Copyright 2018-2022 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.geometry
+
+import kotlin.math.pow
+import kotlin.math.sqrt
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertFalse
+import kotlin.test.assertTrue
+
+class CircleTests {
+
+    @Test
+    fun circle() {
+        val center = Euclidean2DSpace.vector(0.0, 0.0)
+        val radius = 2.0
+        val expectedCircumference = 12.56637
+        val circle = Circle2D(center, radius)
+        assertEquals(expectedCircumference, circle.circumference, 1e-4)
+    }
+
+    @Test
+    fun circleIntersection() = with(Euclidean2DSpace) {
+        assertTrue {
+            intersectsOrInside(
+                circle(0.0, 0.0, 1.0),
+                circle(0.0, 0.0, 2.0)
+            )
+        }
+        assertTrue {
+            intersectsOrInside(
+                circle(0.0, 1.0, 1.0),
+                circle(0.0, 0.0, 1.0)
+            )
+        }
+        assertFalse {
+            intersectsOrInside(
+                circle(0.0, 1.0, 1.0),
+                circle(0.0, -1.1, 1.0)
+            )
+        }
+    }
+
+    @Test
+    fun circleLineIntersection() = with(Euclidean2DSpace) {
+        assertTrue {
+            intersects(circle(0, 0, 1), segment(1, 1, -1, 1))
+        }
+
+        assertTrue {
+            intersects(circle(1, 1, sqrt(2.0)/2), segment(1, 0, 0, 1))
+        }
+
+        assertTrue {
+            intersects(circle(1, 1, 1), segment(2, 2, 0, 2))
+        }
+
+        assertTrue {
+            intersects(circle(0, 0, 1), segment(1, -1, 1, 1))
+        }
+
+        assertTrue {
+            intersects(circle(0, 0, 1), segment(1, 0, -1, 0))
+        }
+
+        assertFalse {
+            intersects(circle(0, 0, 1), segment(1, 2, -1, 2))
+        }
+
+        assertFalse {
+            intersects(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0))
+        }
+    }
+
+    private fun Euclidean2DSpace.oldIntersect(circle: Circle2D, segment: LineSegment2D): Boolean{
+        val begin = segment.begin
+        val end = segment.end
+        val lengthSquared = (begin.x - end.x).pow(2) + (begin.y - end.y).pow(2)
+        val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
+                (begin.y - end.y) * (end.y - circle.center.y))
+        val c = (end.x - circle.center.x).pow(2) + (end.y - circle.center.y).pow(2) -
+                circle.radius.pow(2)
+
+        val aNormalized = lengthSquared / (lengthSquared * lengthSquared + b * b + c * c)
+        val bNormalized = b / (lengthSquared * lengthSquared + b * b + c * c)
+        val cNormalized = c / (lengthSquared * lengthSquared + b * b + c * c)
+
+        val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
+        if (d < 1e-6) {
+            return false
+        } else {
+            val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
+            val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
+            if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
+                return true
+            }
+        }
+        return false
+    }
+
+    @Test
+    fun oldCircleLineIntersection() = with(Euclidean2DSpace){
+        assertTrue {
+            oldIntersect(circle(0, 0, 1.1), segment(1, 1, -1, 1))
+        }
+
+        assertTrue {
+            oldIntersect(circle(1, 1, sqrt(2.0)/2+0.01), segment(1, 0, 0, 1))
+        }
+
+        assertTrue {
+            oldIntersect(circle(1, 1, 1.01), segment(2, 2, 0, 2))
+        }
+
+        assertTrue {
+            oldIntersect(circle(0, 0, 1.01), segment(1, -1, 1, 1))
+        }
+
+        assertTrue {
+            oldIntersect(circle(0, 0, 1.0), segment(2, 0, -2, 0))
+        }
+
+        assertFalse {
+            oldIntersect(circle(0, 0, 1), segment(1, 2, -1, 2))
+        }
+
+        assertFalse {
+            oldIntersect(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0))
+        }
+    }
+}

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

@@ -3,10 +3,8 @@
  * 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.trajectory.segments
+package space.kscience.kmath.geometry
 
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.degrees
 import space.kscience.trajectory.StraightTrajectory2D
 import kotlin.math.pow
 import kotlin.math.sqrt

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

@@ -45,16 +45,16 @@ class DubinsTests {
             val b = path.segments[1]
             val c = path.segments[2] as CircleTrajectory2D
 
-            assertTrue(start.equalsFloat(a.start))
+            assertEquals(start, a.begin)
-            assertTrue(end.equalsFloat(c.end))
+            assertEquals(end, c.end)
 
             // Not working, theta double precision inaccuracy
             if (b is CircleTrajectory2D) {
-                assertTrue(a.end.equalsFloat(b.start))
+                assertEquals(a.end, b.begin)
-                assertTrue(c.start.equalsFloat(b.end))
+                assertEquals(c.begin, b.end)
             } else if (b is StraightTrajectory2D) {
-                assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing)))
+                assertEquals(a.end, DubinsPose2D(b.begin, b.bearing))
-                assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
+                assertEquals(c.begin, DubinsPose2D(b.end, b.bearing))
             }
         }
     }

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

@@ -8,38 +8,51 @@ package space.kscience.trajectory
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace.vector
 import space.kscience.kmath.geometry.degrees
+import kotlin.math.PI
 import kotlin.test.Test
 import kotlin.test.assertEquals
 import kotlin.test.assertTrue
 
 class ObstacleTest {
+
     @Test
-    fun firstPath() {
+    fun equalObstacles() {
+        val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
+        val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
+        assertEquals(circle1, circle2)
+        val obstacle1 = Obstacle(circle1)
+        val obstacle2 = Obstacle(circle2)
+        assertEquals(obstacle1, obstacle2)
+    }
+
+    @Test
+    fun singeObstacle() {
         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 outputTangents = Obstacle.avoidObstacles(
+        val outputTangents = Obstacles.avoidObstacles(
             DubinsPose2D(startPoint, startDirection),
             DubinsPose2D(finalPoint, finalDirection),
             startRadius,
             Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
         )
+        assertTrue { outputTangents.isNotEmpty() }
         val length = outputTangents.minOf { it.length }
         assertEquals(27.2113183, length, 1e-6)
     }
 
     @Test
-    fun secondPath() {
+    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 = Obstacle.avoidObstacles(
+        val paths = Obstacles.avoidObstacles(
             DubinsPose2D(startPoint, startDirection),
             DubinsPose2D(finalPoint, finalDirection),
             radius,
@@ -55,19 +68,36 @@ class ObstacleTest {
                 Circle2D(vector(9.0, 4.0), 0.5)
             )
         )
+        assertTrue { paths.isNotEmpty() }
         val length = paths.minOf { it.length }
         assertEquals(28.9678224, length, 1e-6)
     }
 
     @Test
-    fun nearPoints() {
+    fun circumvention(){
+        val obstacle = 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 circumvention = obstacle.circumvention
+
+        assertEquals(4, circumvention.segments.count { it is CircleTrajectory2D })
+
+        assertEquals(4 + 2* PI, circumvention.length, 1e-4)
+    }
+
+    @Test
+    fun closePoints() {
         val startPoint = vector(-1.0, -1.0)
         val startDirection = vector(0.0, 1.0)
         val startRadius = 1.0
         val finalPoint = vector(-1, -1)
         val finalDirection = vector(1.0, 0)
 
-        val paths = Obstacle.avoidObstacles(
+        val paths = Obstacles.avoidObstacles(
             DubinsPose2D(startPoint, startDirection),
             DubinsPose2D(finalPoint, finalDirection),
             startRadius,
@@ -78,14 +108,15 @@ class ObstacleTest {
                 Circle2D(vector(1.0, 0.0), 1.0)
             )
         )
+        assertTrue { paths.isNotEmpty() }
         val length = paths.minOf { it.length }
         println(length)
         //assertEquals(28.9678224, length, 1e-6)
     }
 
     @Test
-    fun fromMap() {
+    fun largeCoordinates() {
-        val paths = Obstacle.avoidObstacles(
+        val paths = Obstacles.avoidObstacles(
             DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
             DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
             5000.0,
@@ -98,34 +129,6 @@ class ObstacleTest {
             )
         )
         assertTrue { paths.isNotEmpty() }
-        val length = paths.minOf { it.length }
-    }
-    @Test
-    fun fromMapLess() {
-
-        val paths = Obstacle.avoidObstacles(
-            DubinsPose2D(x = 48.4149535516561, y = 299.50862534208703, bearing = 3.401475378237137.degrees),
-            DubinsPose2D(x = 45.66638489126448, y = 283.00541087567504, bearing = 325.32183928982727.degrees),
-            0.5,
-            Obstacle(
-                Circle2D(vector(x=44.60882236175772, y=289.52640759535935), radius=0.5),
-                Circle2D(vector(x=45.558751549431164, y=289.71165594902174), radius=0.5),
-                Circle2D(vector(x=46.590308440141426, y=289.3897500160981), radius=0.5),
-                Circle2D(vector(x=46.242119397653354, y=287.94964842121634), radius=0.5),
-                Circle2D(vector(x=44.92318047505464, y=288.0132403305273), radius=0.5)
-            )
-        )
-        val length = paths.minOf { it.length }
-    }
-
-    @Test
-    fun equalObstacles() {
-        val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
-        val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
-        assertEquals(circle1, circle2)
-        val obstacle1 = ObstacleShell(listOf(circle1))
-        val obstacle2 = ObstacleShell(listOf(circle2))
-        assertEquals(obstacle1, obstacle2)
     }
 
 }

trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/CircleTests.kt

@@ -1,24 +0,0 @@
-/*
- * Copyright 2018-2022 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.trajectory.segments
-
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.circumference
-import kotlin.test.Test
-import kotlin.test.assertEquals
-
-class CircleTests {
-
-    @Test
-    fun arcTest() {
-        val center = Euclidean2DSpace.vector(0.0, 0.0)
-        val radius = 2.0
-        val expectedCircumference = 12.56637
-        val circle = Circle2D(center, radius)
-        assertEquals(expectedCircumference, circle.circumference, 1e-4)
-    }
-}

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

@@ -6,13 +6,15 @@
 package space.kscience.trajectory
 
 import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.equalsFloat
 import space.kscience.kmath.geometry.radians
 import space.kscience.kmath.geometry.sin
 
 
-fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
+fun assertEquals(expected: DubinsPose2D, actual: DubinsPose2D, precision: Double = 1e-6){
-    x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians)
+    kotlin.test.assertEquals(expected.x, actual.x, precision)
+    kotlin.test.assertEquals(expected.y, actual.y, precision)
+    kotlin.test.assertEquals(expected.bearing.radians, actual.bearing.radians, precision)
+}
 
 fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)