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

refactoring directions

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-04 19:02:24 +03:00
parent 639a255aaf
commit 025cb58060
1 changed files with 49 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt
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
)
}
}
@ -281,6 +269,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))
@ -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
@ -471,7 +469,7 @@ internal fun findAllPaths(
finalRadius: Double,
obstacles: List<Obstacle>,
): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing))
fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing))
val initialCircles = constructTangentCircles(
start,
@ -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
)