From 137ddb3ade4f57ee11d2fb05defaa68829d93925 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Fri, 29 Jul 2022 14:12:44 +0300 Subject: [PATCH] Code simplification for Dubins path --- .../trajectory/{dubins => }/DubinsPath.kt | 75 ++++++++++++++++--- .../space/kscience/kmath/trajectory/Pose2D.kt | 33 ++++++++ .../trajectory/{segments => }/Trajectory.kt | 17 +++-- .../kmath/trajectory/TrajectoryCost.kt | 14 ++++ .../trajectory/dubins/trajectoryFunctions.kt | 72 ------------------ .../space/kscience/kmath/trajectory/route.kt | 16 ++++ .../kmath/trajectory/segments/Pose2D.kt | 21 ------ .../space/kscience/kmath/trajectory/Math.kt | 2 - .../kmath/trajectory/dubins/DubinsTests.kt | 16 ++-- .../kmath/trajectory/segments/ArcTests.kt | 1 + .../kmath/trajectory/segments/LineTests.kt | 1 + 11 files changed, 146 insertions(+), 122 deletions(-) rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/{dubins => }/DubinsPath.kt (67%) create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/{segments => }/Trajectory.kt (85%) create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.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/DubinsPath.kt similarity index 67% rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt index f510e3572..134342b33 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -3,24 +3,79 @@ * 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 +package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.ArcSegment -import space.kscience.kmath.trajectory.segments.Pose2D -import space.kscience.kmath.trajectory.segments.StraightSegment -import space.kscience.kmath.trajectory.segments.Trajectory +import kotlin.math.PI import kotlin.math.acos import kotlin.math.cos import kotlin.math.sin +internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first + +internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second + +internal fun Pose2D.getTangentCircles(radius: Double): Pair { + val dX = radius * cos(theta) + val dY = radius * sin(theta) + return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius) +} + +internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, ArcSegment.Direction.LEFT) + +internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, + ArcSegment.Direction.RIGHT +) + +private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment { + val centers = StraightSegment(a.center, b.center) + val p1 = when (side) { + ArcSegment.Direction.LEFT -> Vector2D( + a.center.x - a.radius * cos(centers.theta), + a.center.y + a.radius * sin(centers.theta) + ) + ArcSegment.Direction.RIGHT -> Vector2D( + a.center.x + a.radius * cos(centers.theta), + a.center.y - a.radius * sin(centers.theta) + ) + } + return StraightSegment( + p1, + Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) + ) +} + +internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = + innerTangent(base, direction, ArcSegment.Direction.LEFT) + +internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = + innerTangent(base, direction, ArcSegment.Direction.RIGHT) + +private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? { + val centers = StraightSegment(base.center, direction.center) + if (centers.length < base.radius * 2) return null + val angle = theta( + when (side) { + ArcSegment.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) + ArcSegment.Direction.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 StraightSegment(p1, p2) +} + +internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) + public class DubinsPath( public val a: ArcSegment, public val b: Trajectory, public val c: ArcSegment, -) : Trajectory { +) : CompositeTrajectory(listOf(a,b,c)) { public val type: TYPE = TYPE.valueOf( arrayOf( @@ -30,8 +85,6 @@ public class DubinsPath( ).toCharArray().concatToString() ) - override val length: Double get() = a.length + b.length + c.length - public enum class TYPE { RLR, LRL, RSR, LSL, RSL, LSR } @@ -119,7 +172,7 @@ public class 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 + if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT) val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT) @@ -130,11 +183,11 @@ public class 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 + if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT) val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT) return DubinsPath(a1, s, a3) } } -} +} \ No newline at end of file diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt new file mode 100644 index 000000000..1f7c4a52e --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt @@ -0,0 +1,33 @@ +/* + * 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 + +import space.kscience.kmath.geometry.Vector +import space.kscience.kmath.geometry.Vector2D +import kotlin.math.atan2 + +/** + * Combination of [Vector] and its view angle + */ +public interface Pose2D: Vector2D{ + public val coordinate: Vector2D + public val theta: Double +} + +public class PhaseVector2D( + override val coordinate: Vector2D, + public val velocity: Vector2D +): Pose2D, Vector2D by coordinate{ + override val theta: Double get() = atan2(velocity.y, velocity.x) +} + +internal class Pose2DImpl( + override val coordinate: Vector2D, + override val theta: Double +) : Pose2D, Vector2D by coordinate + + +public fun Pose2D(coordinate: Vector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta) \ No newline at end of file diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt similarity index 85% rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt index 453eef47f..2e97d43b0 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt @@ -1,14 +1,18 @@ -package space.kscience.kmath.trajectory.segments +/* + * 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 import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.circumference -import space.kscience.kmath.trajectory.dubins.theta import kotlin.math.PI import kotlin.math.atan2 -public interface Trajectory { +public sealed interface Trajectory { public val length: Double } @@ -69,7 +73,7 @@ public data class ArcSegment( vector: Vector2D, theta: Double, direction: Direction, - ): Pose2D = Pose2D.of( + ): Pose2D = Pose2D( vector, when (direction) { Direction.LEFT -> theta(theta - PI / 2) @@ -84,6 +88,9 @@ public data class ArcSegment( return ArcSegment(Circle2D(center, s1.length), pose1, pose2) } } - +} + +public open class CompositeTrajectory(public val segments: Collection) : Trajectory { + override val length: Double get() = segments.sumOf { it.length } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt new file mode 100644 index 000000000..170851c43 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt @@ -0,0 +1,14 @@ +/* + * 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 + +public fun interface TrajectoryCost { + public fun estimate(trajectory: Trajectory): Double + + public companion object{ + public val length: TrajectoryCost = TrajectoryCost { it.length } + } +} \ No newline at end of file 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 deleted file mode 100644 index 18384d349..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt +++ /dev/null @@ -1,72 +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.Circle2D -import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Pose2D -import space.kscience.kmath.trajectory.segments.StraightSegment -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): Circle2D = getTangentCircles(radius).first -internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second -internal fun Pose2D.getTangentCircles(radius: Double): Pair { - val dX = radius * cos(theta) - val dY = radius * sin(theta) - return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius) -} - -internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT) -internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT) - -private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment { - val centers = StraightSegment(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 StraightSegment( - p1, - Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) - ) -} - -internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = - innerTangent(base, direction, SIDE.LEFT) - -internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = - innerTangent(base, direction, SIDE.RIGHT) - -private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? { - val centers = StraightSegment(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 StraightSegment(p1, p2) -} - -internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt new file mode 100644 index 000000000..f67ab124d --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt @@ -0,0 +1,16 @@ +/* + * 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 + +public fun interface MaxCurvature { + public fun compute(startPoint: PhaseVector2D): Double +} + +public fun DubinsPath.Companion.shortest( + start: PhaseVector2D, + end: PhaseVector2D, + computer: MaxCurvature, +): DubinsPath = shortest(start, end, computer.compute(start)) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt deleted file mode 100644 index 3df34c107..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt +++ /dev/null @@ -1,21 +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.segments - -import space.kscience.kmath.geometry.Vector2D - -/** - * A [Vector2D] with view direction - */ -public data class Pose2D( - override val x: Double, - override val y: Double, - public val theta: Double -) : Vector2D { - public companion object { - public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta) - } -} diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt index 4f8fda826..7ee68bd92 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt @@ -1,8 +1,6 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Pose2D -import space.kscience.kmath.trajectory.segments.StraightSegment import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin 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 9069f6dd2..09375a400 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 @@ -7,13 +7,7 @@ package space.kscience.kmath.trajectory.dubins import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.equalFloat -import space.kscience.kmath.trajectory.equalsFloat -import space.kscience.kmath.trajectory.inverse -import space.kscience.kmath.trajectory.segments.ArcSegment -import space.kscience.kmath.trajectory.segments.Pose2D -import space.kscience.kmath.trajectory.segments.StraightSegment -import space.kscience.kmath.trajectory.shift +import space.kscience.kmath.trajectory.* import kotlin.test.Test import kotlin.test.assertNotNull import kotlin.test.assertTrue @@ -26,8 +20,8 @@ class DubinsTests { val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) val lineP1 = straight.shift(1, 10.0).inverse() - val start = Pose2D.of(straight.end, straight.theta) - val end = Pose2D.of(lineP1.start, lineP1.theta) + val start = Pose2D(straight.end, straight.theta) + val end = Pose2D(lineP1.start, lineP1.theta) val radius = 2.0 val dubins = DubinsPath.all(start, end, radius) @@ -58,8 +52,8 @@ class DubinsTests { assertTrue(path.c.start.equalsFloat(b.end)) } else if (path.b is StraightSegment) { val b = path.b as StraightSegment - assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta))) - assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta))) + assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta))) + assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta))) } } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt index aa8778315..c66fa201b 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -3,6 +3,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.circumference +import space.kscience.kmath.trajectory.ArcSegment import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.test.Test import kotlin.test.assertEquals diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt index afc473d79..11eaa0fb3 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -2,6 +2,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.StraightSegment import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.math.pow import kotlin.math.sqrt