search for shortest path algorithm
This commit is contained in:
parent
61d43ae5fa
commit
1b6a41c728
@ -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) {
|
||||||
|
@ -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,
|
||||||
|
@ -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)
|
||||||
|
}
|
||||||
}
|
}
|
Loading…
Reference in New Issue
Block a user