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 407810d..1dc2fdc 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt @@ -101,15 +101,15 @@ public class Obstacles(public val obstacles: List) { obstacleIndex: Int, obstacleDirection: Trajectory2D.Direction, arc: CircleTrajectory2D - ): ObstacleTangent = with(Euclidean2DSpace) { + ): ObstacleTangent? = with(Euclidean2DSpace) { val obstacle = obstacles[obstacleIndex] for (circleIndex in obstacle.arcs.indices) { val obstacleArc = obstacle.arcs[circleIndex] - tangentsBetweenArcs( - obstacleArc, - arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle + tangentsBetweenCircles( + obstacleArc.circle, + arc.circle )[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf { - !obstacle.intersects(it) + obstacleArc.containsPoint(it.begin) && !obstacle.intersects(it) }?.let { return ObstacleTangent( it, @@ -118,7 +118,7 @@ public class Obstacles(public val obstacles: List) { ) } } - error("Tangent from obstacle $obstacleIndex to circle ${arc.circle} not found") + return null } @@ -206,7 +206,7 @@ public class Obstacles(public val obstacles: List) { connection.obstacleIndex, connection.direction, endArc - ) + ) ?: return emptySet() if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf( TangentPath(tangents + tangentToEnd) 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 a1a2f01..14ba762 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -7,6 +7,7 @@ package space.kscience.trajectory import space.kscience.kmath.geometry.Angle import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.Degrees import space.kscience.kmath.geometry.Euclidean2DSpace.vector import space.kscience.kmath.geometry.degrees import kotlin.math.PI @@ -29,8 +30,8 @@ class ObstacleTest { @Test fun singeObstacle() { val outputTangents: List = Obstacles.avoidObstacles( - Pose2D(-5,-1, Angle.pi/4), - Pose2D(20,4, Angle.pi*3/4), + Pose2D(-5, -1, Angle.pi / 4), + Pose2D(20, 4, Angle.pi * 3 / 4), 0.5, Obstacle(Circle2D(vector(7.0, 1.0), 5.0)) ) @@ -42,8 +43,8 @@ class ObstacleTest { @Test fun twoObstacles() { val paths = Obstacles.avoidObstacles( - Pose2D(-5,-1, Angle.pi/4), - Pose2D(20,4, Angle.pi*3/4), + Pose2D(-5, -1, Angle.pi / 4), + Pose2D(20, 4, Angle.pi * 3 / 4), 0.5, Obstacle( Circle2D(vector(1.0, 6.5), 0.5), @@ -99,19 +100,35 @@ class ObstacleTest { @Test fun largeCoordinates() { - val paths = Obstacles.avoidObstacles( - Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.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), - Circle2D(vector(x = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0), - Circle2D(vector(x = 465903.08440141426, y = 2893897.500160981), radius = 5000.0), - Circle2D(vector(x = 462421.19397653354, y = 2879496.4842121634), radius = 5000.0), - Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0) - ) + val startPoints = listOf( + Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = Degrees(3.401475378237137)) ) - assertTrue { paths.isNotEmpty() } + + val endPoints = listOf( + Pose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees), + Pose2D(x = 473093.1426061879, y = 2898525.45250675, bearing = Degrees(100.36609537114623)) + ) + + 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), + Circle2D(vector(x = 462421.19397653354, y = 2879496.4842121634), radius = 5000.0), + Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0) + ) + + startPoints.forEach { start-> + endPoints.forEach { end-> + val paths = Obstacles.avoidObstacles( + start, + end, + 5000.0, + obstacle + ) + assertTrue { paths.isNotEmpty() } + } + } + } } \ No newline at end of file