From 1b6a41c728150bb9374625b06045c06f8073c524 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Thu, 9 Mar 2023 08:39:20 +0300 Subject: [PATCH] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 30 ++++++++--------- .../kscience/kmath/trajectory/tangent.kt | 2 +- .../kscience/kmath/trajectory/DubinsTest.kt | 33 +++++++++++++++++++ 3 files changed, 49 insertions(+), 16 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 e1c967e2a..d3ab13f26 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 @@ -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,16 +375,13 @@ public fun findAllPaths( } } - if (nextObstacle == finalObstacle) { - nextTangents = - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and + nextTangents = if (nextObstacle == finalObstacle) { + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and (DubinsPath.toSimpleTypes(it.key)[0] == j)} as MutableMap - } - else { - nextTangents = - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} - as MutableMap + } else { + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} + as MutableMap } val tangentsAlong = mutableListOf() for (tangent in nextTangents.values) { diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index 1a58d64cc..fb1c3927b 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -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, 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 3eefbdaeb..a0697c59c 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 @@ -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) + } } \ No newline at end of file