Obstacle avoidance finished

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

@@ -0,0 +1,14 @@
+/*
+ * 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.geometry
+
+
+/**
+ * A closed polygon in 2D space
+ */
+public interface Polygon<T> {
+    public val points: List<Vector2D<T>>
+}

kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt

@@ -63,7 +63,7 @@ internal class ProjectionOntoLineTest {
 
     @Test
     fun projectionOntoLine3d() = with(Euclidean3DSpace) {
-        val line = Line3D(
+        val line = Line(
             base = vector(1.0, 3.5, 0.07),
             direction = vector(2.0, -0.0037, 11.1111)
         )
@@ -77,7 +77,7 @@ internal class ProjectionOntoLineTest {
                     val result = projectToLine(v, line)
 
                     // assert that result is on the line
-                    assertTrue(isCollinear(result - line.base, line.direction))
+                    assertTrue(isCollinear(result - line.start, line.direction))
                     // assert that PV vector is orthogonal to direction vector
                     assertTrue(isOrthogonal(v - result, line.direction))
                 }

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

@@ -36,7 +36,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): St
         }
         return StraightTrajectory2D(
             p1,
-            vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
+            vector(p1.x + (centers.end.x - centers.begin.x), p1.y + (centers.end.y - centers.begin.y))
         )
     }
 
@@ -207,7 +207,7 @@ 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, R)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
         val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }
@@ -216,7 +216,7 @@ 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, L)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
         val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
@@ -227,7 +227,7 @@ 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, R)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
         val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
         return CompositeTrajectory2D(a1, s, a3)
     }
@@ -238,7 +238,7 @@ 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, L)
+        val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
         val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
         return CompositeTrajectory2D(a1, s, a3)
     }

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

@@ -23,7 +23,7 @@ internal data class Tangent(
     val lineSegment: LineSegment2D,
     val startDirection: Trajectory2D.Direction,
     val endDirection: Trajectory2D.Direction = startDirection,
-): LineSegment2D by lineSegment
+) : LineSegment2D by lineSegment
 
 private class TangentPath(val tangents: List<Tangent>) {
     fun last() = tangents.last()
@@ -143,7 +143,7 @@ private fun dubinsTangentsToCircles(
     }
 }
 
-public class Obstacle(
+internal class Obstacle(
     public val circles: List<Circle2D>,
 ) {
     internal val tangents: List<Tangent> = boundaryTangents().first
@@ -281,7 +281,7 @@ public class Obstacle(
     }
 }
 
-public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
+internal fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
 
 private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
     fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
@@ -594,7 +594,19 @@ internal fun findAllPaths(
 }
 
 
-
+public object Obstacles {
+    public fun allPathsAvoiding(
+        start: DubinsPose2D,
+        finish: DubinsPose2D,
+        trajectoryRadius: Double,
+        obstaclePolygons: List<Polygon<Double>>,
+    ): List<CompositeTrajectory2D> {
+        val obstacles: List<Obstacle> = obstaclePolygons.map { polygon ->
+            Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) })
+        }
+        return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles)
+    }
+}
 
 
 

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

@@ -41,17 +41,15 @@ public sealed interface Trajectory2D {
 @Serializable
 @SerialName("straight")
 public data class StraightTrajectory2D(
-    public val start: DoubleVector2D,
+    override val begin: DoubleVector2D,
-    public val end: DoubleVector2D,
+    override val end: DoubleVector2D,
-) : Trajectory2D {
+) : Trajectory2D, LineSegment2D {
 
-    override val length: Double get() = start.distanceTo(end)
+    override val length: Double get() = begin.distanceTo(end)
 
-    public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized()
+    public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized()
 }
 
-public fun StraightTrajectory2D.toSegment(): LineSegment<Vector2D<Double>> = LineSegment(start, end)
-
 /**
  * An arc segment
  */

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

@@ -20,7 +20,7 @@ class DubinsTests {
         val lineP1 = straight.shift(1, 10.0).inverse()
 
         val start = DubinsPose2D(straight.end, straight.bearing)
-        val end = DubinsPose2D(lineP1.start, lineP1.bearing)
+        val end = DubinsPose2D(lineP1.begin, lineP1.bearing)
         val radius = 2.0
         val dubins = DubinsPath.all(start, end, radius)
 
@@ -53,7 +53,7 @@ class DubinsTests {
                 assertTrue(a.end.equalsFloat(b.start))
                 assertTrue(c.start.equalsFloat(b.end))
             } else if (b is StraightTrajectory2D) {
-                assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing)))
+                assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing)))
                 assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
             }
         }

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

@@ -14,14 +14,14 @@ import space.kscience.kmath.geometry.sin
 fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
     x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians)
 
-fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start)
+fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)
 
 fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) {
     val dX = width * sin(inverse().bearing)
     val dY = width * sin(bearing)
 
     return StraightTrajectory2D(
-        vector(start.x - dX * shift, start.y - dY * shift),
+        vector(begin.x - dX * shift, begin.y - dY * shift),
         vector(end.x - dX * shift, end.y - dY * shift)
     )
 }