kscience/maps-kt
8
0
Fork 0
Code Issues Pull Requests 3 Packages Projects Releases 1 Wiki Activity

Add possibility to skip some paths if target is close to the obstacle

Browse Source
This commit is contained in:
Alexander Nozik 2023-05-02 13:30:41 +03:00
parent 35efce087e
commit 74b86dbc59
2 changed files with 40 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt
View File

@ -101,15 +101,15 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
obstacleIndex: Int, obstacleIndex: Int,
obstacleDirection: Trajectory2D.Direction, obstacleDirection: Trajectory2D.Direction,
arc: CircleTrajectory2D arc: CircleTrajectory2D
): ObstacleTangent = with(Euclidean2DSpace) { ): ObstacleTangent? = with(Euclidean2DSpace) {
val obstacle = obstacles[obstacleIndex] val obstacle = obstacles[obstacleIndex]
for (circleIndex in obstacle.arcs.indices) { for (circleIndex in obstacle.arcs.indices) {
val obstacleArc = obstacle.arcs[circleIndex] val obstacleArc = obstacle.arcs[circleIndex]
tangentsBetweenArcs( tangentsBetweenCircles(
obstacleArc, obstacleArc.circle,
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle arc.circle
)[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf { )[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
!obstacle.intersects(it) obstacleArc.containsPoint(it.begin) && !obstacle.intersects(it)
}?.let { }?.let {
return ObstacleTangent( return ObstacleTangent(
it, it,
@ -118,7 +118,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
) )
} }
} }
error("Tangent from obstacle $obstacleIndex to circle ${arc.circle} not found") return null
} }
@ -206,7 +206,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
connection.obstacleIndex, connection.obstacleIndex,
connection.direction, connection.direction,
endArc endArc
) ) ?: return emptySet()
if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf( if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
TangentPath(tangents + tangentToEnd) TangentPath(tangents + tangentToEnd)

33
trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
View File

@ -7,6 +7,7 @@ package space.kscience.trajectory
import space.kscience.kmath.geometry.Angle import space.kscience.kmath.geometry.Angle
import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Degrees
import space.kscience.kmath.geometry.Euclidean2DSpace.vector import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.degrees import space.kscience.kmath.geometry.degrees
import kotlin.math.PI import kotlin.math.PI
@ -29,8 +30,8 @@ class ObstacleTest {
@Test @Test
fun singeObstacle() { fun singeObstacle() {
val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles( val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-5,-1, Angle.pi/4), Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20,4, Angle.pi*3/4), Pose2D(20, 4, Angle.pi * 3 / 4),
0.5, 0.5,
Obstacle(Circle2D(vector(7.0, 1.0), 5.0)) Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
) )
@ -42,8 +43,8 @@ class ObstacleTest {
@Test @Test
fun twoObstacles() { fun twoObstacles() {
val paths = Obstacles.avoidObstacles( val paths = Obstacles.avoidObstacles(
Pose2D(-5,-1, Angle.pi/4), Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20,4, Angle.pi*3/4), Pose2D(20, 4, Angle.pi * 3 / 4),
0.5, 0.5,
Obstacle( Obstacle(
Circle2D(vector(1.0, 6.5), 0.5), Circle2D(vector(1.0, 6.5), 0.5),
@ -99,19 +100,35 @@ class ObstacleTest {
@Test @Test
fun largeCoordinates() { fun largeCoordinates() {
val paths = Obstacles.avoidObstacles( val startPoints = listOf(
Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees), Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = Degrees(3.401475378237137))
)
val endPoints = listOf(
Pose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees), Pose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
5000.0, Pose2D(x = 473093.1426061879, y = 2898525.45250675, bearing = Degrees(100.36609537114623))
Obstacle( )
val obstacle = Obstacle(
Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0), 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 = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0),
Circle2D(vector(x = 465903.08440141426, y = 2893897.500160981), 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 = 462421.19397653354, y = 2879496.4842121634), radius = 5000.0),
Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), 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() } assertTrue { paths.isNotEmpty() }
} }
}
}
} }