refactoring directions
This commit is contained in:
parent
639a255aaf
commit
025cb58060
@ -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
|
||||
)
|
||||
|
Loading…
Reference in New Issue
Block a user