Hieraechy for trajectory types

kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt

@@ -9,7 +9,7 @@ import kotlin.jvm.JvmInline
 import kotlin.reflect.KClass
 
 /**
- * A entity that contains a set of features defined by their types
+ * An entity that contains a set of features defined by their types
  */
 public interface Featured<F : Any> {
     public fun <T : F> getFeature(type: FeatureKey<T>): T?

kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt

@@ -0,0 +1,22 @@
+/*
+ * 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.misc
+
+/**
+ * The same as [zipWithNext], but includes link between last and first element
+ */
+public inline fun <T, R> List<T>.zipWithNextCircular(transform: (a: T, b: T) -> R): List<R> {
+    if (isEmpty()) return emptyList()
+    return indices.map { i ->
+        if (i == size - 1) {
+            transform(last(), first())
+        } else {
+            transform(get(i), get(i + 1))
+        }
+    }
+}
+
+public inline fun <T> List<T>.zipWithNextCircular(): List<Pair<T,T>> = zipWithNextCircular { l, r -> l to r  }

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

@@ -7,8 +7,7 @@ package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.*
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
-import space.kscience.kmath.trajectory.Trajectory2D.Type
+import space.kscience.kmath.trajectory.Trajectory2D.*
-import space.kscience.kmath.trajectory.Trajectory2D.Type.*
 import kotlin.math.acos
 
 internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
@@ -21,7 +20,7 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circ
     return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
 }
 
-private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): StraightTrajectory2D =
+private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): StraightTrajectory2D =
     with(Euclidean2DSpace) {
         val centers = StraightTrajectory2D(from.center, to.center)
         val p1 = when (direction) {
@@ -34,8 +33,6 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh
                 from.center.x + from.radius * cos(centers.bearing),
                 from.center.y - from.radius * sin(centers.bearing)
             )
-
-            else -> error("S trajectory type not allowed")
         }
         return StraightTrajectory2D(
             p1,
@@ -47,7 +44,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh
 private fun innerTangent(
     from: Circle2D,
     to: Circle2D,
-    direction: Type,
+    direction: Direction,
 ): StraightTrajectory2D? =
     with(Euclidean2DSpace) {
         val centers = StraightTrajectory2D(from.center, to.center)
@@ -55,7 +52,6 @@ private fun innerTangent(
         val angle = when (direction) {
             L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
             R -> centers.bearing - acos(from.radius * 2 / centers.length).radians
-            else -> error("S trajectory type not allowed")
         }.normalized()
 
         val dX = from.radius * sin(angle)
@@ -70,13 +66,13 @@ private fun innerTangent(
 public object DubinsPath {
 
     public data class Type(
-        public val first: Trajectory2D.Type,
+        public val first: Direction,
         public val second: Trajectory2D.Type,
-        public val third: Trajectory2D.Type,
+        public val third: Direction,
     ) {
         public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
 
-        override fun toString(): String = "${first.name}${second.name}${third.name}"
+        override fun toString(): String = "${first}${second}${third}"
 
         public companion object {
             public val RLR: Type = Type(R, L, R)
@@ -98,7 +94,7 @@ public object DubinsPath {
         val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
         return Type(
             a.direction,
-            if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S,
+            if (b is CircleTrajectory2D) b.direction else S,
             c.direction
         )
     }
@@ -137,9 +133,9 @@ public object DubinsPath {
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
-                val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
+                val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
+                val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
+                val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -154,9 +150,9 @@ public object DubinsPath {
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
-                val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
+                val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
+                val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
+                val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -181,9 +177,9 @@ public object DubinsPath {
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
-                val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
+                val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
+                val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
+                val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -198,9 +194,9 @@ public object DubinsPath {
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
-                val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
+                val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
+                val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
+                val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -211,8 +207,8 @@ public object DubinsPath {
         val c1 = start.getRightCircle(turningRadius)
         val c2 = end.getRightCircle(turningRadius)
         val s = outerTangent(c1, c2, L)
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
@@ -220,8 +216,8 @@ public object DubinsPath {
         val c1 = start.getLeftCircle(turningRadius)
         val c2 = end.getLeftCircle(turningRadius)
         val s = outerTangent(c1, c2, R)
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
@@ -231,8 +227,8 @@ public object DubinsPath {
         val s = innerTangent(c1, c2, R)
         if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
 
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
@@ -242,8 +238,8 @@ public object DubinsPath {
         val s = innerTangent(c1, c2, L)
         if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
 
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }
 }

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

@@ -17,10 +17,21 @@ import kotlin.math.atan2
 public sealed interface Trajectory2D {
     public val length: Double
 
-    public enum class Type {
+
-        R,
+    public sealed interface Type
-        S,
+
-        L
+    public sealed interface Direction: Type
+
+    public object R : Direction {
+        override fun toString(): String = "R"
+    }
+
+    public object S : Type {
+        override fun toString(): String = "L"
+    }
+
+    public object L : Direction {
+        override fun toString(): String = "L"
     }
 }
 
@@ -56,7 +67,7 @@ public data class CircleTrajectory2D(
      * Arc length in radians
      */
     val arcLength: Angle
-        get() = if (direction == Trajectory2D.Type.L) {
+        get() = if (direction == Trajectory2D.L) {
             start.bearing - end.bearing
         } else {
             end.bearing - start.bearing
@@ -67,16 +78,16 @@ public data class CircleTrajectory2D(
         circle.radius * arcLength.radians
     }
 
-    public val direction: Trajectory2D.Type by lazy {
+    public val direction: Trajectory2D.Direction by lazy {
         if (start.y < circle.center.y) {
-            if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
+            if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
         } else if (start.y > circle.center.y) {
-            if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
+            if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
         } else {
             if (start.bearing == Angle.zero) {
-                if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
+                if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
             } else {
-                if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
+                if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
             }
         }
     }
@@ -86,18 +97,17 @@ public data class CircleTrajectory2D(
             center: DoubleVector2D,
             start: DoubleVector2D,
             end: DoubleVector2D,
-            direction: Trajectory2D.Type,
+            direction: Trajectory2D.Direction,
         ): CircleTrajectory2D {
             fun calculatePose(
                 vector: DoubleVector2D,
                 theta: Angle,
-                direction: Trajectory2D.Type,
+                direction: Trajectory2D.Direction,
             ): DubinsPose2D = DubinsPose2D(
                 vector,
                 when (direction) {
-                    Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized()
+                    Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
-                    Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized()
+                    Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
-                    else -> error("S trajectory type is not allowed in circle constructor")
                 }
             )
 

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

@@ -38,7 +38,7 @@ class ObstacleTest {
             obstacles
         )
         val length = outputTangents.minOf { it.length }
-        assertEquals(length, 27.2113183, 1e-6)
+        assertEquals(27.2113183, length, 1e-6)
     }
 
     @Test
@@ -77,7 +77,7 @@ class ObstacleTest {
             obstacles
         )
         val length = paths.minOf { it.length }
-        assertEquals(length, 28.9678224, 1e-6)
+        assertEquals(28.9678224, length, 1e-6)
     }
 
     @Test

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt

@@ -23,7 +23,7 @@ class ArcTests {
             circle.center,
             vector(-2.0, 0.0),
             vector(0.0, 2.0),
-            Trajectory2D.Type.R
+            Trajectory2D.R
         )
         assertEquals(circle.circumference / 4, arc.length, 1.0)
         assertEquals(0.0, arc.start.bearing.degrees)