From 0366a69123fc45690d5b6983d83a3c90929d1084 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Fri, 3 Feb 2023 19:33:22 +0300 Subject: [PATCH] Refactor trajectory --- .../space/kscience/kmath/geometry/Circle2D.kt | 4 +- .../kmath/geometry/Euclidean2DSpace.kt | 21 +++ .../kmath/geometry/Euclidean3DSpace.kt | 22 +++ .../space/kscience/kmath/geometry/Line.kt | 11 ++ .../kscience/kmath/trajectory/DubinsPath.kt | 173 ++++++++++-------- .../kscience/kmath/trajectory/DubinsPose2D.kt | 41 ++++- .../kscience/kmath/trajectory/Trajectory2D.kt | 13 +- .../space/kscience/kmath/trajectory/route.kt | 16 -- .../kmath/trajectory/dubins/DubinsTests.kt | 34 ++-- 9 files changed, 217 insertions(+), 118 deletions(-) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt index 162130908..8beef6fee 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt @@ -5,13 +5,15 @@ package space.kscience.kmath.geometry +import kotlinx.serialization.Serializable import kotlin.math.PI /** * A circle in 2D space */ +@Serializable public data class Circle2D( - public val center: DoubleVector2D, + @Serializable(Euclidean2DSpace.VectorSerializer::class) public val center: DoubleVector2D, public val radius: Double ) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt index a8310503c..3df8dba7b 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt @@ -5,6 +5,12 @@ package space.kscience.kmath.geometry +import kotlinx.serialization.KSerializer +import kotlinx.serialization.SerialName +import kotlinx.serialization.Serializable +import kotlinx.serialization.descriptors.SerialDescriptor +import kotlinx.serialization.encoding.Decoder +import kotlinx.serialization.encoding.Encoder import space.kscience.kmath.linear.Point import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.ScaleOperations @@ -33,6 +39,7 @@ public operator fun Vector2D.component1(): T = x public operator fun Vector2D.component2(): T = y public typealias DoubleVector2D = Vector2D +public typealias Float64Vector2D = Vector2D public val Vector2D.r: Double get() = Euclidean2DSpace.norm(this) @@ -44,11 +51,25 @@ public object Euclidean2DSpace : GeometrySpace, ScaleOperations, Norm { + @Serializable + @SerialName("Float64Vector2D") private data class Vector2DImpl( override val x: Double, override val y: Double, ) : DoubleVector2D + public object VectorSerializer : KSerializer { + private val proxySerializer = Vector2DImpl.serializer() + override val descriptor: SerialDescriptor get() = proxySerializer.descriptor + + override fun deserialize(decoder: Decoder): DoubleVector2D = decoder.decodeSerializableValue(proxySerializer) + + override fun serialize(encoder: Encoder, value: DoubleVector2D) { + val vector = value as? Vector2DImpl ?: Vector2DImpl(value.x, value.y) + encoder.encodeSerializableValue(proxySerializer, vector) + } + } + public fun vector(x: Number, y: Number): DoubleVector2D = Vector2DImpl(x.toDouble(), y.toDouble()) override val zero: DoubleVector2D by lazy { vector(0.0, 0.0) } diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index d214e4edf..cc641a3f1 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -5,6 +5,12 @@ package space.kscience.kmath.geometry +import kotlinx.serialization.KSerializer +import kotlinx.serialization.SerialName +import kotlinx.serialization.Serializable +import kotlinx.serialization.descriptors.SerialDescriptor +import kotlinx.serialization.encoding.Decoder +import kotlinx.serialization.encoding.Encoder import space.kscience.kmath.linear.Point import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.ScaleOperations @@ -45,17 +51,33 @@ public fun Buffer.asVector3D(): Vector3D = object : Vector3D { } public typealias DoubleVector3D = Vector3D +public typealias Float64Vector3D = Vector3D public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this) public object Euclidean3DSpace : GeometrySpace, ScaleOperations, Norm { + + @Serializable + @SerialName("Float64Vector3D") private data class Vector3DImpl( override val x: Double, override val y: Double, override val z: Double, ) : DoubleVector3D + public object VectorSerializer : KSerializer { + private val proxySerializer = Vector3DImpl.serializer() + override val descriptor: SerialDescriptor get() = proxySerializer.descriptor + + override fun deserialize(decoder: Decoder): DoubleVector3D = decoder.decodeSerializableValue(proxySerializer) + + override fun serialize(encoder: Encoder, value: DoubleVector3D) { + val vector = value as? Vector3DImpl ?: Vector3DImpl(value.x, value.y, value.z) + encoder.encodeSerializableValue(proxySerializer, vector) + } + } + public fun vector(x: Number, y: Number, z: Number): DoubleVector3D = Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble()) 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 bf14d6d4a..ab322ddca 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 @@ -5,10 +5,13 @@ package space.kscience.kmath.geometry +import kotlinx.serialization.Serializable + /** * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized, * but its length does not affect line properties */ +@Serializable public data class Line(val base: V, val direction: V) public typealias Line2D = Line @@ -17,4 +20,12 @@ public typealias Line3D = Line /** * A directed line segment between [begin] and [end] */ +@Serializable public data class LineSegment(val begin: V, val end: V) + +public fun LineSegment.line(algebra: GeometrySpace): Line = with(algebra) { + Line(begin, end - begin) +} + +public typealias LineSegment2D = LineSegment +public typealias LineSegment3D = LineSegment 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 1ba9936ee..6bbf3e55b 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 @@ -57,7 +57,11 @@ internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTraj internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT) -private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D? = +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 @@ -76,43 +80,49 @@ private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajec internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) + @Suppress("DuplicatedCode") -public class DubinsPath( - public val a: CircleTrajectory2D, - public val b: Trajectory2D, - public val c: CircleTrajectory2D, -) : CompositeTrajectory2D(listOf(a, b, c)) { +public object DubinsPath { - public val type: TYPE = TYPE.valueOf( - arrayOf( - a.direction.name[0], - if (b is CircleTrajectory2D) b.direction.name[0] else 'S', - c.direction.name[0] - ).toCharArray().concatToString() - ) - - public enum class TYPE { + public enum class Type { RLR, LRL, RSR, LSL, RSL, LSR } - public companion object { - public fun all( - start: DubinsPose2D, - end: DubinsPose2D, - 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) + /** + * Return Dubins trajectory type or null if trajectory is not a Dubins path + */ + public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type?{ + if(trajectory2D.segments.size != 3) return null + val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null + val b = trajectory2D.segments[1] + val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null + return Type.valueOf( + arrayOf( + a.direction.name[0], + if (b is CircleTrajectory2D) b.direction.name[0] else 'S', + c.direction.name[0] + ).toCharArray().concatToString() ) + } - public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath = - all(start, end, turningRadius).minBy { it.length } + public fun all( + start: DubinsPose2D, + end: DubinsPose2D, + 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 rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { + public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D = + all(start, end, turningRadius).minBy { it.length } + + public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? = + with(Euclidean2DSpace) { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val centers = StraightTrajectory2D(c1.center, c2.center) @@ -132,7 +142,7 @@ public class DubinsPath( 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) + CompositeTrajectory2D(a1, a2, a3) } val secondVariant = run { @@ -149,13 +159,14 @@ public class DubinsPath( 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) + CompositeTrajectory2D(a1, a2, a3) } return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant } - public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { + public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? = + with(Euclidean2DSpace) { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val centers = StraightTrajectory2D(c1.center, c2.center) @@ -175,10 +186,10 @@ public class DubinsPath( 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) + CompositeTrajectory2D(a1, a2, a3) } - val secondVariant = run{ + val secondVariant = run { var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) @@ -192,50 +203,60 @@ public class DubinsPath( 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) + CompositeTrajectory2D(a1, a2, a3) } return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant } - 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 = 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: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - val s = rightOuterTangent(c1, c2) - 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: 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 = 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: 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 = 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) - } + public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D { + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val s = leftOuterTangent(c1, c2) + 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 CompositeTrajectory2D(a1, s, a3) } -} \ No newline at end of file + + public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D { + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val s = rightOuterTangent(c1, c2) + 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 CompositeTrajectory2D(a1, s, a3) + } + + public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? { + 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 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT) + return CompositeTrajectory2D(a1, s, a3) + } + + public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? { + 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 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT) + return CompositeTrajectory2D(a1, s, a3) + } +} + +public fun interface MaxCurvature { + public fun compute(startPoint: PhaseVector2D): Double +} + +public fun DubinsPath.shortest( + start: PhaseVector2D, + end: PhaseVector2D, + maxCurvature: MaxCurvature, +): CompositeTrajectory2D = shortest(start, end, maxCurvature.compute(start)) + 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 index 5aa8c1455..ff2198bbb 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt @@ -2,35 +2,62 @@ * 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. */ - +@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class) package space.kscience.kmath.trajectory +import kotlinx.serialization.KSerializer +import kotlinx.serialization.SerialName +import kotlinx.serialization.Serializable +import kotlinx.serialization.UseSerializers +import kotlinx.serialization.descriptors.SerialDescriptor +import kotlinx.serialization.encoding.Decoder +import kotlinx.serialization.encoding.Encoder import space.kscience.kmath.geometry.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Vector import kotlin.math.atan2 /** * Combination of [Vector] and its view angle (clockwise from positive y-axis direction) */ +@Serializable(DubinsPose2DSerializer::class) public interface DubinsPose2D : DoubleVector2D { - public val coordinate: DoubleVector2D + public val coordinates: DoubleVector2D public val bearing: Double } +@Serializable public class PhaseVector2D( - override val coordinate: DoubleVector2D, + override val coordinates: DoubleVector2D, public val velocity: DoubleVector2D, -) : DubinsPose2D, DoubleVector2D by coordinate { +) : DubinsPose2D, DoubleVector2D by coordinates { override val bearing: Double get() = atan2(velocity.x, velocity.y) } +@Serializable +@SerialName("DubinsPose2D") private class DubinsPose2DImpl( - override val coordinate: DoubleVector2D, + override val coordinates: DoubleVector2D, override val bearing: Double, -) : DubinsPose2D, DoubleVector2D by coordinate{ +) : DubinsPose2D, DoubleVector2D by coordinates{ - override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)" + override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)" } +public object DubinsPose2DSerializer: KSerializer{ + private val proxySerializer = DubinsPose2DImpl.serializer() + + override val descriptor: SerialDescriptor + get() = proxySerializer.descriptor + + override fun deserialize(decoder: Decoder): DubinsPose2D { + return decoder.decodeSerializableValue(proxySerializer) + } + + override fun serialize(encoder: Encoder, value: DubinsPose2D) { + val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing) + encoder.encodeSerializableValue(proxySerializer, pose) + } +} public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta) \ No newline at end of file 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 ffa23a537..120bedf90 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 @@ -2,15 +2,19 @@ * 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. */ - +@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class) package space.kscience.kmath.trajectory +import kotlinx.serialization.Serializable +import kotlinx.serialization.UseSerializers import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import kotlin.math.PI import kotlin.math.atan2 +@Serializable public sealed interface Trajectory2D { public val length: Double } @@ -18,6 +22,7 @@ public sealed interface Trajectory2D { /** * Straight path segment. The order of start and end defines the direction */ +@Serializable public data class StraightTrajectory2D( public val start: DoubleVector2D, public val end: DoubleVector2D, @@ -30,6 +35,7 @@ public data class StraightTrajectory2D( /** * An arc segment */ +@Serializable public data class CircleTrajectory2D( public val circle: Circle2D, public val start: DubinsPose2D, @@ -102,7 +108,10 @@ public data class CircleTrajectory2D( } } -public open class CompositeTrajectory2D(public val segments: List) : Trajectory2D { +@Serializable +public class CompositeTrajectory2D(public val segments: List) : Trajectory2D { override val length: Double get() = segments.sumOf { it.length } } +public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList()) + 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 deleted file mode 100644 index 56350835d..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt +++ /dev/null @@ -1,16 +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 MaxCurvature { - public fun compute(startPoint: PhaseVector2D): Double -} - -public fun DubinsPath.Companion.shortest( - start: PhaseVector2D, - end: PhaseVector2D, - maxCurvature: MaxCurvature, -): DubinsPath = shortest(start, end, maxCurvature.compute(start)) 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 5bbf8c294..0e14ae736 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 @@ -28,31 +28,33 @@ class DubinsTests { println("Absolute distance: $absoluteDistance") val expectedLengths = mapOf( - DubinsPath.TYPE.RLR to 13.067681939031397, - DubinsPath.TYPE.RSR to 12.28318530717957, - DubinsPath.TYPE.LSL to 32.84955592153878, - DubinsPath.TYPE.RSL to 23.37758938854081, - DubinsPath.TYPE.LSR to 23.37758938854081 + DubinsPath.Type.RLR to 13.067681939031397, + DubinsPath.Type.RSR to 12.28318530717957, + DubinsPath.Type.LSL to 32.84955592153878, + DubinsPath.Type.RSL to 23.37758938854081, + DubinsPath.Type.LSR to 23.37758938854081 ) expectedLengths.forEach { - val path = dubins.find { p -> p.type === it.key } + val path = dubins.find { p -> DubinsPath.trajectoryTypeOf(p) === it.key } assertNotNull(path, "Path ${it.key} not found") println("${it.key}: ${path.length}") assertTrue(it.value.equalFloat(path.length)) - assertTrue(start.equalsFloat(path.a.start)) - assertTrue(end.equalsFloat(path.c.end)) + val a = path.segments[0] as CircleTrajectory2D + val b = path.segments[1] + val c = path.segments[2] as CircleTrajectory2D + + assertTrue(start.equalsFloat(a.start)) + assertTrue(end.equalsFloat(c.end)) // Not working, theta double precision inaccuracy - 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 StraightTrajectory2D) { - val b = path.b as StraightTrajectory2D - assertTrue(path.a.end.equalsFloat(DubinsPose2D(b.start, b.bearing))) - assertTrue(path.c.start.equalsFloat(DubinsPose2D(b.end, b.bearing))) + if (b is CircleTrajectory2D) { + assertTrue(a.end.equalsFloat(b.start)) + assertTrue(c.start.equalsFloat(b.end)) + } else if (b is StraightTrajectory2D) { + assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing))) + assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing))) } } }