Refactor trajectory

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

@@ -5,13 +5,15 @@
 
 package space.kscience.kmath.geometry
 
+import kotlinx.serialization.Serializable
 import kotlin.math.PI
 
 /**
  * A circle in 2D space
  */
+@Serializable
 public data class Circle2D(
-    public val center: DoubleVector2D,
+    @Serializable(Euclidean2DSpace.VectorSerializer::class) public val center: DoubleVector2D,
     public val radius: Double
 )
 

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

@@ -5,6 +5,12 @@
 
 package space.kscience.kmath.geometry
 
+import kotlinx.serialization.KSerializer
+import kotlinx.serialization.SerialName
+import kotlinx.serialization.Serializable
+import kotlinx.serialization.descriptors.SerialDescriptor
+import kotlinx.serialization.encoding.Decoder
+import kotlinx.serialization.encoding.Encoder
 import space.kscience.kmath.linear.Point
 import space.kscience.kmath.operations.Norm
 import space.kscience.kmath.operations.ScaleOperations
@@ -33,6 +39,7 @@ public operator fun <T> Vector2D<T>.component1(): T = x
 public operator fun <T> Vector2D<T>.component2(): T = y
 
 public typealias DoubleVector2D = Vector2D<Double>
+public typealias Float64Vector2D = Vector2D<Double>
 
 public val Vector2D<Double>.r: Double get() = Euclidean2DSpace.norm(this)
 
@@ -44,11 +51,25 @@ public object Euclidean2DSpace : GeometrySpace<DoubleVector2D>,
     ScaleOperations<DoubleVector2D>,
     Norm<DoubleVector2D, Double> {
 
+    @Serializable
+    @SerialName("Float64Vector2D")
     private data class Vector2DImpl(
         override val x: Double,
         override val y: Double,
     ) : DoubleVector2D
 
+    public object VectorSerializer : KSerializer<DoubleVector2D> {
+        private val proxySerializer = Vector2DImpl.serializer()
+        override val descriptor: SerialDescriptor get() = proxySerializer.descriptor
+
+        override fun deserialize(decoder: Decoder): DoubleVector2D = decoder.decodeSerializableValue(proxySerializer)
+
+        override fun serialize(encoder: Encoder, value: DoubleVector2D) {
+            val vector = value as? Vector2DImpl ?: Vector2DImpl(value.x, value.y)
+            encoder.encodeSerializableValue(proxySerializer, vector)
+        }
+    }
+
     public fun vector(x: Number, y: Number): DoubleVector2D = Vector2DImpl(x.toDouble(), y.toDouble())
 
     override val zero: DoubleVector2D by lazy { vector(0.0, 0.0) }

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

@@ -5,6 +5,12 @@
 
 package space.kscience.kmath.geometry
 
+import kotlinx.serialization.KSerializer
+import kotlinx.serialization.SerialName
+import kotlinx.serialization.Serializable
+import kotlinx.serialization.descriptors.SerialDescriptor
+import kotlinx.serialization.encoding.Decoder
+import kotlinx.serialization.encoding.Encoder
 import space.kscience.kmath.linear.Point
 import space.kscience.kmath.operations.Norm
 import space.kscience.kmath.operations.ScaleOperations
@@ -45,17 +51,33 @@ public fun <T> Buffer<T>.asVector3D(): Vector3D<T> = object : Vector3D<T> {
 }
 
 public typealias DoubleVector3D = Vector3D<Double>
+public typealias Float64Vector3D = Vector3D<Double>
 
 public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this)
 
 public object Euclidean3DSpace : GeometrySpace<DoubleVector3D>, ScaleOperations<DoubleVector3D>,
     Norm<DoubleVector3D, Double> {
+
+    @Serializable
+    @SerialName("Float64Vector3D")
     private data class Vector3DImpl(
         override val x: Double,
         override val y: Double,
         override val z: Double,
     ) : DoubleVector3D
 
+    public object VectorSerializer : KSerializer<DoubleVector3D> {
+        private val proxySerializer = Vector3DImpl.serializer()
+        override val descriptor: SerialDescriptor get() = proxySerializer.descriptor
+
+        override fun deserialize(decoder: Decoder): DoubleVector3D = decoder.decodeSerializableValue(proxySerializer)
+
+        override fun serialize(encoder: Encoder, value: DoubleVector3D) {
+            val vector = value as? Vector3DImpl ?: Vector3DImpl(value.x, value.y, value.z)
+            encoder.encodeSerializableValue(proxySerializer, vector)
+        }
+    }
+
     public fun vector(x: Number, y: Number, z: Number): DoubleVector3D =
         Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble())
 

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

@@ -5,10 +5,13 @@
 
 package space.kscience.kmath.geometry
 
+import kotlinx.serialization.Serializable
+
 /**
  * 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
  */
+@Serializable
 public data class Line<out V : Vector>(val base: V, val direction: V)
 
 public typealias Line2D = Line<DoubleVector2D>
@@ -17,4 +20,12 @@ public typealias Line3D = Line<DoubleVector3D>
 /**
  * A directed line segment between [begin] and [end]
  */
+@Serializable
 public data class LineSegment<out V : Vector>(val begin: V, val end: V)
+
+public fun <V : Vector> LineSegment<V>.line(algebra: GeometrySpace<V>): Line<V> = with(algebra) {
+    Line(begin, end - begin)
+}
+
+public typealias LineSegment2D = LineSegment<DoubleVector2D>
+public typealias LineSegment3D = LineSegment<DoubleVector3D>

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

@@ -57,7 +57,11 @@ internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTraj
 internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
     innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT)
 
-private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D? =
+private fun innerTangent(
+    base: Circle2D,
+    direction: Circle2D,
+    side: CircleTrajectory2D.Direction,
+): StraightTrajectory2D? =
     with(Euclidean2DSpace) {
         val centers = StraightTrajectory2D(base.center, direction.center)
         if (centers.length < base.radius * 2) return null
@@ -76,43 +80,49 @@ private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajec
 
 internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
 
+
 @Suppress("DuplicatedCode")
-public class DubinsPath(
+public object DubinsPath {
-    public val a: CircleTrajectory2D,
-    public val b: Trajectory2D,
-    public val c: CircleTrajectory2D,
-) : CompositeTrajectory2D(listOf(a, b, c)) {
 
-    public val type: TYPE = TYPE.valueOf(
+    public enum class Type {
-        arrayOf(
-            a.direction.name[0],
-            if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
-            c.direction.name[0]
-        ).toCharArray().concatToString()
-    )
-
-    public enum class TYPE {
         RLR, LRL, RSR, LSL, RSL, LSR
     }
 
-    public companion object {
+    /**
-        public fun all(
+     * Return Dubins trajectory type or null if trajectory is not a Dubins path
-            start: DubinsPose2D,
+     */
-            end: DubinsPose2D,
+    public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type?{
-            turningRadius: Double,
+        if(trajectory2D.segments.size != 3) return null
-        ): List<DubinsPath> = listOfNotNull(
+        val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
-            rlr(start, end, turningRadius),
+        val b = trajectory2D.segments[1]
-            lrl(start, end, turningRadius),
+        val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
-            rsr(start, end, turningRadius),
+        return Type.valueOf(
-            lsl(start, end, turningRadius),
+            arrayOf(
-            rsl(start, end, turningRadius),
+                a.direction.name[0],
-            lsr(start, end, turningRadius)
+                if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
+                c.direction.name[0]
+            ).toCharArray().concatToString()
         )
+    }
 
-        public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath =
+    public fun all(
-            all(start, end, turningRadius).minBy { it.length }
+        start: DubinsPose2D,
+        end: DubinsPose2D,
+        turningRadius: Double,
+    ): List<CompositeTrajectory2D> = 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 rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
+    public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
+        all(start, end, turningRadius).minBy { it.length }
+
+    public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
+        with(Euclidean2DSpace) {
             val c1 = start.getRightCircle(turningRadius)
             val c2 = end.getRightCircle(turningRadius)
             val centers = StraightTrajectory2D(c1.center, c2.center)
@@ -132,7 +142,7 @@ public class DubinsPath(
                 val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
                 val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
                 val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
-                DubinsPath(a1, a2, a3)
+                CompositeTrajectory2D(a1, a2, a3)
             }
 
             val secondVariant = run {
@@ -149,13 +159,14 @@ public class DubinsPath(
                 val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
                 val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
                 val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
-                DubinsPath(a1, a2, a3)
+                CompositeTrajectory2D(a1, a2, a3)
             }
 
             return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
         }
 
-        public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
+    public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
+        with(Euclidean2DSpace) {
             val c1 = start.getLeftCircle(turningRadius)
             val c2 = end.getLeftCircle(turningRadius)
             val centers = StraightTrajectory2D(c1.center, c2.center)
@@ -175,10 +186,10 @@ public class DubinsPath(
                 val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
                 val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
                 val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
-                DubinsPath(a1, a2, a3)
+                CompositeTrajectory2D(a1, a2, a3)
             }
 
-            val secondVariant = run{
+            val secondVariant = run {
                 var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
                 var dX = turningRadius * sin(theta)
                 var dY = turningRadius * cos(theta)
@@ -192,50 +203,60 @@ public class DubinsPath(
                 val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
                 val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
                 val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
-                DubinsPath(a1, a2, a3)
+                CompositeTrajectory2D(a1, a2, a3)
             }
 
             return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
         }
 
-        public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath {
+    public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
-            val c1 = start.getRightCircle(turningRadius)
+        val c1 = start.getRightCircle(turningRadius)
-            val c2 = end.getRightCircle(turningRadius)
+        val c2 = end.getRightCircle(turningRadius)
-            val s = leftOuterTangent(c1, c2)
+        val s = leftOuterTangent(c1, c2)
-            val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
-            val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
-            return DubinsPath(a1, s, a3)
+        return CompositeTrajectory2D(a1, s, a3)
-        }
-
-        public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath {
-            val c1 = start.getLeftCircle(turningRadius)
-            val c2 = end.getLeftCircle(turningRadius)
-            val s = rightOuterTangent(c1, c2)
-            val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
-            val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
-            return DubinsPath(a1, s, a3)
-        }
-
-        public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? {
-            val c1 = start.getRightCircle(turningRadius)
-            val c2 = end.getLeftCircle(turningRadius)
-            val s = rightInnerTangent(c1, c2)
-            if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
-
-            val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
-            val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
-            return DubinsPath(a1, s, a3)
-        }
-
-        public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? {
-            val c1 = start.getLeftCircle(turningRadius)
-            val c2 = end.getRightCircle(turningRadius)
-            val s = leftInnerTangent(c1, c2)
-            if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
-
-            val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
-            val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
-            return DubinsPath(a1, s, a3)
-        }
     }
-}
+
+    public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
+        val c1 = start.getLeftCircle(turningRadius)
+        val c2 = end.getLeftCircle(turningRadius)
+        val s = rightOuterTangent(c1, c2)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
+        return CompositeTrajectory2D(a1, s, a3)
+    }
+
+    public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
+        val c1 = start.getRightCircle(turningRadius)
+        val c2 = end.getLeftCircle(turningRadius)
+        val s = rightInnerTangent(c1, c2)
+        if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
+
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
+        return CompositeTrajectory2D(a1, s, a3)
+    }
+
+    public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
+        val c1 = start.getLeftCircle(turningRadius)
+        val c2 = end.getRightCircle(turningRadius)
+        val s = leftInnerTangent(c1, c2)
+        if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
+
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
+        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
+        return CompositeTrajectory2D(a1, s, a3)
+    }
+}
+
+public fun interface MaxCurvature {
+    public fun compute(startPoint: PhaseVector2D): Double
+}
+
+public fun DubinsPath.shortest(
+    start: PhaseVector2D,
+    end: PhaseVector2D,
+    maxCurvature: MaxCurvature,
+): CompositeTrajectory2D = shortest(start, end, maxCurvature.compute(start))
+

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

@@ -2,35 +2,62 @@
  * Copyright 2018-2022 KMath contributors.
  * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
  */
-
+@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
 package space.kscience.kmath.trajectory
 
+import kotlinx.serialization.KSerializer
+import kotlinx.serialization.SerialName
+import kotlinx.serialization.Serializable
+import kotlinx.serialization.UseSerializers
+import kotlinx.serialization.descriptors.SerialDescriptor
+import kotlinx.serialization.encoding.Decoder
+import kotlinx.serialization.encoding.Encoder
 import space.kscience.kmath.geometry.DoubleVector2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.kmath.geometry.Vector
 import kotlin.math.atan2
 
 /**
  * Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
  */
+@Serializable(DubinsPose2DSerializer::class)
 public interface DubinsPose2D : DoubleVector2D {
-    public val coordinate: DoubleVector2D
+    public val coordinates: DoubleVector2D
     public val bearing: Double
 }
 
+@Serializable
 public class PhaseVector2D(
-    override val coordinate: DoubleVector2D,
+    override val coordinates: DoubleVector2D,
     public val velocity: DoubleVector2D,
-) : DubinsPose2D, DoubleVector2D by coordinate {
+) : DubinsPose2D, DoubleVector2D by coordinates {
     override val bearing: Double get() = atan2(velocity.x, velocity.y)
 }
 
+@Serializable
+@SerialName("DubinsPose2D")
 private class DubinsPose2DImpl(
-    override val coordinate: DoubleVector2D,
+    override val coordinates: DoubleVector2D,
     override val bearing: Double,
-) : DubinsPose2D, DoubleVector2D by coordinate{
+) : DubinsPose2D, DoubleVector2D by coordinates{
 
-    override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
+    override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
 }
 
+public object DubinsPose2DSerializer: KSerializer<DubinsPose2D>{
+    private val proxySerializer = DubinsPose2DImpl.serializer()
+
+    override val descriptor: SerialDescriptor
+        get() = proxySerializer.descriptor
+
+    override fun deserialize(decoder: Decoder): DubinsPose2D {
+        return decoder.decodeSerializableValue(proxySerializer)
+    }
+
+    override fun serialize(encoder: Encoder, value: DubinsPose2D) {
+        val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing)
+        encoder.encodeSerializableValue(proxySerializer, pose)
+    }
+}
 
 public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)

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

@@ -2,15 +2,19 @@
  * Copyright 2018-2022 KMath contributors.
  * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
  */
-
+@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
 package space.kscience.kmath.trajectory
 
+import kotlinx.serialization.Serializable
+import kotlinx.serialization.UseSerializers
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.DoubleVector2D
+import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import kotlin.math.PI
 import kotlin.math.atan2
 
+@Serializable
 public sealed interface Trajectory2D {
     public val length: Double
 }
@@ -18,6 +22,7 @@ public sealed interface Trajectory2D {
 /**
  * Straight path segment. The order of start and end defines the direction
  */
+@Serializable
 public data class StraightTrajectory2D(
     public val start: DoubleVector2D,
     public val end: DoubleVector2D,
@@ -30,6 +35,7 @@ public data class StraightTrajectory2D(
 /**
  * An arc segment
  */
+@Serializable
 public data class CircleTrajectory2D(
     public val circle: Circle2D,
     public val start: DubinsPose2D,
@@ -102,7 +108,10 @@ public data class CircleTrajectory2D(
     }
 }
 
-public open class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
+@Serializable
+public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
     override val length: Double get() = segments.sumOf { it.length }
 }
 
+public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList())
+

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

@@ -1,16 +0,0 @@
-/*
- * Copyright 2018-2022 KMath contributors.
- * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
- */
-
-package space.kscience.kmath.trajectory
-
-public fun interface MaxCurvature {
-    public fun compute(startPoint: PhaseVector2D): Double
-}
-
-public fun DubinsPath.Companion.shortest(
-    start: PhaseVector2D,
-    end: PhaseVector2D,
-    maxCurvature: MaxCurvature,
-): DubinsPath = shortest(start, end, maxCurvature.compute(start))

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

@@ -28,31 +28,33 @@ class DubinsTests {
         println("Absolute distance: $absoluteDistance")
 
         val expectedLengths = mapOf(
-            DubinsPath.TYPE.RLR to 13.067681939031397,
+            DubinsPath.Type.RLR to 13.067681939031397,
-            DubinsPath.TYPE.RSR to 12.28318530717957,
+            DubinsPath.Type.RSR to 12.28318530717957,
-            DubinsPath.TYPE.LSL to 32.84955592153878,
+            DubinsPath.Type.LSL to 32.84955592153878,
-            DubinsPath.TYPE.RSL to 23.37758938854081,
+            DubinsPath.Type.RSL to 23.37758938854081,
-            DubinsPath.TYPE.LSR to 23.37758938854081
+            DubinsPath.Type.LSR to 23.37758938854081
         )
 
         expectedLengths.forEach {
-            val path = dubins.find { p -> p.type === it.key }
+            val path = dubins.find { p -> DubinsPath.trajectoryTypeOf(p) === it.key }
             assertNotNull(path, "Path ${it.key} not found")
             println("${it.key}: ${path.length}")
             assertTrue(it.value.equalFloat(path.length))
 
-            assertTrue(start.equalsFloat(path.a.start))
+            val a = path.segments[0] as CircleTrajectory2D
-            assertTrue(end.equalsFloat(path.c.end))
+            val b = path.segments[1]
+            val c = path.segments[2] as CircleTrajectory2D
+
+            assertTrue(start.equalsFloat(a.start))
+            assertTrue(end.equalsFloat(c.end))
 
             // Not working, theta double precision inaccuracy
-            if (path.b is CircleTrajectory2D) {
+            if (b is CircleTrajectory2D) {
-                val b = path.b as CircleTrajectory2D
+                assertTrue(a.end.equalsFloat(b.start))
-                assertTrue(path.a.end.equalsFloat(b.start))
+                assertTrue(c.start.equalsFloat(b.end))
-                assertTrue(path.c.start.equalsFloat(b.end))
+            } else if (b is StraightTrajectory2D) {
-            } else if (path.b is StraightTrajectory2D) {
+                assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing)))
-                val b = path.b as StraightTrajectory2D
+                assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
-                assertTrue(path.a.end.equalsFloat(DubinsPose2D(b.start, b.bearing)))
-                assertTrue(path.c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
             }
         }
     }