From 4c1ffdb6d9db3ea37ff159ad95958184bb8fd517 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Tue, 14 Mar 2023 13:50:42 +0300 Subject: [PATCH] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 24 +++++++----- .../kscience/kmath/trajectory/DubinsTest.kt | 39 ++++++++++++++++++- 2 files changed, 52 insertions(+), 11 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 6b247e55b..3aea139ec 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 @@ -136,7 +136,8 @@ public class DubinsObstacle( error("next tangent not found") } - public fun equals(other: DubinsObstacle): Boolean { + override fun equals(other: Any?): Boolean { + if (other == null || other !is DubinsObstacle) return false return this.circles == other.circles } } @@ -258,7 +259,7 @@ public fun constructTangentCircles(point: DoubleVector2D, val center2 = point + normalVectors(direction, r).second val p1 = center1 - point val p2 = center2 - point - return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) { + return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI/2, -3*PI/2)) { mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r), DubinsPath.SimpleType.R to Circle2D(center2, r)) } @@ -270,7 +271,7 @@ public fun constructTangentCircles(point: DoubleVector2D, public fun sortedObstacles(currentObstacle: DubinsObstacle, obstacles: List): List { - return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed() + return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() } public fun tangentsAlongTheObstacle(initialCircle: Circle2D, @@ -389,13 +390,13 @@ public fun findAllPaths( } nextTangents = if (nextObstacle == finalObstacle) { nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and - (DubinsPath.toSimpleTypes(it.key)[0] == j)} + (DubinsPath.toSimpleTypes(it.key)[2] == j)} as MutableMap } else { nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} as MutableMap } - val tangentsAlong = mutableListOf() + var tangentsAlong = mutableListOf() for (tangent in nextTangents.values) { if (tangent.startCircle == line.last().endCircle) { val lengthMaxPossible = arcLength( @@ -413,7 +414,7 @@ public fun findAllPaths( tangent.lineSegment.begin, currentDirection) if (lengthCalculated > lengthMaxPossible) { - val tangentsAlong = tangentsAlongTheObstacle( + tangentsAlong = tangentsAlongTheObstacle( currentCircle, DubinsPath.toType(listOf( currentDirection, @@ -424,11 +425,11 @@ public fun findAllPaths( ) } else { - val tangentsAlong = mutableListOf() + tangentsAlong = mutableListOf() } } else { - val tangentsAlong = tangentsAlongTheObstacle( + tangentsAlong = tangentsAlongTheObstacle( currentCircle, DubinsPath.toType(listOf( currentDirection, @@ -456,6 +457,11 @@ public fun findAllPaths( )] = newOutputTangents } } + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents } for (lineId in outputTangents[listOf( i, @@ -505,7 +511,7 @@ public fun findAllPaths( )]!! + outputTangents[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, - DubinsPath.SimpleType.L)]!! + DubinsPath.SimpleType.R)]!! } 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 16d77c84d..b26b21737 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 @@ -7,6 +7,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.kmath.geometry.Euclidean2DSpace.minus import space.kscience.kmath.geometry.Euclidean2DSpace.vector import kotlin.test.Test import kotlin.test.assertTrue @@ -68,6 +70,18 @@ class DubinsTest { obstacles) val length = pathLength(shortestPath(outputTangents)) println(length) + for (path in outputTangents) { + println(pathLength(path)) + println(path.size) + for (tangent in path) { +// println(tangent.route) +// println(tangent.startCircle) +// println(tangent.endCircle) +// println(Euclidean2DSpace.norm(tangent.lineSegment.end - tangent.lineSegment.begin)) + } + println() + println() + } } @Test fun outerTangentsTest1() { @@ -78,7 +92,6 @@ class DubinsTest { Circle2D(vector(5.0, 5.0), 1.0) ) println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) - assertTrue(false) } @Test fun outerTangentsTest2() { @@ -100,6 +113,28 @@ class DubinsTest { } } } - assertTrue(false) + } + @Test + fun tangentsTest() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + val obstacle1 = DubinsObstacle(listOf(circle1)) + val obstacle2 = DubinsObstacle(listOf(circle2)) + val tangent = dubinsTangentsToCircles(circle1, circle2, obstacle1, obstacle2) + println(tangent) + } + @Test + fun equalCircles() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + println(circle1 == circle2) + } + @Test + fun equalObstacles() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + val obstacle1 = DubinsObstacle(listOf(circle1)) + val obstacle2 = DubinsObstacle(listOf(circle2)) + println(obstacle1 == obstacle2) } } \ No newline at end of file