From 109e050f03abc0b585a024dd8abd743aa434e8d1 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 15:16:33 +0300 Subject: [PATCH] Hieraechy for trajectory types --- .../space/kscience/kmath/misc/Featured.kt | 2 +- .../space/kscience/kmath/misc/collections.kt | 22 +++++++ .../kscience/kmath/trajectory/DubinsPath.kt | 58 +++++++++---------- .../kscience/kmath/trajectory/Trajectory2D.kt | 40 ++++++++----- .../kscience/kmath/trajectory/ObstacleTest.kt | 4 +- .../kmath/trajectory/segments/ArcTests.kt | 2 +- 6 files changed, 78 insertions(+), 50 deletions(-) create mode 100644 kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt index a752a8339..bdda674dc 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt @@ -9,7 +9,7 @@ import kotlin.jvm.JvmInline import kotlin.reflect.KClass /** - * A entity that contains a set of features defined by their types + * An entity that contains a set of features defined by their types */ public interface Featured { public fun getFeature(type: FeatureKey): T? diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt new file mode 100644 index 000000000..90cc5bbfa --- /dev/null +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt @@ -0,0 +1,22 @@ +/* + * 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.misc + +/** + * The same as [zipWithNext], but includes link between last and first element + */ +public inline fun List.zipWithNextCircular(transform: (a: T, b: T) -> R): List { + if (isEmpty()) return emptyList() + return indices.map { i -> + if (i == size - 1) { + transform(last(), first()) + } else { + transform(get(i), get(i + 1)) + } + } +} + +public inline fun List.zipWithNextCircular(): List> = zipWithNextCircular { l, r -> l to r } \ No newline at end of file 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 272cf9e5b..87ea52a69 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 @@ -7,8 +7,7 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.* import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.trajectory.Trajectory2D.Type -import space.kscience.kmath.trajectory.Trajectory2D.Type.* +import space.kscience.kmath.trajectory.Trajectory2D.* import kotlin.math.acos internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first @@ -21,7 +20,7 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair error("S trajectory type not allowed") } return StraightTrajectory2D( p1, @@ -47,7 +44,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh private fun innerTangent( from: Circle2D, to: Circle2D, - direction: Type, + direction: Direction, ): StraightTrajectory2D? = with(Euclidean2DSpace) { val centers = StraightTrajectory2D(from.center, to.center) @@ -55,7 +52,6 @@ private fun innerTangent( val angle = when (direction) { L -> centers.bearing + acos(from.radius * 2 / centers.length).radians R -> centers.bearing - acos(from.radius * 2 / centers.length).radians - else -> error("S trajectory type not allowed") }.normalized() val dX = from.radius * sin(angle) @@ -70,13 +66,13 @@ private fun innerTangent( public object DubinsPath { public data class Type( - public val first: Trajectory2D.Type, + public val first: Direction, public val second: Trajectory2D.Type, - public val third: Trajectory2D.Type, + public val third: Direction, ) { public fun toList(): List = listOf(first, second, third) - override fun toString(): String = "${first.name}${second.name}${third.name}" + override fun toString(): String = "${first}${second}${third}" public companion object { public val RLR: Type = Type(R, L, R) @@ -98,7 +94,7 @@ public object DubinsPath { val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null return Type( a.direction, - if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S, + if (b is CircleTrajectory2D) b.direction else S, c.direction ) } @@ -137,9 +133,9 @@ public object DubinsPath { dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, R) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, L) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, R) CompositeTrajectory2D(a1, a2, a3) } @@ -154,9 +150,9 @@ public object DubinsPath { dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, R) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, L) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, R) CompositeTrajectory2D(a1, a2, a3) } @@ -181,9 +177,9 @@ public object DubinsPath { dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, L) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, R) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, L) CompositeTrajectory2D(a1, a2, a3) } @@ -198,9 +194,9 @@ public object DubinsPath { dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = vector(e.center.x + dX, e.center.y + dY) - val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, L) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, R) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, L) CompositeTrajectory2D(a1, a2, a3) } @@ -211,8 +207,8 @@ public object DubinsPath { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = outerTangent(c1, c2, L) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } @@ -220,8 +216,8 @@ public object DubinsPath { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = outerTangent(c1, c2, R) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -231,8 +227,8 @@ public object DubinsPath { val s = innerTangent(c1, c2, R) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -242,8 +238,8 @@ public object DubinsPath { val s = innerTangent(c1, c2, L) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt index 7bf17fc26..d6974b105 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -17,10 +17,21 @@ import kotlin.math.atan2 public sealed interface Trajectory2D { public val length: Double - public enum class Type { - R, - S, - L + + public sealed interface Type + + public sealed interface Direction: Type + + public object R : Direction { + override fun toString(): String = "R" + } + + public object S : Type { + override fun toString(): String = "L" + } + + public object L : Direction { + override fun toString(): String = "L" } } @@ -56,7 +67,7 @@ public data class CircleTrajectory2D( * Arc length in radians */ val arcLength: Angle - get() = if (direction == Trajectory2D.Type.L) { + get() = if (direction == Trajectory2D.L) { start.bearing - end.bearing } else { end.bearing - start.bearing @@ -67,16 +78,16 @@ public data class CircleTrajectory2D( circle.radius * arcLength.radians } - public val direction: Trajectory2D.Type by lazy { + public val direction: Trajectory2D.Direction by lazy { if (start.y < circle.center.y) { - if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L } else if (start.y > circle.center.y) { - if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L } else { if (start.bearing == Angle.zero) { - if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L } else { - if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L } } } @@ -86,18 +97,17 @@ public data class CircleTrajectory2D( center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, - direction: Trajectory2D.Type, + direction: Trajectory2D.Direction, ): CircleTrajectory2D { fun calculatePose( vector: DoubleVector2D, theta: Angle, - direction: Trajectory2D.Type, + direction: Trajectory2D.Direction, ): DubinsPose2D = DubinsPose2D( vector, when (direction) { - Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized() - Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized() - else -> error("S trajectory type is not allowed in circle constructor") + Trajectory2D.L -> (theta - Angle.piDiv2).normalized() + Trajectory2D.R -> (theta + Angle.piDiv2).normalized() } ) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt index 446e0a4d3..150b370d0 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt @@ -38,7 +38,7 @@ class ObstacleTest { obstacles ) val length = outputTangents.minOf { it.length } - assertEquals(length, 27.2113183, 1e-6) + assertEquals(27.2113183, length, 1e-6) } @Test @@ -77,7 +77,7 @@ class ObstacleTest { obstacles ) val length = paths.minOf { it.length } - assertEquals(length, 28.9678224, 1e-6) + assertEquals(28.9678224, length, 1e-6) } @Test 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 f149004bb..b3825b93b 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 @@ -23,7 +23,7 @@ class ArcTests { circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), - Trajectory2D.Type.R + Trajectory2D.R ) assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(0.0, arc.start.bearing.degrees)