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

[WIP] refactoring directions

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-04 15:28:02 +03:00
parent 109e050f03
commit fd35d7c614
1 changed files with 17 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -350,13 +350,13 @@ private fun arcLength(
circle: Circle2D, circle: Circle2D,
point1: DoubleVector2D, point1: DoubleVector2D,
point2: DoubleVector2D, point2: DoubleVector2D,
route: Trajectory2D.Type, direction: Trajectory2D.Direction,
): Double { ): Double {
val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x) val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x) val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
var angle = 0.0 var angle = 0.0
when (route) { when (direction) {
Trajectory2D.Type.L -> { Trajectory2D.L -> {
angle = if (phi2 >= phi1) { angle = if (phi2 >= phi1) {
phi2 - phi1 phi2 - phi1
} else { } else {
@ -364,17 +364,13 @@ private fun arcLength(
} }
} }
Trajectory2D.Type.R -> { Trajectory2D.R -> {
angle = if (phi2 >= phi1) { angle = if (phi2 >= phi1) {
2 * PI - (phi2 - phi1) 2 * PI - (phi2 - phi1)
} else { } else {
-(phi2 - phi1) -(phi2 - phi1)
} }
} }
Trajectory2D.Type.S -> {
error("L or R route is expected")
}
} }
return circle.radius * angle return circle.radius * angle
} }
@ -396,13 +392,13 @@ private fun constructTangentCircles(
val p1 = center1 - point val p1 = center1 - point
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) { return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
mapOf( mapOf(
Trajectory2D.Type.L to Circle2D(center1, r), Trajectory2D.L to Circle2D(center1, r),
Trajectory2D.Type.R to Circle2D(center2, r) Trajectory2D.R to Circle2D(center2, r)
) )
} else { } else {
mapOf( mapOf(
Trajectory2D.Type.L to Circle2D(center2, r), Trajectory2D.L to Circle2D(center2, r),
Trajectory2D.Type.R to Circle2D(center1, r) Trajectory2D.R to Circle2D(center1, r)
) )
} }
} }
@ -483,8 +479,8 @@ internal fun findAllPaths(
finalRadius finalRadius
) )
val trajectories = mutableListOf<CompositeTrajectory2D>() val trajectories = mutableListOf<CompositeTrajectory2D>()
for (i in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) { for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
for (j in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) { for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
val finalCircle = finalCircles[j]!! val finalCircle = finalCircles[j]!!
val finalObstacle = Obstacle(listOf(finalCircle)) val finalObstacle = Obstacle(listOf(finalCircle))
var currentPaths: List<TangentPath> = listOf( var currentPaths: List<TangentPath> = listOf(
@ -495,7 +491,7 @@ internal fun findAllPaths(
Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)),
Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)),
LineSegment2D(startingPoint, startingPoint), LineSegment2D(startingPoint, startingPoint),
listOf(i, Trajectory2D.Type.S, i) listOf(i, Trajectory2D.S, i)
) )
) )
) )
@ -503,13 +499,13 @@ internal fun findAllPaths(
val newPaths = mutableListOf<TangentPath>() val newPaths = mutableListOf<TangentPath>()
for (tangentPath: TangentPath in currentPaths) { for (tangentPath: TangentPath in currentPaths) {
val currentCircle = tangentPath.last().endCircle val currentCircle = tangentPath.last().endCircle
val currentDirection = tangentPath.last().trajectoryType.last() val currentDirection: Trajectory2D.Direction = tangentPath.last().trajectoryType.last()
val currentObstacle = tangentPath.last().endObstacle val currentObstacle = tangentPath.last().endObstacle
var nextObstacle: Obstacle? = null var nextObstacle: Obstacle? = null
if (currentObstacle != finalObstacle) { if (currentObstacle != finalObstacle) {
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.Type.S, Trajectory2D.S,
j j
)] )]
for (obstacle in sortedObstacles(currentObstacle, obstacles)) { for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
@ -538,7 +534,7 @@ internal fun findAllPaths(
tangent.startCircle, tangent.startCircle,
DubinsPath.Type( DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.Type.S, Trajectory2D.S,
currentDirection currentDirection
), ),
).lineSegment.begin, ).lineSegment.begin,
@ -555,7 +551,7 @@ internal fun findAllPaths(
currentCircle, currentCircle,
DubinsPath.Type( DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.Type.S, Trajectory2D.S,
currentDirection currentDirection
), ),
tangent.startCircle, tangent.startCircle,
@ -569,7 +565,7 @@ internal fun findAllPaths(
currentCircle, currentCircle,
DubinsPath.Type( DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.Type.S, Trajectory2D.S,
currentDirection currentDirection
), ),
tangent.startCircle, tangent.startCircle,
@ -599,7 +595,7 @@ internal fun findAllPaths(
LineSegment2D(finalPoint, finalPoint), LineSegment2D(finalPoint, finalPoint),
listOf( listOf(
lastDirection, lastDirection,
Trajectory2D.Type.S, Trajectory2D.S,
j j
) )
) )