Fix circle line intersection and add a special case for a single-point obstacle

demo/trajectory-playground/src/jvmMain/kotlin/Main.kt

@@ -20,6 +20,8 @@ import kotlin.random.Random
 
 private fun DoubleVector2D.toXY() = XY(x.toFloat(), y.toFloat())
 
+private val random = Random(123)
+
 fun FeatureGroup<XY>.trajectory(
     trajectory: Trajectory2D,
     colorPicker: (Trajectory2D) -> Color = { Color.Blue },
@@ -57,8 +59,8 @@ fun FeatureGroup<XY>.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) ->
     polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray)
 }
 
-fun FeatureGroup<XY>.pose(pose2D: Pose2D) = with(Euclidean2DSpace){
+fun FeatureGroup<XY>.pose(pose2D: Pose2D) = with(Euclidean2DSpace) {
-    line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY() )
+    line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY())
 }
 
 @Composable
@@ -72,18 +74,25 @@ fun closePoints() {
             Circle2D(Euclidean2DSpace.vector(1.0, 1.0), 1.0),
             Circle2D(Euclidean2DSpace.vector(1.0, 0.0), 1.0)
         )
+        val enter = Pose2D(-0.8, -0.8, Angle.pi)
+        val exit = Pose2D(-0.8, -0.8, Angle.piDiv2)
+
+        pose(enter)
+        pose(exit)
 
         val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
-            Pose2D(-1, -1, Angle.pi),
+            enter,
-            Pose2D(-1, -1, Angle.piDiv2),
+            exit,
             1.0,
             obstacle
         )
 
         obstacle(obstacle)
 
-        trajectory(paths.first()) { Color.Green }
+        paths.forEach {
-        trajectory(paths.last()) { Color.Magenta }
+            val color = Color(random.nextInt())
+            trajectory(it) { color }
+        }
 
     }
 }
@@ -93,14 +102,21 @@ fun closePoints() {
 fun singleObstacle() {
     SchemeView {
         val obstacle = Obstacle(Circle2D(Euclidean2DSpace.vector(7.0, 1.0), 5.0))
+        val enter = Pose2D(-5, -1, Angle.pi / 4)
+        val exit = Pose2D(20, 4, Angle.pi * 3 / 4)
+
+        pose(enter)
+        pose(exit)
         obstacle(obstacle)
+
         Obstacles.avoidObstacles(
-            Pose2D(-5, -1, Angle.pi / 4),
+            enter,
-            Pose2D(20, 4, Angle.pi * 3 / 4),
+            exit,
             0.5,
             obstacle
         ).forEach {
-            trajectory(it).color(Color(Random.nextInt()))
+            val color = Color(random.nextInt())
+            trajectory(it) { color }
         }
     }
 }
@@ -123,7 +139,7 @@ fun doubleObstacle() {
             )
         )
 
-        obstacles.forEach { obstacle(it)  }
+        obstacles.forEach { obstacle(it) }
         val enter = Pose2D(-5, -1, Angle.pi / 4)
         val exit = Pose2D(20, 4, Angle.pi * 3 / 4)
         pose(enter)
@@ -135,7 +151,8 @@ fun doubleObstacle() {
             0.5,
             *obstacles
         ).forEach {
-            trajectory(it).color(Color(Random.nextInt()))
+            val color = Color(random.nextInt())
+            trajectory(it) { color }
         }
     }
 }

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

@@ -39,7 +39,7 @@ public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D)
     val t1 = (-b - discriminant) / (2 * a) // first intersection point in relative coordinates
     val t2 = (-b + discriminant) / (2 * a) //second intersection point in relative coordinates
 
-    return t1.sign != t2.sign || (t1-1.0).sign != (t2-1).sign
+    return t1 in 0.0..1.0 || t2 in 0.0..1.0
 }
 
 

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

@@ -14,4 +14,8 @@ public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, circle: Circle2
     polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, circle) }
 
 public fun Euclidean2DSpace.intersectsTrajectory(polygon: Polygon<Double>, trajectory: Trajectory2D): Boolean =
-    polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { edge -> intersectsTrajectory(edge, trajectory) }
+    polygon.points.zipWithNextCircular { l, r ->
+        segment(l, r)
+    }.any { edge ->
+        intersectsTrajectory(edge, trajectory)
+    }

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

@@ -14,19 +14,14 @@ public interface Obstacle {
     public val center: Vector2D<Double>
 
     /**
-     * A closed right-handed circuit minimal path circumvention of an obstacle.
+     * A closed right-handed circuit minimal path circumvention of the obstacle.
      */
     public val circumvention: CompositeTrajectory2D
 
-    public val polygon: Polygon<Double>
-
-
     /**
-     * Check if obstacle has intersection with given [Trajectory2D]
+     * A polygon created from the arc centers of the obstacle
      */
-    public fun intersects(trajectory: Trajectory2D): Boolean =
+    public val core: Polygon<Double>
-        Euclidean2DSpace.intersectsTrajectory(polygon, trajectory)
-
 
     public companion object {
 
@@ -45,7 +40,7 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) :
         )
     }
 
-    override val polygon: Polygon<Double> by lazy {
+    override val core: Polygon<Double> by lazy {
         Euclidean2DSpace.polygon(arcs.map { it.circle.center })
     }
 

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

@@ -4,6 +4,25 @@ import space.kscience.kmath.geometry.*
 import kotlin.collections.component1
 import kotlin.collections.component2
 
+/**
+ * The same as [intersectsTrajectory], but bypasses same circles or same straights
+ */
+private fun Euclidean2DSpace.intersectsOtherTrajectory(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
+    is CircleTrajectory2D -> when (b) {
+        is CircleTrajectory2D -> a != b && intersectsOrInside(a.circle, b.circle)
+        is StraightTrajectory2D -> intersects(a.circle, b)
+        is CompositeTrajectory2D -> b.segments.any { intersectsOtherTrajectory(it, a) }
+    }
+
+    is StraightTrajectory2D -> when (b) {
+        is CircleTrajectory2D -> intersects(a, b.circle)
+        is StraightTrajectory2D -> a != b && intersects(a, b)
+        is CompositeTrajectory2D -> b.segments.any { intersectsOtherTrajectory(it, a) }
+    }
+
+    is CompositeTrajectory2D -> a.segments.any { intersectsOtherTrajectory(it, b) }
+}
+
 
 public class Obstacles(public val obstacles: List<Obstacle>) {
 
@@ -22,11 +41,16 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         val to: ObstacleConnection?,
     ) {
         /**
-         * If false this tangent intersects another obstacle
+         * If false, this tangent intersects another obstacle
          */
         val isValid by lazy {
-            obstacles.indices.none {
+            with(Euclidean2DSpace) {
-                it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory)
+                obstacles.indices.none {
+                    it != from?.obstacleIndex && it != to?.obstacleIndex && intersectsTrajectory(
+                        obstacles[it].core,
+                        tangentTrajectory
+                    )
+                }
             }
         }
     }
@@ -53,7 +77,10 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                         firstCircle,
                         secondCircle
                     )) {
-                        if (!first.intersects(segment) && !second.intersects(segment)) {
+                        if (
+                            !intersectsTrajectory(first.core, segment)
+                            && !intersectsTrajectory(second.core, segment)
+                        ) {
                             put(
                                 pathType,
                                 ObstacleTangent(
@@ -82,7 +109,11 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                     arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
                     obstacleArc
                 )) {
-                    if (pathType.first == arc.direction && !intersects(obstacle.polygon, segment)) {
+                    if (pathType.first == arc.direction && !intersectsTrajectory(
+                            obstacle.core,
+                            segment
+                        )
+                    ) {
                         put(
                             pathType,
                             ObstacleTangent(
@@ -100,7 +131,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
     private fun tangentToArc(
         obstacleIndex: Int,
         obstacleDirection: Trajectory2D.Direction,
-        arc: CircleTrajectory2D
+        arc: CircleTrajectory2D,
     ): ObstacleTangent? = with(Euclidean2DSpace) {
         val obstacle = obstacles[obstacleIndex]
         for (circleIndex in obstacle.arcs.indices) {
@@ -109,7 +140,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
                 obstacleArc.circle,
                 arc.circle
             )[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
-                obstacleArc.containsPoint(it.begin) && !obstacle.intersects(it)
+                obstacleArc.containsPoint(it.begin) && !intersectsTrajectory(obstacle.core, it)
             }?.let {
                 return ObstacleTangent(
                     it,
@@ -131,7 +162,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
 
 
     /**
-     * Circumvention trajectory alongside obstacle. Replacing first and last arcs with appropriate cuts
+     * Circumvention trajectory alongside the obstacle. Replacing first and last arcs with appropriate cuts
      */
     private fun trajectoryBetween(tangent1: ObstacleTangent, tangent2: ObstacleTangent): CompositeTrajectory2D {
         require(tangent1.to != null)
@@ -149,7 +180,7 @@ 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 tangent and end of previous arc (begin of the next one)
+        //arc between the end of the tangent and end of the previous arc (begin of the next one)
         circumvention[0] = CircleTrajectory2D(
             first.circle,
             tangent1.tangentTrajectory.endPose,
@@ -169,7 +200,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         val isFinished get() = tangents.last().to == null
 
         fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
-            buildList<Trajectory2D> {
+            buildList {
                 add(tangents.first().tangentTrajectory)
                 tangents.zipWithNext().forEach { (l, r) ->
                     addAll(trajectoryBetween(l, r).segments)
@@ -183,7 +214,12 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         dubinsPath: CompositeTrajectory2D,
     ): Collection<Trajectory2D> = with(Euclidean2DSpace) {
         //fast return if no obstacles intersect the direct path
-        if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
+        if (
+            obstacles.none {
+                (it.arcs.size == 1 && intersectsTrajectory(it.circumvention, dubinsPath)) // special case for one-point obstacles
+                        || intersectsTrajectory(it.core, dubinsPath)
+            }
+        ) return listOf(dubinsPath)
 
         val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
         val endArc = dubinsPath.segments.last() as CircleTrajectory2D
@@ -198,7 +234,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
             val connection = tangents.last().to
             require(connection != null)
 
-            //indices of obstacles that are not on previous path
+            //indices of obstacles that are not on the previous path
             val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }.toSet()
 
             //a tangent to end point, null if tangent could not be constructed
@@ -209,14 +245,27 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
             ) ?: return emptySet()
 
             // if no intersections, finish
-            if (obstacles.indices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
+            if (obstacles.indices.none {
+                    intersectsTrajectory(
+                        obstacles[it].core,
+                        tangentToEnd.tangentTrajectory
+                    )
+                }) return setOf(
                 TangentPath(tangents + tangentToEnd)
             )
 
             // tangents to other obstacles
             return remainingObstacleIndices.sortedWith(
-                compareByDescending<Int> { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles
+                compareByDescending<Int> {
-                    .thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest
+                    intersectsTrajectory(
+                        obstacles[it].core,
+                        tangentToEnd.tangentTrajectory
+                    )
+                    //take intersecting obstacles
+                }.thenBy {
+                    connection.circle.center.distanceTo(obstacles[it].center)
+                    //then nearest
+                }
             ).firstNotNullOf { nextObstacleIndex ->
                 //all tangents from cache
                 getAllTangents(connection.obstacleIndex, nextObstacleIndex).filter {
@@ -229,19 +278,24 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
         }
 
 
-        //find nearest obstacle that has valid tangents to
+        //find the nearest obstacle that has valid tangents to
         val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
-            compareByDescending<Int> { obstacles[it].intersects(dubinsPath) } //take intersecting obstacles
+            compareByDescending<Int> {
-                .thenBy { beginArc.circle.center.distanceTo(obstacles[it].center) } //then nearest
+                intersectsTrajectory(obstacles[it].core, dubinsPath)
+                //take intersecting obstacles
+            }.thenBy {
+                beginArc.circle.center.distanceTo(obstacles[it].center)
+                //then nearest
+            }
         ).firstNotNullOfOrNull { obstacleIndex ->
             tangentsFromArc(beginArc, obstacleIndex).values
                 .filter { it.isValid }.takeIf { it.isNotEmpty() }
-        }?: return emptySet()
+        } ?: return emptySet()
 
         var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
 
         while (!paths.all { it.isFinished }) {
-            paths = paths.flatMap { if(it.isFinished) listOf(it) else  it.nextSteps() }
+            paths = paths.flatMap { if (it.isFinished) listOf(it) else it.nextSteps() }
         }
         return paths.map {
             CompositeTrajectory2D(
@@ -279,24 +333,6 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
 
 
     public companion object {
-//        private data class LR<T>(val l: T, val r: T) {
-//            operator fun get(direction: Trajectory2D.Direction) = when (direction) {
-//                Trajectory2D.L -> l
-//                Trajectory2D.R -> r
-//            }
-//        }
-//
-//        private fun constructTangentCircles(
-//            pose: Pose2D,
-//            r: Double,
-//        ): LR<Circle2D> = with(Euclidean2DSpace) {
-//            val center1 = pose + vector(r*sin(pose.bearing + Angle.piDiv2), r*cos(pose.bearing + Angle.piDiv2))
-//            val center2 = pose + vector(r*sin(pose.bearing - Angle.piDiv2), r*cos(pose.bearing - Angle.piDiv2))
-//            LR(
-//                Circle2D(center2, r),
-//                Circle2D(center1, r)
-//            )
-//        }
 
         public fun avoidObstacles(
             start: Pose2D,

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

@@ -6,7 +6,6 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.trajectory.CircleTrajectory2D
-import space.kscience.trajectory.Pose2D
 import space.kscience.trajectory.Trajectory2D
 import kotlin.math.PI
 import kotlin.test.Test
@@ -35,11 +34,11 @@ class ArcTests {
         val circle = circle(1, 0, 1)
         val arc = CircleTrajectory2D(
             circle,
-            Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
+            (PI/2).radians,
-            Pose2D(x = 1.0, y = -1.0, bearing = (PI * 3 / 2).radians)
+            (PI/2).radians
         )
         assertEquals(Trajectory2D.R, arc.direction)
-        assertEquals(PI / 2, arc.length, 1e-4)
+        assertEquals(PI, arc.arcEnd.radians, 1e-4)
     }
 
     @Test

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

@@ -48,7 +48,15 @@ class CircleTests {
     @Test
     fun circleLineIntersection() = with(Euclidean2DSpace) {
         assertTrue {
-            intersects(circle(0, 0, 1), segment(1, 1, -1, 1))
+            intersects(circle(0, 0, 1.0), segment(1, 1, -1, 1))
+        }
+
+        assertFalse {
+            intersects(circle(0, 0, 1.0), segment(1, 1, 0.5, 1))
+        }
+
+        assertFalse {
+            intersects(circle(0, 0, 1.0), segment(0, 0.5, 0, -0.5))
         }
 
         assertTrue {

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

@@ -28,7 +28,7 @@ class ObstacleTest {
     }
 
     @Test
-    fun singeObstacle() {
+    fun singePoint() {
         val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles(
             Pose2D(-5, -1, Angle.pi / 4),
             Pose2D(20, 4, Angle.pi * 3 / 4),
@@ -110,7 +110,7 @@ class ObstacleTest {
             Pose2D(x = 473093.1426061879, y = 2898525.45250675, bearing = Degrees(100.36609537114623))
         )
 
-        val obstacle =                     Obstacle(
+        val obstacle = Obstacle(
             Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0),
             Circle2D(vector(x = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0),
             Circle2D(vector(x = 465903.08440141426, y = 2893897.500160981), radius = 5000.0),
@@ -118,8 +118,8 @@ class ObstacleTest {
             Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0)
         )
 
-        startPoints.forEach { start->
+        startPoints.forEach { start ->
-            endPoints.forEach { end->
+            endPoints.forEach { end ->
                 val paths = Obstacles.avoidObstacles(
                     start,
                     end,