From 323e8b6872137eb6da1b8c91aff1ce8b4e8d31d7 Mon Sep 17 00:00:00 2001
From: Alexander Nozik <altavir@gmail.com>
Date: Tue, 26 Jul 2022 09:19:04 +0300
Subject: [PATCH 1/2] Code simplification for Dubins path

---
 .../space/kscience/kmath/geometry/Circle2D.kt | 18 ++++
 .../space/kscience/kmath/geometry/Line.kt     |  4 +
 .../kmath/trajectory/dubins/DubinsPath.kt     | 82 +++++++++--------
 ...oryFunctions.kt => trajectoryFunctions.kt} | 41 +++++----
 .../kscience/kmath/trajectory/segments/Arc.kt | 59 ------------
 .../kmath/trajectory/segments/Pose2D.kt       | 21 +++++
 .../kmath/trajectory/segments/Segment.kt      |  5 --
 .../kmath/trajectory/segments/Straight.kt     | 18 ----
 .../kmath/trajectory/segments/Trajectory.kt   | 89 +++++++++++++++++++
 .../trajectory/segments/components/Circle.kt  | 11 ---
 .../trajectory/segments/components/Pose2D.kt  | 13 ---
 .../space/kscience/kmath/trajectory/Math.kt   | 10 +--
 .../kmath/trajectory/dubins/DubinsTests.kt    | 16 ++--
 .../kmath/trajectory/segments/ArcTests.kt     |  7 +-
 .../segments/{components => }/CircleTests.kt  |  6 +-
 .../kmath/trajectory/segments/LineTests.kt    | 12 +--
 16 files changed, 225 insertions(+), 187 deletions(-)
 create mode 100644 kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt
 rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/{TrajectoryFunctions.kt => trajectoryFunctions.kt} (51%)
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
 create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
 create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
 rename kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/{components => }/CircleTests.kt (74%)

diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt
new file mode 100644
index 000000000..8623335b9
--- /dev/null
+++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt
@@ -0,0 +1,18 @@
+/*
+ * Copyright 2018-2021 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.PI
+
+/**
+ * A circle in 2D space
+ */
+public class Circle2D(
+    public val center: Vector2D,
+    public val radius: Double
+)
+
+public val Circle2D.circumference: Double get() = radius * 2 * PI
diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt
index 8c6ccb55e..85bfcdd8f 100644
--- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt
+++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt
@@ -5,6 +5,10 @@
 
 package space.kscience.kmath.geometry
 
+/**
+ * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized,
+ * but its length does not affect line properties
+ */
 public data class Line<out V : Vector>(val base: V, val direction: V)
 
 public typealias Line2D = Line<Vector2D>
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
index ee4f38662..f510e3572 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
@@ -5,91 +5,95 @@
 
 package space.kscience.kmath.trajectory.dubins
 
+import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Arc
-import space.kscience.kmath.trajectory.segments.Segment
-import space.kscience.kmath.trajectory.segments.Straight
-import space.kscience.kmath.trajectory.segments.components.Circle
-import space.kscience.kmath.trajectory.segments.components.Pose2D
+import space.kscience.kmath.trajectory.segments.ArcSegment
+import space.kscience.kmath.trajectory.segments.Pose2D
+import space.kscience.kmath.trajectory.segments.StraightSegment
+import space.kscience.kmath.trajectory.segments.Trajectory
 import kotlin.math.acos
 import kotlin.math.cos
 import kotlin.math.sin
 
 public class DubinsPath(
-    public val a: Arc,
-    public val b: Segment,
-    public val c: Arc,
-) {
+    public val a: ArcSegment,
+    public val b: Trajectory,
+    public val c: ArcSegment,
+) : Trajectory {
 
     public val type: TYPE = TYPE.valueOf(
         arrayOf(
             a.direction.name[0],
-            if (b is Arc) b.direction.name[0] else 'S',
+            if (b is ArcSegment) b.direction.name[0] else 'S',
             c.direction.name[0]
         ).toCharArray().concatToString()
     )
 
-    public val length: Double = a.length + b.length + c.length
+    override val length: Double get() = a.length + b.length + c.length
 
     public enum class TYPE {
         RLR, LRL, RSR, LSL, RSL, LSR
     }
 
     public companion object {
-        public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List<DubinsPath> =
-            listOfNotNull(
-                rlr(start, end, turningRadius),
-                lrl(start, end, turningRadius),
-                rsr(start, end, turningRadius),
-                lsl(start, end, turningRadius),
-                rsl(start, end, turningRadius),
-                lsr(start, end, turningRadius)
-            )
+        public fun all(
+            start: Pose2D,
+            end: Pose2D,
+            turningRadius: Double,
+        ): List<DubinsPath> = listOfNotNull(
+            rlr(start, end, turningRadius),
+            lrl(start, end, turningRadius),
+            rsr(start, end, turningRadius),
+            lsl(start, end, turningRadius),
+            rsl(start, end, turningRadius),
+            lsr(start, end, turningRadius)
+        )
+
         public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
-            all(start, end, turningRadius).minByOrNull { it.length }!!
+            all(start, end, turningRadius).minBy { it.length }
 
         public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
             val c1 = start.getRightCircle(turningRadius)
             val c2 = end.getRightCircle(turningRadius)
-            val centers = Straight(c1.center, c2.center)
+            val centers = StraightSegment(c1.center, c2.center)
             if (centers.length > turningRadius * 4) return null
 
             var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
             var dX = turningRadius * sin(theta)
             var dY = turningRadius * cos(theta)
             val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
-            val e = Circle(p, turningRadius)
+            val e = Circle2D(p, turningRadius)
             val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
             theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
             dX = turningRadius * sin(theta)
             dY = turningRadius * cos(theta)
             val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
-            val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
-            val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
-            val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
+            val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.RIGHT)
+            val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT)
+            val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT)
             return DubinsPath(a1, a2, a3)
         }
 
         public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
             val c1 = start.getLeftCircle(turningRadius)
             val c2 = end.getLeftCircle(turningRadius)
-            val centers = Straight(c1.center, c2.center)
+            val centers = StraightSegment(c1.center, c2.center)
             if (centers.length > turningRadius * 4) return null
 
             var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
             var dX = turningRadius * sin(theta)
             var dY = turningRadius * cos(theta)
             val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
-            val e = Circle(p, turningRadius)
+            val e = Circle2D(p, turningRadius)
             val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
             theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
             dX = turningRadius * sin(theta)
             dY = turningRadius * cos(theta)
             val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
-            val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
-            val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
-            val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
+            val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.LEFT)
+            val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT)
+            val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT)
             return DubinsPath(a1, a2, a3)
         }
 
@@ -97,8 +101,8 @@ public class DubinsPath(
             val c1 = start.getRightCircle(turningRadius)
             val c2 = end.getRightCircle(turningRadius)
             val s = leftOuterTangent(c1, c2)
-            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
-            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
+            val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
+            val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
             return DubinsPath(a1, s, a3)
         }
 
@@ -106,8 +110,8 @@ public class DubinsPath(
             val c1 = start.getLeftCircle(turningRadius)
             val c2 = end.getLeftCircle(turningRadius)
             val s = rightOuterTangent(c1, c2)
-            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
-            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
+            val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
+            val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
             return DubinsPath(a1, s, a3)
         }
 
@@ -117,8 +121,8 @@ public class DubinsPath(
             val s = rightInnerTangent(c1, c2)
             if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
 
-            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
-            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
+            val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
+            val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
             return DubinsPath(a1, s, a3)
         }
 
@@ -128,8 +132,8 @@ public class DubinsPath(
             val s = leftInnerTangent(c1, c2)
             if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
 
-            val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
-            val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
+            val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
+            val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
             return DubinsPath(a1, s, a3)
         }
     }
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
similarity index 51%
rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt
rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
index 547cb99fc..18384d349 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
@@ -5,10 +5,10 @@
 
 package space.kscience.kmath.trajectory.dubins
 
+import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Straight
-import space.kscience.kmath.trajectory.segments.components.Circle
-import space.kscience.kmath.trajectory.segments.components.Pose2D
+import space.kscience.kmath.trajectory.segments.Pose2D
+import space.kscience.kmath.trajectory.segments.StraightSegment
 import kotlin.math.PI
 import kotlin.math.acos
 import kotlin.math.cos
@@ -18,18 +18,19 @@ private enum class SIDE {
     LEFT, RIGHT
 }
 
-internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first
-internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second
-internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle, Circle> {
+internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
+internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
+internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
     val dX = radius * cos(theta)
     val dY = radius * sin(theta)
-    return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius)
+    return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
 }
 
-internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT)
-internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT)
-private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
-    val centers = Straight(a.center, b.center)
+internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT)
+internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT)
+
+private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment {
+    val centers = StraightSegment(a.center, b.center)
     val p1 = when (side) {
         SIDE.LEFT -> Vector2D(
             a.center.x - a.radius * cos(centers.theta),
@@ -40,16 +41,20 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
             a.center.y - a.radius * sin(centers.theta)
         )
     }
-    return Straight(
+    return StraightSegment(
         p1,
         Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
     )
 }
 
-internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT)
-internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT)
-private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? {
-    val centers = Straight(base.center, direction.center)
+internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
+    innerTangent(base, direction, SIDE.LEFT)
+
+internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
+    innerTangent(base, direction, SIDE.RIGHT)
+
+private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? {
+    val centers = StraightSegment(base.center, direction.center)
     if (centers.length < base.radius * 2) return null
     val angle = theta(
         when (side) {
@@ -61,7 +66,7 @@ private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight?
     val dY = base.radius * cos(angle)
     val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
     val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
-    return Straight(p1, p2)
+    return StraightSegment(p1, p2)
 }
 
-internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
+internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
deleted file mode 100644
index 1c02dd952..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
+++ /dev/null
@@ -1,59 +0,0 @@
-package space.kscience.kmath.trajectory.segments
-
-import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.dubins.theta
-import space.kscience.kmath.trajectory.segments.components.Circle
-import space.kscience.kmath.trajectory.segments.components.Pose2D
-import kotlin.math.PI
-
-public data class Arc(
-    public val circle: Circle,
-    public val start: Pose2D,
-    public val end: Pose2D
-) : Segment {
-
-    internal companion object {
-        fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
-            val s1 = Straight(center, start)
-            val s2 = Straight(center, end)
-            val pose1 = calculatePose(start, s1.theta, direction)
-            val pose2 = calculatePose(end, s2.theta, direction)
-            return Arc(Circle(center, s1.length), pose1, pose2)
-        }
-
-        private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
-            Pose2D.of(
-                vector,
-                when (direction) {
-                    Direction.LEFT -> theta(theta - PI / 2)
-                    Direction.RIGHT -> theta(theta + PI / 2)
-                }
-            )
-    }
-
-    internal enum class Direction {
-        LEFT, RIGHT
-    }
-
-    override val length: Double
-        get() {
-            val angle: Double =
-                theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
-            val proportion = angle / (2 * PI)
-            return circle.circumference * proportion
-        }
-
-    internal val direction: Direction
-        get() = if (start.y < circle.center.y) {
-            if (start.theta > PI) Direction.RIGHT else Direction.LEFT
-        } else if (start.y > circle.center.y) {
-            if (start.theta < PI) Direction.RIGHT else Direction.LEFT
-        } else {
-            if (start.theta == 0.0) {
-                if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
-            } else {
-                if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
-            }
-        }
-
-}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
new file mode 100644
index 000000000..3df34c107
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
@@ -0,0 +1,21 @@
+/*
+ * Copyright 2018-2021 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.segments
+
+import space.kscience.kmath.geometry.Vector2D
+
+/**
+ * A [Vector2D] with view direction
+ */
+public data class Pose2D(
+    override val x: Double,
+    override val y: Double,
+    public val theta: Double
+) : Vector2D {
+    public companion object {
+        public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta)
+    }
+}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
deleted file mode 100644
index 8a1d086fc..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
+++ /dev/null
@@ -1,5 +0,0 @@
-package space.kscience.kmath.trajectory.segments
-
-public interface Segment {
-    public val length: Double
-}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
deleted file mode 100644
index 444025d83..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
+++ /dev/null
@@ -1,18 +0,0 @@
-package space.kscience.kmath.trajectory.segments
-
-import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
-import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.dubins.theta
-import kotlin.math.PI
-import kotlin.math.atan2
-
-public data class Straight(
-    internal val start: Vector2D,
-    internal val end: Vector2D
-) : Segment {
-    override val length: Double
-        get() = start.distanceTo(end)
-
-    internal val theta: Double
-        get() = theta(atan2(end.x - start.x, end.y - start.y))
-}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt
new file mode 100644
index 000000000..453eef47f
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt
@@ -0,0 +1,89 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Circle2D
+import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.geometry.circumference
+import space.kscience.kmath.trajectory.dubins.theta
+import kotlin.math.PI
+import kotlin.math.atan2
+
+public interface Trajectory {
+    public val length: Double
+}
+
+/**
+ * Straight path segment. The order of start and end defines the direction
+ */
+public data class StraightSegment(
+    internal val start: Vector2D,
+    internal val end: Vector2D,
+) : Trajectory {
+    override val length: Double get() = start.distanceTo(end)
+
+    internal val theta: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
+}
+
+/**
+ * An arc segment
+ */
+public data class ArcSegment(
+    public val circle: Circle2D,
+    public val start: Pose2D,
+    public val end: Pose2D,
+) : Trajectory {
+
+    public enum class Direction {
+        LEFT, RIGHT
+    }
+
+    override val length: Double by lazy {
+        val angle: Double = theta(
+            if (direction == Direction.LEFT) {
+                start.theta - end.theta
+            } else {
+                end.theta - start.theta
+            }
+        )
+        val proportion = angle / (2 * PI)
+        circle.circumference * proportion
+    }
+
+    internal val direction: Direction by lazy {
+        if (start.y < circle.center.y) {
+            if (start.theta > PI) Direction.RIGHT else Direction.LEFT
+        } else if (start.y > circle.center.y) {
+            if (start.theta < PI) Direction.RIGHT else Direction.LEFT
+        } else {
+            if (start.theta == 0.0) {
+                if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
+            } else {
+                if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
+            }
+        }
+    }
+
+    public companion object {
+        public fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): ArcSegment {
+            fun calculatePose(
+                vector: Vector2D,
+                theta: Double,
+                direction: Direction,
+            ): Pose2D = Pose2D.of(
+                vector,
+                when (direction) {
+                    Direction.LEFT -> theta(theta - PI / 2)
+                    Direction.RIGHT -> theta(theta + PI / 2)
+                }
+            )
+
+            val s1 = StraightSegment(center, start)
+            val s2 = StraightSegment(center, end)
+            val pose1 = calculatePose(start, s1.theta, direction)
+            val pose2 = calculatePose(end, s2.theta, direction)
+            return ArcSegment(Circle2D(center, s1.length), pose1, pose2)
+        }
+    }
+
+}
+
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
deleted file mode 100644
index 946dd8c6e..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
+++ /dev/null
@@ -1,11 +0,0 @@
-package space.kscience.kmath.trajectory.segments.components
-
-import space.kscience.kmath.geometry.Vector2D
-import kotlin.math.PI
-
-public open class Circle(
-    internal val center: Vector2D,
-    internal val radius: Double
-) {
-    internal val circumference = radius * 2 * PI
-}
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
deleted file mode 100644
index c49da3187..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
+++ /dev/null
@@ -1,13 +0,0 @@
-package space.kscience.kmath.trajectory.segments.components
-
-import space.kscience.kmath.geometry.Vector2D
-
-public data class Pose2D(
-    override val x: Double,
-    override val y: Double,
-    public val theta: Double
-) : Vector2D {
-    internal companion object {
-        internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
-    }
-}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
index 92b2f1df9..4f8fda826 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
@@ -1,8 +1,8 @@
 package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Straight
-import space.kscience.kmath.trajectory.segments.components.Pose2D
+import space.kscience.kmath.trajectory.segments.Pose2D
+import space.kscience.kmath.trajectory.segments.StraightSegment
 import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.sin
@@ -14,12 +14,12 @@ fun Double.radiansToDegrees() = this * 180 / PI
 fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
 fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
 
-fun Straight.inverse() = Straight(end, start)
-fun Straight.shift(shift: Int, width: Double): Straight {
+fun StraightSegment.inverse() = StraightSegment(end, start)
+fun StraightSegment.shift(shift: Int, width: Double): StraightSegment {
     val dX = width * sin(inverse().theta)
     val dY = width * sin(theta)
 
-    return Straight(
+    return StraightSegment(
         Vector2D(start.x - dX * shift, start.y - dY * shift),
         Vector2D(end.x - dX * shift, end.y - dY * shift)
     )
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
index 47e6ac2ef..9069f6dd2 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
@@ -10,9 +10,9 @@ import space.kscience.kmath.geometry.Vector2D
 import space.kscience.kmath.trajectory.equalFloat
 import space.kscience.kmath.trajectory.equalsFloat
 import space.kscience.kmath.trajectory.inverse
-import space.kscience.kmath.trajectory.segments.Arc
-import space.kscience.kmath.trajectory.segments.Straight
-import space.kscience.kmath.trajectory.segments.components.Pose2D
+import space.kscience.kmath.trajectory.segments.ArcSegment
+import space.kscience.kmath.trajectory.segments.Pose2D
+import space.kscience.kmath.trajectory.segments.StraightSegment
 import space.kscience.kmath.trajectory.shift
 import kotlin.test.Test
 import kotlin.test.assertNotNull
@@ -23,7 +23,7 @@ class DubinsTests {
 
     @Test
     fun dubinsTest() {
-        val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
+        val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
         val lineP1 = straight.shift(1, 10.0).inverse()
 
         val start = Pose2D.of(straight.end, straight.theta)
@@ -52,12 +52,12 @@ class DubinsTests {
             assertTrue(end.equalsFloat(path.c.end))
 
             // Not working, theta double precision inaccuracy
-            if (path.b is Arc) {
-                val b = path.b as Arc
+            if (path.b is ArcSegment) {
+                val b = path.b as ArcSegment
                 assertTrue(path.a.end.equalsFloat(b.start))
                 assertTrue(path.c.start.equalsFloat(b.end))
-            } else if (path.b is Straight) {
-                val b = path.b as Straight
+            } else if (path.b is StraightSegment) {
+                val b = path.b as StraightSegment
                 assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
                 assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
             }
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
index 5b4ae6d7a..aa8778315 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
@@ -1,8 +1,9 @@
 package space.kscience.kmath.trajectory.segments
 
+import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.geometry.circumference
 import space.kscience.kmath.trajectory.radiansToDegrees
-import space.kscience.kmath.trajectory.segments.components.Circle
 import kotlin.test.Test
 import kotlin.test.assertEquals
 
@@ -10,8 +11,8 @@ class ArcTests {
 
     @Test
     fun arcTest() {
-        val circle = Circle(Vector2D(0.0, 0.0), 2.0)
-        val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
+        val circle = Circle2D(Vector2D(0.0, 0.0), 2.0)
+        val arc = ArcSegment.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), ArcSegment.Direction.RIGHT)
         assertEquals(circle.circumference / 4, arc.length, 1.0)
         assertEquals(0.0, arc.start.theta.radiansToDegrees())
         assertEquals(90.0, arc.end.theta.radiansToDegrees())
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt
similarity index 74%
rename from kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt
rename to kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt
index 6f28885e0..5170c1db5 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt
@@ -3,9 +3,11 @@
  * 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.segments.components
+package space.kscience.kmath.trajectory.segments
 
+import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.geometry.circumference
 import space.kscience.kmath.trajectory.maxFloatDelta
 import kotlin.test.Test
 import kotlin.test.assertEquals
@@ -17,7 +19,7 @@ class CircleTests {
         val center = Vector2D(0.0, 0.0)
         val radius = 2.0
         val expectedCircumference = 12.56637
-        val circle = Circle(center, radius)
+        val circle = Circle2D(center, radius)
         assertEquals(expectedCircumference, circle.circumference, maxFloatDelta)
     }
 }
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
index e8184e178..afc473d79 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
@@ -12,21 +12,21 @@ class LineTests {
 
     @Test
     fun lineTest() {
-        val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
+        val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
         assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
         assertEquals(45.0, straight.theta.radiansToDegrees())
     }
 
     @Test
     fun lineAngleTest() {
-        val zero = Vector2D(0.0, 0.0)
-        val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
+        //val zero = Vector2D(0.0, 0.0)
+        val north = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
         assertEquals(0.0, north.theta.radiansToDegrees())
-        val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
+        val east = StraightSegment(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
         assertEquals(90.0, east.theta.radiansToDegrees())
-        val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
+        val south = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
         assertEquals(180.0, south.theta.radiansToDegrees())
-        val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
+        val west = StraightSegment(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
         assertEquals(270.0, west.theta.radiansToDegrees())
     }
 }

From 137ddb3ade4f57ee11d2fb05defaa68829d93925 Mon Sep 17 00:00:00 2001
From: Alexander Nozik <altavir@gmail.com>
Date: Fri, 29 Jul 2022 14:12:44 +0300
Subject: [PATCH 2/2] Code simplification for Dubins path

---
 .../trajectory/{dubins => }/DubinsPath.kt     | 75 ++++++++++++++++---
 .../space/kscience/kmath/trajectory/Pose2D.kt | 33 ++++++++
 .../trajectory/{segments => }/Trajectory.kt   | 17 +++--
 .../kmath/trajectory/TrajectoryCost.kt        | 14 ++++
 .../trajectory/dubins/trajectoryFunctions.kt  | 72 ------------------
 .../space/kscience/kmath/trajectory/route.kt  | 16 ++++
 .../kmath/trajectory/segments/Pose2D.kt       | 21 ------
 .../space/kscience/kmath/trajectory/Math.kt   |  2 -
 .../kmath/trajectory/dubins/DubinsTests.kt    | 16 ++--
 .../kmath/trajectory/segments/ArcTests.kt     |  1 +
 .../kmath/trajectory/segments/LineTests.kt    |  1 +
 11 files changed, 146 insertions(+), 122 deletions(-)
 rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/{dubins => }/DubinsPath.kt (67%)
 create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt
 rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/{segments => }/Trajectory.kt (85%)
 create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
 create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt
 delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt

diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
similarity index 67%
rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
index f510e3572..134342b33 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
@@ -3,24 +3,79 @@
  * 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.dubins
+package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.ArcSegment
-import space.kscience.kmath.trajectory.segments.Pose2D
-import space.kscience.kmath.trajectory.segments.StraightSegment
-import space.kscience.kmath.trajectory.segments.Trajectory
+import kotlin.math.PI
 import kotlin.math.acos
 import kotlin.math.cos
 import kotlin.math.sin
 
+internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
+
+internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
+
+internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
+    val dX = radius * cos(theta)
+    val dY = radius * sin(theta)
+    return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
+}
+
+internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, ArcSegment.Direction.LEFT)
+
+internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b,
+    ArcSegment.Direction.RIGHT
+)
+
+private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment {
+    val centers = StraightSegment(a.center, b.center)
+    val p1 = when (side) {
+        ArcSegment.Direction.LEFT -> Vector2D(
+            a.center.x - a.radius * cos(centers.theta),
+            a.center.y + a.radius * sin(centers.theta)
+        )
+        ArcSegment.Direction.RIGHT -> Vector2D(
+            a.center.x + a.radius * cos(centers.theta),
+            a.center.y - a.radius * sin(centers.theta)
+        )
+    }
+    return StraightSegment(
+        p1,
+        Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
+    )
+}
+
+internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
+    innerTangent(base, direction, ArcSegment.Direction.LEFT)
+
+internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
+    innerTangent(base, direction, ArcSegment.Direction.RIGHT)
+
+private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? {
+    val centers = StraightSegment(base.center, direction.center)
+    if (centers.length < base.radius * 2) return null
+    val angle = theta(
+        when (side) {
+            ArcSegment.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
+            ArcSegment.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
+        }
+    )
+    val dX = base.radius * sin(angle)
+    val dY = base.radius * cos(angle)
+    val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
+    val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
+    return StraightSegment(p1, p2)
+}
+
+internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
+
 public class DubinsPath(
     public val a: ArcSegment,
     public val b: Trajectory,
     public val c: ArcSegment,
-) : Trajectory {
+) : CompositeTrajectory(listOf(a,b,c)) {
 
     public val type: TYPE = TYPE.valueOf(
         arrayOf(
@@ -30,8 +85,6 @@ public class DubinsPath(
         ).toCharArray().concatToString()
     )
 
-    override val length: Double get() = a.length + b.length + c.length
-
     public enum class TYPE {
         RLR, LRL, RSR, LSL, RSL, LSR
     }
@@ -119,7 +172,7 @@ public class DubinsPath(
             val c1 = start.getRightCircle(turningRadius)
             val c2 = end.getLeftCircle(turningRadius)
             val s = rightInnerTangent(c1, c2)
-            if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
+            if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
 
             val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
             val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
@@ -130,11 +183,11 @@ public class DubinsPath(
             val c1 = start.getLeftCircle(turningRadius)
             val c2 = end.getRightCircle(turningRadius)
             val s = leftInnerTangent(c1, c2)
-            if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
+            if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
 
             val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
             val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
             return DubinsPath(a1, s, a3)
         }
     }
-}
+}
\ No newline at end of file
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt
new file mode 100644
index 000000000..1f7c4a52e
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2018-2021 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.Vector
+import space.kscience.kmath.geometry.Vector2D
+import kotlin.math.atan2
+
+/**
+ * Combination of [Vector] and its view angle
+ */
+public interface Pose2D: Vector2D{
+    public val coordinate: Vector2D
+    public val theta: Double
+}
+
+public class PhaseVector2D(
+    override val coordinate: Vector2D,
+    public val velocity: Vector2D
+): Pose2D, Vector2D by coordinate{
+    override val theta: Double get() = atan2(velocity.y, velocity.x)
+}
+
+internal class Pose2DImpl(
+    override val coordinate: Vector2D,
+    override val theta: Double
+) : Pose2D, Vector2D by coordinate
+
+
+public fun Pose2D(coordinate: Vector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta)
\ No newline at end of file
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt
similarity index 85%
rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt
rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt
index 453eef47f..2e97d43b0 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt
@@ -1,14 +1,18 @@
-package space.kscience.kmath.trajectory.segments
+/*
+ * Copyright 2018-2021 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.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.kmath.geometry.Vector2D
 import space.kscience.kmath.geometry.circumference
-import space.kscience.kmath.trajectory.dubins.theta
 import kotlin.math.PI
 import kotlin.math.atan2
 
-public interface Trajectory {
+public sealed interface Trajectory {
     public val length: Double
 }
 
@@ -69,7 +73,7 @@ public data class ArcSegment(
                 vector: Vector2D,
                 theta: Double,
                 direction: Direction,
-            ): Pose2D = Pose2D.of(
+            ): Pose2D = Pose2D(
                 vector,
                 when (direction) {
                     Direction.LEFT -> theta(theta - PI / 2)
@@ -84,6 +88,9 @@ public data class ArcSegment(
             return ArcSegment(Circle2D(center, s1.length), pose1, pose2)
         }
     }
-
+}
+
+public open class CompositeTrajectory(public val segments: Collection<Trajectory>) : Trajectory {
+    override val length: Double get() = segments.sumOf { it.length }
 }
 
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt
new file mode 100644
index 000000000..170851c43
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt
@@ -0,0 +1,14 @@
+/*
+ * Copyright 2018-2021 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
+
+public fun interface TrajectoryCost {
+    public fun estimate(trajectory: Trajectory): Double
+
+    public companion object{
+        public val length: TrajectoryCost = TrajectoryCost { it.length }
+    }
+}
\ No newline at end of file
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
deleted file mode 100644
index 18384d349..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * Copyright 2018-2021 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.dubins
-
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Pose2D
-import space.kscience.kmath.trajectory.segments.StraightSegment
-import kotlin.math.PI
-import kotlin.math.acos
-import kotlin.math.cos
-import kotlin.math.sin
-
-private enum class SIDE {
-    LEFT, RIGHT
-}
-
-internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
-internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
-internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
-    val dX = radius * cos(theta)
-    val dY = radius * sin(theta)
-    return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
-}
-
-internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT)
-internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT)
-
-private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment {
-    val centers = StraightSegment(a.center, b.center)
-    val p1 = when (side) {
-        SIDE.LEFT -> Vector2D(
-            a.center.x - a.radius * cos(centers.theta),
-            a.center.y + a.radius * sin(centers.theta)
-        )
-        SIDE.RIGHT -> Vector2D(
-            a.center.x + a.radius * cos(centers.theta),
-            a.center.y - a.radius * sin(centers.theta)
-        )
-    }
-    return StraightSegment(
-        p1,
-        Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
-    )
-}
-
-internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
-    innerTangent(base, direction, SIDE.LEFT)
-
-internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
-    innerTangent(base, direction, SIDE.RIGHT)
-
-private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? {
-    val centers = StraightSegment(base.center, direction.center)
-    if (centers.length < base.radius * 2) return null
-    val angle = theta(
-        when (side) {
-            SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
-            SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
-        }
-    )
-    val dX = base.radius * sin(angle)
-    val dY = base.radius * cos(angle)
-    val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
-    val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
-    return StraightSegment(p1, p2)
-}
-
-internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt
new file mode 100644
index 000000000..f67ab124d
--- /dev/null
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt
@@ -0,0 +1,16 @@
+/*
+ * Copyright 2018-2021 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
+
+public fun interface MaxCurvature {
+    public fun compute(startPoint: PhaseVector2D): Double
+}
+
+public fun DubinsPath.Companion.shortest(
+    start: PhaseVector2D,
+    end: PhaseVector2D,
+    computer: MaxCurvature,
+): DubinsPath = shortest(start, end, computer.compute(start))
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
deleted file mode 100644
index 3df34c107..000000000
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * Copyright 2018-2021 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.segments
-
-import space.kscience.kmath.geometry.Vector2D
-
-/**
- * A [Vector2D] with view direction
- */
-public data class Pose2D(
-    override val x: Double,
-    override val y: Double,
-    public val theta: Double
-) : Vector2D {
-    public companion object {
-        public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta)
-    }
-}
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
index 4f8fda826..7ee68bd92 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
@@ -1,8 +1,6 @@
 package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Pose2D
-import space.kscience.kmath.trajectory.segments.StraightSegment
 import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.sin
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
index 9069f6dd2..09375a400 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
@@ -7,13 +7,7 @@ package space.kscience.kmath.trajectory.dubins
 
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.equalFloat
-import space.kscience.kmath.trajectory.equalsFloat
-import space.kscience.kmath.trajectory.inverse
-import space.kscience.kmath.trajectory.segments.ArcSegment
-import space.kscience.kmath.trajectory.segments.Pose2D
-import space.kscience.kmath.trajectory.segments.StraightSegment
-import space.kscience.kmath.trajectory.shift
+import space.kscience.kmath.trajectory.*
 import kotlin.test.Test
 import kotlin.test.assertNotNull
 import kotlin.test.assertTrue
@@ -26,8 +20,8 @@ class DubinsTests {
         val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
         val lineP1 = straight.shift(1, 10.0).inverse()
 
-        val start = Pose2D.of(straight.end, straight.theta)
-        val end = Pose2D.of(lineP1.start, lineP1.theta)
+        val start = Pose2D(straight.end, straight.theta)
+        val end = Pose2D(lineP1.start, lineP1.theta)
         val radius = 2.0
         val dubins = DubinsPath.all(start, end, radius)
 
@@ -58,8 +52,8 @@ class DubinsTests {
                 assertTrue(path.c.start.equalsFloat(b.end))
             } else if (path.b is StraightSegment) {
                 val b = path.b as StraightSegment
-                assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
-                assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
+                assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta)))
+                assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta)))
             }
         }
     }
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
index aa8778315..c66fa201b 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
@@ -3,6 +3,7 @@ package space.kscience.kmath.trajectory.segments
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Vector2D
 import space.kscience.kmath.geometry.circumference
+import space.kscience.kmath.trajectory.ArcSegment
 import space.kscience.kmath.trajectory.radiansToDegrees
 import kotlin.test.Test
 import kotlin.test.assertEquals
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
index afc473d79..11eaa0fb3 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
@@ -2,6 +2,7 @@ package space.kscience.kmath.trajectory.segments
 
 import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.StraightSegment
 import space.kscience.kmath.trajectory.radiansToDegrees
 import kotlin.math.pow
 import kotlin.math.sqrt