From 8faa312424f37bf6aa4b6db3c33e166709c346d2 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 14:56:21 +0200 Subject: [PATCH] Dubins factory functions --- .../kmath/trajectory/dubins/DubinsPath.kt | 106 +++++++++++ .../trajectory/dubins/DubinsPathFactory.kt | 169 ------------------ .../trajectory/dubins/TrajectoryFunctions.kt | 67 +++++++ .../kmath/trajectory/dubins/DubinsTests.kt | 4 +- 4 files changed, 175 insertions(+), 171 deletions(-) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt index 005d7fd60..ad67fa11d 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt @@ -5,8 +5,16 @@ package space.kscience.kmath.trajectory.dubins +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.segments.Arc import space.kscience.kmath.trajectory.segments.Segment +import space.kscience.kmath.trajectory.segments.Straight +import space.kscience.kmath.trajectory.segments.components.Circle +import space.kscience.kmath.trajectory.segments.components.Pose2D +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin public class DubinsPath( public val a: Arc, @@ -27,4 +35,102 @@ public class DubinsPath( public enum class TYPE { RLR, LRL, RSR, LSL, RSL, LSR } + + public companion object { + public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List = + listOfNotNull( + rlr(start, end, turningRadius), + lrl(start, end, turningRadius), + rsr(start, end, turningRadius), + lsl(start, end, turningRadius), + rsl(start, end, turningRadius), + lsr(start, end, turningRadius) + ) + public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath = + all(start, end, turningRadius).minByOrNull { it.length }!! + + public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val centers = Straight(c1.center, c2.center) + if (centers.length > turningRadius * 4) return null + + var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + var dX = turningRadius * sin(theta) + var dY = turningRadius * cos(theta) + val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val e = Circle(p, turningRadius) + val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + dX = turningRadius * sin(theta) + dY = turningRadius * cos(theta) + val p2 = Vector2D(e.center.x + dX, e.center.y + dY) + val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) + val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) + return DubinsPath(a1, a2, a3) + } + + public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val centers = Straight(c1.center, c2.center) + if (centers.length > turningRadius * 4) return null + + var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + var dX = turningRadius * sin(theta) + var dY = turningRadius * cos(theta) + val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val e = Circle(p, turningRadius) + val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + dX = turningRadius * sin(theta) + dY = turningRadius * cos(theta) + val p2 = Vector2D(e.center.x + dX, e.center.y + dY) + val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) + val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) + return DubinsPath(a1, a2, a3) + } + + public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath { + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val s = leftOuterTangent(c1, c2) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + return DubinsPath(a1, s, a3) + } + + public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath { + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val s = rightOuterTangent(c1, c2) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + return DubinsPath(a1, s, a3) + } + + public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val s = rightInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null + + val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + return DubinsPath(a1, s, a3) + } + + public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val s = leftInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null + + val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + return DubinsPath(a1, s, a3) + } + } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt deleted file mode 100644 index 04c639576..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Copyright 2018-2021 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.dubins - -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.* -import space.kscience.kmath.trajectory.segments.components.Circle -import space.kscience.kmath.trajectory.segments.components.Pose2D -import kotlin.math.PI -import kotlin.math.acos -import kotlin.math.cos -import kotlin.math.sin - -public class DubinsPathFactory( - private val start: Pose2D, - private val end: Pose2D, - private val turningRadius: Double -) { - - public val all: List get() = listOfNotNull(rlr, lrl, rsr, lsl, rsl, lsr) - public val shortest: DubinsPath get() = all.minByOrNull { it.length }!! - public operator fun get(type: DubinsPath.TYPE): DubinsPath? = all.find { it.type == type } - - public val rlr: DubinsPath? - get() { - val c1 = start.getRightCircle(turningRadius) - val c2 = end.getRightCircle(turningRadius) - val centers = Straight(c1.center, c2.center) - if (centers.length > turningRadius * 4) return null - - var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) - var dX = turningRadius * sin(theta) - var dY = turningRadius * cos(theta) - val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) - val e = Circle(p, turningRadius) - val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) - dX = turningRadius * sin(theta) - dY = turningRadius * cos(theta) - val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) - val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) - val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) - return DubinsPath(a1, a2, a3) - } - - private val lrl: DubinsPath? - get() { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - val centers = Straight(c1.center, c2.center) - if (centers.length > turningRadius * 4) return null - - var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) - var dX = turningRadius * sin(theta) - var dY = turningRadius * cos(theta) - val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) - val e = Circle(p, turningRadius) - val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) - dX = turningRadius * sin(theta) - dY = turningRadius * cos(theta) - val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) - val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) - return DubinsPath(a1, a2, a3) - } - - public val rsr: DubinsPath - get() { - val c1 = start.getRightCircle(turningRadius) - val c2 = end.getRightCircle(turningRadius) - val s = leftOuterTangent(c1, c2) - val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) - return DubinsPath(a1, s, a3) - } - - public val lsl: DubinsPath - get() { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - val s = rightOuterTangent(c1, c2) - val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) - return DubinsPath(a1, s, a3) - } - - public val rsl: DubinsPath? - get() { - val c1 = start.getRightCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - val s = rightInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - - val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) - return DubinsPath(a1, s, a3) - } - - public val lsr: DubinsPath? - get() { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getRightCircle(turningRadius) - val s = leftInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - - val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) - return DubinsPath(a1, s, a3) - } -} - -private enum class SIDE { - LEFT, RIGHT -} - -private fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first -private fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second -private fun Pose2D.getTangentCircles(radius: Double): Pair { - val dX = radius * cos(theta) - val dY = radius * sin(theta) - return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius) -} - -private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) -private fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) -private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { - val centers = Straight(a.center, b.center) - val p1 = when (side) { - SIDE.LEFT -> Vector2D( - a.center.x - a.radius * cos(centers.theta), - a.center.y + a.radius * sin(centers.theta) - ) - SIDE.RIGHT -> Vector2D( - a.center.x + a.radius * cos(centers.theta), - a.center.y - a.radius * sin(centers.theta) - ) - } - return Straight( - p1, - Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) - ) -} - -private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) -private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) -private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? { - val centers = Straight(base.center, direction.center) - if (centers.length < base.radius * 2) return null - val angle = theta( - when (side) { - SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) - SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) - } - ) - val dX = base.radius * sin(angle) - val dY = base.radius * cos(angle) - val p1 = Vector2D(base.center.x + dX, base.center.y + dY) - val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) - return Straight(p1, p2) -} - -internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt new file mode 100644 index 000000000..547cb99fc --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt @@ -0,0 +1,67 @@ +/* + * Copyright 2018-2021 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.dubins + +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.Straight +import space.kscience.kmath.trajectory.segments.components.Circle +import space.kscience.kmath.trajectory.segments.components.Pose2D +import kotlin.math.PI +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin + +private enum class SIDE { + LEFT, RIGHT +} + +internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first +internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second +internal fun Pose2D.getTangentCircles(radius: Double): Pair { + val dX = radius * cos(theta) + val dY = radius * sin(theta) + return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius) +} + +internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) +internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) +private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { + val centers = Straight(a.center, b.center) + val p1 = when (side) { + SIDE.LEFT -> Vector2D( + a.center.x - a.radius * cos(centers.theta), + a.center.y + a.radius * sin(centers.theta) + ) + SIDE.RIGHT -> Vector2D( + a.center.x + a.radius * cos(centers.theta), + a.center.y - a.radius * sin(centers.theta) + ) + } + return Straight( + p1, + Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) + ) +} + +internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) +internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) +private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? { + val centers = Straight(base.center, direction.center) + if (centers.length < base.radius * 2) return null + val angle = theta( + when (side) { + SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) + SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) + } + ) + val dX = base.radius * sin(angle) + val dY = base.radius * cos(angle) + val p1 = Vector2D(base.center.x + dX, base.center.y + dY) + val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) + return Straight(p1, p2) +} + +internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt index 025aab2b7..a5288e40d 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -29,7 +29,7 @@ class DubinsTests { val start = Pose2D.of(straight.end, straight.theta) val end = Pose2D.of(lineP1.start, lineP1.theta) val radius = 2.0 - val dubins = DubinsPathFactory(start, end, radius) + val dubins = DubinsPath.all(start, end, radius) val absoluteDistance = start.distanceTo(end) println("Absolute distance: $absoluteDistance") @@ -43,7 +43,7 @@ class DubinsTests { ) expectedLengths.forEach { - val path = dubins[it.key] + val path = dubins.find { p -> p.type === it.key } assertNotNull(path, "Path ${it.key} not found") println("${it.key}: ${path.length}") assertTrue(it.value.equalFloat(path.length))