kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt

@@ -6,6 +6,7 @@
 package space.kscience.kmath.geometry
 
 import kotlinx.serialization.Serializable
+import space.kscience.kmath.operations.DoubleField.pow
 
 /**
  * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized,

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

@@ -0,0 +1,413 @@
+/*
+ * Copyright 2018-2023 KMath contributors.
+ * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
+ */
+
+package space.kscience.kmath.trajectory
+
+import space.kscience.kmath.geometry.*
+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.geometry.Euclidean2DSpace.vector
+import space.kscience.kmath.geometry.Euclidean2DSpace.norm
+import space.kscience.kmath.operations.DoubleField.pow
+import kotlin.math.*
+
+public fun LineSegment2D.length(): Double {
+    return ((end.y - begin.y).pow(2.0) + (end.x - begin.x).pow(2.0)).pow(0.5)
+}
+public class DubinsObstacle(
+    public val circles: List<Circle2D>
+) {
+    public val tangents: List<DubinsTangent> = boundaryTangents().first
+    public val boundaryRoute: DubinsPath.Type = boundaryTangents().second
+    public val center: Vector2D<Double> =
+        vector(this.circles.sumOf{it.center.x} / this.circles.size,
+               this.circles.sumOf{it.center.y} / this.circles.size)
+    private fun boundaryTangents(): Pair<List<DubinsTangent>, DubinsPath.Type> {
+        // outer tangents for a polygon circles can be either lsl or rsr
+
+        fun Circle2D.dubinsTangentsToCircles(
+            other: Circle2D,
+        ): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
+            val line = LineSegment(center, other.center)
+            val d = line.begin.distanceTo(line.end)
+            val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
+            var r: Double
+            var angle2: Double
+            val routes = mapOf(
+                DubinsPath.Type.RSR to Pair(radius, other.radius),
+                DubinsPath.Type.RSL to Pair(radius, -other.radius),
+                DubinsPath.Type.LSR to Pair(-radius, other.radius),
+                DubinsPath.Type.LSL to Pair(-radius, -other.radius)
+            )
+            return buildMap {
+                for ((route, r1r2) in routes) {
+                    val r1 = r1r2.first
+                    val r2 = r1r2.second
+                    r = if (r1.sign == r2.sign) {
+                        r1.absoluteValue - r2.absoluteValue
+                    } else {
+                        r1.absoluteValue + r2.absoluteValue
+                    }
+                    if (d * d > r * r) {
+                        val l = (d * d - r * r).pow(0.5)
+                        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))
+                        put(route, DubinsTangent(Circle2D(center, radius),
+                            other,
+                            this@DubinsObstacle,
+                            this@DubinsObstacle,
+                            LineSegment2D(
+                                center + w * r1,
+                                other.center + w * r2
+                            ),
+                            DubinsPath.toSimpleTypes(route))
+                        )
+                    } else {
+                        throw Exception("Circles should not intersect")
+                    }
+                }
+            }
+        }
+        val firstCircles = this.circles.slice(-this.circles.size..-1)
+        val secondCircles = this.circles.slice(-this.circles.size+1..0)
+        val lslTangents = firstCircles.zip(secondCircles)
+        {a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!}
+        val rsrTangents = firstCircles.zip(secondCircles)
+        {a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.RSR]!!}
+        val center = vector(
+            this.circles.sumOf { it.center.x } / this.circles.size,
+            this.circles.sumOf { it.center.y } / this.circles.size
+        )
+        val lslToCenter = lslTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
+                lslTangents.sumOf { it.lineSegment.end.distanceTo(center) }
+        val rsrToCenter = rsrTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
+                rsrTangents.sumOf { it.lineSegment.end.distanceTo(center) }
+        return if (rsrToCenter >= lslToCenter) {
+            Pair(rsrTangents, DubinsPath.Type.RSR)
+        } else {
+            Pair(lslTangents, DubinsPath.Type.LSL)
+        }
+    }
+
+    public fun nextTangent(circle: Circle2D, route: DubinsPath.Type): DubinsTangent {
+        if (route == this.boundaryRoute) {
+            for (i in this.circles.indices) {
+                if (this.circles[i] == circle) {
+                    return this.tangents[i]
+                }
+            }
+        }
+        else {
+            for (i in this.circles.indices) {
+                if (this.circles[i] == circle) {
+                    return DubinsTangent(this.circles[i],
+                                         this.circles[i-1],
+                                         this,
+                                         this,
+                                         LineSegment2D(this.tangents[i-1].lineSegment.end,
+                                                        this.tangents[i-1].lineSegment.begin),
+                                         DubinsPath.toSimpleTypes(route))
+                }
+            }
+        }
+
+    error("next tangent not found")
+    }
+
+    public fun equals(other: DubinsObstacle): Boolean {
+        return this.circles == other.circles
+    }
+}
+
+public data class DubinsTangent(val startCircle: Circle2D,
+                                val endCircle: Circle2D,
+                                val startObstacle: DubinsObstacle,
+                                val endObstacle: DubinsObstacle,
+                                val lineSegment: LineSegment2D,
+                                val route: PathTypes)
+
+public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
+    fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
+        return v1.x * v2.y - v1.y * v2.x
+    }
+    if (crossProduct(other.begin - this.begin, other.end - this.begin).sign ==
+        crossProduct(other.begin - this.end, other.end - this.end).sign) {
+        return false
+    }
+    if (crossProduct(this.begin - other.begin, this.end - other.begin).sign ==
+        crossProduct(this.begin - other.end, this.end - other.end).sign) {
+        return false
+    }
+    return true
+}
+
+public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
+    val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0)
+    val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) +
+            (this.begin.y - this.end.y) * (this.end.y - circle.center.y))
+    val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) -
+            circle.radius.pow(2.0)
+    val d = b.pow(2.0) - 4 * a * c
+    if (d < 1e-6) {
+        return false
+    }
+    else {
+        val t1 = (-b - d.pow(0.5)) * 0.5 / a
+        val t2 = (-b + d.pow(0.5)) * 0.5 / a
+        if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
+            return true
+        }
+    }
+    return false
+}
+
+public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean {
+    for (tangent in obstacle.tangents) {
+        if (this.lineSegment.intersectSegment(tangent.lineSegment)) {
+            return true
+        }
+    }
+    for (circle in obstacle.circles) {
+        if (this.lineSegment.intersectCircle(circle)) {
+            return true
+        }
+    }
+    return false
+}
+
+public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> {
+    return buildMap {
+        for (circle1 in first.circles) {
+            for (circle2 in second.circles) {
+                for (tangent in dubinsTangentsToCircles(circle1, circle2, first, second)) {
+                    if (!(tangent.value.intersectObstacle(first))
+                        and !(tangent.value.intersectObstacle(second))) {
+                        put(
+                            tangent.key,
+                            tangent.value
+                        )
+                    }
+                }
+            }
+        }
+    }.toMutableMap()
+}
+
+public fun arcLength(circle: Circle2D,
+                     point1: DoubleVector2D,
+                     point2: DoubleVector2D,
+                     route: DubinsPath.SimpleType): 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 (route) {
+        DubinsPath.SimpleType.L -> {
+            angle = if (phi2 >= phi1) {
+                phi2 - phi1
+            } else {
+                2 * PI + phi2 - phi1
+            }
+        }
+        DubinsPath.SimpleType.R -> {
+            angle = if (phi2 >= phi1) {
+                2 * PI - (phi2 - phi1)
+            } else {
+                -(phi2 - phi1)
+            }
+        }
+        DubinsPath.SimpleType.S -> {
+            error("L or R route is expected")
+        }
+    }
+    return circle.radius * angle
+}
+
+public fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
+    return Pair(
+        r * vector(v.y / norm(v), -v.x / norm(v)),
+        r * vector(-v.y / norm(v), v.x / norm(v))
+    )
+}
+
+public fun constructTangentCircles(point: DoubleVector2D,
+                                   direction: DoubleVector2D,
+                                   r: Double): Map<DubinsPath.SimpleType, Circle2D> {
+    val center1 = point + normalVectors(direction, r).first
+    val center2 = point + normalVectors(direction, r).second
+    val p1 = center1 - point
+    val p2 = center2 - point
+    return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) {
+        mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r),
+            DubinsPath.SimpleType.R to Circle2D(center2, r))
+    }
+    else {
+        mapOf(DubinsPath.SimpleType.L to Circle2D(center2, r),
+            DubinsPath.SimpleType.R to Circle2D(center1, r))
+    }
+}
+
+public fun sortedObstacles(currentObstacle: DubinsObstacle,
+                           obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
+    return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed()
+}
+
+public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
+                                    initialRoute: DubinsPath.Type,
+                                    finalCircle: Circle2D,
+                                    obstacle: DubinsObstacle): MutableList<DubinsTangent> {
+    val dubinsTangents = mutableListOf<DubinsTangent>()
+    var tangent = obstacle.nextTangent(initialCircle, initialRoute)
+    dubinsTangents.add(tangent)
+    while (tangent.endCircle != finalCircle) {
+        tangent = obstacle.nextTangent(tangent.endCircle, initialRoute)
+        dubinsTangents.add(tangent)
+    }
+    return dubinsTangents
+}
+
+public fun allFinished(paths: List<List<DubinsTangent>>,
+                       finalObstacle: DubinsObstacle): Boolean {
+    for (path in paths) {
+        if (path[-1].endObstacle != finalObstacle) {
+            return false
+        }
+    }
+    return true
+}
+
+public fun pathLength(path: List<DubinsTangent>): Double {
+    val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)}
+    val arcsLength = buildList<Double>{
+        for (i in 1..path.size) {
+            add(arcLength(path[i].startCircle,
+                path[i-1].lineSegment.end,
+                path[i].lineSegment.begin,
+                path[i].route[0]))
+        }
+    }.sum()
+    return tangentsLength + arcsLength
+}
+
+public fun shortestPath(path: List<List<DubinsTangent>>): List<List<DubinsTangent>> {
+    return path.sortedBy { pathLength(it) }
+}
+
+public typealias Path = List<DubinsTangent>
+public fun findAllPaths(
+    startingPoint: DoubleVector2D,
+    startingDirection: DoubleVector2D,
+    startingRadius: Double,
+    finalPoint: DoubleVector2D,
+    finalDirection: DoubleVector2D,
+    finalRadius: Double,
+    obstacles: List<DubinsObstacle>
+) {
+    val initialCircles = constructTangentCircles(
+        startingPoint,
+        startingDirection,
+        startingRadius)
+    val finalCircles = constructTangentCircles(
+        finalPoint,
+        finalDirection,
+        finalRadius)
+    var outputTangents = mutableMapOf<PathTypes, List<Path>>()
+    for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
+        for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
+            val finalCircle = finalCircles[j]!!
+            val finalObstacle = DubinsObstacle(listOf(finalCircle))
+            outputTangents[listOf(i,
+                           DubinsPath.SimpleType.S,
+                           j)] = listOf(
+                                    listOf(DubinsTangent(
+                                        initialCircles[i]!!,
+                                        initialCircles[i]!!,
+                                        DubinsObstacle(listOf(initialCircles[i]!!)),
+                                        DubinsObstacle(listOf(initialCircles[i]!!)),
+                                        LineSegment2D(startingPoint, startingPoint),
+                                        listOf(i, DubinsPath.SimpleType.S, i)
+                                    )))
+            var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
+            while (!allFinished(outputTangents[listOf(i,
+                    DubinsPath.SimpleType.S,
+                    j)]!!, finalObstacle)) {
+                var newOutputTangents = listOf<Path>()
+                for (line in outputTangents[listOf(i,
+                    DubinsPath.SimpleType.S,
+                    j)]!!) {
+                    var currentCircle = line[-1].endCircle
+                    var currentDirection = line[-1].route[-1]
+                    var currentObstacle = line[-1].endObstacle
+                    var nextObstacle = DubinsObstacle(listOf())
+                    if (currentObstacle != finalObstacle) {
+                        var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf(
+                            currentDirection,
+                            DubinsPath.SimpleType.S,
+                            j)
+                        )]
+                        for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
+                            if (tangentToFinal!!.intersectObstacle(obstacle)) {
+                                nextObstacle = obstacle
+                                break
+                            }
+                        }
+                        if (nextObstacle == DubinsObstacle(listOf())) {
+                            nextObstacle = finalObstacle
+                        }
+                        var nextTangents = outerTangents(currentObstacle, nextObstacle)
+//                        for (pathType in listOf(
+//                            listOf(DubinsPath.SimpleType.L,
+//                                DubinsPath.SimpleType.S,
+//                                DubinsPath.SimpleType.L),
+//                            listOf(DubinsPath.SimpleType.L,
+//                                DubinsPath.SimpleType.S,
+//                                DubinsPath.SimpleType.R),
+//                            listOf(DubinsPath.SimpleType.R,
+//                                DubinsPath.SimpleType.S,
+//                                DubinsPath.SimpleType.L),
+//                            listOf(DubinsPath.SimpleType.R,
+//                                DubinsPath.SimpleType.S,
+//                                DubinsPath.SimpleType.R)
+//                        )) {
+                        for (pathType in nextTangents.keys) {
+                            for (obstacle in obstacles) {
+                                // in Python code here try/except was used, but seems unneeded
+                                if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
+                                    nextTangents.remove(pathType)
+                                }
+
+                            }
+                        }
+                        if (nextObstacle == finalObstacle) {
+                            nextTangents =
+                                nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
+                                    (DubinsPath.toSimpleTypes(it.key)[0] == j)}
+                                    as MutableMap<DubinsPath.Type, DubinsTangent>
+                        }
+                        else {
+                            nextTangents =
+                                nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
+                                        as MutableMap<DubinsPath.Type, DubinsTangent>
+                        }
+                        TODO("rewrite fragment from Python")
+                    }
+                }
+            }
+        }
+    }
+}
+
+
+
+
+
+
+
+

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

@@ -77,10 +77,118 @@ private fun innerTangent(
 @Suppress("DuplicatedCode")
 public object DubinsPath {
 
+//    public class ArcType(private val type: Type){
+//        public val first: SimpleType
+//            get() {
+//                if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) {
+//                    return SimpleType.R
+//                }
+//                else if (type in listOf(Type.LSL, Type.LSR, Type.LRL)) {
+//                    return SimpleType.L
+//                }
+//                error("Wrong DubinsPath.Type")
+//            }
+//
+//        public val last: SimpleType
+//            get() {
+//                if (type in listOf(Type.RSR, Type.LSR, Type.RLR)) {
+//                    return SimpleType.R
+//                }
+//                else if (type in listOf(Type.LSL, Type.RSL, Type.LRL)) {
+//                    return SimpleType.L
+//                }
+//                error("Wrong DubinsPath.Type")
+//            }
+//        public val intermediate: SimpleType
+//            get() {
+//                if (type == Type.RLR) {
+//                    return SimpleType.L
+//                }
+//                else if (type == Type.LRL) {
+//                    return SimpleType.R
+//                }
+//                error("This DubinsPath.Type doesn't contain intermediate arc")
+//            }
+//    }
+
+    public enum class SimpleType {
+        R, S, L
+    }
+
     public enum class Type {
         RLR, LRL, RSR, LSL, RSL, LSR
     }
 
+    public fun toSimpleTypes(type: Type): List<SimpleType> {
+        when (type) {
+            Type.RLR -> {
+                return listOf(SimpleType.R, SimpleType.L, SimpleType.R)
+            }
+            Type.LRL -> {
+                return listOf(SimpleType.L, SimpleType.R, SimpleType.L)
+            }
+            Type.RSR -> {
+                return listOf(SimpleType.R, SimpleType.S, SimpleType.R)
+            }
+            Type.LSL -> {
+                return listOf(SimpleType.L, SimpleType.S, SimpleType.L)
+            }
+            Type.RSL -> {
+                return listOf(SimpleType.R, SimpleType.S, SimpleType.L)
+            }
+            Type.LSR -> {
+                return listOf(SimpleType.L, SimpleType.S, SimpleType.R)
+            }
+            else -> error("This type doesn't exist")
+        }
+    }
+
+    public fun toType(types: List<SimpleType>): Type {
+        when (types) {
+            listOf(SimpleType.R, SimpleType.L, SimpleType.R) -> {
+                return Type.RLR
+            }
+            listOf(SimpleType.L, SimpleType.R, SimpleType.L) -> {
+                return Type.LRL
+            }
+            listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
+                return Type.RSR
+            }
+            listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
+                return Type.LSL
+            }
+            listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
+                return Type.RSL
+            }
+            listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
+                return Type.LSR
+            }
+            else -> error("This type doesn't exist")
+        }
+    }
+
+//    public class PathTypes(private val inputTypes: List<SimpleType>) {
+//        public val type: Type
+//            get() {
+//                when (this.inputTypes) {
+//                    listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
+//                        return Type.RSR
+//                    }
+//                    listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
+//                        return Type.RSL
+//                    }
+//                    listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
+//                        return Type.LSR
+//                    }
+//                    listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
+//                        return Type.LSL
+//                    }
+//                    else -> error("Wrong list of SimpleTypes")
+//                }
+//            }
+//        public val chain: List<SimpleType> = this.inputTypes
+//    }
+
     /**
      * Return Dubins trajectory type or null if trajectory is not a Dubins path
      */
@@ -243,6 +351,8 @@ public object DubinsPath {
     }
 }
 
+public typealias PathTypes = List<DubinsPath.SimpleType>
+
 public fun interface MaxCurvature {
     public fun compute(startPoint: PhaseVector2D): Double
 }

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

@@ -5,10 +5,10 @@
 
 package space.kscience.kmath.trajectory
 
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.DoubleVector2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.LineSegment
+import space.kscience.kmath.geometry.*
+import space.kscience.kmath.geometry.Euclidean2DSpace.plus
+import space.kscience.kmath.geometry.Euclidean2DSpace.times
+import space.kscience.kmath.operations.DoubleField.pow
 import kotlin.math.*
 
 /**
@@ -17,7 +17,7 @@ import kotlin.math.*
  */
 public fun Circle2D.tangentsToCircle(
     other: Circle2D,
-): Map<DubinsPath.Type, LineSegment<DoubleVector2D>> = with(Euclidean2DSpace) {
+): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
     val line = LineSegment(center, other.center)
     val d = line.begin.distanceTo(line.end)
     val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
@@ -48,14 +48,66 @@ public fun Circle2D.tangentsToCircle(
                 val w = vector(-cos(angle2), sin(angle2))
                 put(
                     route,
-                    LineSegment(
+                    LineSegment2D(
                         center + w * r1,
                         other.center + w * r2
                     )
                 )
             }
             else {
-                throw Exception("Circles should not be")
+                throw Exception("Circles should not intersect")
+            }
+        }
+    }
+}
+
+public fun dubinsTangentsToCircles(
+    firstCircle: Circle2D,
+    secondCircle: Circle2D,
+    firstObstacle: DubinsObstacle,
+    secondObstacle: DubinsObstacle
+): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
+    val line = LineSegment(firstCircle.center, secondCircle.center)
+    val d = line.begin.distanceTo(line.end)
+    val angle1 = atan2(secondCircle.center.x - firstCircle.center.x,
+        secondCircle.center.y - firstCircle.center.y)
+    var r: Double
+    var angle2: Double
+    val routes = mapOf(
+        DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
+        DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
+        DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
+        DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
+    )
+    return buildMap {
+        for ((route, r1r2) in routes) {
+            val r1 = r1r2.first
+            val r2 = r1r2.second
+            r = if (r1.sign == r2.sign) {
+                r1.absoluteValue - r2.absoluteValue
+            } else {
+                r1.absoluteValue + r2.absoluteValue
+            }
+            if (d * d > r * r) {
+                val l = (d * d - r * r).pow(0.5)
+                angle2 = if (r1.absoluteValue > r2.absoluteValue) {
+                    angle1 + r1.sign * atan2(r.absoluteValue, l)
+                } else {
+                    angle1 - r2.sign * atan2(r.absoluteValue, l)
+                }
+                val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
+                put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
+                    secondCircle,
+                    firstObstacle,
+                    secondObstacle,
+                    LineSegment2D(
+                        firstCircle.center + w * r1,
+                        secondCircle.center + w * r2
+                    ),
+                    DubinsPath.toSimpleTypes(route))
+                )
+            } else {
+                throw Exception("Circles should not intersect")
             }
         }
     }