From 614ca8d6f388ae356eeca6bc82c7ab91c5c2dc36 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Mon, 1 May 2023 09:14:26 +0300 Subject: [PATCH] [WIP] refactor CircleTrajectory2D logic to be more correct --- .../kmath/geometry/geometryExtensions.kt | 16 ++ .../space/kscience/trajectory/DubinsPath.kt | 64 ++++---- .../space/kscience/trajectory/Obstacle.kt | 14 +- .../space/kscience/trajectory/Obstacles.kt | 25 ++-- .../trajectory/{DubinsPose2D.kt => Pose2D.kt} | 55 ++++--- .../space/kscience/trajectory/Trajectory2D.kt | 138 ++++++++++-------- .../space/kscience/kmath/geometry/ArcTests.kt | 12 +- .../space/kscience/trajectory/DubinsTests.kt | 25 ++-- .../space/kscience/trajectory/ObstacleTest.kt | 16 +- .../space/kscience/trajectory/testutils.kt | 2 +- 10 files changed, 208 insertions(+), 159 deletions(-) rename trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/{DubinsPose2D.kt => Pose2D.kt} (56%) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt index 2ece6f0..9ddd0db 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt @@ -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 = 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) +} + diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt index 9650e73..a4736e9 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt @@ -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 = with(Euclidean2DSpace) { +internal fun Pose2D.getTangentCircles(radius: Double): Pair = 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 = 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) } } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt index e84c465..01ce191 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt @@ -48,14 +48,14 @@ private class ObstacleImpl(override val circles: List) : 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) ) } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt index fae7a7d..1432925 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt @@ -158,14 +158,15 @@ public class Obstacles(public val obstacles: List) { //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) { } private fun constructTangentCircles( - pose: DubinsPose2D, + pose: Pose2D, r: Double, ): LR = 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) { } public fun avoidObstacles( - start: DubinsPose2D, - finish: DubinsPose2D, + start: Pose2D, + finish: Pose2D, startingRadius: Double, obstacleList: List, finalRadius: Double = startingRadius, @@ -328,15 +329,15 @@ public class Obstacles(public val obstacles: List) { } public fun avoidObstacles( - start: DubinsPose2D, - finish: DubinsPose2D, + start: Pose2D, + finish: Pose2D, trajectoryRadius: Double, vararg obstacles: Obstacle, ): List = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList()) public fun avoidPolygons( - start: DubinsPose2D, - finish: DubinsPose2D, + start: Pose2D, + finish: Pose2D, trajectoryRadius: Double, vararg polygons: Polygon, ): List { @@ -348,8 +349,8 @@ public class Obstacles(public val obstacles: List) { public fun avoidPolygons( - start: DubinsPose2D, - finish: DubinsPose2D, + start: Pose2D, + finish: Pose2D, trajectoryRadius: Double, polygons: Collection>, ): List { diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Pose2D.kt similarity index 56% rename from trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt rename to trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Pose2D.kt index 0bc476f..9e86f77 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Pose2D.kt @@ -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 = @@ -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 { - private val proxySerializer = DubinsPose2DImpl.serializer() +public object Pose2DSerializer : KSerializer { + 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) \ No newline at end of file +public fun Pose2D(x: Number, y: Number, bearing: Angle): Pose2D = + Pose2DImpl(Euclidean2DSpace.vector(x, y), bearing) \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt index 5dcfdcb..84ffa0d 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt @@ -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 { 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()) } diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt index 625ddb7..1dabfc9 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt @@ -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) diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt index 1d9f24b..77617d4 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt @@ -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 = 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) } } } diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt index 41a5311..3cac316 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -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), diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt index ff9c595..86f8054 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt @@ -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)