forked from kscience/kmath
Hieraechy for trajectory types
This commit is contained in:
parent
f809e40791
commit
109e050f03
@ -9,7 +9,7 @@ import kotlin.jvm.JvmInline
|
|||||||
import kotlin.reflect.KClass
|
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<F : Any> {
|
public interface Featured<F : Any> {
|
||||||
public fun <T : F> getFeature(type: FeatureKey<T>): T?
|
public fun <T : F> getFeature(type: FeatureKey<T>): T?
|
||||||
|
@ -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 <T, R> List<T>.zipWithNextCircular(transform: (a: T, b: T) -> R): List<R> {
|
||||||
|
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 <T> List<T>.zipWithNextCircular(): List<Pair<T,T>> = zipWithNextCircular { l, r -> l to r }
|
@ -7,8 +7,7 @@ package space.kscience.kmath.trajectory
|
|||||||
|
|
||||||
import space.kscience.kmath.geometry.*
|
import space.kscience.kmath.geometry.*
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
import space.kscience.kmath.trajectory.Trajectory2D.Type
|
import space.kscience.kmath.trajectory.Trajectory2D.*
|
||||||
import space.kscience.kmath.trajectory.Trajectory2D.Type.*
|
|
||||||
import kotlin.math.acos
|
import kotlin.math.acos
|
||||||
|
|
||||||
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
||||||
@ -21,7 +20,7 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circ
|
|||||||
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
||||||
}
|
}
|
||||||
|
|
||||||
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): StraightTrajectory2D =
|
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): StraightTrajectory2D =
|
||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
val centers = StraightTrajectory2D(from.center, to.center)
|
val centers = StraightTrajectory2D(from.center, to.center)
|
||||||
val p1 = when (direction) {
|
val p1 = when (direction) {
|
||||||
@ -34,8 +33,6 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh
|
|||||||
from.center.x + from.radius * cos(centers.bearing),
|
from.center.x + from.radius * cos(centers.bearing),
|
||||||
from.center.y - from.radius * sin(centers.bearing)
|
from.center.y - from.radius * sin(centers.bearing)
|
||||||
)
|
)
|
||||||
|
|
||||||
else -> error("S trajectory type not allowed")
|
|
||||||
}
|
}
|
||||||
return StraightTrajectory2D(
|
return StraightTrajectory2D(
|
||||||
p1,
|
p1,
|
||||||
@ -47,7 +44,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh
|
|||||||
private fun innerTangent(
|
private fun innerTangent(
|
||||||
from: Circle2D,
|
from: Circle2D,
|
||||||
to: Circle2D,
|
to: Circle2D,
|
||||||
direction: Type,
|
direction: Direction,
|
||||||
): StraightTrajectory2D? =
|
): StraightTrajectory2D? =
|
||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
val centers = StraightTrajectory2D(from.center, to.center)
|
val centers = StraightTrajectory2D(from.center, to.center)
|
||||||
@ -55,7 +52,6 @@ private fun innerTangent(
|
|||||||
val angle = when (direction) {
|
val angle = when (direction) {
|
||||||
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
|
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
|
||||||
R -> 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()
|
}.normalized()
|
||||||
|
|
||||||
val dX = from.radius * sin(angle)
|
val dX = from.radius * sin(angle)
|
||||||
@ -70,13 +66,13 @@ private fun innerTangent(
|
|||||||
public object DubinsPath {
|
public object DubinsPath {
|
||||||
|
|
||||||
public data class Type(
|
public data class Type(
|
||||||
public val first: Trajectory2D.Type,
|
public val first: Direction,
|
||||||
public val second: Trajectory2D.Type,
|
public val second: Trajectory2D.Type,
|
||||||
public val third: Trajectory2D.Type,
|
public val third: Direction,
|
||||||
) {
|
) {
|
||||||
public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
|
public fun toList(): List<Trajectory2D.Type> = 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 companion object {
|
||||||
public val RLR: Type = Type(R, L, R)
|
public val RLR: Type = Type(R, L, R)
|
||||||
@ -98,7 +94,7 @@ public object DubinsPath {
|
|||||||
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
||||||
return Type(
|
return Type(
|
||||||
a.direction,
|
a.direction,
|
||||||
if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S,
|
if (b is CircleTrajectory2D) b.direction else S,
|
||||||
c.direction
|
c.direction
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
@ -137,9 +133,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -154,9 +150,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -181,9 +177,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -198,9 +194,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -211,8 +207,8 @@ public object DubinsPath {
|
|||||||
val c1 = start.getRightCircle(turningRadius)
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
val c2 = end.getRightCircle(turningRadius)
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
val s = outerTangent(c1, c2, L)
|
val s = outerTangent(c1, c2, L)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -220,8 +216,8 @@ public object DubinsPath {
|
|||||||
val c1 = start.getLeftCircle(turningRadius)
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
val c2 = end.getLeftCircle(turningRadius)
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
val s = outerTangent(c1, c2, R)
|
val s = outerTangent(c1, c2, R)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -231,8 +227,8 @@ public object DubinsPath {
|
|||||||
val s = innerTangent(c1, c2, R)
|
val s = innerTangent(c1, c2, R)
|
||||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
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 a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -242,8 +238,8 @@ public object DubinsPath {
|
|||||||
val s = innerTangent(c1, c2, L)
|
val s = innerTangent(c1, c2, L)
|
||||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
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 a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -17,10 +17,21 @@ import kotlin.math.atan2
|
|||||||
public sealed interface Trajectory2D {
|
public sealed interface Trajectory2D {
|
||||||
public val length: Double
|
public val length: Double
|
||||||
|
|
||||||
public enum class Type {
|
|
||||||
R,
|
public sealed interface Type
|
||||||
S,
|
|
||||||
L
|
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
|
* Arc length in radians
|
||||||
*/
|
*/
|
||||||
val arcLength: Angle
|
val arcLength: Angle
|
||||||
get() = if (direction == Trajectory2D.Type.L) {
|
get() = if (direction == Trajectory2D.L) {
|
||||||
start.bearing - end.bearing
|
start.bearing - end.bearing
|
||||||
} else {
|
} else {
|
||||||
end.bearing - start.bearing
|
end.bearing - start.bearing
|
||||||
@ -67,16 +78,16 @@ public data class CircleTrajectory2D(
|
|||||||
circle.radius * arcLength.radians
|
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.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) {
|
} 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 {
|
} else {
|
||||||
if (start.bearing == Angle.zero) {
|
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 {
|
} 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,
|
center: DoubleVector2D,
|
||||||
start: DoubleVector2D,
|
start: DoubleVector2D,
|
||||||
end: DoubleVector2D,
|
end: DoubleVector2D,
|
||||||
direction: Trajectory2D.Type,
|
direction: Trajectory2D.Direction,
|
||||||
): CircleTrajectory2D {
|
): CircleTrajectory2D {
|
||||||
fun calculatePose(
|
fun calculatePose(
|
||||||
vector: DoubleVector2D,
|
vector: DoubleVector2D,
|
||||||
theta: Angle,
|
theta: Angle,
|
||||||
direction: Trajectory2D.Type,
|
direction: Trajectory2D.Direction,
|
||||||
): DubinsPose2D = DubinsPose2D(
|
): DubinsPose2D = DubinsPose2D(
|
||||||
vector,
|
vector,
|
||||||
when (direction) {
|
when (direction) {
|
||||||
Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized()
|
Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
|
||||||
Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized()
|
Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
|
||||||
else -> error("S trajectory type is not allowed in circle constructor")
|
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
@ -38,7 +38,7 @@ class ObstacleTest {
|
|||||||
obstacles
|
obstacles
|
||||||
)
|
)
|
||||||
val length = outputTangents.minOf { it.length }
|
val length = outputTangents.minOf { it.length }
|
||||||
assertEquals(length, 27.2113183, 1e-6)
|
assertEquals(27.2113183, length, 1e-6)
|
||||||
}
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
@ -77,7 +77,7 @@ class ObstacleTest {
|
|||||||
obstacles
|
obstacles
|
||||||
)
|
)
|
||||||
val length = paths.minOf { it.length }
|
val length = paths.minOf { it.length }
|
||||||
assertEquals(length, 28.9678224, 1e-6)
|
assertEquals(28.9678224, length, 1e-6)
|
||||||
}
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
|
@ -23,7 +23,7 @@ class ArcTests {
|
|||||||
circle.center,
|
circle.center,
|
||||||
vector(-2.0, 0.0),
|
vector(-2.0, 0.0),
|
||||||
vector(0.0, 2.0),
|
vector(0.0, 2.0),
|
||||||
Trajectory2D.Type.R
|
Trajectory2D.R
|
||||||
)
|
)
|
||||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||||
assertEquals(0.0, arc.start.bearing.degrees)
|
assertEquals(0.0, arc.start.bearing.degrees)
|
||||||
|
Loading…
Reference in New Issue
Block a user