refactoring directions

This commit is contained in:
Alexander Nozik 2023-04-04 16:50:30 +03:00
parent fd35d7c614
commit 1e46ffbd98

View File

@ -21,7 +21,8 @@ internal data class Tangent(
val startObstacle: Obstacle, val startObstacle: Obstacle,
val endObstacle: Obstacle, val endObstacle: Obstacle,
val lineSegment: LineSegment2D, val lineSegment: LineSegment2D,
val trajectoryType: List<Trajectory2D.Type>, val startDirection: Trajectory2D.Direction,
val endDirection: Trajectory2D.Direction = startDirection
) )
private class TangentPath(val tangents: List<Tangent>) { private class TangentPath(val tangents: List<Tangent>) {
@ -131,7 +132,8 @@ private fun dubinsTangentsToCircles(
firstCircle.center + w * r1, firstCircle.center + w * r1,
secondCircle.center + w * r2 secondCircle.center + w * r2
), ),
trajectoryType = route.toList() startDirection = route.first,
endDirection = route.third
) )
) )
} else { } else {
@ -194,7 +196,8 @@ public class Obstacle(
center + w * r1, center + w * r1,
other.center + w * r2 other.center + w * r2
), ),
routeType.toList() startDirection = routeType.first,
endDirection = routeType.third
) )
) )
} else { } else {
@ -256,7 +259,8 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
tangents[i - 1].lineSegment.end, tangents[i - 1].lineSegment.end,
tangents[i - 1].lineSegment.begin tangents[i - 1].lineSegment.begin
), ),
routeType.toList() startDirection = routeType.first,
endDirection = routeType.third
) )
} else { } else {
return Tangent( return Tangent(
@ -268,7 +272,8 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
tangents.last().lineSegment.end, tangents.last().lineSegment.end,
tangents.last().lineSegment.begin tangents.last().lineSegment.begin
), ),
routeType.toList() startDirection = routeType.first,
endDirection = routeType.third
) )
} }
} }
@ -450,7 +455,7 @@ private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTraject
right.startCircle.center, right.startCircle.center,
left.lineSegment.end, left.lineSegment.end,
right.lineSegment.begin, right.lineSegment.begin,
right.trajectoryType.first() right.startDirection
) )
) )
} }
@ -491,7 +496,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.S, i) i
) )
) )
) )
@ -499,7 +504,7 @@ 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: Trajectory2D.Direction = tangentPath.last().trajectoryType.last() val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val currentObstacle = tangentPath.last().endObstacle val currentObstacle = tangentPath.last().endObstacle
var nextObstacle: Obstacle? = null var nextObstacle: Obstacle? = null
if (currentObstacle != finalObstacle) { if (currentObstacle != finalObstacle) {
@ -583,7 +588,7 @@ internal fun findAllPaths(
} }
trajectories += currentPaths.map { tangentPath -> trajectories += currentPaths.map { tangentPath ->
val lastDirection: Trajectory2D.Type = tangentPath.last().trajectoryType[2] val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val end = finalCircles[j]!! val end = finalCircles[j]!!
TangentPath( TangentPath(
tangentPath.tangents + tangentPath.tangents +
@ -593,11 +598,8 @@ internal fun findAllPaths(
Obstacle(end), Obstacle(end),
Obstacle(end), Obstacle(end),
LineSegment2D(finalPoint, finalPoint), LineSegment2D(finalPoint, finalPoint),
listOf( startDirection = lastDirection,
lastDirection, endDirection = j
Trajectory2D.S,
j
)
) )
) )
}.map { it.toTrajectory() } }.map { it.toTrajectory() }