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

Obstacle avoidance refactoring

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-23 21:20:32 +03:00
parent 4e7ef35280
commit a6a5baa352
4 changed files with 96 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt
View File

@ -32,9 +32,6 @@ public interface DubinsPose2D : DoubleVector2D {
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" } require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
return atan2(vector2D.y, vector2D.x).radians return atan2(vector2D.y, vector2D.x).radians
} }
public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
DubinsPose2D(point, vectorToBearing(direction))
} }
} }
@ -72,4 +69,10 @@ public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
} }
} }
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta) public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, bearing)
public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))
public fun DubinsPose2D(x: Number, y: Number, bearing: Angle): DubinsPose2D =
DubinsPose2DImpl(Euclidean2DSpace.vector(x, y), bearing)

38
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt
View File

@ -17,8 +17,10 @@ public interface Obstacle {
public fun intersects(segment: LineSegment2D): Boolean public fun intersects(segment: LineSegment2D): Boolean
public fun intersects(circle: Circle2D): Boolean
public companion object { public companion object {
public fun allPathsAvoiding( public fun avoidObstacles(
start: DubinsPose2D, start: DubinsPose2D,
finish: DubinsPose2D, finish: DubinsPose2D,
trajectoryRadius: Double, trajectoryRadius: Double,
@ -30,7 +32,7 @@ public interface Obstacle {
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
} }
public fun allPathsAvoiding( public fun avoidPolygons(
start: DubinsPose2D, start: DubinsPose2D,
finish: DubinsPose2D, finish: DubinsPose2D,
trajectoryRadius: Double, trajectoryRadius: Double,
@ -42,11 +44,43 @@ public interface Obstacle {
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
} }
public fun avoidObstacles(
start: DubinsPose2D,
finish: DubinsPose2D,
trajectoryRadius: Double,
obstacles: Collection<Obstacle>,
): List<CompositeTrajectory2D> {
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
ObstacleShell(polygon.circles)
}
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
}
public fun avoidPolygons(
start: DubinsPose2D,
finish: DubinsPose2D,
trajectoryRadius: Double,
obstacles: Collection<Polygon<Double>>,
): List<CompositeTrajectory2D> {
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
}
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
}
} }
} }
public fun Obstacle.intersectsTrajectory(trajectory: Trajectory2D): Boolean = when (trajectory) {
is CircleTrajectory2D -> intersects(trajectory.circle)
is StraightTrajectory2D -> intersects(trajectory)
is CompositeTrajectory2D -> trajectory.segments.any { intersectsTrajectory(it) }
}
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles)) public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles))
public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
ObstacleShell(points.map { Circle2D(it, radius) })
//public fun Trajectory2D.intersects( //public fun Trajectory2D.intersects(
// polygon: Polygon<Double>, // polygon: Polygon<Double>,
// radius: Double, // radius: Double,

40
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt
View File

@ -14,7 +14,7 @@ internal data class ObstacleNode(
val obstacle: Obstacle, val obstacle: Obstacle,
val nodeIndex: Int, val nodeIndex: Int,
val direction: Trajectory2D.Direction, val direction: Trajectory2D.Direction,
){ ) {
val circle get() = obstacle.circles[nodeIndex] val circle get() = obstacle.circles[nodeIndex]
} }
@ -22,7 +22,7 @@ internal data class ObstacleTangent(
val lineSegment: LineSegment2D, val lineSegment: LineSegment2D,
val beginNode: ObstacleNode, val beginNode: ObstacleNode,
val endNode: ObstacleNode, val endNode: ObstacleNode,
) : LineSegment2D by lineSegment{ ) : LineSegment2D by lineSegment {
val startCircle get() = beginNode.circle val startCircle get() = beginNode.circle
val startDirection get() = beginNode.direction val startDirection get() = beginNode.direction
val endCircle get() = endNode.circle val endCircle get() = endNode.circle
@ -164,10 +164,13 @@ internal class ObstacleShell(
shell.any { tangent -> segment.intersectsSegment(tangent) } shell.any { tangent -> segment.intersectsSegment(tangent) }
|| circles.any { circle -> segment.intersectsCircle(circle) } || circles.any { circle -> segment.intersectsCircle(circle) }
override fun intersects(circle: Circle2D): Boolean =
shell.any{tangent -> tangent.intersectsCircle(circle) }
/** /**
* Tangent to next obstacle node in given direction * Tangent to next obstacle node in given direction
*/ */
fun nextTangent(circleIndex:Int, direction: Trajectory2D.Direction): ObstacleTangent { fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent {
if (circleIndex == -1) error("Circle does not belong to this tangent") if (circleIndex == -1) error("Circle does not belong to this tangent")
val nextCircleIndex = if (direction == this.shellDirection) { val nextCircleIndex = if (direction == this.shellDirection) {
@ -181,15 +184,18 @@ internal class ObstacleShell(
shell[nextCircleIndex].end, shell[nextCircleIndex].end,
shell[nextCircleIndex].begin shell[nextCircleIndex].begin
), ),
ObstacleNode(this,circleIndex, direction), ObstacleNode(this, circleIndex, direction),
ObstacleNode(this,nextCircleIndex, direction), ObstacleNode(this, nextCircleIndex, direction),
) )
} }
/**
* All tangents in given direction
*/
internal fun tangentsAlong( internal fun tangentsAlong(
initialCircleIndex: Int, initialCircleIndex: Int,
direction: Trajectory2D.Direction,
finalCircleIndex: Int, finalCircleIndex: Int,
direction: Trajectory2D.Direction,
): List<ObstacleTangent> { ): List<ObstacleTangent> {
return buildList { return buildList {
var currentIndex = initialCircleIndex var currentIndex = initialCircleIndex
@ -389,8 +395,7 @@ internal fun findAllPaths(
finalRadius: Double, finalRadius: Double,
obstacles: List<ObstacleShell>, obstacles: List<ObstacleShell>,
): List<CompositeTrajectory2D> { ): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = fun DubinsPose2D.direction() = Euclidean2DSpace.vector(cos(bearing), sin(bearing))
Euclidean2DSpace.vector(space.kscience.kmath.geometry.cos(bearing), space.kscience.kmath.geometry.sin(bearing))
// two circles for the initial point // two circles for the initial point
val initialCircles = constructTangentCircles( val initialCircles = constructTangentCircles(
@ -439,11 +444,12 @@ internal fun findAllPaths(
if (currentObstacle == finalObstacle) { if (currentObstacle == finalObstacle) {
newPaths.add(tangentPath) newPaths.add(tangentPath)
} else { } else {
val tangentToFinal: ObstacleTangent = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( val tangentToFinal: ObstacleTangent =
currentDirection, outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
Trajectory2D.S, currentDirection,
j Trajectory2D.S,
)] ?: break j
)] ?: break
// searching for the nearest obstacle that intersects with the direct path // searching for the nearest obstacle that intersects with the direct path
val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle -> val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle ->
@ -463,7 +469,7 @@ internal fun findAllPaths(
}.values }.values
for (tangent in nextTangents) { for (tangent in nextTangents) {
val tangentsAlong: List<ObstacleTangent> = if (tangent.startCircle === tangentPath.last().endCircle) { val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) {
//if the previous segment last circle is the same as first circle of the next segment //if the previous segment last circle is the same as first circle of the next segment
//If obstacle consists of single circle, do not walk around //If obstacle consists of single circle, do not walk around
@ -490,8 +496,8 @@ internal fun findAllPaths(
if (lengthCalculated > lengthMaxPossible) { if (lengthCalculated > lengthMaxPossible) {
currentObstacle.tangentsAlong( currentObstacle.tangentsAlong(
currentNode.nodeIndex, currentNode.nodeIndex,
currentDirection,
tangent.beginNode.nodeIndex, tangent.beginNode.nodeIndex,
currentDirection,
) )
} else { } else {
emptyList() emptyList()
@ -500,8 +506,8 @@ internal fun findAllPaths(
} else { } else {
currentObstacle.tangentsAlong( currentObstacle.tangentsAlong(
currentNode.nodeIndex, currentNode.nodeIndex,
currentDirection,
tangent.beginNode.nodeIndex, tangent.beginNode.nodeIndex,
currentDirection,
) )
} }
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
@ -512,7 +518,7 @@ internal fun findAllPaths(
} }
trajectories += currentPaths.map { tangentPath -> trajectories += currentPaths.map { tangentPath ->
val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection // val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val end = Obstacle(finalCircles[j]) val end = Obstacle(finalCircles[j])
TangentPath( TangentPath(
tangentPath.tangents + tangentPath.tangents +

41
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.Circle2D import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.vector import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.degrees
import kotlin.test.Test import kotlin.test.Test
import kotlin.test.assertEquals import kotlin.test.assertEquals
@ -19,9 +20,9 @@ class ObstacleTest {
val finalPoint = vector(20.0, 4.0) val finalPoint = vector(20.0, 4.0)
val finalDirection = vector(1.0, -1.0) val finalDirection = vector(1.0, -1.0)
val outputTangents = Obstacle.allPathsAvoiding( val outputTangents = Obstacle.avoidObstacles(
DubinsPose2D.of(startPoint, startDirection), DubinsPose2D(startPoint, startDirection),
DubinsPose2D.of(finalPoint, finalDirection), DubinsPose2D(finalPoint, finalDirection),
startRadius, startRadius,
Obstacle(Circle2D(vector(7.0, 1.0), 5.0)) Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
) )
@ -37,9 +38,9 @@ class ObstacleTest {
val finalPoint = vector(20.0, 4.0) val finalPoint = vector(20.0, 4.0)
val finalDirection = vector(1.0, -1.0) val finalDirection = vector(1.0, -1.0)
val paths = Obstacle.allPathsAvoiding( val paths = Obstacle.avoidObstacles(
DubinsPose2D.of(startPoint, startDirection), DubinsPose2D(startPoint, startDirection),
DubinsPose2D.of(finalPoint, finalDirection), DubinsPose2D(finalPoint, finalDirection),
radius, radius,
Obstacle( Obstacle(
Circle2D(vector(1.0, 6.5), 0.5), Circle2D(vector(1.0, 6.5), 0.5),
@ -59,15 +60,15 @@ class ObstacleTest {
@Test @Test
fun nearPoints() { fun nearPoints() {
val startPoint = vector(-1.0, 0.0) val startPoint = vector(-1.0, -1.0)
val startDirection = vector(0.0, 1.0) val startDirection = vector(0.0, 1.0)
val startRadius = 1.0 val startRadius = 1.0
val finalPoint = vector(0, -1) val finalPoint = vector(-1, -1)
val finalDirection = vector(1.0, 0) val finalDirection = vector(1.0, 0)
val paths = Obstacle.allPathsAvoiding( val paths = Obstacle.avoidObstacles(
DubinsPose2D.of(startPoint, startDirection), DubinsPose2D(startPoint, startDirection),
DubinsPose2D.of(finalPoint, finalDirection), DubinsPose2D(finalPoint, finalDirection),
startRadius, startRadius,
Obstacle( Obstacle(
Circle2D(vector(0.0, 0.0), 1.0), Circle2D(vector(0.0, 0.0), 1.0),
@ -81,6 +82,24 @@ class ObstacleTest {
//assertEquals(28.9678224, length, 1e-6) //assertEquals(28.9678224, length, 1e-6)
} }
@Test
fun fromMap() {
val paths = Obstacle.avoidObstacles(
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
DubinsPose2D(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 length = paths.minOf { it.length }
}
@Test @Test
fun equalObstacles() { fun equalObstacles() {
val circle1 = Circle2D(vector(1.0, 6.5), 0.5) val circle1 = Circle2D(vector(1.0, 6.5), 0.5)