From 11b11118fa8675feb6c3483e0c3dd225bb5b4522 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sat, 6 May 2023 12:50:44 +0300 Subject: [PATCH] Fix circle line intersection and add a special case for a single-point obstacle --- .../src/jvmMain/kotlin/Main.kt | 39 ++++-- .../kmath/geometry/geometryExtensions.kt | 2 +- .../kmath/geometry/polygonExtensions.kt | 6 +- .../space/kscience/trajectory/Obstacle.kt | 13 +- .../space/kscience/trajectory/Obstacles.kt | 112 ++++++++++++------ .../space/kscience/kmath/geometry/ArcTests.kt | 7 +- .../kscience/kmath/geometry/CircleTests.kt | 10 +- .../space/kscience/trajectory/ObstacleTest.kt | 8 +- 8 files changed, 128 insertions(+), 69 deletions(-) diff --git a/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt b/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt index 69a1795..41550cf 100644 --- a/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt +++ b/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.trajectory( trajectory: Trajectory2D, colorPicker: (Trajectory2D) -> Color = { Color.Blue }, @@ -57,8 +59,8 @@ fun FeatureGroup.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) -> polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray) } -fun FeatureGroup.pose(pose2D: Pose2D) = with(Euclidean2DSpace){ - line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY() ) +fun FeatureGroup.pose(pose2D: Pose2D) = with(Euclidean2DSpace) { + 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 = Obstacles.avoidObstacles( - Pose2D(-1, -1, Angle.pi), - Pose2D(-1, -1, Angle.piDiv2), + enter, + exit, 1.0, obstacle ) obstacle(obstacle) - trajectory(paths.first()) { Color.Green } - trajectory(paths.last()) { Color.Magenta } + paths.forEach { + 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), - Pose2D(20, 4, Angle.pi * 3 / 4), + enter, + 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 } } } } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt index b8b4686..3a06630 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt +++ b/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 } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt index 389e949..efa3277 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt @@ -14,4 +14,8 @@ public fun Euclidean2DSpace.intersects(polygon: Polygon, circle: Circle2 polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, circle) } public fun Euclidean2DSpace.intersectsTrajectory(polygon: Polygon, trajectory: Trajectory2D): Boolean = - polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { edge -> intersectsTrajectory(edge, trajectory) } \ No newline at end of file + polygon.points.zipWithNextCircular { l, r -> + segment(l, r) + }.any { edge -> + intersectsTrajectory(edge, trajectory) + } \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt index fc451a1..68e82c0 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt @@ -14,19 +14,14 @@ public interface Obstacle { public val center: Vector2D /** - * 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 - - /** - * Check if obstacle has intersection with given [Trajectory2D] + * A polygon created from the arc centers of the obstacle */ - public fun intersects(trajectory: Trajectory2D): Boolean = - Euclidean2DSpace.intersectsTrajectory(polygon, trajectory) - + public val core: Polygon public companion object { @@ -45,7 +40,7 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) : ) } - override val polygon: Polygon by lazy { + override val core: Polygon by lazy { Euclidean2DSpace.polygon(arcs.map { it.circle.center }) } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt index 3bc03c8..e64115f 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt +++ b/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) { @@ -22,11 +41,16 @@ public class Obstacles(public val obstacles: List) { val to: ObstacleConnection?, ) { /** - * If false this tangent intersects another obstacle + * If false, this tangent intersects another obstacle */ val isValid by lazy { - obstacles.indices.none { - it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory) + with(Euclidean2DSpace) { + 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) { 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) { 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) { 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) { 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) { /** - * 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) { //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) { val isFinished get() = tangents.last().to == null fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D( - buildList { + 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) { dubinsPath: CompositeTrajectory2D, ): Collection = 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) { 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) { ) ?: 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 { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles - .thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest + compareByDescending { + 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) { } - //find nearest obstacle that has valid tangents to + //find the nearest obstacle that has valid tangents to val tangentsToFirstObstacle: Collection = obstacles.indices.sortedWith( - compareByDescending { obstacles[it].intersects(dubinsPath) } //take intersecting obstacles - .thenBy { beginArc.circle.center.distanceTo(obstacles[it].center) } //then nearest + compareByDescending { + 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) { public companion object { -// private data class LR(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 = 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, diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt index 2289a79..793c367 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt +++ b/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), - Pose2D(x = 1.0, y = -1.0, bearing = (PI * 3 / 2).radians) + (PI/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 diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt index 5f4020e..4c0bc15 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt +++ b/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 { diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt index 5d0d06b..0920501 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -28,7 +28,7 @@ class ObstacleTest { } @Test - fun singeObstacle() { + fun singePoint() { val outputTangents: List = 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-> - endPoints.forEach { end-> + startPoints.forEach { start -> + endPoints.forEach { end -> val paths = Obstacles.avoidObstacles( start, end,