search for shortest path algorithm
This commit is contained in:
parent
2bce369c5d
commit
4c1ffdb6d9
@ -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)]!!
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -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)
|
||||||
}
|
}
|
||||||
}
|
}
|
Loading…
Reference in New Issue
Block a user