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-14 13:50:42 +03:00
parent 2bce369c5d
commit 4c1ffdb6d9
2 changed files with 52 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<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,
@ -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<DubinsPath.Type, DubinsTangent>
} else {
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
as MutableMap<DubinsPath.Type, DubinsTangent>
}
val tangentsAlong = mutableListOf<DubinsTangent>()
var tangentsAlong = mutableListOf<DubinsTangent>()
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<DubinsTangent>()
tangentsAlong = mutableListOf<DubinsTangent>()
}
}
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)]!!
}

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

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