kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

0.3.1-dev-11 #510

Merged
altavir merged 80 commits from dev into master 2023-04-05 18:46:36 +03:00
Conversation 0 Commits 80 Files Changed 264 +49 -63
1 changed files with 49 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 025cb58060 - Show all commits

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 { internal fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent {
if (other == null || other !is Obstacle) return false if (direction == boundaryRoute.first) {
return circles == other.circles
}
override fun hashCode(): Int {
return circles.hashCode()
}
}
private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent {
if (routeType == boundaryRoute) {
for (i in circles.indices) { for (i in circles.indices) {
if (circles[i] == circle) { if (circles[i] == circle) {
return tangents[i] 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.end,
tangents[i - 1].lineSegment.begin tangents[i - 1].lineSegment.begin
), ),
startDirection = routeType.first, direction
endDirection = routeType.third
) )
} else { } else {
return Tangent( return Tangent(
@ -272,8 +261,7 @@ 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
), ),
startDirection = routeType.first, direction
endDirection = routeType.third
) )
} }
} }
@ -281,6 +269,16 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type):
} }
error("next tangent not found") 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)) public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
@ -417,15 +415,15 @@ private fun sortedObstacles(
private fun tangentsAlongTheObstacle( private fun tangentsAlongTheObstacle(
initialCircle: Circle2D, initialCircle: Circle2D,
initialRoute: DubinsPath.Type, direction: Trajectory2D.Direction,
finalCircle: Circle2D, finalCircle: Circle2D,
obstacle: Obstacle, obstacle: Obstacle,
): List<Tangent> { ): List<Tangent> {
val dubinsTangents = mutableListOf<Tangent>() val dubinsTangents = mutableListOf<Tangent>()
var tangent = obstacle.nextTangent(initialCircle, initialRoute) var tangent = obstacle.nextTangent(initialCircle, direction)
dubinsTangents.add(tangent) dubinsTangents.add(tangent)
while (tangent.endCircle != finalCircle) { while (tangent.endCircle != finalCircle) {
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) tangent = obstacle.nextTangent(tangent.endCircle, direction)
dubinsTangents.add(tangent) dubinsTangents.add(tangent)
} }
return dubinsTangents return dubinsTangents
@ -471,7 +469,7 @@ internal fun findAllPaths(
finalRadius: Double, finalRadius: Double,
obstacles: List<Obstacle>, obstacles: List<Obstacle>,
): List<CompositeTrajectory2D> { ): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing)) fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing))
val initialCircles = constructTangentCircles( val initialCircles = constructTangentCircles(
start, start,
@ -537,11 +535,7 @@ internal fun findAllPaths(
tangentPath.last().lineSegment.end, tangentPath.last().lineSegment.end,
tangent.startObstacle.nextTangent( tangent.startObstacle.nextTangent(
tangent.startCircle, tangent.startCircle,
DubinsPath.Type(
currentDirection,
Trajectory2D.S,
currentDirection currentDirection
),
).lineSegment.begin, ).lineSegment.begin,
currentDirection currentDirection
) )
@ -554,11 +548,7 @@ internal fun findAllPaths(
tangentsAlong = if (lengthCalculated > lengthMaxPossible) { tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
tangentsAlongTheObstacle( tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle, tangent.startCircle,
currentObstacle currentObstacle
) )
@ -568,11 +558,7 @@ internal fun findAllPaths(
} else { } else {
tangentsAlong = tangentsAlongTheObstacle( tangentsAlong = tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.S,
currentDirection
),
tangent.startCircle, tangent.startCircle,
currentObstacle currentObstacle
) )