forked from kscience/kmath
search for shortest path algorithm
This commit is contained in:
parent
61d43ae5fa
commit
1b6a41c728
@ -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,14 +375,11 @@ public fun findAllPaths(
|
||||
|
||||
}
|
||||
}
|
||||
if (nextObstacle == finalObstacle) {
|
||||
nextTangents =
|
||||
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 =
|
||||
} else {
|
||||
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
|
||||
as MutableMap<DubinsPath.Type, DubinsTangent>
|
||||
}
|
||||
|
@ -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,
|
||||
|
@ -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)
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user