[WIP] refactor CircleTrajectory2D logic to be more correct

trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt

@@ -1,6 +1,8 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.kmath.operations.DoubleField.pow
+import space.kscience.trajectory.Pose2D
+import space.kscience.trajectory.Trajectory2D
 import kotlin.math.sign
 
 public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
@@ -47,3 +49,17 @@ public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSe
     }
 }
 
+/**
+ * Compute tangent pose to a circle
+ *
+ * @param bearing is counted the same way as in [Pose2D], from positive y clockwise
+ */
+public fun Circle2D.tangent(bearing: Angle, direction: Trajectory2D.Direction): Pose2D = with(Euclidean2DSpace) {
+    val coordinates: Vector2D<Double> = vector(center.x + radius * sin(bearing), center.y + radius * cos(bearing))
+    val tangentAngle = when (direction) {
+        Trajectory2D.R -> bearing + Angle.piDiv2
+        Trajectory2D.L -> bearing - Angle.piDiv2
+    }.normalized()
+    Pose2D(coordinates, tangentAngle)
+}
+

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

@@ -10,11 +10,11 @@ import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.trajectory.Trajectory2D.*
 import kotlin.math.acos
 
-internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
+internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
 
-internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
+internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
 
-internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
+internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
     val dX = radius * cos(bearing)
     val dY = radius * sin(bearing)
     return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
@@ -102,8 +102,8 @@ public object DubinsPath {
     }
 
     public fun all(
-        start: DubinsPose2D,
+        start: Pose2D,
-        end: DubinsPose2D,
+        end: Pose2D,
         turningRadius: Double,
     ): List<CompositeTrajectory2D> = listOfNotNull(
         rlr(start, end, turningRadius),
@@ -114,10 +114,10 @@ public object DubinsPath {
         lsr(start, end, turningRadius)
     )
 
-    public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
+    public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D =
         all(start, end, turningRadius).minBy { it.length }
 
-    public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
+    public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? =
         with(Euclidean2DSpace) {
             val c1 = start.getRightCircle(turningRadius)
             val c2 = end.getRightCircle(turningRadius)
@@ -135,9 +135,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, R)
+                val a1 = CircleTrajectory2D(c1.center, start, p1, R)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
+                val a2 = CircleTrajectory2D(e.center, p1, p2, L)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
+                val a3 = CircleTrajectory2D(c2.center, p2, end, R)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -152,16 +152,16 @@ 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, R)
+                val a1 = CircleTrajectory2D(c1.center, start, p1, R)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
+                val a2 = CircleTrajectory2D(e.center, p1, p2, L)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
+                val a3 = CircleTrajectory2D(c2.center, p2, end, R)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
             return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
         }
 
-    public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
+    public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? =
         with(Euclidean2DSpace) {
             val c1 = start.getLeftCircle(turningRadius)
             val c2 = end.getLeftCircle(turningRadius)
@@ -179,9 +179,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, L)
+                val a1 = CircleTrajectory2D(c1.center, start, p1, L)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
+                val a2 = CircleTrajectory2D(e.center, p1, p2, R)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
+                val a3 = CircleTrajectory2D(c2.center, p2, end, L)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
@@ -196,52 +196,52 @@ 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, L)
+                val a1 = CircleTrajectory2D(c1.center, start, p1, L)
-                val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
+                val a2 = CircleTrajectory2D(e.center, p1, p2, R)
-                val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
+                val a3 = CircleTrajectory2D(c2.center, p2, end, L)
                 CompositeTrajectory2D(a1, a2, a3)
             }
 
             return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
         }
 
-    public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
+    public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D {
         val c1 = start.getRightCircle(turningRadius)
         val c2 = end.getRightCircle(turningRadius)
         val s = outerTangent(c1, c2, L)
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
+        val a1 = CircleTrajectory2D(c1.center, start, s.begin, R)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
+        val a3 = CircleTrajectory2D(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
-    public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
+    public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D {
         val c1 = start.getLeftCircle(turningRadius)
         val c2 = end.getLeftCircle(turningRadius)
         val s = outerTangent(c1, c2, R)
-        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
+        val a1 = CircleTrajectory2D(c1.center, start, s.begin, L)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
+        val a3 = CircleTrajectory2D(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
-    public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
+    public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? {
         val c1 = start.getRightCircle(turningRadius)
         val c2 = end.getLeftCircle(turningRadius)
         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.begin, R)
+        val a1 = CircleTrajectory2D(c1.center, start, s.begin, R)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
+        val a3 = CircleTrajectory2D(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
 
-    public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
+    public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? {
         val c1 = start.getLeftCircle(turningRadius)
         val c2 = end.getRightCircle(turningRadius)
         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.begin, L)
+        val a1 = CircleTrajectory2D(c1.center, start, s.begin, L)
-        val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
+        val a3 = CircleTrajectory2D(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }
 }

trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt

@@ -48,14 +48,14 @@ private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
 
             if (circles.size == 1) {
                 // a circumvention consisting of a single circle, starting on top
-                val circle = circles.first()
+//                val circle = circles.first()
-                val top = vector(circle.center.x + circle.radius, circle.center.y)
+//                val top = vector(circle.center.x + circle.radius, circle.center.y)
-                val startEnd = DubinsPose2D(
+//                val start = DubinsPose2D(
-                    top,
+//                    top,
-                    Angle.piDiv2
+//                    Angle.piDiv2
-                )
+//                )
                 return@lazy CompositeTrajectory2D(
-                    CircleTrajectory2D(circle, startEnd, startEnd)
+                    CircleTrajectory2D(circles.first(), Angle.zero, Angle.zero)
                 )
             }
 

trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt

@@ -158,14 +158,15 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         //cutting first and last arcs to accommodate connection points
         val first = circumvention.first() as CircleTrajectory2D
         val last = circumvention.last() as CircleTrajectory2D
+        //arc between end of the nangent and end of previous arc (begin of the the next one)
         circumvention[0] = CircleTrajectory2D(
             first.circle,
             tangent1.tangentTrajectory.endPose,
-            first.end
+            first.endPose,
         )
         circumvention[circumvention.lastIndex] = CircleTrajectory2D(
             last.circle,
-            last.begin,
+            last.beginPose,
             tangent2.tangentTrajectory.beginPose
         )
         return CompositeTrajectory2D(circumvention)
@@ -271,10 +272,10 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
             }
 
         private fun constructTangentCircles(
-            pose: DubinsPose2D,
+            pose: Pose2D,
             r: Double,
         ): LR<Circle2D> = with(Euclidean2DSpace) {
-            val direction = DubinsPose2D.bearingToVector(pose.bearing)
+            val direction = Pose2D.bearingToVector(pose.bearing)
             //TODO optimize to use bearing
             val center1 = pose + normalVectors(direction, r).first
             val center2 = pose + normalVectors(direction, r).second
@@ -293,8 +294,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         }
 
         public fun avoidObstacles(
-            start: DubinsPose2D,
+            start: Pose2D,
-            finish: DubinsPose2D,
+            finish: Pose2D,
             startingRadius: Double,
             obstacleList: List<Obstacle>,
             finalRadius: Double = startingRadius,
@@ -328,15 +329,15 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         }
 
         public fun avoidObstacles(
-            start: DubinsPose2D,
+            start: Pose2D,
-            finish: DubinsPose2D,
+            finish: Pose2D,
             trajectoryRadius: Double,
             vararg obstacles: Obstacle,
         ): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
 
         public fun avoidPolygons(
-            start: DubinsPose2D,
+            start: Pose2D,
-            finish: DubinsPose2D,
+            finish: Pose2D,
             trajectoryRadius: Double,
             vararg polygons: Polygon<Double>,
         ): List<Trajectory2D> {
@@ -348,8 +349,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
 
 
         public fun avoidPolygons(
-            start: DubinsPose2D,
+            start: Pose2D,
-            finish: DubinsPose2D,
+            finish: Pose2D,
             trajectoryRadius: Double,
             polygons: Collection<Polygon<Double>>,
         ): List<Trajectory2D> {

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

@@ -19,15 +19,15 @@ import kotlin.math.atan2
 /**
  * Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
  */
-@Serializable(DubinsPose2DSerializer::class)
+@Serializable(Pose2DSerializer::class)
-public interface DubinsPose2D : DoubleVector2D {
+public interface Pose2D : DoubleVector2D {
     public val coordinates: DoubleVector2D
     public val bearing: Angle
 
     /**
      * Reverse the direction of this pose to the opposite, keeping other parameters the same
      */
-    public fun reversed(): DubinsPose2D
+    public fun reversed(): Pose2D
 
     public companion object {
         public fun bearingToVector(bearing: Angle): Vector2D<Double> =
@@ -45,45 +45,62 @@ public interface DubinsPose2D : DoubleVector2D {
 public class PhaseVector2D(
     override val coordinates: DoubleVector2D,
     public val velocity: DoubleVector2D,
-) : DubinsPose2D, DoubleVector2D by coordinates {
+) : Pose2D, DoubleVector2D by coordinates {
     override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
 
-    override fun reversed(): DubinsPose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
+    override fun reversed(): Pose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
 }
 
 @Serializable
 @SerialName("DubinsPose2D")
-private class DubinsPose2DImpl(
+private class Pose2DImpl(
     override val coordinates: DoubleVector2D,
     override val bearing: Angle,
-) : DubinsPose2D, DoubleVector2D by coordinates {
+) : Pose2D, DoubleVector2D by coordinates {
+
+    override fun reversed(): Pose2D = Pose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
+
 
-    override fun reversed(): DubinsPose2D = DubinsPose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
 
     override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (other == null || this::class != other::class) return false
+
+        other as Pose2DImpl
+
+        if (coordinates != other.coordinates) return false
+        return bearing == other.bearing
+    }
+
+    override fun hashCode(): Int {
+        var result = coordinates.hashCode()
+        result = 31 * result + bearing.hashCode()
+        return result
+    }
 }
 
-public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
+public object Pose2DSerializer : KSerializer<Pose2D> {
-    private val proxySerializer = DubinsPose2DImpl.serializer()
+    private val proxySerializer = Pose2DImpl.serializer()
 
     override val descriptor: SerialDescriptor
         get() = proxySerializer.descriptor
 
-    override fun deserialize(decoder: Decoder): DubinsPose2D {
+    override fun deserialize(decoder: Decoder): Pose2D {
         return decoder.decodeSerializableValue(proxySerializer)
     }
 
-    override fun serialize(encoder: Encoder, value: DubinsPose2D) {
+    override fun serialize(encoder: Encoder, value: Pose2D) {
-        val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing)
+        val pose = value as? Pose2DImpl ?: Pose2DImpl(value.coordinates, value.bearing)
         encoder.encodeSerializableValue(proxySerializer, pose)
     }
 }
 
-public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D =
+public fun Pose2D(coordinate: DoubleVector2D, bearing: Angle): Pose2D =
-    DubinsPose2DImpl(coordinate, bearing)
+    Pose2DImpl(coordinate, bearing)
 
-public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
+public fun Pose2D(point: DoubleVector2D, direction: DoubleVector2D): Pose2D =
-    DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))
+    Pose2D(point, Pose2D.vectorToBearing(direction))
 
-public fun DubinsPose2D(x: Number, y: Number, bearing: Angle): DubinsPose2D =
+public fun Pose2D(x: Number, y: Number, bearing: Angle): Pose2D =
-    DubinsPose2DImpl(Euclidean2DSpace.vector(x, y), bearing)
+    Pose2DImpl(Euclidean2DSpace.vector(x, y), bearing)

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

@@ -18,8 +18,8 @@ import kotlin.math.atan2
 public sealed interface Trajectory2D {
     public val length: Double
 
-    public val beginPose: DubinsPose2D
+    public val beginPose: Pose2D
-    public val endPose: DubinsPose2D
+    public val endPose: Pose2D
 
     /**
      * Produce a trajectory with reversed order of points
@@ -60,8 +60,8 @@ public data class StraightTrajectory2D(
 
     public val bearing: Angle get() = (end - begin).bearing
 
-    override val beginPose: DubinsPose2D get() = DubinsPose2D(begin, bearing)
+    override val beginPose: Pose2D get() = Pose2D(begin, bearing)
-    override val endPose: DubinsPose2D get() = DubinsPose2D(end, bearing)
+    override val endPose: Pose2D get() = Pose2D(end, bearing)
 
     override fun reversed(): StraightTrajectory2D = StraightTrajectory2D(end, begin)
 }
@@ -74,72 +74,88 @@ public fun StraightTrajectory2D(segment: LineSegment2D): StraightTrajectory2D =
  */
 @Serializable
 @SerialName("arc")
-public data class CircleTrajectory2D (
+public data class CircleTrajectory2D(
     public val circle: Circle2D,
-    public val begin: DubinsPose2D,
+    public val arcStart: Angle,
-    public val end: DubinsPose2D,
+    public val arcAngle: Angle,
 ) : Trajectory2D {
+    public val direction: Trajectory2D.Direction = if (arcAngle > Angle.zero) Trajectory2D.R else Trajectory2D.L
 
-    override val beginPose: DubinsPose2D get() = begin
+    public val arcEnd: Angle = arcStart + arcAngle
-    override val endPose: DubinsPose2D get() = end
+    override val beginPose: Pose2D get() = circle.tangent(arcStart, direction)
-
+    override val endPose: Pose2D get() = circle.tangent(arcEnd, direction)
-    /**
-     * Arc length in radians
-     */
-    val arcAngle: Angle
-        get() = if (direction == Trajectory2D.L) {
-            begin.bearing - end.bearing
-        } else {
-            end.bearing - begin.bearing
-        }.normalized()
-
 
     override val length: Double by lazy {
-        circle.radius * arcAngle.radians
+        circle.radius * kotlin.math.abs(arcAngle.radians)
     }
 
-    public val direction: Trajectory2D.Direction by lazy {
+
-        when {
+    override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, arcEnd, -arcAngle)
-            begin.y < circle.center.y -> if (begin.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
+
-            begin.y > circle.center.y -> if (begin.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
+    public companion object
-            else -> if (begin.bearing == Angle.zero) {
+}
-                if (begin.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
+
+public fun CircleTrajectory2D(
+    center: DoubleVector2D,
+    start: DoubleVector2D,
+    end: DoubleVector2D,
+    direction: Trajectory2D.Direction,
+): CircleTrajectory2D = with(Euclidean2DSpace) {
+//    fun calculatePose(
+//        vector: DoubleVector2D,
+//        theta: Angle,
+//        direction: Trajectory2D.Direction,
+//    ): DubinsPose2D = DubinsPose2D(
+//        vector,
+//        when (direction) {
+//            Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
+//            Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
+//        }
+//    )
+//
+//    val s1 = StraightTrajectory2D(center, start)
+//    val s2 = StraightTrajectory2D(center, end)
+//    val pose1 = calculatePose(start, s1.bearing, direction)
+//    val pose2 = calculatePose(end, s2.bearing, direction)
+//    val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
+//    if (trajectory.direction != direction) error("Trajectory direction mismatch")
+//    return trajectory
+    val startVector = start - center
+    val endVector = end - center
+    val startRadius = norm(startVector)
+    val endRadius = norm(endVector)
+    require((startRadius - endRadius) / startRadius < 1e-6) { "Start and end points have different radii" }
+    val radius = (startRadius + endRadius) / 2
+    val startBearing = startVector.bearing
+    val endBearing = endVector.bearing
+    CircleTrajectory2D(
+        Circle2D(center, radius),
+        startBearing,
+        when (direction) {
+            Trajectory2D.L -> if (endBearing >= startBearing) {
+                endBearing - startBearing - Angle.piTimes2
             } else {
-                if (begin.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
+                endBearing - startBearing
+            }
+
+            Trajectory2D.R -> if (endBearing >= startBearing) {
+                endBearing - startBearing
+            } else {
+                endBearing + Angle.piTimes2 - startBearing
             }
         }
-    }
+    )
+}
 
-    override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, end.reversed(), begin.reversed())
+public fun CircleTrajectory2D(
-
+    circle: Circle2D,
-    public companion object {
+    beginPose: Pose2D,
-        public fun of(
+    endPose: Pose2D,
-            center: DoubleVector2D,
+): CircleTrajectory2D = with(Euclidean2DSpace) {
-            start: DoubleVector2D,
+    val vectorToBegin = beginPose - circle.center
-            end: DoubleVector2D,
+    val vectorToEnd = endPose - circle.center
-            direction: Trajectory2D.Direction,
+    //TODO check pose bearing
-        ): CircleTrajectory2D {
+    return CircleTrajectory2D(circle, vectorToBegin.bearing, vectorToEnd.bearing - vectorToBegin.bearing)
-            fun calculatePose(
-                vector: DoubleVector2D,
-                theta: Angle,
-                direction: Trajectory2D.Direction,
-            ): DubinsPose2D = DubinsPose2D(
-                vector,
-                when (direction) {
-                    Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
-                    Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
-                }
-            )
-
-            val s1 = StraightTrajectory2D(center, start)
-            val s2 = StraightTrajectory2D(center, end)
-            val pose1 = calculatePose(start, s1.bearing, direction)
-            val pose2 = calculatePose(end, s2.bearing, direction)
-            val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
-            if (trajectory.direction != direction) error("Trajectory direction mismatch")
-            return trajectory
-        }
-    }
 }
 
 @Serializable
@@ -147,8 +163,8 @@ public data class CircleTrajectory2D (
 public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
     override val length: Double get() = segments.sumOf { it.length }
 
-    override val beginPose: DubinsPose2D get() = segments.first().beginPose
+    override val beginPose: Pose2D get() = segments.first().beginPose
-    override val endPose: DubinsPose2D get() = segments.last().endPose
+    override val endPose: Pose2D get() = segments.last().endPose
 
     override fun reversed(): CompositeTrajectory2D = CompositeTrajectory2D(segments.map { it.reversed() }.reversed())
 }

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

@@ -6,7 +6,7 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.trajectory.CircleTrajectory2D
-import space.kscience.trajectory.DubinsPose2D
+import space.kscience.trajectory.Pose2D
 import space.kscience.trajectory.Trajectory2D
 import kotlin.math.PI
 import kotlin.test.Test
@@ -17,15 +17,15 @@ class ArcTests {
     @Test
     fun arc() = with(Euclidean2DSpace) {
         val circle = Circle2D(vector(0.0, 0.0), 2.0)
-        val arc = CircleTrajectory2D.of(
+        val arc = CircleTrajectory2D(
             circle.center,
             vector(-2.0, 0.0),
             vector(0.0, 2.0),
             Trajectory2D.R
         )
         assertEquals(circle.circumference / 4, arc.length, 1.0)
-        assertEquals(0.0, arc.begin.bearing.degrees)
+        assertEquals(0.0, arc.beginPose.bearing.degrees)
-        assertEquals(90.0, arc.end.bearing.degrees)
+        assertEquals(90.0, arc.endPose.bearing.degrees)
     }
 
     @Test
@@ -33,8 +33,8 @@ class ArcTests {
         val circle = circle(1, 0, 1)
         val arc = CircleTrajectory2D(
             circle,
-            DubinsPose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
+            Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
-            DubinsPose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
+            Pose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
         )
         assertEquals(Trajectory2D.R, arc.direction)
         assertEquals(PI / 2, arc.length, 1e-4)

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

@@ -6,23 +6,22 @@
 package space.kscience.trajectory
 
 import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.equalsFloat
 import kotlin.test.Test
+import kotlin.test.assertEquals
 import kotlin.test.assertNotNull
-import kotlin.test.assertTrue
 
 
 class DubinsTests {
 
     @Test
-    fun dubinsTest() = with(Euclidean2DSpace){
+    fun dubinsTest() = with(Euclidean2DSpace) {
         val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
         val lineP1 = straight.shift(1, 10.0).inverse()
 
-        val start = DubinsPose2D(straight.end, straight.bearing)
+        val start = Pose2D(straight.end, straight.bearing)
-        val end = DubinsPose2D(lineP1.begin, lineP1.bearing)
+        val end = Pose2D(lineP1.begin, lineP1.bearing)
         val radius = 2.0
-        val dubins = DubinsPath.all(start, end, radius)
+        val dubins: List<CompositeTrajectory2D> = DubinsPath.all(start, end, radius)
 
         val absoluteDistance = start.distanceTo(end)
         println("Absolute distance: $absoluteDistance")
@@ -39,22 +38,22 @@ class DubinsTests {
             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.equalsFloat(path.length))
+            assertEquals(it.value, path.length, 1e-4)
 
             val a = path.segments[0] as CircleTrajectory2D
             val b = path.segments[1]
             val c = path.segments[2] as CircleTrajectory2D
 
-            assertEquals(start, a.begin)
+            assertEquals(start, a.beginPose, 1e-4)
-            assertEquals(end, c.end)
+            assertEquals(end, c.endPose, 1e-4)
 
             // Not working, theta double precision inaccuracy
             if (b is CircleTrajectory2D) {
-                assertEquals(a.end, b.begin)
+                assertEquals(a.endPose, b.beginPose, 1e-4)
-                assertEquals(c.begin, b.end)
+                assertEquals(c.beginPose, b.endPose, 1e-4)
             } else if (b is StraightTrajectory2D) {
-                assertEquals(a.end, DubinsPose2D(b.begin, b.bearing))
+                assertEquals(a.endPose, Pose2D(b.begin, b.bearing), 1e-4)
-                assertEquals(c.begin, DubinsPose2D(b.end, b.bearing))
+                assertEquals(c.beginPose, Pose2D(b.end, b.bearing),1e-4)
             }
         }
     }

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

@@ -34,8 +34,8 @@ class ObstacleTest {
         val finalDirection = vector(1.0, -1.0)
 
         val outputTangents = Obstacles.avoidObstacles(
-            DubinsPose2D(startPoint, startDirection),
+            Pose2D(startPoint, startDirection),
-            DubinsPose2D(finalPoint, finalDirection),
+            Pose2D(finalPoint, finalDirection),
             startRadius,
             Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
         )
@@ -53,8 +53,8 @@ class ObstacleTest {
         val finalDirection = vector(1.0, -1.0)
 
         val paths = Obstacles.avoidObstacles(
-            DubinsPose2D(startPoint, startDirection),
+            Pose2D(startPoint, startDirection),
-            DubinsPose2D(finalPoint, finalDirection),
+            Pose2D(finalPoint, finalDirection),
             radius,
             Obstacle(
                 Circle2D(vector(1.0, 6.5), 0.5),
@@ -98,8 +98,8 @@ class ObstacleTest {
         val finalDirection = vector(1.0, 0)
 
         val paths = Obstacles.avoidObstacles(
-            DubinsPose2D(startPoint, startDirection),
+            Pose2D(startPoint, startDirection),
-            DubinsPose2D(finalPoint, finalDirection),
+            Pose2D(finalPoint, finalDirection),
             startRadius,
             Obstacle(
                 Circle2D(vector(0.0, 0.0), 1.0),
@@ -117,8 +117,8 @@ class ObstacleTest {
     @Test
     fun largeCoordinates() {
         val paths = Obstacles.avoidObstacles(
-            DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
+            Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
-            DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
+            Pose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
             5000.0,
             Obstacle(
                 Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0),

trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt

@@ -10,7 +10,7 @@ import space.kscience.kmath.geometry.radians
 import space.kscience.kmath.geometry.sin
 
 
-fun assertEquals(expected: DubinsPose2D, actual: DubinsPose2D, precision: Double = 1e-6){
+fun assertEquals(expected: Pose2D, actual: Pose2D, precision: Double = 1e-6){
     kotlin.test.assertEquals(expected.x, actual.x, precision)
     kotlin.test.assertEquals(expected.y, actual.y, precision)
     kotlin.test.assertEquals(expected.bearing.radians, actual.bearing.radians, precision)