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

search for shortest path algorithm

Browse Source
This commit is contained in:
Artyom Degtyarev 2023-03-04 21:31:06 +03:00
parent 6288eb9d97
commit 61d43ae5fa
3 changed files with 225 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt
View File

@ -276,7 +276,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
public fun allFinished(paths: List<List<DubinsTangent>>,
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<DubinsTangent>): Double {
return tangentsLength + arcsLength
}
public fun shortestPath(path: List<List<DubinsTangent>>): List<List<DubinsTangent>> {
return path.sortedBy { pathLength(it) }
public fun shortestPath(path: List<List<DubinsTangent>>): List<DubinsTangent> {
return path.sortedBy { pathLength(it) }[0]
}
public typealias Path = List<DubinsTangent>
@ -309,7 +309,7 @@ public fun findAllPaths(
finalDirection: DoubleVector2D,
finalRadius: Double,
obstacles: List<DubinsObstacle>
) {
): List<MutableList<DubinsTangent>> {
val initialCircles = constructTangentCircles(
startingPoint,
startingDirection,
@ -318,15 +318,15 @@ public fun findAllPaths(
finalPoint,
finalDirection,
finalRadius)
var outputTangents = mutableMapOf<PathTypes, List<Path>>()
var outputTangents = mutableMapOf<PathTypes, MutableList<MutableList<DubinsTangent>>>()
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<Path>()
var newOutputTangents = mutableListOf<MutableList<DubinsTangent>>()
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,13 +383,118 @@ public fun findAllPaths(
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
as MutableMap<DubinsPath.Type, DubinsTangent>
}
TODO("rewrite fragment from Python")
val tangentsAlong = mutableListOf<DubinsTangent>()
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<DubinsTangent>()
}
}
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)]!!
}

58
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt
View File

@ -39,14 +39,14 @@ public fun Circle2D.tangentsToCircle(
} else {
r1.absoluteValue + r2.absoluteValue
}
if (distance <= r) TODO("Intersecting circles are not supported yet")
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 = vector(-cos(angle2), sin(angle2))
val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
put(
route,
LineSegment(
@ -54,7 +54,61 @@ public fun Circle2D.tangentsToCircle(
other.center + w * r2
)
)
} else {
throw Exception("Circles should not intersect")
}
}
}
}
public fun dubinsTangentsToCircles(
firstCircle: Circle2D,
secondCircle: Circle2D,
firstObstacle: DubinsObstacle,
secondObstacle: DubinsObstacle
): Map<DubinsPath.Type, DubinsTangent> = 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")
}
}
}
}

39
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt Normal file
View File

@ -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)
}
}