teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

search for shortest path algorithm

Browse Source
This commit is contained in:
Artyom Degtyarev 2023-03-21 12:04:27 +03:00
parent 4c1ffdb6d9
commit 81213eb943
2 changed files with 45 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt
View File

@ -39,8 +39,6 @@ public class DubinsObstacle(
var angle2: Double var angle2: Double
val routes = mapOf( val routes = mapOf(
DubinsPath.Type.RSR to Pair(radius, other.radius), 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) DubinsPath.Type.LSL to Pair(-radius, -other.radius)
) )
return buildMap { 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 firstCircles = this.circles
val secondCircles = this.circles.slice(1..this.circles.lastIndex) + val secondCircles = this.circles.slice(1..this.circles.lastIndex) +
this.circles[0] this.circles[0]
@ -149,7 +146,7 @@ public data class DubinsTangent(val startCircle: Circle2D,
val lineSegment: LineSegment2D, val lineSegment: LineSegment2D,
val route: PathTypes) val route: PathTypes)
public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
return v1.x * v2.y - v1.y * v2.x return v1.x * v2.y - v1.y * v2.x
} }
@ -164,7 +161,7 @@ public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
return true 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 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))
@ -184,7 +181,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
return false return false
} }
public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { private fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean {
for (tangent in obstacle.tangents) { for (tangent in obstacle.tangents) {
if (this.lineSegment.intersectSegment(tangent.lineSegment)) { if (this.lineSegment.intersectSegment(tangent.lineSegment)) {
return true return true
@ -198,7 +195,7 @@ public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean {
return false return false
} }
public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> { private fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> {
return buildMap { return buildMap {
for (circle1 in first.circles) { for (circle1 in first.circles) {
for (circle2 in second.circles) { for (circle2 in second.circles) {
@ -216,7 +213,7 @@ public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): Mutable
}.toMutableMap() }.toMutableMap()
} }
public fun arcLength(circle: Circle2D, private fun arcLength(circle: Circle2D,
point1: DoubleVector2D, point1: DoubleVector2D,
point2: DoubleVector2D, point2: DoubleVector2D,
route: DubinsPath.SimpleType): Double { route: DubinsPath.SimpleType): Double {
@ -245,14 +242,14 @@ public fun arcLength(circle: Circle2D,
return circle.radius * angle return circle.radius * angle
} }
public fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> { private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
return Pair( return Pair(
r * vector(v.y / norm(v), -v.x / norm(v)), r * vector(v.y / norm(v), -v.x / norm(v)),
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, direction: DoubleVector2D,
r: Double): Map<DubinsPath.SimpleType, Circle2D> { r: Double): Map<DubinsPath.SimpleType, Circle2D> {
val center1 = point + normalVectors(direction, r).first 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<DubinsObstacle>): List<DubinsObstacle> { obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed()
} }
public fun tangentsAlongTheObstacle(initialCircle: Circle2D, private fun tangentsAlongTheObstacle(initialCircle: Circle2D,
initialRoute: DubinsPath.Type, initialRoute: DubinsPath.Type,
finalCircle: Circle2D, finalCircle: Circle2D,
obstacle: DubinsObstacle): MutableList<DubinsTangent> { obstacle: DubinsObstacle): MutableList<DubinsTangent> {
@ -288,7 +285,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
return dubinsTangents return dubinsTangents
} }
public fun allFinished(paths: List<List<DubinsTangent>>, private fun allFinished(paths: List<List<DubinsTangent>>,
finalObstacle: DubinsObstacle): Boolean { finalObstacle: DubinsObstacle): Boolean {
for (path in paths) { for (path in paths) {
if (path.last().endObstacle != finalObstacle) { if (path.last().endObstacle != finalObstacle) {
@ -333,28 +330,28 @@ public fun findAllPaths(
finalPoint, finalPoint,
finalDirection, finalDirection,
finalRadius) finalRadius)
var outputTangents = mutableMapOf<PathTypes, MutableList<MutableList<DubinsTangent>>>() var path = mutableMapOf<PathTypes, MutableList<MutableList<DubinsTangent>>>()
for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
val finalCircle = finalCircles[j]!! val finalCircle = finalCircles[j]!!
val finalObstacle = DubinsObstacle(listOf(finalCircle)) val finalObstacle = DubinsObstacle(listOf(finalCircle))
outputTangents[listOf(i, path[listOf(i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j)] = mutableListOf( j)] = mutableListOf(
mutableListOf(DubinsTangent( mutableListOf(DubinsTangent(
initialCircles[i]!!, initialCircles[i]!!,
initialCircles[i]!!, initialCircles[i]!!,
DubinsObstacle(listOf(initialCircles[i]!!)), DubinsObstacle(listOf(initialCircles[i]!!)),
DubinsObstacle(listOf(initialCircles[i]!!)), DubinsObstacle(listOf(initialCircles[i]!!)),
LineSegment2D(startingPoint, startingPoint), LineSegment2D(startingPoint, startingPoint),
listOf(i, DubinsPath.SimpleType.S, i) listOf(i, DubinsPath.SimpleType.S, i)
))) )))
var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) //var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
while (!allFinished(outputTangents[listOf(i, while (!allFinished(path[listOf(i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j)]!!, finalObstacle)) { j)]!!, finalObstacle)) {
var newOutputTangents = mutableListOf<MutableList<DubinsTangent>>() var newPaths = mutableListOf<MutableList<DubinsTangent>>()
for (line in outputTangents[listOf(i, for (line in path[listOf(i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j)]!!) { j)]!!) {
var currentCircle = line.last().endCircle var currentCircle = line.last().endCircle
@ -439,41 +436,41 @@ public fun findAllPaths(
currentObstacle currentObstacle
) )
} }
newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList()) newPaths.add((line + tangentsAlong + listOf(tangent)).toMutableList())
} }
outputTangents[listOf( path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
)] = newOutputTangents )] = newPaths
} }
else { else {
// minor changes from Python code // minor changes from Python code
newOutputTangents.add(line) newPaths.add(line)
outputTangents[listOf( path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
)] = newOutputTangents )] = newPaths
} }
} }
outputTangents[listOf( path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
)] = newOutputTangents )] = newPaths
} }
for (lineId in outputTangents[listOf( for (lineId in path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
)]!!.indices) { )]!!.indices) {
val lastDirection = outputTangents[listOf( val lastDirection = path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
)]!![lineId].last().route[2] )]!![lineId].last().route[2]
outputTangents[listOf( path[listOf(
i, i,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
j j
@ -496,19 +493,19 @@ public fun findAllPaths(
} }
} }
} }
return outputTangents[listOf( return path[listOf(
DubinsPath.SimpleType.L, DubinsPath.SimpleType.L,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
DubinsPath.SimpleType.L DubinsPath.SimpleType.L
)]!! + outputTangents[listOf( )]!! + path[listOf(
DubinsPath.SimpleType.L, DubinsPath.SimpleType.L,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
DubinsPath.SimpleType.R DubinsPath.SimpleType.R
)]!! + outputTangents[listOf( )]!! + path[listOf(
DubinsPath.SimpleType.R, DubinsPath.SimpleType.R,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
DubinsPath.SimpleType.L DubinsPath.SimpleType.L
)]!! + outputTangents[listOf( )]!! + path[listOf(
DubinsPath.SimpleType.R, DubinsPath.SimpleType.R,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
DubinsPath.SimpleType.R)]!! DubinsPath.SimpleType.R)]!!

50
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt
View File

@ -6,12 +6,8 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D 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 space.kscience.kmath.geometry.Euclidean2DSpace.vector
import kotlin.test.Test import kotlin.test.Test
import kotlin.test.assertTrue
class DubinsTest { class DubinsTest {
@Test @Test
@ -60,7 +56,7 @@ class DubinsTest {
Circle2D(vector(9.0, 4.0), 0.5) Circle2D(vector(9.0, 4.0), 0.5)
)) ))
) )
val outputTangents = findAllPaths( val paths = findAllPaths(
startPoint, startPoint,
startDirection, startDirection,
startRadius, startRadius,
@ -68,9 +64,9 @@ class DubinsTest {
finalDirection, finalDirection,
finalRadius, finalRadius,
obstacles) obstacles)
val length = pathLength(shortestPath(outputTangents)) val length = pathLength(shortestPath(paths))
println(length) println(length)
for (path in outputTangents) { for (path in paths) {
println(pathLength(path)) println(pathLength(path))
println(path.size) println(path.size)
for (tangent in path) { for (tangent in path) {
@ -84,46 +80,6 @@ class DubinsTest {
} }
} }
@Test @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() { fun equalCircles() {
val circle1 = Circle2D(vector(1.0, 6.5), 0.5) val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
val circle2 = Circle2D(vector(1.0, 6.5), 0.5) val circle2 = Circle2D(vector(1.0, 6.5), 0.5)