From 81213eb94340fc87ae6958d30e598d234439dc35 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Tue, 21 Mar 2023 12:04:27 +0300 Subject: [PATCH] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 87 +++++++++---------- .../kscience/kmath/trajectory/DubinsTest.kt | 50 +---------- 2 files changed, 45 insertions(+), 92 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 3aea139ec..896803fba 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,6 @@ 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.LSL to Pair(-radius, -other.radius) ) return buildMap { @@ -76,8 +74,7 @@ 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 val secondCircles = this.circles.slice(1..this.circles.lastIndex) + this.circles[0] @@ -149,7 +146,7 @@ public data class DubinsTangent(val startCircle: Circle2D, val lineSegment: LineSegment2D, val route: PathTypes) -public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { +private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { return v1.x * v2.y - v1.y * v2.x } @@ -164,7 +161,7 @@ public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { return true } -public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { +private 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)) @@ -184,7 +181,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { return false } -public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { +private fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { for (tangent in obstacle.tangents) { if (this.lineSegment.intersectSegment(tangent.lineSegment)) { return true @@ -198,7 +195,7 @@ public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { return false } -public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { +private fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { return buildMap { for (circle1 in first.circles) { for (circle2 in second.circles) { @@ -216,7 +213,7 @@ public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): Mutable }.toMutableMap() } -public fun arcLength(circle: Circle2D, +private fun arcLength(circle: Circle2D, point1: DoubleVector2D, point2: DoubleVector2D, route: DubinsPath.SimpleType): Double { @@ -245,14 +242,14 @@ public fun arcLength(circle: Circle2D, return circle.radius * angle } -public fun normalVectors(v: DoubleVector2D, r: Double): Pair { +private fun normalVectors(v: DoubleVector2D, r: Double): Pair { return Pair( r * vector(v.y / norm(v), -v.x / norm(v)), r * vector(-v.y / norm(v), v.x / norm(v)) ) } -public fun constructTangentCircles(point: DoubleVector2D, +private fun constructTangentCircles(point: DoubleVector2D, direction: DoubleVector2D, r: Double): Map { val center1 = point + normalVectors(direction, r).first @@ -269,12 +266,12 @@ public fun constructTangentCircles(point: DoubleVector2D, } } -public fun sortedObstacles(currentObstacle: DubinsObstacle, +private fun sortedObstacles(currentObstacle: DubinsObstacle, obstacles: List): List { return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() } -public fun tangentsAlongTheObstacle(initialCircle: Circle2D, +private fun tangentsAlongTheObstacle(initialCircle: Circle2D, initialRoute: DubinsPath.Type, finalCircle: Circle2D, obstacle: DubinsObstacle): MutableList { @@ -288,7 +285,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D, return dubinsTangents } -public fun allFinished(paths: List>, +private fun allFinished(paths: List>, finalObstacle: DubinsObstacle): Boolean { for (path in paths) { if (path.last().endObstacle != finalObstacle) { @@ -333,28 +330,28 @@ public fun findAllPaths( finalPoint, finalDirection, finalRadius) - var outputTangents = mutableMapOf>>() + var path = mutableMapOf>>() for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { val finalCircle = finalCircles[j]!! val finalObstacle = DubinsObstacle(listOf(finalCircle)) - outputTangents[listOf(i, - DubinsPath.SimpleType.S, - j)] = mutableListOf( - mutableListOf(DubinsTangent( - initialCircles[i]!!, - initialCircles[i]!!, - DubinsObstacle(listOf(initialCircles[i]!!)), - DubinsObstacle(listOf(initialCircles[i]!!)), - LineSegment2D(startingPoint, startingPoint), - listOf(i, DubinsPath.SimpleType.S, i) - ))) - var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) - while (!allFinished(outputTangents[listOf(i, + path[listOf(i, + DubinsPath.SimpleType.S, + j)] = mutableListOf( + mutableListOf(DubinsTangent( + initialCircles[i]!!, + initialCircles[i]!!, + DubinsObstacle(listOf(initialCircles[i]!!)), + DubinsObstacle(listOf(initialCircles[i]!!)), + LineSegment2D(startingPoint, startingPoint), + listOf(i, DubinsPath.SimpleType.S, i) + ))) + //var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) + while (!allFinished(path[listOf(i, DubinsPath.SimpleType.S, j)]!!, finalObstacle)) { - var newOutputTangents = mutableListOf>() - for (line in outputTangents[listOf(i, + var newPaths = mutableListOf>() + for (line in path[listOf(i, DubinsPath.SimpleType.S, j)]!!) { var currentCircle = line.last().endCircle @@ -439,41 +436,41 @@ public fun findAllPaths( currentObstacle ) } - newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList()) + newPaths.add((line + tangentsAlong + listOf(tangent)).toMutableList()) } - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } else { // minor changes from Python code - newOutputTangents.add(line) - outputTangents[listOf( + newPaths.add(line) + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } } - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } - for (lineId in outputTangents[listOf( + for (lineId in path[listOf( i, DubinsPath.SimpleType.S, j )]!!.indices) { - val lastDirection = outputTangents[listOf( + val lastDirection = path[listOf( i, DubinsPath.SimpleType.S, j )]!![lineId].last().route[2] - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j @@ -496,19 +493,19 @@ public fun findAllPaths( } } } - return outputTangents[listOf( + return path[listOf( DubinsPath.SimpleType.L, DubinsPath.SimpleType.S, DubinsPath.SimpleType.L - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.L, DubinsPath.SimpleType.S, DubinsPath.SimpleType.R - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, DubinsPath.SimpleType.L - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, 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 b26b21737..a78a5317d 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 @@ -6,12 +6,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 class DubinsTest { @Test @@ -60,7 +56,7 @@ class DubinsTest { Circle2D(vector(9.0, 4.0), 0.5) )) ) - val outputTangents = findAllPaths( + val paths = findAllPaths( startPoint, startDirection, startRadius, @@ -68,9 +64,9 @@ class DubinsTest { finalDirection, finalRadius, obstacles) - val length = pathLength(shortestPath(outputTangents)) + val length = pathLength(shortestPath(paths)) println(length) - for (path in outputTangents) { + for (path in paths) { println(pathLength(path)) println(path.size) for (tangent in path) { @@ -84,46 +80,6 @@ class DubinsTest { } } @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))) - } - @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) - } - } - } - } - @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)