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 13:44:07 +03:00
parent cac5841401
commit 4e7ef35280
1 changed files with 73 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,15 +10,24 @@ import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.* import kotlin.math.*
internal data class Tangent( internal data class ObstacleNode(
val startCircle: Circle2D, val obstacle: Obstacle,
val endCircle: Circle2D, val nodeIndex: Int,
val startObstacle: ObstacleShell, val direction: Trajectory2D.Direction,
val endObstacle: ObstacleShell, ){
val circle get() = obstacle.circles[nodeIndex]
}
internal data class ObstacleTangent(
val lineSegment: LineSegment2D, val lineSegment: LineSegment2D,
val startDirection: Trajectory2D.Direction, val beginNode: ObstacleNode,
val endDirection: Trajectory2D.Direction = startDirection, val endNode: ObstacleNode,
) : LineSegment2D by lineSegment ) : LineSegment2D by lineSegment{
val startCircle get() = beginNode.circle
val startDirection get() = beginNode.direction
val endCircle get() = endNode.circle
val endDirection get() = endNode.direction
}
private class LR<T>(val l: T, val r: T) { private class LR<T>(val l: T, val r: T) {
@ -28,11 +37,11 @@ private class LR<T>(val l: T, val r: T) {
} }
} }
private class TangentPath(val tangents: List<Tangent>) { private class TangentPath(val tangents: List<ObstacleTangent>) {
fun last() = tangents.last() fun last() = tangents.last()
} }
private fun TangentPath(vararg tangents: Tangent) = TangentPath(listOf(*tangents)) private fun TangentPath(vararg tangents: ObstacleTangent) = TangentPath(listOf(*tangents))
/** /**
* Create inner and outer tangents between two circles. * Create inner and outer tangents between two circles.
@ -146,6 +155,8 @@ internal class ObstacleShell(
} }
} }
constructor(obstacle: Obstacle) : this(obstacle.circles)
/** /**
* Check if segment has any intersections with this obstacle * Check if segment has any intersections with this obstacle
*/ */
@ -153,8 +164,10 @@ 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) }
fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent { /**
val circleIndex = circles.indexOf(circle) * Tangent to next obstacle node in given direction
*/
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) {
@ -163,32 +176,29 @@ internal class ObstacleShell(
if (circleIndex == 0) circles.lastIndex else circleIndex - 1 if (circleIndex == 0) circles.lastIndex else circleIndex - 1
} }
return Tangent( return ObstacleTangent(
circle,
circles[nextCircleIndex],
this,
this,
LineSegment( LineSegment(
shell[nextCircleIndex].end, shell[nextCircleIndex].end,
shell[nextCircleIndex].begin shell[nextCircleIndex].begin
), ),
direction ObstacleNode(this,circleIndex, direction),
ObstacleNode(this,nextCircleIndex, direction),
) )
} }
internal fun tangentsAlong( internal fun tangentsAlong(
initialCircle: Circle2D, initialCircleIndex: Int,
direction: Trajectory2D.Direction, direction: Trajectory2D.Direction,
finalCircle: Circle2D, finalCircleIndex: Int,
): List<Tangent> { ): List<ObstacleTangent> {
val dubinsTangents = mutableListOf<Tangent>() return buildList {
var tangent = nextTangent(initialCircle, direction) var currentIndex = initialCircleIndex
dubinsTangents.add(tangent) do {
while (tangent.endCircle != finalCircle) { val tangent = nextTangent(currentIndex, direction)
tangent = nextTangent(tangent.endCircle, direction) add(tangent)
dubinsTangents.add(tangent) currentIndex = tangent.endNode.nodeIndex
} while (currentIndex != finalCircleIndex)
} }
return dubinsTangents
} }
override fun equals(other: Any?): Boolean { override fun equals(other: Any?): Boolean {
@ -246,19 +256,18 @@ private fun LineSegment2D.intersectsCircle(circle: Circle2D): Boolean {
* In general generates 4 paths. * In general generates 4 paths.
* TODO check intersections. * TODO check intersections.
*/ */
private fun outerTangents(first: ObstacleShell, second: ObstacleShell): Map<DubinsPath.Type, Tangent> = buildMap { private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, ObstacleTangent> = buildMap {
for (firstCircle in first.circles) { for (firstCircleIndex in first.circles.indices) {
for (secondCircle in second.circles) { for (secondCircleIndex in second.circles.indices) {
for ((pathType, segment) in tangentsBetweenCircles(firstCircle, secondCircle)) { for ((pathType, segment) in tangentsBetweenCircles(
val tangent = Tangent( first.circles[firstCircleIndex],
firstCircle, second.circles[secondCircleIndex]
secondCircle, )) {
first, val tangent = ObstacleTangent(
second,
segment, segment,
pathType.first, ObstacleNode(first, firstCircleIndex, pathType.first),
pathType.third ObstacleNode(second, secondCircleIndex, pathType.third)
) )
if (!(first.intersects(tangent)) && !(second.intersects(tangent))) { if (!(first.intersects(tangent)) && !(second.intersects(tangent))) {
@ -331,9 +340,9 @@ private fun constructTangentCircles(
} }
private fun sortedObstacles( private fun sortedObstacles(
currentObstacle: ObstacleShell, currentObstacle: Obstacle,
obstacles: List<ObstacleShell>, obstacles: List<Obstacle>,
): List<ObstacleShell> { ): List<Obstacle> {
return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) } return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) }
} }
@ -345,7 +354,7 @@ private fun allFinished(
finalObstacle: Obstacle, finalObstacle: Obstacle,
): Boolean { ): Boolean {
for (path in paths) { for (path in paths) {
if (path.last().endObstacle != finalObstacle) { if (path.last().endNode.obstacle !== finalObstacle) {
return false return false
} }
} }
@ -357,7 +366,7 @@ private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D( private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
buildList { buildList {
tangents.zipWithNext().forEach { (left, right) -> tangents.zipWithNext().forEach { (left, right: ObstacleTangent) ->
add(left.lineSegment.toTrajectory()) add(left.lineSegment.toTrajectory())
add( add(
CircleTrajectory2D.of( CircleTrajectory2D.of(
@ -403,18 +412,16 @@ internal fun findAllPaths(
for (i in listOf(Trajectory2D.L, Trajectory2D.R)) { for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
for (j in listOf(Trajectory2D.L, Trajectory2D.R)) { for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
//Using obstacle to minimize code bloat //Using obstacle to minimize code bloat
val initialObstacle = ObstacleShell(initialCircles[i])
val finalObstacle = ObstacleShell(finalCircles[j]) val finalObstacle = ObstacleShell(finalCircles[j])
var currentPaths: List<TangentPath> = listOf( var currentPaths: List<TangentPath> = listOf(
TangentPath( TangentPath(
//We need only the direction of the final segment from this //We need only the direction of the final segment from this
Tangent( ObstacleTangent(
initialCircles[i],
initialCircles[i],
ObstacleShell(initialCircles[i]),
ObstacleShell(initialCircles[i]),
LineSegment(start, start), LineSegment(start, start),
i ObstacleNode(initialObstacle, 0, i),
ObstacleNode(initialObstacle, 0, i),
) )
) )
) )
@ -423,16 +430,16 @@ internal fun findAllPaths(
val newPaths = mutableListOf<TangentPath>() val newPaths = mutableListOf<TangentPath>()
// for each path propagate it one obstacle further // for each path propagate it one obstacle further
for (tangentPath: TangentPath in currentPaths) { for (tangentPath: TangentPath in currentPaths) {
val currentCircle = tangentPath.last().endCircle val currentNode = tangentPath.last().endNode
val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val currentObstacle = tangentPath.last().endObstacle val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle)
// If path is finished, ignore it // If path is finished, ignore it
// TODO avoid returning to ignored obstacle on the next cycle // TODO avoid returning to ignored obstacle on the next cycle
if (currentObstacle == finalObstacle) { if (currentObstacle == finalObstacle) {
newPaths.add(tangentPath) newPaths.add(tangentPath)
} else { } else {
val tangentToFinal: Tangent = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( val tangentToFinal: ObstacleTangent = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.S, Trajectory2D.S,
j j
@ -446,7 +453,7 @@ internal fun findAllPaths(
//TODO add break check for end of path //TODO add break check for end of path
// All valid tangents from current obstacle to the next one // All valid tangents from current obstacle to the next one
val nextTangents: Collection<Tangent> = outerTangents( val nextTangents: Collection<ObstacleTangent> = outerTangents(
currentObstacle, currentObstacle,
nextObstacle nextObstacle
).filter { (key, tangent) -> ).filter { (key, tangent) ->
@ -456,18 +463,18 @@ internal fun findAllPaths(
}.values }.values
for (tangent in nextTangents) { for (tangent in nextTangents) {
val tangentsAlong = if (tangent.startCircle == tangentPath.last().endCircle) { val tangentsAlong: List<ObstacleTangent> = 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
if (tangent.startObstacle.circles.size < 2) { if (currentObstacle.circles.size < 2) {
emptyList() emptyList()
} else { } else {
val lengthMaxPossible = arcLength( val lengthMaxPossible = arcLength(
tangent.startCircle, tangent.startCircle,
tangentPath.last().lineSegment.end, tangentPath.last().lineSegment.end,
tangent.startObstacle.nextTangent( currentObstacle.nextTangent(
tangent.startCircle, tangent.beginNode.nodeIndex,
currentDirection currentDirection
).lineSegment.begin, ).lineSegment.begin,
currentDirection currentDirection
@ -482,9 +489,9 @@ internal fun findAllPaths(
// ensure that path does not go inside the obstacle // ensure that path does not go inside the obstacle
if (lengthCalculated > lengthMaxPossible) { if (lengthCalculated > lengthMaxPossible) {
currentObstacle.tangentsAlong( currentObstacle.tangentsAlong(
currentCircle, currentNode.nodeIndex,
currentDirection, currentDirection,
tangent.startCircle, tangent.beginNode.nodeIndex,
) )
} else { } else {
emptyList() emptyList()
@ -492,9 +499,9 @@ internal fun findAllPaths(
} }
} else { } else {
currentObstacle.tangentsAlong( currentObstacle.tangentsAlong(
currentCircle, currentNode.nodeIndex,
currentDirection, currentDirection,
tangent.startCircle, tangent.beginNode.nodeIndex,
) )
} }
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
@ -506,17 +513,13 @@ 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 = finalCircles[j] val end = Obstacle(finalCircles[j])
TangentPath( TangentPath(
tangentPath.tangents + tangentPath.tangents +
Tangent( ObstacleTangent(
end,
end,
ObstacleShell(end),
ObstacleShell(end),
LineSegment(finish, finish), LineSegment(finish, finish),
startDirection = lastDirection, ObstacleNode(end, 0, j),
endDirection = j ObstacleNode(end, 0, j)
) )
) )
}.map { it.toTrajectory() } }.map { it.toTrajectory() }