From 2c13386646a6f3f38523e9546eccf38343251163 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Wed, 1 Mar 2023 10:40:54 +0300 Subject: [PATCH] search for shortest path algorithm --- .../space/kscience/kmath/geometry/Line.kt | 1 + .../kmath/trajectory/DubinsObstacle.kt | 413 ++++++++++++++++++ .../kscience/kmath/trajectory/DubinsPath.kt | 110 +++++ .../kscience/kmath/trajectory/tangent.kt | 66 ++- 4 files changed, 583 insertions(+), 7 deletions(-) create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt index ab322ddca..e593150f1 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt @@ -6,6 +6,7 @@ package space.kscience.kmath.geometry 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, diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt new file mode 100644 index 000000000..56aa88e4a --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -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 +) { + public val tangents: List = boundaryTangents().first + public val boundaryRoute: DubinsPath.Type = boundaryTangents().second + public val center: Vector2D = + vector(this.circles.sumOf{it.center.x} / this.circles.size, + this.circles.sumOf{it.center.y} / this.circles.size) + private fun boundaryTangents(): Pair, DubinsPath.Type> { + // outer tangents for a polygon circles can be either lsl or rsr + + fun Circle2D.dubinsTangentsToCircles( + other: Circle2D, + ): Map = 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 { + 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 { + 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 { + 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): List { + return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed() +} + +public fun tangentsAlongTheObstacle(initialCircle: Circle2D, + initialRoute: DubinsPath.Type, + finalCircle: Circle2D, + obstacle: DubinsObstacle): MutableList { + val dubinsTangents = mutableListOf() + 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>, + finalObstacle: DubinsObstacle): Boolean { + for (path in paths) { + if (path[-1].endObstacle != finalObstacle) { + return false + } + } + return true +} + +public fun pathLength(path: List): Double { + val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)} + val arcsLength = buildList{ + 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> { + return path.sortedBy { pathLength(it) } +} + +public typealias Path = List +public fun findAllPaths( + startingPoint: DoubleVector2D, + startingDirection: DoubleVector2D, + startingRadius: Double, + finalPoint: DoubleVector2D, + finalDirection: DoubleVector2D, + finalRadius: Double, + obstacles: List +) { + val initialCircles = constructTangentCircles( + startingPoint, + startingDirection, + startingRadius) + val finalCircles = constructTangentCircles( + finalPoint, + finalDirection, + finalRadius) + var outputTangents = mutableMapOf>() + 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() + 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 + } + else { + nextTangents = + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} + as MutableMap + } + TODO("rewrite fragment from Python") + } + } + } + } + } +} + + + + + + + + diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt index 568ef691a..5654d10ae 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -77,10 +77,118 @@ private fun innerTangent( @Suppress("DuplicatedCode") 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 { RLR, LRL, RSR, LSL, RSL, LSR } + public fun toSimpleTypes(type: Type): List { + 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): 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) { +// 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 = this.inputTypes +// } + /** * Return Dubins trajectory type or null if trajectory is not a Dubins path */ @@ -243,6 +351,8 @@ public object DubinsPath { } } +public typealias PathTypes = List + public fun interface MaxCurvature { public fun compute(startPoint: PhaseVector2D): Double } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index 945b140c2..2d935aa00 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -5,10 +5,10 @@ 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.LineSegment +import space.kscience.kmath.geometry.* +import space.kscience.kmath.geometry.Euclidean2DSpace.plus +import space.kscience.kmath.geometry.Euclidean2DSpace.times +import space.kscience.kmath.operations.DoubleField.pow import kotlin.math.* /** @@ -17,7 +17,7 @@ import kotlin.math.* */ public fun Circle2D.tangentsToCircle( other: Circle2D, -): Map> = with(Euclidean2DSpace) { +): Map = 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) @@ -48,14 +48,66 @@ public fun Circle2D.tangentsToCircle( val w = vector(-cos(angle2), sin(angle2)) put( route, - LineSegment( + LineSegment2D( center + w * r1, other.center + w * r2 ) ) } 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 = 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") } } }