diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index 56aa88e4a..e1c967e2a 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -276,7 +276,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D, public fun allFinished(paths: List>, finalObstacle: DubinsObstacle): Boolean { for (path in paths) { - if (path[-1].endObstacle != finalObstacle) { + if (path.last().endObstacle != finalObstacle) { return false } } @@ -296,8 +296,8 @@ public fun pathLength(path: List): Double { return tangentsLength + arcsLength } -public fun shortestPath(path: List>): List> { - return path.sortedBy { pathLength(it) } +public fun shortestPath(path: List>): List { + return path.sortedBy { pathLength(it) }[0] } public typealias Path = List @@ -309,7 +309,7 @@ public fun findAllPaths( finalDirection: DoubleVector2D, finalRadius: Double, obstacles: List -) { +): List> { val initialCircles = constructTangentCircles( startingPoint, startingDirection, @@ -318,15 +318,15 @@ public fun findAllPaths( finalPoint, finalDirection, finalRadius) - var outputTangents = mutableMapOf>() + var outputTangents = mutableMapOf>>() for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { val finalCircle = finalCircles[j]!! val finalObstacle = DubinsObstacle(listOf(finalCircle)) outputTangents[listOf(i, DubinsPath.SimpleType.S, - j)] = listOf( - listOf(DubinsTangent( + j)] = mutableListOf( + mutableListOf(DubinsTangent( initialCircles[i]!!, initialCircles[i]!!, DubinsObstacle(listOf(initialCircles[i]!!)), @@ -338,13 +338,13 @@ public fun findAllPaths( while (!allFinished(outputTangents[listOf(i, DubinsPath.SimpleType.S, j)]!!, finalObstacle)) { - var newOutputTangents = listOf() + var newOutputTangents = mutableListOf>() for (line in outputTangents[listOf(i, DubinsPath.SimpleType.S, j)]!!) { - var currentCircle = line[-1].endCircle - var currentDirection = line[-1].route[-1] - var currentObstacle = line[-1].endObstacle + var currentCircle = line.last().endCircle + var currentDirection = line.last().route.last() + var currentObstacle = line.last().endObstacle var nextObstacle = DubinsObstacle(listOf()) if (currentObstacle != finalObstacle) { var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( @@ -362,20 +362,7 @@ public fun findAllPaths( nextObstacle = finalObstacle } var nextTangents = outerTangents(currentObstacle, nextObstacle) -// for (pathType in listOf( -// listOf(DubinsPath.SimpleType.L, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.L), -// listOf(DubinsPath.SimpleType.L, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.R), -// listOf(DubinsPath.SimpleType.R, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.L), -// listOf(DubinsPath.SimpleType.R, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.R) -// )) { + for (pathType in nextTangents.keys) { for (obstacle in obstacles) { // in Python code here try/except was used, but seems unneeded @@ -396,12 +383,117 @@ public fun findAllPaths( nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} as MutableMap } - TODO("rewrite fragment from Python") + val tangentsAlong = mutableListOf() + for (tangent in nextTangents.values) { + if (tangent.startCircle == line.last().endCircle) { + val lengthMaxPossible = arcLength( + tangent.startCircle, + line.last().lineSegment.end, + tangent.startObstacle.nextTangent( + tangent.startCircle, + DubinsPath.toType(listOf(currentDirection, DubinsPath.SimpleType.S, currentDirection)), + ).lineSegment.begin, + currentDirection + ) + val lengthCalculated = arcLength( + tangent.startCircle, + line.last().lineSegment.end, + tangent.lineSegment.begin, + currentDirection) + if (lengthCalculated > lengthMaxPossible) { + val tangentsAlong = tangentsAlongTheObstacle( + currentCircle, + DubinsPath.toType(listOf( + currentDirection, + DubinsPath.SimpleType.S, + currentDirection)), + tangent.startCircle, + currentObstacle + ) + } + else { + val tangentsAlong = mutableListOf() + } + } + else { + val tangentsAlong = tangentsAlongTheObstacle( + currentCircle, + DubinsPath.toType(listOf( + currentDirection, + DubinsPath.SimpleType.S, + currentDirection)), + tangent.startCircle, + currentObstacle + ) + } + newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList()) + } + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents + } + else { + // minor changes from Python code + newOutputTangents.add(line) + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents } } } + for (lineId in outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!!.indices) { + val lastDirection = outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!![lineId].last().route[2] + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!![lineId].add(DubinsTangent( + finalCircles[j]!!, + finalCircles[j]!!, + DubinsObstacle( + listOf(finalCircles[j]!!) + ), + DubinsObstacle( + listOf(finalCircles[j]!!) + ), + LineSegment2D(finalPoint, finalPoint), + listOf( + lastDirection, + DubinsPath.SimpleType.S, + j + ) + )) + } } } + return outputTangents[listOf( + DubinsPath.SimpleType.L, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.L, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.R + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.R, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.R, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L)]!! } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index d3165e162..1a58d64cc 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -39,22 +39,76 @@ public fun Circle2D.tangentsToCircle( } else { r1.absoluteValue + r2.absoluteValue } - if (distance <= r) TODO("Intersecting circles are not supported yet") - val l = sqrt(distance * distance - r * r) - angle2 = if (r1.absoluteValue > r2.absoluteValue) { - angle1 + r1.sign * atan2(r.absoluteValue, l) - } else { - angle1 - r2.sign * atan2(r.absoluteValue, l) - } - val w = vector(-cos(angle2), sin(angle2)) - put( - route, - LineSegment( - center + w * r1, - other.center + w * r2 + if (distance * distance >= r * r) { + val l = sqrt(distance * distance - r * r) + angle2 = if (r1.absoluteValue > r2.absoluteValue) { + angle1 + r1.sign * atan2(r.absoluteValue, l) + } else { + angle1 - r2.sign * atan2(r.absoluteValue, l) + } + val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + put( + route, + LineSegment( + center + w * r1, + other.center + w * r2 + ) ) - ) + } else { + throw Exception("Circles should not intersect") + } + } + } +} +public fun dubinsTangentsToCircles( + firstCircle: Circle2D, + secondCircle: Circle2D, + firstObstacle: DubinsObstacle, + secondObstacle: DubinsObstacle +): Map = with(Euclidean2DSpace) { + val line = LineSegment(firstCircle.center, secondCircle.center) + val distance = line.begin.distanceTo(line.end) + val angle1 = atan2(secondCircle.center.x - firstCircle.center.x, + secondCircle.center.y - firstCircle.center.y) + var r: Double + var angle2: Double + val routes = mapOf( + DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius), + DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius), + DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius), + DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius) + ) + return buildMap { + for ((route, r1r2) in routes) { + val r1 = r1r2.first + val r2 = r1r2.second + r = if (r1.sign == r2.sign) { + r1.absoluteValue - r2.absoluteValue + } else { + r1.absoluteValue + r2.absoluteValue + } + if (distance * distance >= r * r) { + val l = sqrt(distance * distance - r * r) + angle2 = if (r1.absoluteValue > r2.absoluteValue) { + angle1 + r1.sign * atan2(r.absoluteValue, l) + } else { + angle1 - r2.sign * atan2(r.absoluteValue, l) + } + val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), + secondCircle, + firstObstacle, + secondObstacle, + LineSegment2D( + firstCircle.center + w * r1, + secondCircle.center + w * r2 + ), + DubinsPath.toSimpleTypes(route)) + ) + } else { + throw Exception("Circles should not intersect") + } } } } \ No newline at end of file diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt new file mode 100644 index 000000000..3eefbdaeb --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -0,0 +1,39 @@ +/* + * Copyright 2018-2023 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +package space.kscience.kmath.trajectory + +import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace.vector +import kotlin.test.Test +import kotlin.test.assertTrue + +class DubinsTest { + @Test + fun firstPath() { + val startPoint = vector(-5.0, -1.0) + val startDirection = vector(1.0, 1.0) + val startRadius = 0.5 + val finalPoint = vector(20.0, 4.0) + val finalDirection = vector(1.0, -1.0) + val finalRadius = 0.5 + + val obstacles = listOf(DubinsObstacle(listOf( + Circle2D(vector(7.0, 1.0), 5.0)))) + + val outputTangents = findAllPaths( + startPoint, + startDirection, + startRadius, + finalPoint, + finalDirection, + finalRadius, + obstacles) + val length = pathLength(shortestPath(outputTangents)) + TODO("fix negative indices in boundaryTangents and accomplish test") + assertTrue(false) + } +} \ No newline at end of file