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