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-09 08:39:20 +03:00
parent 61d43ae5fa
commit 1b6a41c728
3 changed files with 49 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,8 +39,8 @@ public class DubinsObstacle(
var angle2: Double var angle2: Double
val routes = mapOf( val routes = mapOf(
DubinsPath.Type.RSR to Pair(radius, other.radius), DubinsPath.Type.RSR to Pair(radius, other.radius),
DubinsPath.Type.RSL to Pair(radius, -other.radius), // DubinsPath.Type.RSL to Pair(radius, -other.radius),
DubinsPath.Type.LSR to Pair(-radius, other.radius), // DubinsPath.Type.LSR to Pair(-radius, other.radius),
DubinsPath.Type.LSL to Pair(-radius, -other.radius) DubinsPath.Type.LSL to Pair(-radius, -other.radius)
) )
return buildMap { return buildMap {
@ -52,7 +52,7 @@ public class DubinsObstacle(
} else { } else {
r1.absoluteValue + r2.absoluteValue r1.absoluteValue + r2.absoluteValue
} }
if (d * d > r * r) { if (d * d >= r * r) {
val l = (d * d - r * r).pow(0.5) val l = (d * d - r * r).pow(0.5)
angle2 = if (r1.absoluteValue > r2.absoluteValue) { angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l) angle1 + r1.sign * atan2(r.absoluteValue, l)
@ -76,8 +76,11 @@ public class DubinsObstacle(
} }
} }
} }
val firstCircles = this.circles.slice(-this.circles.size..-1) // val firstCircles = this.circles.slice(-this.circles.size..-1)
val secondCircles = this.circles.slice(-this.circles.size+1..0) // val secondCircles = this.circles.slice(-this.circles.size+1..0)
val firstCircles = this.circles
val secondCircles = this.circles.slice(1..this.circles.lastIndex) +
this.circles[0]
val lslTangents = firstCircles.zip(secondCircles) val lslTangents = firstCircles.zip(secondCircles)
{a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!} {a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!}
val rsrTangents = firstCircles.zip(secondCircles) val rsrTangents = firstCircles.zip(secondCircles)
@ -345,7 +348,7 @@ public fun findAllPaths(
var currentCircle = line.last().endCircle var currentCircle = line.last().endCircle
var currentDirection = line.last().route.last() var currentDirection = line.last().route.last()
var currentObstacle = line.last().endObstacle var currentObstacle = line.last().endObstacle
var nextObstacle = DubinsObstacle(listOf()) var nextObstacle: DubinsObstacle? = null
if (currentObstacle != finalObstacle) { if (currentObstacle != finalObstacle) {
var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf(
currentDirection, currentDirection,
@ -358,7 +361,7 @@ public fun findAllPaths(
break break
} }
} }
if (nextObstacle == DubinsObstacle(listOf())) { if (nextObstacle == null) {
nextObstacle = finalObstacle nextObstacle = finalObstacle
} }
var nextTangents = outerTangents(currentObstacle, nextObstacle) var nextTangents = outerTangents(currentObstacle, nextObstacle)
@ -372,16 +375,13 @@ public fun findAllPaths(
} }
} }
if (nextObstacle == finalObstacle) { nextTangents = if (nextObstacle == finalObstacle) {
nextTangents = nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
(DubinsPath.toSimpleTypes(it.key)[0] == j)} (DubinsPath.toSimpleTypes(it.key)[0] == j)}
as MutableMap<DubinsPath.Type, DubinsTangent> as MutableMap<DubinsPath.Type, DubinsTangent>
} } else {
else { nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
nextTangents = as MutableMap<DubinsPath.Type, DubinsTangent>
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
as MutableMap<DubinsPath.Type, DubinsTangent>
} }
val tangentsAlong = mutableListOf<DubinsTangent>() val tangentsAlong = mutableListOf<DubinsTangent>()
for (tangent in nextTangents.values) { for (tangent in nextTangents.values) {

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

@ -95,7 +95,7 @@ public fun dubinsTangentsToCircles(
} else { } else {
angle1 - r2.sign * atan2(r.absoluteValue, l) angle1 - r2.sign * atan2(r.absoluteValue, l)
} }
val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) val w = vector(-cos(angle2), sin(angle2))
put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
secondCircle, secondCircle,
firstObstacle, firstObstacle,

33
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt
View File

@ -36,4 +36,37 @@ class DubinsTest {
TODO("fix negative indices in boundaryTangents and accomplish test") TODO("fix negative indices in boundaryTangents and accomplish test")
assertTrue(false) assertTrue(false)
} }
@Test
fun outerTangentsTest1() {
// works incorrectly
val circles1 = listOf(
Circle2D(vector(0.0, 0.0), 1.0))
val circles2 = listOf(
Circle2D(vector(5.0, 5.0), 1.0)
)
println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2)))
assertTrue(false)
}
@Test
fun outerTangentsTest2() {
// works incorrectly
val circles1 = listOf(
Circle2D(vector(0.0, 0.0), 1.0),
Circle2D(vector( 2.0, 0.0), 1.0))
val circles2 = listOf(
Circle2D(vector(5.0, 5.0), 1.0),
Circle2D(vector(7.0, 5.0), 1.0)
)
println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2)))
for (circle1 in circles1) {
for (circle2 in circles2) {
for (tangent in dubinsTangentsToCircles(circle1, circle2,
DubinsObstacle(circles1), DubinsObstacle(circles2))) {
println(tangent)
}
}
}
assertTrue(false)
}
} }