Refactor trajectory

This commit is contained in:
Alexander Nozik 2023-02-03 19:33:22 +03:00
parent db30913542
commit 0366a69123
9 changed files with 217 additions and 118 deletions

View File

@ -5,13 +5,15 @@
package space.kscience.kmath.geometry package space.kscience.kmath.geometry
import kotlinx.serialization.Serializable
import kotlin.math.PI import kotlin.math.PI
/** /**
* A circle in 2D space * A circle in 2D space
*/ */
@Serializable
public data class Circle2D( public data class Circle2D(
public val center: DoubleVector2D, @Serializable(Euclidean2DSpace.VectorSerializer::class) public val center: DoubleVector2D,
public val radius: Double public val radius: Double
) )

View File

@ -5,6 +5,12 @@
package space.kscience.kmath.geometry 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.linear.Point
import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations import space.kscience.kmath.operations.ScaleOperations
@ -33,6 +39,7 @@ public operator fun <T> Vector2D<T>.component1(): T = x
public operator fun <T> Vector2D<T>.component2(): T = y public operator fun <T> Vector2D<T>.component2(): T = y
public typealias DoubleVector2D = Vector2D<Double> public typealias DoubleVector2D = Vector2D<Double>
public typealias Float64Vector2D = Vector2D<Double>
public val Vector2D<Double>.r: Double get() = Euclidean2DSpace.norm(this) public val Vector2D<Double>.r: Double get() = Euclidean2DSpace.norm(this)
@ -44,11 +51,25 @@ public object Euclidean2DSpace : GeometrySpace<DoubleVector2D>,
ScaleOperations<DoubleVector2D>, ScaleOperations<DoubleVector2D>,
Norm<DoubleVector2D, Double> { Norm<DoubleVector2D, Double> {
@Serializable
@SerialName("Float64Vector2D")
private data class Vector2DImpl( private data class Vector2DImpl(
override val x: Double, override val x: Double,
override val y: Double, override val y: Double,
) : DoubleVector2D ) : DoubleVector2D
public object VectorSerializer : KSerializer<DoubleVector2D> {
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()) public fun vector(x: Number, y: Number): DoubleVector2D = Vector2DImpl(x.toDouble(), y.toDouble())
override val zero: DoubleVector2D by lazy { vector(0.0, 0.0) } override val zero: DoubleVector2D by lazy { vector(0.0, 0.0) }

View File

@ -5,6 +5,12 @@
package space.kscience.kmath.geometry 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.linear.Point
import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations import space.kscience.kmath.operations.ScaleOperations
@ -45,17 +51,33 @@ public fun <T> Buffer<T>.asVector3D(): Vector3D<T> = object : Vector3D<T> {
} }
public typealias DoubleVector3D = Vector3D<Double> public typealias DoubleVector3D = Vector3D<Double>
public typealias Float64Vector3D = Vector3D<Double>
public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this) public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this)
public object Euclidean3DSpace : GeometrySpace<DoubleVector3D>, ScaleOperations<DoubleVector3D>, public object Euclidean3DSpace : GeometrySpace<DoubleVector3D>, ScaleOperations<DoubleVector3D>,
Norm<DoubleVector3D, Double> { Norm<DoubleVector3D, Double> {
@Serializable
@SerialName("Float64Vector3D")
private data class Vector3DImpl( private data class Vector3DImpl(
override val x: Double, override val x: Double,
override val y: Double, override val y: Double,
override val z: Double, override val z: Double,
) : DoubleVector3D ) : DoubleVector3D
public object VectorSerializer : KSerializer<DoubleVector3D> {
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 = public fun vector(x: Number, y: Number, z: Number): DoubleVector3D =
Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble()) Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble())

View File

@ -5,10 +5,13 @@
package space.kscience.kmath.geometry 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, * 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 * but its length does not affect line properties
*/ */
@Serializable
public data class Line<out V : Vector>(val base: V, val direction: V) public data class Line<out V : Vector>(val base: V, val direction: V)
public typealias Line2D = Line<DoubleVector2D> public typealias Line2D = Line<DoubleVector2D>
@ -17,4 +20,12 @@ public typealias Line3D = Line<DoubleVector3D>
/** /**
* A directed line segment between [begin] and [end] * A directed line segment between [begin] and [end]
*/ */
@Serializable
public data class LineSegment<out V : Vector>(val begin: V, val end: V) public data class LineSegment<out V : Vector>(val begin: V, val end: V)
public fun <V : Vector> LineSegment<V>.line(algebra: GeometrySpace<V>): Line<V> = with(algebra) {
Line(begin, end - begin)
}
public typealias LineSegment2D = LineSegment<DoubleVector2D>
public typealias LineSegment3D = LineSegment<DoubleVector3D>

View File

@ -57,7 +57,11 @@ internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTraj
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT) 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) { with(Euclidean2DSpace) {
val centers = StraightTrajectory2D(base.center, direction.center) val centers = StraightTrajectory2D(base.center, direction.center)
if (centers.length < base.radius * 2) return null if (centers.length < base.radius * 2) return null
@ -76,31 +80,36 @@ private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajec
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) 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 val type: TYPE = TYPE.valueOf( @Suppress("DuplicatedCode")
public object DubinsPath {
public enum class Type {
RLR, LRL, RSR, LSL, RSL, LSR
}
/**
* 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( arrayOf(
a.direction.name[0], a.direction.name[0],
if (b is CircleTrajectory2D) b.direction.name[0] else 'S', if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
c.direction.name[0] c.direction.name[0]
).toCharArray().concatToString() ).toCharArray().concatToString()
) )
public enum class TYPE {
RLR, LRL, RSR, LSL, RSL, LSR
} }
public companion object {
public fun all( public fun all(
start: DubinsPose2D, start: DubinsPose2D,
end: DubinsPose2D, end: DubinsPose2D,
turningRadius: Double, turningRadius: Double,
): List<DubinsPath> = listOfNotNull( ): List<CompositeTrajectory2D> = listOfNotNull(
rlr(start, end, turningRadius), rlr(start, end, turningRadius),
lrl(start, end, turningRadius), lrl(start, end, turningRadius),
rsr(start, end, turningRadius), rsr(start, end, turningRadius),
@ -109,10 +118,11 @@ public class DubinsPath(
lsr(start, end, turningRadius) lsr(start, end, turningRadius)
) )
public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath = public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
all(start, end, turningRadius).minBy { it.length } all(start, end, turningRadius).minBy { it.length }
public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) { public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
with(Euclidean2DSpace) {
val c1 = start.getRightCircle(turningRadius) val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius)
val centers = StraightTrajectory2D(c1.center, c2.center) 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 a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT) val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
DubinsPath(a1, a2, a3) CompositeTrajectory2D(a1, a2, a3)
} }
val secondVariant = run { val secondVariant = run {
@ -149,13 +159,14 @@ public class DubinsPath(
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.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 a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) 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 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 c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val centers = StraightTrajectory2D(c1.center, c2.center) 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 a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT) val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) 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 theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
var dX = turningRadius * sin(theta) var dX = turningRadius * sin(theta)
var dY = turningRadius * cos(theta) var dY = turningRadius * cos(theta)
@ -192,31 +203,31 @@ public class DubinsPath(
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.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 a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) 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 return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
} }
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath { public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getRightCircle(turningRadius) val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius)
val s = leftOuterTangent(c1, c2) val s = leftOuterTangent(c1, c2)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.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) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
return DubinsPath(a1, s, a3) return CompositeTrajectory2D(a1, s, a3)
} }
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath { public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getLeftCircle(turningRadius) val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val s = rightOuterTangent(c1, c2) val s = rightOuterTangent(c1, c2)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.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) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
return DubinsPath(a1, s, a3) return CompositeTrajectory2D(a1, s, a3)
} }
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? { public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getRightCircle(turningRadius) val c1 = start.getRightCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val s = rightInnerTangent(c1, c2) val s = rightInnerTangent(c1, c2)
@ -224,10 +235,10 @@ public class DubinsPath(
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.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.LEFT) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
return DubinsPath(a1, s, a3) return CompositeTrajectory2D(a1, s, a3)
} }
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? { public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getLeftCircle(turningRadius) val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius)
val s = leftInnerTangent(c1, c2) val s = leftInnerTangent(c1, c2)
@ -235,7 +246,17 @@ public class DubinsPath(
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.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.RIGHT) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
return DubinsPath(a1, s, a3) 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))

View File

@ -2,35 +2,62 @@
* Copyright 2018-2022 KMath contributors. * 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. * 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 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.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Vector import space.kscience.kmath.geometry.Vector
import kotlin.math.atan2 import kotlin.math.atan2
/** /**
* Combination of [Vector] and its view angle (clockwise from positive y-axis direction) * Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
*/ */
@Serializable(DubinsPose2DSerializer::class)
public interface DubinsPose2D : DoubleVector2D { public interface DubinsPose2D : DoubleVector2D {
public val coordinate: DoubleVector2D public val coordinates: DoubleVector2D
public val bearing: Double public val bearing: Double
} }
@Serializable
public class PhaseVector2D( public class PhaseVector2D(
override val coordinate: DoubleVector2D, override val coordinates: DoubleVector2D,
public val velocity: DoubleVector2D, public val velocity: DoubleVector2D,
) : DubinsPose2D, DoubleVector2D by coordinate { ) : DubinsPose2D, DoubleVector2D by coordinates {
override val bearing: Double get() = atan2(velocity.x, velocity.y) override val bearing: Double get() = atan2(velocity.x, velocity.y)
} }
@Serializable
@SerialName("DubinsPose2D")
private class DubinsPose2DImpl( private class DubinsPose2DImpl(
override val coordinate: DoubleVector2D, override val coordinates: DoubleVector2D,
override val bearing: Double, 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<DubinsPose2D>{
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) public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)

View File

@ -2,15 +2,19 @@
* Copyright 2018-2022 KMath contributors. * 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. * 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 package space.kscience.kmath.trajectory
import kotlinx.serialization.Serializable
import kotlinx.serialization.UseSerializers
import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.DoubleVector2D import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import kotlin.math.PI import kotlin.math.PI
import kotlin.math.atan2 import kotlin.math.atan2
@Serializable
public sealed interface Trajectory2D { public sealed interface Trajectory2D {
public val length: Double public val length: Double
} }
@ -18,6 +22,7 @@ public sealed interface Trajectory2D {
/** /**
* Straight path segment. The order of start and end defines the direction * Straight path segment. The order of start and end defines the direction
*/ */
@Serializable
public data class StraightTrajectory2D( public data class StraightTrajectory2D(
public val start: DoubleVector2D, public val start: DoubleVector2D,
public val end: DoubleVector2D, public val end: DoubleVector2D,
@ -30,6 +35,7 @@ public data class StraightTrajectory2D(
/** /**
* An arc segment * An arc segment
*/ */
@Serializable
public data class CircleTrajectory2D( public data class CircleTrajectory2D(
public val circle: Circle2D, public val circle: Circle2D,
public val start: DubinsPose2D, public val start: DubinsPose2D,
@ -102,7 +108,10 @@ public data class CircleTrajectory2D(
} }
} }
public open class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D { @Serializable
public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
override val length: Double get() = segments.sumOf { it.length } override val length: Double get() = segments.sumOf { it.length }
} }
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList())

View File

@ -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))

View File

@ -28,31 +28,33 @@ class DubinsTests {
println("Absolute distance: $absoluteDistance") println("Absolute distance: $absoluteDistance")
val expectedLengths = mapOf( val expectedLengths = mapOf(
DubinsPath.TYPE.RLR to 13.067681939031397, DubinsPath.Type.RLR to 13.067681939031397,
DubinsPath.TYPE.RSR to 12.28318530717957, DubinsPath.Type.RSR to 12.28318530717957,
DubinsPath.TYPE.LSL to 32.84955592153878, DubinsPath.Type.LSL to 32.84955592153878,
DubinsPath.TYPE.RSL to 23.37758938854081, DubinsPath.Type.RSL to 23.37758938854081,
DubinsPath.TYPE.LSR to 23.37758938854081 DubinsPath.Type.LSR to 23.37758938854081
) )
expectedLengths.forEach { 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") assertNotNull(path, "Path ${it.key} not found")
println("${it.key}: ${path.length}") println("${it.key}: ${path.length}")
assertTrue(it.value.equalFloat(path.length)) assertTrue(it.value.equalFloat(path.length))
assertTrue(start.equalsFloat(path.a.start)) val a = path.segments[0] as CircleTrajectory2D
assertTrue(end.equalsFloat(path.c.end)) 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 // Not working, theta double precision inaccuracy
if (path.b is CircleTrajectory2D) { if (b is CircleTrajectory2D) {
val b = path.b as CircleTrajectory2D assertTrue(a.end.equalsFloat(b.start))
assertTrue(path.a.end.equalsFloat(b.start)) assertTrue(c.start.equalsFloat(b.end))
assertTrue(path.c.start.equalsFloat(b.end)) } else if (b is StraightTrajectory2D) {
} else if (path.b is StraightTrajectory2D) { assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing)))
val b = path.b as StraightTrajectory2D assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
assertTrue(path.a.end.equalsFloat(DubinsPose2D(b.start, b.bearing)))
assertTrue(path.c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
} }
} }
} }