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

@@ -3,6 +3,7 @@
  * 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
@@ -22,6 +23,19 @@ import kotlin.math.atan2
 public interface DubinsPose2D : DoubleVector2D {
     public val coordinates: DoubleVector2D
     public val bearing: Angle
+
+    public companion object {
+        public fun bearingToVector(bearing: Angle): Vector2D<Double> =
+            Euclidean2DSpace.vector(cos(bearing), sin(bearing))
+
+        public fun vectorToBearing(vector2D: DoubleVector2D): Angle {
+            require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
+            return atan2(vector2D.y, vector2D.x).radians
+        }
+
+        public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
+            DubinsPose2D(point, vectorToBearing(direction))
+    }
 }
 
 @Serializable
@@ -37,12 +51,12 @@ public class PhaseVector2D(
 private class DubinsPose2DImpl(
     override val coordinates: DoubleVector2D,
     override val bearing: Angle,
-) : DubinsPose2D, DoubleVector2D by coordinates{
+) : DubinsPose2D, DoubleVector2D by coordinates {
 
     override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
 }
 
-public object DubinsPose2DSerializer: KSerializer<DubinsPose2D>{
+public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
     private val proxySerializer = DubinsPose2DImpl.serializer()
 
     override val descriptor: SerialDescriptor

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

@@ -22,7 +22,7 @@ internal data class Tangent(
     val endObstacle: Obstacle,
     val lineSegment: LineSegment2D,
     val startDirection: Trajectory2D.Direction,
-    val endDirection: Trajectory2D.Direction = startDirection
+    val endDirection: Trajectory2D.Direction = startDirection,
 )
 
 private class TangentPath(val tangents: List<Tangent>) {
@@ -465,22 +465,22 @@ private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTraject
 )
 
 internal fun findAllPaths(
-    startingPoint: DoubleVector2D,
+    start: DubinsPose2D,
-    startingDirection: DoubleVector2D,
     startingRadius: Double,
-    finalPoint: DoubleVector2D,
+    finish: DubinsPose2D,
-    finalDirection: DoubleVector2D,
     finalRadius: Double,
     obstacles: List<Obstacle>,
 ): List<CompositeTrajectory2D> {
+    fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing))
+
     val initialCircles = constructTangentCircles(
-        startingPoint,
+        start,
-        startingDirection,
+        start.direction(),
         startingRadius
     )
     val finalCircles = constructTangentCircles(
-        finalPoint,
+        finish,
-        finalDirection,
+        finish.direction(),
         finalRadius
     )
     val trajectories = mutableListOf<CompositeTrajectory2D>()
@@ -495,7 +495,7 @@ internal fun findAllPaths(
                         initialCircles[i]!!,
                         Obstacle(listOf(initialCircles[i]!!)),
                         Obstacle(listOf(initialCircles[i]!!)),
-                        LineSegment2D(startingPoint, startingPoint),
+                        LineSegment2D(start, start),
                         i
                     )
                 )
@@ -596,7 +596,7 @@ internal fun findAllPaths(
                                 end,
                                 Obstacle(end),
                                 Obstacle(end),
-                                LineSegment2D(finalPoint, finalPoint),
+                                LineSegment2D(finish, finish),
                                 startDirection = lastDirection,
                                 endDirection = j
                             )

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

@@ -29,11 +29,9 @@ class ObstacleTest {
         )
 
         val outputTangents = findAllPaths(
-            startPoint,
+            DubinsPose2D.of(startPoint, startDirection),
-            startDirection,
             startRadius,
-            finalPoint,
+            DubinsPose2D.of(finalPoint, finalDirection),
-            finalDirection,
             finalRadius,
             obstacles
         )
@@ -68,11 +66,9 @@ class ObstacleTest {
             )
         )
         val paths = findAllPaths(
-            startPoint,
+            DubinsPose2D.of(startPoint, startDirection),
-            startDirection,
             startRadius,
-            finalPoint,
+            DubinsPose2D.of(finalPoint, finalDirection),
-            finalDirection,
             finalRadius,
             obstacles
         )