kscience/maps-kt
8
0
Fork 0
Code Issues Pull Requests 3 Packages Projects Releases 1 Wiki Activity

[WIP] refactor CircleTrajectory2D logic to be more correct

Browse Source
This commit is contained in:
Alexander Nozik 2023-05-01 09:14:26 +03:00
parent 8bc1987acf
commit 614ca8d6f3
10 changed files with 208 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt
View File

@ -1,6 +1,8 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.operations.DoubleField.pow
import space.kscience.trajectory.Pose2D
import space.kscience.trajectory.Trajectory2D
import kotlin.math.sign
public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
@ -47,3 +49,17 @@ public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSe
}
}
/**
* Compute tangent pose to a circle
*
* @param bearing is counted the same way as in [Pose2D], from positive y clockwise
*/
public fun Circle2D.tangent(bearing: Angle, direction: Trajectory2D.Direction): Pose2D = with(Euclidean2DSpace) {
val coordinates: Vector2D<Double> = vector(center.x + radius * sin(bearing), center.y + radius * cos(bearing))
val tangentAngle = when (direction) {
Trajectory2D.R -> bearing + Angle.piDiv2
Trajectory2D.L -> bearing - Angle.piDiv2
}.normalized()
Pose2D(coordinates, tangentAngle)
}

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

@ -10,11 +10,11 @@ import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.trajectory.Trajectory2D.*
import kotlin.math.acos
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
val dX = radius * cos(bearing)
val dY = radius * sin(bearing)
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
@ -102,8 +102,8 @@ public object DubinsPath {
}
public fun all(
start: DubinsPose2D,
end: DubinsPose2D,
start: Pose2D,
end: Pose2D,
turningRadius: Double,
): List<CompositeTrajectory2D> = listOfNotNull(
rlr(start, end, turningRadius),
@ -114,10 +114,10 @@ public object DubinsPath {
lsr(start, end, turningRadius)
)
public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D =
all(start, end, turningRadius).minBy { it.length }
public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? =
with(Euclidean2DSpace) {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
@ -135,9 +135,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, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
val a1 = CircleTrajectory2D(c1.center, start, p1, R)
val a2 = CircleTrajectory2D(e.center, p1, p2, L)
val a3 = CircleTrajectory2D(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
@ -152,16 +152,16 @@ 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, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
val a1 = CircleTrajectory2D(c1.center, start, p1, R)
val a2 = CircleTrajectory2D(e.center, p1, p2, L)
val a3 = CircleTrajectory2D(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? =
with(Euclidean2DSpace) {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
@ -179,9 +179,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, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
val a1 = CircleTrajectory2D(c1.center, start, p1, L)
val a2 = CircleTrajectory2D(e.center, p1, p2, R)
val a3 = CircleTrajectory2D(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
@ -196,52 +196,52 @@ 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, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
val a1 = CircleTrajectory2D(c1.center, start, p1, L)
val a2 = CircleTrajectory2D(e.center, p1, p2, R)
val a3 = CircleTrajectory2D(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = outerTangent(c1, c2, L)
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
val a1 = CircleTrajectory2D(c1.center, start, s.begin, R)
val a3 = CircleTrajectory2D(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = outerTangent(c1, c2, R)
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
val a1 = CircleTrajectory2D(c1.center, start, s.begin, L)
val a3 = CircleTrajectory2D(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
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.begin, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
val a1 = CircleTrajectory2D(c1.center, start, s.begin, R)
val a3 = CircleTrajectory2D(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
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.begin, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
val a1 = CircleTrajectory2D(c1.center, start, s.begin, L)
val a3 = CircleTrajectory2D(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
}

14
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt
View File

@ -48,14 +48,14 @@ private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
if (circles.size == 1) {
// a circumvention consisting of a single circle, starting on top
val circle = circles.first()
val top = vector(circle.center.x + circle.radius, circle.center.y)
val startEnd = DubinsPose2D(
top,
Angle.piDiv2
)
// val circle = circles.first()
// val top = vector(circle.center.x + circle.radius, circle.center.y)
// val start = DubinsPose2D(
// top,
// Angle.piDiv2
// )
return@lazy CompositeTrajectory2D(
CircleTrajectory2D(circle, startEnd, startEnd)
CircleTrajectory2D(circles.first(), Angle.zero, Angle.zero)
)
}

25
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt
View File

@ -158,14 +158,15 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
//cutting first and last arcs to accommodate connection points
val first = circumvention.first() as CircleTrajectory2D
val last = circumvention.last() as CircleTrajectory2D
//arc between end of the nangent and end of previous arc (begin of the the next one)
circumvention[0] = CircleTrajectory2D(
first.circle,
tangent1.tangentTrajectory.endPose,
first.end
first.endPose,
)
circumvention[circumvention.lastIndex] = CircleTrajectory2D(
last.circle,
last.begin,
last.beginPose,
tangent2.tangentTrajectory.beginPose
)
return CompositeTrajectory2D(circumvention)
@ -271,10 +272,10 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
private fun constructTangentCircles(
pose: DubinsPose2D,
pose: Pose2D,
r: Double,
): LR<Circle2D> = with(Euclidean2DSpace) {
val direction = DubinsPose2D.bearingToVector(pose.bearing)
val direction = Pose2D.bearingToVector(pose.bearing)
//TODO optimize to use bearing
val center1 = pose + normalVectors(direction, r).first
val center2 = pose + normalVectors(direction, r).second
@ -293,8 +294,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
public fun avoidObstacles(
start: DubinsPose2D,
finish: DubinsPose2D,
start: Pose2D,
finish: Pose2D,
startingRadius: Double,
obstacleList: List<Obstacle>,
finalRadius: Double = startingRadius,
@ -328,15 +329,15 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
public fun avoidObstacles(
start: DubinsPose2D,
finish: DubinsPose2D,
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
vararg obstacles: Obstacle,
): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
public fun avoidPolygons(
start: DubinsPose2D,
finish: DubinsPose2D,
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
vararg polygons: Polygon<Double>,
): List<Trajectory2D> {
@ -348,8 +349,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
public fun avoidPolygons(
start: DubinsPose2D,
finish: DubinsPose2D,
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
polygons: Collection<Polygon<Double>>,
): List<Trajectory2D> {

55
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt → trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Pose2D.kt
View File

@ -19,15 +19,15 @@ import kotlin.math.atan2
/**
* Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
*/
@Serializable(DubinsPose2DSerializer::class)
public interface DubinsPose2D : DoubleVector2D {
@Serializable(Pose2DSerializer::class)
public interface Pose2D : DoubleVector2D {
public val coordinates: DoubleVector2D
public val bearing: Angle
/**
* Reverse the direction of this pose to the opposite, keeping other parameters the same
*/
public fun reversed(): DubinsPose2D
public fun reversed(): Pose2D
public companion object {
public fun bearingToVector(bearing: Angle): Vector2D<Double> =
@ -45,45 +45,62 @@ public interface DubinsPose2D : DoubleVector2D {
public class PhaseVector2D(
override val coordinates: DoubleVector2D,
public val velocity: DoubleVector2D,
) : DubinsPose2D, DoubleVector2D by coordinates {
) : Pose2D, DoubleVector2D by coordinates {
override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
override fun reversed(): DubinsPose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
override fun reversed(): Pose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
}
@Serializable
@SerialName("DubinsPose2D")
private class DubinsPose2DImpl(
private class Pose2DImpl(
override val coordinates: DoubleVector2D,
override val bearing: Angle,
) : DubinsPose2D, DoubleVector2D by coordinates {
) : Pose2D, DoubleVector2D by coordinates {
override fun reversed(): Pose2D = Pose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
override fun reversed(): DubinsPose2D = DubinsPose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
override fun equals(other: Any?): Boolean {
if (this === other) return true
if (other == null || this::class != other::class) return false
other as Pose2DImpl
if (coordinates != other.coordinates) return false
return bearing == other.bearing
}
override fun hashCode(): Int {
var result = coordinates.hashCode()
result = 31 * result + bearing.hashCode()
return result
}
}
public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
private val proxySerializer = DubinsPose2DImpl.serializer()
public object Pose2DSerializer : KSerializer<Pose2D> {
private val proxySerializer = Pose2DImpl.serializer()
override val descriptor: SerialDescriptor
get() = proxySerializer.descriptor
override fun deserialize(decoder: Decoder): DubinsPose2D {
override fun deserialize(decoder: Decoder): Pose2D {
return decoder.decodeSerializableValue(proxySerializer)
}
override fun serialize(encoder: Encoder, value: DubinsPose2D) {
val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing)
override fun serialize(encoder: Encoder, value: Pose2D) {
val pose = value as? Pose2DImpl ?: Pose2DImpl(value.coordinates, value.bearing)
encoder.encodeSerializableValue(proxySerializer, pose)
}
}
public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D =
DubinsPose2DImpl(coordinate, bearing)
public fun Pose2D(coordinate: DoubleVector2D, bearing: Angle): Pose2D =
Pose2DImpl(coordinate, bearing)
public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))
public fun Pose2D(point: DoubleVector2D, direction: DoubleVector2D): Pose2D =
Pose2D(point, Pose2D.vectorToBearing(direction))
public fun DubinsPose2D(x: Number, y: Number, bearing: Angle): DubinsPose2D =
DubinsPose2DImpl(Euclidean2DSpace.vector(x, y), bearing)
public fun Pose2D(x: Number, y: Number, bearing: Angle): Pose2D =
Pose2DImpl(Euclidean2DSpace.vector(x, y), bearing)

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

@ -18,8 +18,8 @@ import kotlin.math.atan2
public sealed interface Trajectory2D {
public val length: Double
public val beginPose: DubinsPose2D
public val endPose: DubinsPose2D
public val beginPose: Pose2D
public val endPose: Pose2D
/**
* Produce a trajectory with reversed order of points
@ -60,8 +60,8 @@ public data class StraightTrajectory2D(
public val bearing: Angle get() = (end - begin).bearing
override val beginPose: DubinsPose2D get() = DubinsPose2D(begin, bearing)
override val endPose: DubinsPose2D get() = DubinsPose2D(end, bearing)
override val beginPose: Pose2D get() = Pose2D(begin, bearing)
override val endPose: Pose2D get() = Pose2D(end, bearing)
override fun reversed(): StraightTrajectory2D = StraightTrajectory2D(end, begin)
}
@ -74,72 +74,88 @@ public fun StraightTrajectory2D(segment: LineSegment2D): StraightTrajectory2D =
*/
@Serializable
@SerialName("arc")
public data class CircleTrajectory2D (
public data class CircleTrajectory2D(
public val circle: Circle2D,
public val begin: DubinsPose2D,
public val end: DubinsPose2D,
public val arcStart: Angle,
public val arcAngle: Angle,
) : Trajectory2D {
public val direction: Trajectory2D.Direction = if (arcAngle > Angle.zero) Trajectory2D.R else Trajectory2D.L
override val beginPose: DubinsPose2D get() = begin
override val endPose: DubinsPose2D get() = end
/**
* Arc length in radians
*/
val arcAngle: Angle
get() = if (direction == Trajectory2D.L) {
begin.bearing - end.bearing
} else {
end.bearing - begin.bearing
}.normalized()
public val arcEnd: Angle = arcStart + arcAngle
override val beginPose: Pose2D get() = circle.tangent(arcStart, direction)
override val endPose: Pose2D get() = circle.tangent(arcEnd, direction)
override val length: Double by lazy {
circle.radius * arcAngle.radians
circle.radius * kotlin.math.abs(arcAngle.radians)
}
public val direction: Trajectory2D.Direction by lazy {
when {
begin.y < circle.center.y -> if (begin.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
begin.y > circle.center.y -> if (begin.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
else -> if (begin.bearing == Angle.zero) {
if (begin.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, arcEnd, -arcAngle)
public companion object
}
public fun CircleTrajectory2D(
center: DoubleVector2D,
start: DoubleVector2D,
end: DoubleVector2D,
direction: Trajectory2D.Direction,
): CircleTrajectory2D = with(Euclidean2DSpace) {
// fun calculatePose(
// vector: DoubleVector2D,
// theta: Angle,
// direction: Trajectory2D.Direction,
// ): DubinsPose2D = DubinsPose2D(
// vector,
// when (direction) {
// Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
// Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
// }
// )
//
// val s1 = StraightTrajectory2D(center, start)
// val s2 = StraightTrajectory2D(center, end)
// val pose1 = calculatePose(start, s1.bearing, direction)
// val pose2 = calculatePose(end, s2.bearing, direction)
// val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
// if (trajectory.direction != direction) error("Trajectory direction mismatch")
// return trajectory
val startVector = start - center
val endVector = end - center
val startRadius = norm(startVector)
val endRadius = norm(endVector)
require((startRadius - endRadius) / startRadius < 1e-6) { "Start and end points have different radii" }
val radius = (startRadius + endRadius) / 2
val startBearing = startVector.bearing
val endBearing = endVector.bearing
CircleTrajectory2D(
Circle2D(center, radius),
startBearing,
when (direction) {
Trajectory2D.L -> if (endBearing >= startBearing) {
endBearing - startBearing - Angle.piTimes2
} else {
if (begin.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
endBearing - startBearing
}
Trajectory2D.R -> if (endBearing >= startBearing) {
endBearing - startBearing
} else {
endBearing + Angle.piTimes2 - startBearing
}
}
}
)
}
override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, end.reversed(), begin.reversed())
public companion object {
public fun of(
center: DoubleVector2D,
start: DoubleVector2D,
end: DoubleVector2D,
direction: Trajectory2D.Direction,
): CircleTrajectory2D {
fun calculatePose(
vector: DoubleVector2D,
theta: Angle,
direction: Trajectory2D.Direction,
): DubinsPose2D = DubinsPose2D(
vector,
when (direction) {
Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
}
)
val s1 = StraightTrajectory2D(center, start)
val s2 = StraightTrajectory2D(center, end)
val pose1 = calculatePose(start, s1.bearing, direction)
val pose2 = calculatePose(end, s2.bearing, direction)
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
if (trajectory.direction != direction) error("Trajectory direction mismatch")
return trajectory
}
}
public fun CircleTrajectory2D(
circle: Circle2D,
beginPose: Pose2D,
endPose: Pose2D,
): CircleTrajectory2D = with(Euclidean2DSpace) {
val vectorToBegin = beginPose - circle.center
val vectorToEnd = endPose - circle.center
//TODO check pose bearing
return CircleTrajectory2D(circle, vectorToBegin.bearing, vectorToEnd.bearing - vectorToBegin.bearing)
}
@Serializable
@ -147,8 +163,8 @@ public data class CircleTrajectory2D (
public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
override val length: Double get() = segments.sumOf { it.length }
override val beginPose: DubinsPose2D get() = segments.first().beginPose
override val endPose: DubinsPose2D get() = segments.last().endPose
override val beginPose: Pose2D get() = segments.first().beginPose
override val endPose: Pose2D get() = segments.last().endPose
override fun reversed(): CompositeTrajectory2D = CompositeTrajectory2D(segments.map { it.reversed() }.reversed())
}

12
trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt
View File

@ -6,7 +6,7 @@
package space.kscience.kmath.geometry
import space.kscience.trajectory.CircleTrajectory2D
import space.kscience.trajectory.DubinsPose2D
import space.kscience.trajectory.Pose2D
import space.kscience.trajectory.Trajectory2D
import kotlin.math.PI
import kotlin.test.Test
@ -17,15 +17,15 @@ class ArcTests {
@Test
fun arc() = with(Euclidean2DSpace) {
val circle = Circle2D(vector(0.0, 0.0), 2.0)
val arc = CircleTrajectory2D.of(
val arc = CircleTrajectory2D(
circle.center,
vector(-2.0, 0.0),
vector(0.0, 2.0),
Trajectory2D.R
)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.begin.bearing.degrees)
assertEquals(90.0, arc.end.bearing.degrees)
assertEquals(0.0, arc.beginPose.bearing.degrees)
assertEquals(90.0, arc.endPose.bearing.degrees)
}
@Test
@ -33,8 +33,8 @@ class ArcTests {
val circle = circle(1, 0, 1)
val arc = CircleTrajectory2D(
circle,
DubinsPose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
DubinsPose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
Pose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
)
assertEquals(Trajectory2D.R, arc.direction)
assertEquals(PI / 2, arc.length, 1e-4)

25
trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt
View File

@ -6,23 +6,22 @@
package space.kscience.trajectory
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.equalsFloat
import kotlin.test.Test
import kotlin.test.assertEquals
import kotlin.test.assertNotNull
import kotlin.test.assertTrue
class DubinsTests {
@Test
fun dubinsTest() = with(Euclidean2DSpace){
fun dubinsTest() = with(Euclidean2DSpace) {
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = DubinsPose2D(straight.end, straight.bearing)
val end = DubinsPose2D(lineP1.begin, lineP1.bearing)
val start = Pose2D(straight.end, straight.bearing)
val end = Pose2D(lineP1.begin, lineP1.bearing)
val radius = 2.0
val dubins = DubinsPath.all(start, end, radius)
val dubins: List<CompositeTrajectory2D> = DubinsPath.all(start, end, radius)
val absoluteDistance = start.distanceTo(end)
println("Absolute distance: $absoluteDistance")
@ -39,22 +38,22 @@ class DubinsTests {
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.equalsFloat(path.length))
assertEquals(it.value, path.length, 1e-4)
val a = path.segments[0] as CircleTrajectory2D
val b = path.segments[1]
val c = path.segments[2] as CircleTrajectory2D
assertEquals(start, a.begin)
assertEquals(end, c.end)
assertEquals(start, a.beginPose, 1e-4)
assertEquals(end, c.endPose, 1e-4)
// Not working, theta double precision inaccuracy
if (b is CircleTrajectory2D) {
assertEquals(a.end, b.begin)
assertEquals(c.begin, b.end)
assertEquals(a.endPose, b.beginPose, 1e-4)
assertEquals(c.beginPose, b.endPose, 1e-4)
} else if (b is StraightTrajectory2D) {
assertEquals(a.end, DubinsPose2D(b.begin, b.bearing))
assertEquals(c.begin, DubinsPose2D(b.end, b.bearing))
assertEquals(a.endPose, Pose2D(b.begin, b.bearing), 1e-4)
assertEquals(c.beginPose, Pose2D(b.end, b.bearing),1e-4)
}
}
}

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

@ -34,8 +34,8 @@ class ObstacleTest {
val finalDirection = vector(1.0, -1.0)
val outputTangents = Obstacles.avoidObstacles(
DubinsPose2D(startPoint, startDirection),
DubinsPose2D(finalPoint, finalDirection),
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
startRadius,
Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
)
@ -53,8 +53,8 @@ class ObstacleTest {
val finalDirection = vector(1.0, -1.0)
val paths = Obstacles.avoidObstacles(
DubinsPose2D(startPoint, startDirection),
DubinsPose2D(finalPoint, finalDirection),
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
radius,
Obstacle(
Circle2D(vector(1.0, 6.5), 0.5),
@ -98,8 +98,8 @@ class ObstacleTest {
val finalDirection = vector(1.0, 0)
val paths = Obstacles.avoidObstacles(
DubinsPose2D(startPoint, startDirection),
DubinsPose2D(finalPoint, finalDirection),
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
startRadius,
Obstacle(
Circle2D(vector(0.0, 0.0), 1.0),
@ -117,8 +117,8 @@ class ObstacleTest {
@Test
fun largeCoordinates() {
val paths = Obstacles.avoidObstacles(
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
Pose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
Pose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
5000.0,
Obstacle(
Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0),

2
trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt
View File

@ -10,7 +10,7 @@ import space.kscience.kmath.geometry.radians
import space.kscience.kmath.geometry.sin
fun assertEquals(expected: DubinsPose2D, actual: DubinsPose2D, precision: Double = 1e-6){
fun assertEquals(expected: Pose2D, actual: Pose2D, precision: Double = 1e-6){
kotlin.test.assertEquals(expected.x, actual.x, precision)
kotlin.test.assertEquals(expected.y, actual.y, precision)
kotlin.test.assertEquals(expected.bearing.radians, actual.bearing.radians, precision)