search for shortest path algorithm
This commit is contained in:
parent
1b6a41c728
commit
2bce369c5d
@ -111,13 +111,24 @@ public class DubinsObstacle(
|
||||
else {
|
||||
for (i in this.circles.indices) {
|
||||
if (this.circles[i] == circle) {
|
||||
return DubinsTangent(this.circles[i],
|
||||
this.circles[i-1],
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(this.tangents[i-1].lineSegment.end,
|
||||
this.tangents[i-1].lineSegment.begin),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
if (i > 0) {
|
||||
return DubinsTangent(this.circles[i],
|
||||
this.circles[i-1],
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(this.tangents[i-1].lineSegment.end,
|
||||
this.tangents[i-1].lineSegment.begin),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
}
|
||||
else {
|
||||
return DubinsTangent(this.circles[0],
|
||||
this.circles.last(),
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(this.tangents.last().lineSegment.end,
|
||||
this.tangents.last().lineSegment.begin),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -156,7 +167,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
|
||||
val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0)
|
||||
val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) +
|
||||
(this.begin.y - this.end.y) * (this.end.y - circle.center.y))
|
||||
val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) -
|
||||
val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y).pow(2.0) -
|
||||
circle.radius.pow(2.0)
|
||||
val d = b.pow(2.0) - 4 * a * c
|
||||
if (d < 1e-6) {
|
||||
@ -289,7 +300,7 @@ public fun allFinished(paths: List<List<DubinsTangent>>,
|
||||
public fun pathLength(path: List<DubinsTangent>): Double {
|
||||
val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)}
|
||||
val arcsLength = buildList<Double>{
|
||||
for (i in 1..path.size) {
|
||||
for (i in 1..path.lastIndex) {
|
||||
add(arcLength(path[i].startCircle,
|
||||
path[i-1].lineSegment.end,
|
||||
path[i].lineSegment.begin,
|
||||
@ -366,13 +377,14 @@ public fun findAllPaths(
|
||||
}
|
||||
var nextTangents = outerTangents(currentObstacle, nextObstacle)
|
||||
|
||||
for (pathType in nextTangents.keys) {
|
||||
for (pathType in DubinsPath.Type.values()) {
|
||||
for (obstacle in obstacles) {
|
||||
// in Python code here try/except was used, but seems unneeded
|
||||
if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
|
||||
nextTangents.remove(pathType)
|
||||
if (nextTangents.containsKey(pathType)) {
|
||||
if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
|
||||
nextTangents.remove(pathType)
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
nextTangents = if (nextObstacle == finalObstacle) {
|
||||
|
@ -33,8 +33,41 @@ class DubinsTest {
|
||||
finalRadius,
|
||||
obstacles)
|
||||
val length = pathLength(shortestPath(outputTangents))
|
||||
TODO("fix negative indices in boundaryTangents and accomplish test")
|
||||
assertTrue(false)
|
||||
println(length)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun secondPath() {
|
||||
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(1.0, 6.5), 0.5),
|
||||
Circle2D(vector(2.0, 1.0), 0.5),
|
||||
Circle2D(vector(6.0, 0.0), 0.5),
|
||||
Circle2D(vector(5.0, 5.0), 0.5)
|
||||
)), DubinsObstacle(listOf(
|
||||
Circle2D(vector(10.0, 1.0), 0.5),
|
||||
Circle2D(vector(16.0, 0.0), 0.5),
|
||||
Circle2D(vector(14.0, 6.0), 0.5),
|
||||
Circle2D(vector(9.0, 4.0), 0.5)
|
||||
))
|
||||
)
|
||||
val outputTangents = findAllPaths(
|
||||
startPoint,
|
||||
startDirection,
|
||||
startRadius,
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius,
|
||||
obstacles)
|
||||
val length = pathLength(shortestPath(outputTangents))
|
||||
println(length)
|
||||
}
|
||||
@Test
|
||||
fun outerTangentsTest1() {
|
||||
|
Loading…
Reference in New Issue
Block a user