Refactor trajectory
This commit is contained in:
parent
db30913542
commit
0366a69123
@ -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
|
||||||
)
|
)
|
||||||
|
|
||||||
|
@ -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) }
|
||||||
|
@ -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())
|
||||||
|
|
||||||
|
@ -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>
|
||||||
|
@ -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))
|
||||||
|
|
||||||
|
@ -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)
|
@ -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())
|
||||||
|
|
||||||
|
@ -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))
|
|
@ -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)))
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user