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

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

@ -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<DubinsPath.Type, DubinsTangent> {
private fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> {
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<DoubleVector2D, DoubleVector2D> {
private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
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<DubinsPath.SimpleType, Circle2D> {
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> {
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<DubinsTangent> {
@ -288,7 +285,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
return dubinsTangents
}
public fun allFinished(paths: List<List<DubinsTangent>>,
private fun allFinished(paths: List<List<DubinsTangent>>,
finalObstacle: DubinsObstacle): Boolean {
for (path in paths) {
if (path.last().endObstacle != finalObstacle) {
@ -333,12 +330,12 @@ public fun findAllPaths(
finalPoint,
finalDirection,
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 (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
val finalCircle = finalCircles[j]!!
val finalObstacle = DubinsObstacle(listOf(finalCircle))
outputTangents[listOf(i,
path[listOf(i,
DubinsPath.SimpleType.S,
j)] = mutableListOf(
mutableListOf(DubinsTangent(
@ -349,12 +346,12 @@ public fun findAllPaths(
LineSegment2D(startingPoint, startingPoint),
listOf(i, DubinsPath.SimpleType.S, i)
)))
var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
while (!allFinished(outputTangents[listOf(i,
//var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
while (!allFinished(path[listOf(i,
DubinsPath.SimpleType.S,
j)]!!, finalObstacle)) {
var newOutputTangents = mutableListOf<MutableList<DubinsTangent>>()
for (line in outputTangents[listOf(i,
var newPaths = mutableListOf<MutableList<DubinsTangent>>()
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)]!!

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

@ -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)