From 2bce369c5dcdae8b4ba16f498fd19d31807b29df Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Thu, 9 Mar 2023 16:03:48 +0300 Subject: [PATCH] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 38 ++++++++++++------- .../kscience/kmath/trajectory/DubinsTest.kt | 37 +++++++++++++++++- 2 files changed, 60 insertions(+), 15 deletions(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index d3ab13f26..6b247e55b 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -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>, public fun pathLength(path: List): Double { val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)} val arcsLength = buildList{ - 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) { diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index a0697c59c..16d77c84d 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -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() {