search for shortest path algorithm

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

View File

@ -136,7 +136,8 @@ public class DubinsObstacle(
error("next tangent not found") 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 return this.circles == other.circles
} }
} }
@ -258,7 +259,7 @@ public fun constructTangentCircles(point: DoubleVector2D,
val center2 = point + normalVectors(direction, r).second val center2 = point + normalVectors(direction, r).second
val p1 = center1 - point val p1 = center1 - point
val p2 = center2 - 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), mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r),
DubinsPath.SimpleType.R to Circle2D(center2, r)) DubinsPath.SimpleType.R to Circle2D(center2, r))
} }
@ -270,7 +271,7 @@ public fun constructTangentCircles(point: DoubleVector2D,
public fun sortedObstacles(currentObstacle: DubinsObstacle, public 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, public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
@ -389,13 +390,13 @@ public fun findAllPaths(
} }
nextTangents = if (nextObstacle == finalObstacle) { nextTangents = if (nextObstacle == finalObstacle) {
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and 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> as MutableMap<DubinsPath.Type, DubinsTangent>
} else { } else {
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
as MutableMap<DubinsPath.Type, DubinsTangent> as MutableMap<DubinsPath.Type, DubinsTangent>
} }
val tangentsAlong = mutableListOf<DubinsTangent>() var tangentsAlong = mutableListOf<DubinsTangent>()
for (tangent in nextTangents.values) { for (tangent in nextTangents.values) {
if (tangent.startCircle == line.last().endCircle) { if (tangent.startCircle == line.last().endCircle) {
val lengthMaxPossible = arcLength( val lengthMaxPossible = arcLength(
@ -413,7 +414,7 @@ public fun findAllPaths(
tangent.lineSegment.begin, tangent.lineSegment.begin,
currentDirection) currentDirection)
if (lengthCalculated > lengthMaxPossible) { if (lengthCalculated > lengthMaxPossible) {
val tangentsAlong = tangentsAlongTheObstacle( tangentsAlong = tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.toType(listOf( DubinsPath.toType(listOf(
currentDirection, currentDirection,
@ -424,11 +425,11 @@ public fun findAllPaths(
) )
} }
else { else {
val tangentsAlong = mutableListOf<DubinsTangent>() tangentsAlong = mutableListOf<DubinsTangent>()
} }
} }
else { else {
val tangentsAlong = tangentsAlongTheObstacle( tangentsAlong = tangentsAlongTheObstacle(
currentCircle, currentCircle,
DubinsPath.toType(listOf( DubinsPath.toType(listOf(
currentDirection, currentDirection,
@ -456,6 +457,11 @@ public fun findAllPaths(
)] = newOutputTangents )] = newOutputTangents
} }
} }
outputTangents[listOf(
i,
DubinsPath.SimpleType.S,
j
)] = newOutputTangents
} }
for (lineId in outputTangents[listOf( for (lineId in outputTangents[listOf(
i, i,
@ -505,7 +511,7 @@ public fun findAllPaths(
)]!! + outputTangents[listOf( )]!! + outputTangents[listOf(
DubinsPath.SimpleType.R, DubinsPath.SimpleType.R,
DubinsPath.SimpleType.S, DubinsPath.SimpleType.S,
DubinsPath.SimpleType.L)]!! DubinsPath.SimpleType.R)]!!
} }

View File

@ -7,6 +7,8 @@ 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.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 import kotlin.test.assertTrue
@ -68,6 +70,18 @@ class DubinsTest {
obstacles) obstacles)
val length = pathLength(shortestPath(outputTangents)) val length = pathLength(shortestPath(outputTangents))
println(length) 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 @Test
fun outerTangentsTest1() { fun outerTangentsTest1() {
@ -78,7 +92,6 @@ class DubinsTest {
Circle2D(vector(5.0, 5.0), 1.0) Circle2D(vector(5.0, 5.0), 1.0)
) )
println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2)))
assertTrue(false)
} }
@Test @Test
fun outerTangentsTest2() { 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)
} }
} }