refactoring directions

This commit is contained in:
Alexander Nozik 2023-04-04 19:02:24 +03:00
parent 639a255aaf
commit 025cb58060

View File

@ -229,18 +229,8 @@ public class Obstacle(
}
}
override fun equals(other: Any?): Boolean {
if (other == null || other !is Obstacle) return false
return circles == other.circles
}
override fun hashCode(): Int {
return circles.hashCode()
}
}
private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent {
if (routeType == boundaryRoute) {
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]
@ -259,8 +249,7 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
tangents[i - 1].lineSegment.end,
tangents[i - 1].lineSegment.begin
),
startDirection = routeType.first,
endDirection = routeType.third
direction
)
} else {
return Tangent(
@ -272,8 +261,7 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
tangents.last().lineSegment.end,
tangents.last().lineSegment.begin
),
startDirection = routeType.first,
endDirection = routeType.third
direction
)
}
}
@ -283,6 +271,16 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
error("next tangent not found")
}
override fun equals(other: Any?): Boolean {
if (other == null || other !is Obstacle) return false
return circles == other.circles
}
override fun hashCode(): Int {
return circles.hashCode()
}
}
public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
@ -417,15 +415,15 @@ private fun sortedObstacles(
private fun tangentsAlongTheObstacle(
initialCircle: Circle2D,
initialRoute: DubinsPath.Type,
direction: Trajectory2D.Direction,
finalCircle: Circle2D,
obstacle: Obstacle,
): List<Tangent> {
val dubinsTangents = mutableListOf<Tangent>()
var tangent = obstacle.nextTangent(initialCircle, initialRoute)
var tangent = obstacle.nextTangent(initialCircle, direction)
dubinsTangents.add(tangent)
while (tangent.endCircle != finalCircle) {
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute)
tangent = obstacle.nextTangent(tangent.endCircle, direction)
dubinsTangents.add(tangent)
}
return dubinsTangents
@ -537,11 +535,7 @@ internal fun findAllPaths(
tangentPath.last().lineSegment.end,
tangent.startObstacle.nextTangent(
tangent.startCircle,
DubinsPath.Type(
currentDirection,
Trajectory2D.S,
currentDirection
),
).lineSegment.begin,
currentDirection
)
@ -554,11 +548,7 @@ internal fun findAllPaths(
tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
tangentsAlongTheObstacle(
currentCircle,
DubinsPath.Type(
currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle,
currentObstacle
)
@ -568,11 +558,7 @@ internal fun findAllPaths(
} else {
tangentsAlong = tangentsAlongTheObstacle(
currentCircle,
DubinsPath.Type(
currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle,
currentObstacle
)