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