From e24463c58bc6368709c2db681f9d1b8518f36f7d Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sat, 15 Oct 2022 18:45:06 +0300 Subject: [PATCH] Refactor Dubins path --- build.gradle.kts | 2 +- .../kmath/tensorflow/TensorFlowAlgebra.kt | 3 + .../kscience/kmath/trajectory/DubinsPath.kt | 177 +++++++++--------- .../kscience/kmath/trajectory/DubinsPose2D.kt | 36 ++++ .../space/kscience/kmath/trajectory/Pose2D.kt | 33 ---- .../{Trajectory.kt => Trajectory2D.kt} | 64 ++++--- .../kmath/trajectory/TrajectoryCost.kt | 14 -- .../space/kscience/kmath/trajectory/route.kt | 4 +- .../space/kscience/kmath/trajectory/Math.kt | 12 +- .../kmath/trajectory/dubins/DubinsTests.kt | 18 +- .../kmath/trajectory/segments/ArcTests.kt | 8 +- .../kmath/trajectory/segments/LineTests.kt | 22 +-- 12 files changed, 201 insertions(+), 192 deletions(-) create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/{Trajectory.kt => Trajectory2D.kt} (53%) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt diff --git a/build.gradle.kts b/build.gradle.kts index c7e2e5892..bf732c893 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -14,7 +14,7 @@ allprojects { } group = "space.kscience" - version = "0.3.1-dev-5" + version = "0.3.1-dev-6" } subprojects { diff --git a/kmath-tensorflow/src/main/kotlin/space/kscience/kmath/tensorflow/TensorFlowAlgebra.kt b/kmath-tensorflow/src/main/kotlin/space/kscience/kmath/tensorflow/TensorFlowAlgebra.kt index 435b01b80..bc5fb9616 100644 --- a/kmath-tensorflow/src/main/kotlin/space/kscience/kmath/tensorflow/TensorFlowAlgebra.kt +++ b/kmath-tensorflow/src/main/kotlin/space/kscience/kmath/tensorflow/TensorFlowAlgebra.kt @@ -77,11 +77,13 @@ public abstract class TensorFlowOutput( } } + @PerformancePitfall override fun get(index: IntArray): T = actualTensor[index] @PerformancePitfall override fun elements(): Sequence> = actualTensor.elements() + @PerformancePitfall override fun set(index: IntArray, value: T) { actualTensor[index] = value } @@ -101,6 +103,7 @@ public abstract class TensorFlowAlgebra> internal c protected abstract fun const(value: T): Constant + @OptIn(PerformancePitfall::class) override fun StructureND.valueOrNull(): T? = if (shape contentEquals ShapeND(1)) get(intArrayOf(0)) else null 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 6a340f6b9..1ba9936ee 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 @@ -13,75 +13,80 @@ import kotlin.math.acos import kotlin.math.cos import kotlin.math.sin -internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first +internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first -internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second +internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second -internal fun Pose2D.getTangentCircles(radius: Double): Pair = with(Euclidean2DSpace) { - val dX = radius * cos(theta) - val dY = radius * sin(theta) +internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair = with(Euclidean2DSpace) { + val dX = radius * cos(bearing) + val dY = radius * sin(bearing) return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius) } -internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b, CircleTrajectory.Direction.LEFT) +internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D = + outerTangent(a, b, CircleTrajectory2D.Direction.LEFT) -internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b, - CircleTrajectory.Direction.RIGHT +internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D = outerTangent( + a, b, + CircleTrajectory2D.Direction.RIGHT ) -private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory = with(Euclidean2DSpace){ - val centers = StraightTrajectory(a.center, b.center) - val p1 = when (side) { - CircleTrajectory.Direction.LEFT -> vector( - a.center.x - a.radius * cos(centers.theta), - a.center.y + a.radius * sin(centers.theta) - ) - CircleTrajectory.Direction.RIGHT -> vector( - a.center.x + a.radius * cos(centers.theta), - a.center.y - a.radius * sin(centers.theta) +private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D = + with(Euclidean2DSpace) { + val centers = StraightTrajectory2D(a.center, b.center) + val p1 = when (side) { + CircleTrajectory2D.Direction.LEFT -> vector( + a.center.x - a.radius * cos(centers.bearing), + a.center.y + a.radius * sin(centers.bearing) + ) + + CircleTrajectory2D.Direction.RIGHT -> vector( + a.center.x + a.radius * cos(centers.bearing), + a.center.y - a.radius * sin(centers.bearing) + ) + } + return StraightTrajectory2D( + p1, + vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) ) } - return StraightTrajectory( - p1, - vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) - ) -} -internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? = - innerTangent(base, direction, CircleTrajectory.Direction.LEFT) +internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = + innerTangent(base, direction, CircleTrajectory2D.Direction.LEFT) -internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? = - innerTangent(base, direction, CircleTrajectory.Direction.RIGHT) +internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = + innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT) -private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory? = with(Euclidean2DSpace){ - val centers = StraightTrajectory(base.center, direction.center) - if (centers.length < base.radius * 2) return null - val angle = theta( - when (side) { - CircleTrajectory.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) - CircleTrajectory.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) - } - ) - val dX = base.radius * sin(angle) - val dY = base.radius * cos(angle) - val p1 = vector(base.center.x + dX, base.center.y + dY) - val p2 = vector(direction.center.x - dX, direction.center.y - dY) - return StraightTrajectory(p1, p2) -} +private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D? = + with(Euclidean2DSpace) { + val centers = StraightTrajectory2D(base.center, direction.center) + if (centers.length < base.radius * 2) return null + val angle = theta( + when (side) { + CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length) + CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length) + } + ) + val dX = base.radius * sin(angle) + val dY = base.radius * cos(angle) + val p1 = vector(base.center.x + dX, base.center.y + dY) + val p2 = vector(direction.center.x - dX, direction.center.y - dY) + return StraightTrajectory2D(p1, p2) + } internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) @Suppress("DuplicatedCode") public class DubinsPath( - public val a: CircleTrajectory, - public val b: Trajectory, - public val c: CircleTrajectory, -) : CompositeTrajectory(listOf(a,b,c)) { + public val a: CircleTrajectory2D, + public val b: Trajectory2D, + public val c: CircleTrajectory2D, +) : CompositeTrajectory2D(listOf(a, b, c)) { public val type: TYPE = TYPE.valueOf( arrayOf( a.direction.name[0], - if (b is CircleTrajectory) b.direction.name[0] else 'S', + if (b is CircleTrajectory2D) b.direction.name[0] else 'S', c.direction.name[0] ).toCharArray().concatToString() ) @@ -92,8 +97,8 @@ public class DubinsPath( public companion object { public fun all( - start: Pose2D, - end: Pose2D, + start: DubinsPose2D, + end: DubinsPose2D, turningRadius: Double, ): List = listOfNotNull( rlr(start, end, turningRadius), @@ -104,132 +109,132 @@ public class DubinsPath( lsr(start, end, turningRadius) ) - public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath = + public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath = all(start, end, turningRadius).minBy { it.length } - public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { + public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) - val centers = StraightTrajectory(c1.center, c2.center) + val centers = StraightTrajectory2D(c1.center, c2.center) if (centers.length > turningRadius * 4) return null val firstVariant = run { - var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) val p1 = vector(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT) - val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT) - val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) DubinsPath(a1, a2, a3) } val secondVariant = run { - var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) val p1 = vector(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT) - val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT) - val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) DubinsPath(a1, a2, a3) } return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant } - public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { + public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) - val centers = StraightTrajectory(c1.center, c2.center) + val centers = StraightTrajectory2D(c1.center, c2.center) if (centers.length > turningRadius * 4) return null val firstVariant = run { - var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) val p1 = vector(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT) - val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT) - val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) DubinsPath(a1, a2, a3) } val secondVariant = run{ - var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) + var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) val p1 = vector(c1.center.x + dX, c1.center.y + dY) - theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) + theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT) - val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT) - val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) DubinsPath(a1, a2, a3) } return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant } - public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath { + public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = leftOuterTangent(c1, c2) - val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT) - val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT) return DubinsPath(a1, s, a3) } - public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath { + public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = rightOuterTangent(c1, c2) - val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT) - val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT) return DubinsPath(a1, s, a3) } - public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? { val c1 = start.getRightCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = rightInnerTangent(c1, c2) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT) - val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT) return DubinsPath(a1, s, a3) } - public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = leftInnerTangent(c1, c2) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT) - val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT) return DubinsPath(a1, s, a3) } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt new file mode 100644 index 000000000..ba001b4a5 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt @@ -0,0 +1,36 @@ +/* + * Copyright 2018-2022 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.DoubleVector2D +import space.kscience.kmath.geometry.Vector +import kotlin.math.atan2 + +/** + * Combination of [Vector] and its view angle (clockwise from positive y-axis direction) + */ +public interface DubinsPose2D : DoubleVector2D { + public val coordinate: DoubleVector2D + public val bearing: Double +} + +public class PhaseVector2D( + override val coordinate: DoubleVector2D, + public val velocity: DoubleVector2D, +) : DubinsPose2D, DoubleVector2D by coordinate { + override val bearing: Double get() = atan2(velocity.x, velocity.y) +} + +internal class Pose2DImpl( + override val coordinate: DoubleVector2D, + override val bearing: Double, +) : DubinsPose2D, DoubleVector2D by coordinate{ + + override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)" +} + + +public fun Pose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = Pose2DImpl(coordinate, theta) \ 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 deleted file mode 100644 index 788cf57af..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright 2018-2022 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.DoubleVector2D -import space.kscience.kmath.geometry.Vector -import kotlin.math.atan2 - -/** - * Combination of [Vector] and its view angle - */ -public interface Pose2D: DoubleVector2D{ - public val coordinate: DoubleVector2D - public val theta: Double -} - -public class PhaseVector2D( - override val coordinate: DoubleVector2D, - public val velocity: DoubleVector2D -): Pose2D, DoubleVector2D by coordinate{ - override val theta: Double get() = atan2(velocity.y, velocity.x) -} - -internal class Pose2DImpl( - override val coordinate: DoubleVector2D, - override val theta: Double -) : Pose2D, DoubleVector2D by coordinate - - -public fun Pose2D(coordinate: DoubleVector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta) \ No newline at end of file diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt similarity index 53% rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt index 1085f7847..f1ae20722 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -8,58 +8,61 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.DoubleVector2D import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.circumference import kotlin.math.PI import kotlin.math.atan2 -public sealed interface Trajectory { +public sealed interface Trajectory2D { public val length: Double } /** * Straight path segment. The order of start and end defines the direction */ -public data class StraightTrajectory( +public data class StraightTrajectory2D( internal val start: DoubleVector2D, internal val end: DoubleVector2D, -) : Trajectory { +) : Trajectory2D { override val length: Double get() = start.distanceTo(end) - internal val theta: Double get() = theta(atan2(end.x - start.x, end.y - start.y)) + internal val bearing: Double get() = theta(atan2(end.x - start.x, end.y - start.y)) } /** * An arc segment */ -public data class CircleTrajectory( +public data class CircleTrajectory2D( public val circle: Circle2D, - public val start: Pose2D, - public val end: Pose2D, -) : Trajectory { + public val start: DubinsPose2D, + public val end: DubinsPose2D, +) : Trajectory2D { public enum class Direction { LEFT, RIGHT } - override val length: Double by lazy { - val angle: Double = theta( + /** + * Arc length in radians + */ + val arcLength: Double + get() = theta( if (direction == Direction.LEFT) { - start.theta - end.theta + start.bearing - end.bearing } else { - end.theta - start.theta + end.bearing - start.bearing } ) - val proportion = angle / (2 * PI) - circle.circumference * proportion + + override val length: Double by lazy { + circle.radius * arcLength } internal val direction: Direction by lazy { if (start.y < circle.center.y) { - if (start.theta > PI) Direction.RIGHT else Direction.LEFT + if (start.bearing > PI) Direction.RIGHT else Direction.LEFT } else if (start.y > circle.center.y) { - if (start.theta < PI) Direction.RIGHT else Direction.LEFT + if (start.bearing < PI) Direction.RIGHT else Direction.LEFT } else { - if (start.theta == 0.0) { + if (start.bearing == 0.0) { if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT } else { if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT @@ -68,12 +71,17 @@ public data class CircleTrajectory( } public companion object { - public fun of(center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, direction: Direction): CircleTrajectory { + public fun of( + center: DoubleVector2D, + start: DoubleVector2D, + end: DoubleVector2D, + direction: Direction, + ): CircleTrajectory2D { fun calculatePose( vector: DoubleVector2D, theta: Double, direction: Direction, - ): Pose2D = Pose2D( + ): DubinsPose2D = Pose2D( vector, when (direction) { Direction.LEFT -> theta(theta - PI / 2) @@ -81,16 +89,20 @@ public data class CircleTrajectory( } ) - val s1 = StraightTrajectory(center, start) - val s2 = StraightTrajectory(center, end) - val pose1 = calculatePose(start, s1.theta, direction) - val pose2 = calculatePose(end, s2.theta, direction) - return CircleTrajectory(Circle2D(center, s1.length), pose1, pose2) + val s1 = StraightTrajectory2D(center, start) + val s2 = StraightTrajectory2D(center, end) + val pose1 = calculatePose(start, s1.bearing, direction) + val pose2 = calculatePose(end, s2.bearing, direction) + val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2) + if(trajectory.direction != direction){ + error("Trajectory direction mismatch") + } + return trajectory } } } -public open class CompositeTrajectory(public val segments: Collection) : Trajectory { +public open class CompositeTrajectory2D(public val segments: Collection) : Trajectory2D { 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 deleted file mode 100644 index 3782a9a32..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt +++ /dev/null @@ -1,14 +0,0 @@ -/* - * Copyright 2018-2022 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/route.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt index 2e8e43be9..56350835d 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt @@ -12,5 +12,5 @@ public fun interface MaxCurvature { public fun DubinsPath.Companion.shortest( start: PhaseVector2D, end: PhaseVector2D, - computer: MaxCurvature, -): DubinsPath = shortest(start, end, computer.compute(start)) + maxCurvature: MaxCurvature, +): DubinsPath = shortest(start, end, maxCurvature.compute(start)) 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 64513f6e2..4599f30f8 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 @@ -15,14 +15,14 @@ const val maxFloatDelta = 0.000001 fun Double.radiansToDegrees() = this * 180 / PI fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta -fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta) +fun DubinsPose2D.equalsFloat(other: DubinsPose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.equalFloat(other.bearing) -fun StraightTrajectory.inverse() = StraightTrajectory(end, start) -fun StraightTrajectory.shift(shift: Int, width: Double): StraightTrajectory = with(Euclidean2DSpace){ - val dX = width * sin(inverse().theta) - val dY = width * sin(theta) +fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start) +fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace){ + val dX = width * sin(inverse().bearing) + val dY = width * sin(bearing) - return StraightTrajectory( + return StraightTrajectory2D( vector(start.x - dX * shift, start.y - dY * shift), vector(end.x - dX * shift, end.y - dY * shift) ) 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 b545b7c94..8cea22b45 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 @@ -16,11 +16,11 @@ class DubinsTests { @Test fun dubinsTest() = with(Euclidean2DSpace){ - val straight = StraightTrajectory(vector(0.0, 0.0), vector(100.0, 100.0)) + val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0)) val lineP1 = straight.shift(1, 10.0).inverse() - val start = Pose2D(straight.end, straight.theta) - val end = Pose2D(lineP1.start, lineP1.theta) + val start = Pose2D(straight.end, straight.bearing) + val end = Pose2D(lineP1.start, lineP1.bearing) val radius = 2.0 val dubins = DubinsPath.all(start, end, radius) @@ -45,14 +45,14 @@ class DubinsTests { assertTrue(end.equalsFloat(path.c.end)) // Not working, theta double precision inaccuracy - if (path.b is CircleTrajectory) { - val b = path.b as CircleTrajectory + if (path.b is CircleTrajectory2D) { + val b = path.b as CircleTrajectory2D assertTrue(path.a.end.equalsFloat(b.start)) assertTrue(path.c.start.equalsFloat(b.end)) - } else if (path.b is StraightTrajectory) { - val b = path.b as StraightTrajectory - assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta))) - assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta))) + } else if (path.b is StraightTrajectory2D) { + val b = path.b as StraightTrajectory2D + assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.bearing))) + assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.bearing))) } } } 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 17277c35e..af1444ade 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 @@ -8,7 +8,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.circumference -import space.kscience.kmath.trajectory.CircleTrajectory +import space.kscience.kmath.trajectory.CircleTrajectory2D import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.test.Test import kotlin.test.assertEquals @@ -18,9 +18,9 @@ class ArcTests { @Test fun arcTest() = with(Euclidean2DSpace){ val circle = Circle2D(vector(0.0, 0.0), 2.0) - val arc = CircleTrajectory.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory.Direction.RIGHT) + val arc = CircleTrajectory2D.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory2D.Direction.RIGHT) assertEquals(circle.circumference / 4, arc.length, 1.0) - assertEquals(0.0, arc.start.theta.radiansToDegrees()) - assertEquals(90.0, arc.end.theta.radiansToDegrees()) + assertEquals(0.0, arc.start.bearing.radiansToDegrees()) + assertEquals(90.0, arc.end.bearing.radiansToDegrees()) } } 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 4b54d775c..03320a3df 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 @@ -6,7 +6,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.trajectory.StraightTrajectory +import space.kscience.kmath.trajectory.StraightTrajectory2D import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.math.pow import kotlin.math.sqrt @@ -17,21 +17,21 @@ class LineTests { @Test fun lineTest() = with(Euclidean2DSpace){ - val straight = StraightTrajectory(vector(0.0, 0.0), vector(100.0, 100.0)) + val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0)) assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length) - assertEquals(45.0, straight.theta.radiansToDegrees()) + assertEquals(45.0, straight.bearing.radiansToDegrees()) } @Test fun lineAngleTest() = with(Euclidean2DSpace){ //val zero = Vector2D(0.0, 0.0) - val north = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, 2.0)) - assertEquals(0.0, north.theta.radiansToDegrees()) - val east = StraightTrajectory(Euclidean2DSpace.zero, vector(2.0, 0.0)) - assertEquals(90.0, east.theta.radiansToDegrees()) - val south = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, -2.0)) - assertEquals(180.0, south.theta.radiansToDegrees()) - val west = StraightTrajectory(Euclidean2DSpace.zero, vector(-2.0, 0.0)) - assertEquals(270.0, west.theta.radiansToDegrees()) + val north = StraightTrajectory2D(zero, vector(0.0, 2.0)) + assertEquals(0.0, north.bearing.radiansToDegrees()) + val east = StraightTrajectory2D(zero, vector(2.0, 0.0)) + assertEquals(90.0, east.bearing.radiansToDegrees()) + val south = StraightTrajectory2D(zero, vector(0.0, -2.0)) + assertEquals(180.0, south.bearing.radiansToDegrees()) + val west = StraightTrajectory2D(zero, vector(-2.0, 0.0)) + assertEquals(270.0, west.bearing.radiansToDegrees()) } }