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,
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<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.direction,
endArc
)
) ?: return emptySet()
if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
TangentPath(tangents + tangentToEnd)

49
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.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<Trajectory2D> = 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() }
}
}
}
}