refactoring directions
This commit is contained in:
parent
fd35d7c614
commit
1e46ffbd98
@ -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() }
|
||||||
|
Loading…
Reference in New Issue
Block a user