search for shortest path algorithm

This commit is contained in:
Artyom Degtyarev 2023-03-01 10:40:54 +03:00
parent cc0fb2a718
commit 2c13386646
4 changed files with 583 additions and 7 deletions

View File

@ -6,6 +6,7 @@
package space.kscience.kmath.geometry package space.kscience.kmath.geometry
import kotlinx.serialization.Serializable import kotlinx.serialization.Serializable
import space.kscience.kmath.operations.DoubleField.pow
/** /**
* A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized, * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized,

View File

@ -0,0 +1,413 @@
/*
* Copyright 2018-2023 KMath contributors.
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
*/
package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Euclidean2DSpace.minus
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
import space.kscience.kmath.geometry.Euclidean2DSpace.times
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.Euclidean2DSpace.norm
import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.*
public fun LineSegment2D.length(): Double {
return ((end.y - begin.y).pow(2.0) + (end.x - begin.x).pow(2.0)).pow(0.5)
}
public class DubinsObstacle(
public val circles: List<Circle2D>
) {
public val tangents: List<DubinsTangent> = boundaryTangents().first
public val boundaryRoute: DubinsPath.Type = boundaryTangents().second
public val center: Vector2D<Double> =
vector(this.circles.sumOf{it.center.x} / this.circles.size,
this.circles.sumOf{it.center.y} / this.circles.size)
private fun boundaryTangents(): Pair<List<DubinsTangent>, DubinsPath.Type> {
// outer tangents for a polygon circles can be either lsl or rsr
fun Circle2D.dubinsTangentsToCircles(
other: Circle2D,
): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
val line = LineSegment(center, other.center)
val d = line.begin.distanceTo(line.end)
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
var r: Double
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 {
for ((route, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (d * d > r * r) {
val l = (d * d - r * r).pow(0.5)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = vector(-cos(angle2), sin(angle2))
put(route, DubinsTangent(Circle2D(center, radius),
other,
this@DubinsObstacle,
this@DubinsObstacle,
LineSegment2D(
center + w * r1,
other.center + w * r2
),
DubinsPath.toSimpleTypes(route))
)
} else {
throw Exception("Circles should not intersect")
}
}
}
}
val firstCircles = this.circles.slice(-this.circles.size..-1)
val secondCircles = this.circles.slice(-this.circles.size+1..0)
val lslTangents = firstCircles.zip(secondCircles)
{a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!}
val rsrTangents = firstCircles.zip(secondCircles)
{a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.RSR]!!}
val center = vector(
this.circles.sumOf { it.center.x } / this.circles.size,
this.circles.sumOf { it.center.y } / this.circles.size
)
val lslToCenter = lslTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
lslTangents.sumOf { it.lineSegment.end.distanceTo(center) }
val rsrToCenter = rsrTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
rsrTangents.sumOf { it.lineSegment.end.distanceTo(center) }
return if (rsrToCenter >= lslToCenter) {
Pair(rsrTangents, DubinsPath.Type.RSR)
} else {
Pair(lslTangents, DubinsPath.Type.LSL)
}
}
public fun nextTangent(circle: Circle2D, route: DubinsPath.Type): DubinsTangent {
if (route == this.boundaryRoute) {
for (i in this.circles.indices) {
if (this.circles[i] == circle) {
return this.tangents[i]
}
}
}
else {
for (i in this.circles.indices) {
if (this.circles[i] == circle) {
return DubinsTangent(this.circles[i],
this.circles[i-1],
this,
this,
LineSegment2D(this.tangents[i-1].lineSegment.end,
this.tangents[i-1].lineSegment.begin),
DubinsPath.toSimpleTypes(route))
}
}
}
error("next tangent not found")
}
public fun equals(other: DubinsObstacle): Boolean {
return this.circles == other.circles
}
}
public data class DubinsTangent(val startCircle: Circle2D,
val endCircle: Circle2D,
val startObstacle: DubinsObstacle,
val endObstacle: DubinsObstacle,
val lineSegment: LineSegment2D,
val route: PathTypes)
public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
return v1.x * v2.y - v1.y * v2.x
}
if (crossProduct(other.begin - this.begin, other.end - this.begin).sign ==
crossProduct(other.begin - this.end, other.end - this.end).sign) {
return false
}
if (crossProduct(this.begin - other.begin, this.end - other.begin).sign ==
crossProduct(this.begin - other.end, this.end - other.end).sign) {
return false
}
return true
}
public 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))
val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) -
circle.radius.pow(2.0)
val d = b.pow(2.0) - 4 * a * c
if (d < 1e-6) {
return false
}
else {
val t1 = (-b - d.pow(0.5)) * 0.5 / a
val t2 = (-b + d.pow(0.5)) * 0.5 / a
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
return true
}
}
return false
}
public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean {
for (tangent in obstacle.tangents) {
if (this.lineSegment.intersectSegment(tangent.lineSegment)) {
return true
}
}
for (circle in obstacle.circles) {
if (this.lineSegment.intersectCircle(circle)) {
return true
}
}
return false
}
public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> {
return buildMap {
for (circle1 in first.circles) {
for (circle2 in second.circles) {
for (tangent in dubinsTangentsToCircles(circle1, circle2, first, second)) {
if (!(tangent.value.intersectObstacle(first))
and !(tangent.value.intersectObstacle(second))) {
put(
tangent.key,
tangent.value
)
}
}
}
}
}.toMutableMap()
}
public fun arcLength(circle: Circle2D,
point1: DoubleVector2D,
point2: DoubleVector2D,
route: DubinsPath.SimpleType): Double {
val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
var angle = 0.0
when (route) {
DubinsPath.SimpleType.L -> {
angle = if (phi2 >= phi1) {
phi2 - phi1
} else {
2 * PI + phi2 - phi1
}
}
DubinsPath.SimpleType.R -> {
angle = if (phi2 >= phi1) {
2 * PI - (phi2 - phi1)
} else {
-(phi2 - phi1)
}
}
DubinsPath.SimpleType.S -> {
error("L or R route is expected")
}
}
return circle.radius * angle
}
public 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,
direction: DoubleVector2D,
r: Double): Map<DubinsPath.SimpleType, Circle2D> {
val center1 = point + normalVectors(direction, r).first
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)) {
mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r),
DubinsPath.SimpleType.R to Circle2D(center2, r))
}
else {
mapOf(DubinsPath.SimpleType.L to Circle2D(center2, r),
DubinsPath.SimpleType.R to Circle2D(center1, r))
}
}
public fun sortedObstacles(currentObstacle: DubinsObstacle,
obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed()
}
public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
initialRoute: DubinsPath.Type,
finalCircle: Circle2D,
obstacle: DubinsObstacle): MutableList<DubinsTangent> {
val dubinsTangents = mutableListOf<DubinsTangent>()
var tangent = obstacle.nextTangent(initialCircle, initialRoute)
dubinsTangents.add(tangent)
while (tangent.endCircle != finalCircle) {
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute)
dubinsTangents.add(tangent)
}
return dubinsTangents
}
public fun allFinished(paths: List<List<DubinsTangent>>,
finalObstacle: DubinsObstacle): Boolean {
for (path in paths) {
if (path[-1].endObstacle != finalObstacle) {
return false
}
}
return true
}
public fun pathLength(path: List<DubinsTangent>): Double {
val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)}
val arcsLength = buildList<Double>{
for (i in 1..path.size) {
add(arcLength(path[i].startCircle,
path[i-1].lineSegment.end,
path[i].lineSegment.begin,
path[i].route[0]))
}
}.sum()
return tangentsLength + arcsLength
}
public fun shortestPath(path: List<List<DubinsTangent>>): List<List<DubinsTangent>> {
return path.sortedBy { pathLength(it) }
}
public typealias Path = List<DubinsTangent>
public fun findAllPaths(
startingPoint: DoubleVector2D,
startingDirection: DoubleVector2D,
startingRadius: Double,
finalPoint: DoubleVector2D,
finalDirection: DoubleVector2D,
finalRadius: Double,
obstacles: List<DubinsObstacle>
) {
val initialCircles = constructTangentCircles(
startingPoint,
startingDirection,
startingRadius)
val finalCircles = constructTangentCircles(
finalPoint,
finalDirection,
finalRadius)
var outputTangents = mutableMapOf<PathTypes, List<Path>>()
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,
DubinsPath.SimpleType.S,
j)] = listOf(
listOf(DubinsTangent(
initialCircles[i]!!,
initialCircles[i]!!,
DubinsObstacle(listOf(initialCircles[i]!!)),
DubinsObstacle(listOf(initialCircles[i]!!)),
LineSegment2D(startingPoint, startingPoint),
listOf(i, DubinsPath.SimpleType.S, i)
)))
var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
while (!allFinished(outputTangents[listOf(i,
DubinsPath.SimpleType.S,
j)]!!, finalObstacle)) {
var newOutputTangents = listOf<Path>()
for (line in outputTangents[listOf(i,
DubinsPath.SimpleType.S,
j)]!!) {
var currentCircle = line[-1].endCircle
var currentDirection = line[-1].route[-1]
var currentObstacle = line[-1].endObstacle
var nextObstacle = DubinsObstacle(listOf())
if (currentObstacle != finalObstacle) {
var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf(
currentDirection,
DubinsPath.SimpleType.S,
j)
)]
for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
if (tangentToFinal!!.intersectObstacle(obstacle)) {
nextObstacle = obstacle
break
}
}
if (nextObstacle == DubinsObstacle(listOf())) {
nextObstacle = finalObstacle
}
var nextTangents = outerTangents(currentObstacle, nextObstacle)
// for (pathType in listOf(
// listOf(DubinsPath.SimpleType.L,
// DubinsPath.SimpleType.S,
// DubinsPath.SimpleType.L),
// listOf(DubinsPath.SimpleType.L,
// DubinsPath.SimpleType.S,
// DubinsPath.SimpleType.R),
// listOf(DubinsPath.SimpleType.R,
// DubinsPath.SimpleType.S,
// DubinsPath.SimpleType.L),
// listOf(DubinsPath.SimpleType.R,
// DubinsPath.SimpleType.S,
// DubinsPath.SimpleType.R)
// )) {
for (pathType in nextTangents.keys) {
for (obstacle in obstacles) {
// in Python code here try/except was used, but seems unneeded
if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
nextTangents.remove(pathType)
}
}
}
if (nextObstacle == finalObstacle) {
nextTangents =
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
(DubinsPath.toSimpleTypes(it.key)[0] == j)}
as MutableMap<DubinsPath.Type, DubinsTangent>
}
else {
nextTangents =
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
as MutableMap<DubinsPath.Type, DubinsTangent>
}
TODO("rewrite fragment from Python")
}
}
}
}
}
}

View File

@ -77,10 +77,118 @@ private fun innerTangent(
@Suppress("DuplicatedCode") @Suppress("DuplicatedCode")
public object DubinsPath { public object DubinsPath {
// public class ArcType(private val type: Type){
// public val first: SimpleType
// get() {
// if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) {
// return SimpleType.R
// }
// else if (type in listOf(Type.LSL, Type.LSR, Type.LRL)) {
// return SimpleType.L
// }
// error("Wrong DubinsPath.Type")
// }
//
// public val last: SimpleType
// get() {
// if (type in listOf(Type.RSR, Type.LSR, Type.RLR)) {
// return SimpleType.R
// }
// else if (type in listOf(Type.LSL, Type.RSL, Type.LRL)) {
// return SimpleType.L
// }
// error("Wrong DubinsPath.Type")
// }
// public val intermediate: SimpleType
// get() {
// if (type == Type.RLR) {
// return SimpleType.L
// }
// else if (type == Type.LRL) {
// return SimpleType.R
// }
// error("This DubinsPath.Type doesn't contain intermediate arc")
// }
// }
public enum class SimpleType {
R, S, L
}
public enum class Type { public enum class Type {
RLR, LRL, RSR, LSL, RSL, LSR RLR, LRL, RSR, LSL, RSL, LSR
} }
public fun toSimpleTypes(type: Type): List<SimpleType> {
when (type) {
Type.RLR -> {
return listOf(SimpleType.R, SimpleType.L, SimpleType.R)
}
Type.LRL -> {
return listOf(SimpleType.L, SimpleType.R, SimpleType.L)
}
Type.RSR -> {
return listOf(SimpleType.R, SimpleType.S, SimpleType.R)
}
Type.LSL -> {
return listOf(SimpleType.L, SimpleType.S, SimpleType.L)
}
Type.RSL -> {
return listOf(SimpleType.R, SimpleType.S, SimpleType.L)
}
Type.LSR -> {
return listOf(SimpleType.L, SimpleType.S, SimpleType.R)
}
else -> error("This type doesn't exist")
}
}
public fun toType(types: List<SimpleType>): Type {
when (types) {
listOf(SimpleType.R, SimpleType.L, SimpleType.R) -> {
return Type.RLR
}
listOf(SimpleType.L, SimpleType.R, SimpleType.L) -> {
return Type.LRL
}
listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
return Type.RSR
}
listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
return Type.LSL
}
listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
return Type.RSL
}
listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
return Type.LSR
}
else -> error("This type doesn't exist")
}
}
// public class PathTypes(private val inputTypes: List<SimpleType>) {
// public val type: Type
// get() {
// when (this.inputTypes) {
// listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
// return Type.RSR
// }
// listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
// return Type.RSL
// }
// listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
// return Type.LSR
// }
// listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
// return Type.LSL
// }
// else -> error("Wrong list of SimpleTypes")
// }
// }
// public val chain: List<SimpleType> = this.inputTypes
// }
/** /**
* Return Dubins trajectory type or null if trajectory is not a Dubins path * Return Dubins trajectory type or null if trajectory is not a Dubins path
*/ */
@ -243,6 +351,8 @@ public object DubinsPath {
} }
} }
public typealias PathTypes = List<DubinsPath.SimpleType>
public fun interface MaxCurvature { public fun interface MaxCurvature {
public fun compute(startPoint: PhaseVector2D): Double public fun compute(startPoint: PhaseVector2D): Double
} }

View File

@ -5,10 +5,10 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.DoubleVector2D import space.kscience.kmath.geometry.Euclidean2DSpace.plus
import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Euclidean2DSpace.times
import space.kscience.kmath.geometry.LineSegment import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.* import kotlin.math.*
/** /**
@ -17,7 +17,7 @@ import kotlin.math.*
*/ */
public fun Circle2D.tangentsToCircle( public fun Circle2D.tangentsToCircle(
other: Circle2D, other: Circle2D,
): Map<DubinsPath.Type, LineSegment<DoubleVector2D>> = with(Euclidean2DSpace) { ): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
val line = LineSegment(center, other.center) val line = LineSegment(center, other.center)
val d = line.begin.distanceTo(line.end) val d = line.begin.distanceTo(line.end)
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y) val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
@ -48,14 +48,66 @@ public fun Circle2D.tangentsToCircle(
val w = vector(-cos(angle2), sin(angle2)) val w = vector(-cos(angle2), sin(angle2))
put( put(
route, route,
LineSegment( LineSegment2D(
center + w * r1, center + w * r1,
other.center + w * r2 other.center + w * r2
) )
) )
} }
else { else {
throw Exception("Circles should not be") throw Exception("Circles should not intersect")
}
}
}
}
public fun dubinsTangentsToCircles(
firstCircle: Circle2D,
secondCircle: Circle2D,
firstObstacle: DubinsObstacle,
secondObstacle: DubinsObstacle
): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
val line = LineSegment(firstCircle.center, secondCircle.center)
val d = line.begin.distanceTo(line.end)
val angle1 = atan2(secondCircle.center.x - firstCircle.center.x,
secondCircle.center.y - firstCircle.center.y)
var r: Double
var angle2: Double
val routes = mapOf(
DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
)
return buildMap {
for ((route, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (d * d > r * r) {
val l = (d * d - r * r).pow(0.5)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
secondCircle,
firstObstacle,
secondObstacle,
LineSegment2D(
firstCircle.center + w * r1,
secondCircle.center + w * r2
),
DubinsPath.toSimpleTypes(route))
)
} else {
throw Exception("Circles should not intersect")
} }
} }
} }