teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Hieraechy for trajectory types

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-04 15:16:33 +03:00
parent f809e40791
commit 109e050f03
6 changed files with 78 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt
View File

@ -9,7 +9,7 @@ import kotlin.jvm.JvmInline
import kotlin.reflect.KClass
/**
* A entity that contains a set of features defined by their types
* An entity that contains a set of features defined by their types
*/
public interface Featured<F : Any> {
public fun <T : F> getFeature(type: FeatureKey<T>): T?

22
kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt Normal file
View File

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

58
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
View File

@ -7,8 +7,7 @@ package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.trajectory.Trajectory2D.Type
import space.kscience.kmath.trajectory.Trajectory2D.Type.*
import space.kscience.kmath.trajectory.Trajectory2D.*
import kotlin.math.acos
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
@ -21,7 +20,7 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circ
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) {
val centers = StraightTrajectory2D(from.center, to.center)
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.y - from.radius * sin(centers.bearing)
)
else -> error("S trajectory type not allowed")
}
return StraightTrajectory2D(
p1,
@ -47,7 +44,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh
private fun innerTangent(
from: Circle2D,
to: Circle2D,
direction: Type,
direction: Direction,
): StraightTrajectory2D? =
with(Euclidean2DSpace) {
val centers = StraightTrajectory2D(from.center, to.center)
@ -55,7 +52,6 @@ private fun innerTangent(
val angle = when (direction) {
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
R -> centers.bearing - acos(from.radius * 2 / centers.length).radians
else -> error("S trajectory type not allowed")
}.normalized()
val dX = from.radius * sin(angle)
@ -70,13 +66,13 @@ private fun innerTangent(
public object DubinsPath {
public data class Type(
public val first: Trajectory2D.Type,
public val first: Direction,
public val second: Trajectory2D.Type,
public val third: Trajectory2D.Type,
public val third: Direction,
) {
public fun toList(): List<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 val RLR: Type = Type(R, L, R)
@ -98,7 +94,7 @@ public object DubinsPath {
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
return Type(
a.direction,
if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S,
if (b is CircleTrajectory2D) b.direction else S,
c.direction
)
}
@ -137,9 +133,9 @@ public object DubinsPath {
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
@ -154,9 +150,9 @@ public object DubinsPath {
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
@ -181,9 +177,9 @@ public object DubinsPath {
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
@ -198,9 +194,9 @@ public object DubinsPath {
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
@ -211,8 +207,8 @@ public object DubinsPath {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = outerTangent(c1, c2, L)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
@ -220,8 +216,8 @@ public object DubinsPath {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = outerTangent(c1, c2, R)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
@ -231,8 +227,8 @@ public object DubinsPath {
val s = innerTangent(c1, c2, R)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
@ -242,8 +238,8 @@ public object DubinsPath {
val s = innerTangent(c1, c2, L)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
}

40
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt
View File

@ -17,10 +17,21 @@ import kotlin.math.atan2
public sealed interface Trajectory2D {
public val length: Double
public enum class Type {
R,
S,
L
public sealed interface Type
public sealed interface Direction: Type
public object R : Direction {
override fun toString(): String = "R"
}
public object S : Type {
override fun toString(): String = "L"
}
public object L : Direction {
override fun toString(): String = "L"
}
}
@ -56,7 +67,7 @@ public data class CircleTrajectory2D(
* Arc length in radians
*/
val arcLength: Angle
get() = if (direction == Trajectory2D.Type.L) {
get() = if (direction == Trajectory2D.L) {
start.bearing - end.bearing
} else {
end.bearing - start.bearing
@ -67,16 +78,16 @@ public data class CircleTrajectory2D(
circle.radius * arcLength.radians
}
public val direction: Trajectory2D.Type by lazy {
public val direction: Trajectory2D.Direction by lazy {
if (start.y < circle.center.y) {
if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
} else if (start.y > circle.center.y) {
if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
} else {
if (start.bearing == Angle.zero) {
if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
} else {
if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
}
}
}
@ -86,18 +97,17 @@ public data class CircleTrajectory2D(
center: DoubleVector2D,
start: DoubleVector2D,
end: DoubleVector2D,
direction: Trajectory2D.Type,
direction: Trajectory2D.Direction,
): CircleTrajectory2D {
fun calculatePose(
vector: DoubleVector2D,
theta: Angle,
direction: Trajectory2D.Type,
direction: Trajectory2D.Direction,
): DubinsPose2D = DubinsPose2D(
vector,
when (direction) {
Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized()
Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized()
else -> error("S trajectory type is not allowed in circle constructor")
Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
}
)

4
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt
View File

@ -38,7 +38,7 @@ class ObstacleTest {
obstacles
)
val length = outputTangents.minOf { it.length }
assertEquals(length, 27.2113183, 1e-6)
assertEquals(27.2113183, length, 1e-6)
}
@Test
@ -77,7 +77,7 @@ class ObstacleTest {
obstacles
)
val length = paths.minOf { it.length }
assertEquals(length, 28.9678224, 1e-6)
assertEquals(28.9678224, length, 1e-6)
}
@Test

2
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
View File

@ -23,7 +23,7 @@ class ArcTests {
circle.center,
vector(-2.0, 0.0),
vector(0.0, 2.0),
Trajectory2D.Type.R
Trajectory2D.R
)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.start.bearing.degrees)