Code simplification for Dubins path

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 kotlin.math.PI
-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
 
+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)
         }
     }
-}
+}

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)

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 }
 }
 

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 }
+    }
+}

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt

@@ -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)

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))

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt

@@ -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)
-    }
-}

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

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.*
-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 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 start = Pose2D(straight.end, straight.theta)
-        val end = Pose2D.of(lineP1.start, lineP1.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.a.end.equalsFloat(Pose2D(b.start, b.theta)))
-                assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
+                assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta)))
             }
         }
     }

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

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