teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

refactoring directions

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-04 19:02:24 +03:00
parent 639a255aaf
commit 025cb58060
1 changed files with 49 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -229,6 +229,48 @@ public class Obstacle(
} }
} }
internal fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent {
if (direction == boundaryRoute.first) {
for (i in circles.indices) {
if (circles[i] == circle) {
return tangents[i]
}
}
} else {
for (i in circles.indices) {
if (circles[i] == circle) {
if (i > 0) {
return Tangent(
circles[i],
circles[i - 1],
this,
this,
LineSegment2D(
tangents[i - 1].lineSegment.end,
tangents[i - 1].lineSegment.begin
),
direction
)
} else {
return Tangent(
circles[0],
circles.last(),
this,
this,
LineSegment2D(
tangents.last().lineSegment.end,
tangents.last().lineSegment.begin
),
direction
)
}
}
}
}
error("next tangent not found")
}
override fun equals(other: Any?): Boolean { override fun equals(other: Any?): Boolean {
if (other == null || other !is Obstacle) return false if (other == null || other !is Obstacle) return false
return circles == other.circles return circles == other.circles
@ -239,50 +281,6 @@ public class Obstacle(
} }
} }
private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent {
if (routeType == boundaryRoute) {
for (i in circles.indices) {
if (circles[i] == circle) {
return tangents[i]
}
}
} else {
for (i in circles.indices) {
if (circles[i] == circle) {
if (i > 0) {
return Tangent(
circles[i],
circles[i - 1],
this,
this,
LineSegment2D(
tangents[i - 1].lineSegment.end,
tangents[i - 1].lineSegment.begin
),
startDirection = routeType.first,
endDirection = routeType.third
)
} else {
return Tangent(
circles[0],
circles.last(),
this,
this,
LineSegment2D(
tangents.last().lineSegment.end,
tangents.last().lineSegment.begin
),
startDirection = routeType.first,
endDirection = routeType.third
)
}
}
}
}
error("next tangent not found")
}
public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
@ -417,15 +415,15 @@ private fun sortedObstacles(
private fun tangentsAlongTheObstacle( private fun tangentsAlongTheObstacle(
initialCircle: Circle2D, initialCircle: Circle2D,
initialRoute: DubinsPath.Type, direction: Trajectory2D.Direction,
finalCircle: Circle2D, finalCircle: Circle2D,
obstacle: Obstacle, obstacle: Obstacle,
): List<Tangent> { ): List<Tangent> {
val dubinsTangents = mutableListOf<Tangent>() val dubinsTangents = mutableListOf<Tangent>()
var tangent = obstacle.nextTangent(initialCircle, initialRoute) var tangent = obstacle.nextTangent(initialCircle, direction)
dubinsTangents.add(tangent) dubinsTangents.add(tangent)
while (tangent.endCircle != finalCircle) { while (tangent.endCircle != finalCircle) {
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) tangent = obstacle.nextTangent(tangent.endCircle, direction)
dubinsTangents.add(tangent) dubinsTangents.add(tangent)
} }
return dubinsTangents return dubinsTangents
@ -471,7 +469,7 @@ internal fun findAllPaths(
finalRadius: Double, finalRadius: Double,
obstacles: List<Obstacle>, obstacles: List<Obstacle>,
): List<CompositeTrajectory2D> { ): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing)) fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing))
val initialCircles = constructTangentCircles( val initialCircles = constructTangentCircles(
start, start,
@ -537,11 +535,7 @@ internal fun findAllPaths(
tangentPath.last().lineSegment.end, tangentPath.last().lineSegment.end,
tangent.startObstacle.nextTangent( tangent.startObstacle.nextTangent(
tangent.startCircle, tangent.startCircle,
DubinsPath.Type( currentDirection
currentDirection,
Trajectory2D.S,
currentDirection
),
).lineSegment.begin, ).lineSegment.begin,
currentDirection currentDirection
) )
@ -554,11 +548,7 @@ internal fun findAllPaths(
tangentsAlong = if (lengthCalculated > lengthMaxPossible) { tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
tangentsAlongTheObstacle( tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.Type( currentDirection,
currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle, tangent.startCircle,
currentObstacle currentObstacle
) )
@ -568,11 +558,7 @@ internal fun findAllPaths(
} else { } else {
tangentsAlong = tangentsAlongTheObstacle( tangentsAlong = tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.Type( currentDirection,
currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle, tangent.startCircle,
currentObstacle currentObstacle
) )