From 323e8b6872137eb6da1b8c91aff1ce8b4e8d31d7 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 26 Jul 2022 09:19:04 +0300 Subject: [PATCH] Code simplification for Dubins path --- .../space/kscience/kmath/geometry/Circle2D.kt | 18 ++++ .../space/kscience/kmath/geometry/Line.kt | 4 + .../kmath/trajectory/dubins/DubinsPath.kt | 82 +++++++++-------- ...oryFunctions.kt => trajectoryFunctions.kt} | 41 +++++---- .../kscience/kmath/trajectory/segments/Arc.kt | 59 ------------ .../kmath/trajectory/segments/Pose2D.kt | 21 +++++ .../kmath/trajectory/segments/Segment.kt | 5 -- .../kmath/trajectory/segments/Straight.kt | 18 ---- .../kmath/trajectory/segments/Trajectory.kt | 89 +++++++++++++++++++ .../trajectory/segments/components/Circle.kt | 11 --- .../trajectory/segments/components/Pose2D.kt | 13 --- .../space/kscience/kmath/trajectory/Math.kt | 10 +-- .../kmath/trajectory/dubins/DubinsTests.kt | 16 ++-- .../kmath/trajectory/segments/ArcTests.kt | 7 +- .../segments/{components => }/CircleTests.kt | 6 +- .../kmath/trajectory/segments/LineTests.kt | 12 +-- 16 files changed, 225 insertions(+), 187 deletions(-) create mode 100644 kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt rename kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/{TrajectoryFunctions.kt => trajectoryFunctions.kt} (51%) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt rename kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/{components => }/CircleTests.kt (74%) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt new file mode 100644 index 000000000..8623335b9 --- /dev/null +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt @@ -0,0 +1,18 @@ +/* + * Copyright 2018-2021 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.geometry + +import kotlin.math.PI + +/** + * A circle in 2D space + */ +public class Circle2D( + public val center: Vector2D, + public val radius: Double +) + +public val Circle2D.circumference: Double get() = radius * 2 * PI diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt index 8c6ccb55e..85bfcdd8f 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt @@ -5,6 +5,10 @@ package space.kscience.kmath.geometry +/** + * 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 + */ public data class Line(val base: V, val direction: V) public typealias Line2D = Line diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt index ee4f38662..f510e3572 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt @@ -5,91 +5,95 @@ package space.kscience.kmath.trajectory.dubins +import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Arc -import space.kscience.kmath.trajectory.segments.Segment -import space.kscience.kmath.trajectory.segments.Straight -import space.kscience.kmath.trajectory.segments.components.Circle -import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.ArcSegment +import space.kscience.kmath.trajectory.segments.Pose2D +import space.kscience.kmath.trajectory.segments.StraightSegment +import space.kscience.kmath.trajectory.segments.Trajectory import kotlin.math.acos import kotlin.math.cos import kotlin.math.sin public class DubinsPath( - public val a: Arc, - public val b: Segment, - public val c: Arc, -) { + public val a: ArcSegment, + public val b: Trajectory, + public val c: ArcSegment, +) : Trajectory { public val type: TYPE = TYPE.valueOf( arrayOf( a.direction.name[0], - if (b is Arc) b.direction.name[0] else 'S', + if (b is ArcSegment) b.direction.name[0] else 'S', c.direction.name[0] ).toCharArray().concatToString() ) - public val length: Double = a.length + b.length + c.length + override val length: Double get() = a.length + b.length + c.length public enum class TYPE { RLR, LRL, RSR, LSL, RSL, LSR } public companion object { - public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List = - listOfNotNull( - rlr(start, end, turningRadius), - lrl(start, end, turningRadius), - rsr(start, end, turningRadius), - lsl(start, end, turningRadius), - rsl(start, end, turningRadius), - lsr(start, end, turningRadius) - ) + public fun all( + start: Pose2D, + end: Pose2D, + turningRadius: Double, + ): List = listOfNotNull( + rlr(start, end, turningRadius), + lrl(start, end, turningRadius), + rsr(start, end, turningRadius), + lsl(start, end, turningRadius), + rsl(start, end, turningRadius), + lsr(start, end, turningRadius) + ) + public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath = - all(start, end, turningRadius).minByOrNull { it.length }!! + all(start, end, turningRadius).minBy { it.length } public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) - val centers = Straight(c1.center, c2.center) + val centers = StraightSegment(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) - val e = Circle(p, turningRadius) + val e = Circle2D(p, turningRadius) val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT) - val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT) - val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT) + val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.RIGHT) + val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT) + val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT) return DubinsPath(a1, a2, a3) } public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) - val centers = Straight(c1.center, c2.center) + val centers = StraightSegment(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) - val e = Circle(p, turningRadius) + val e = Circle2D(p, turningRadius) val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT) - val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT) - val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT) + val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.LEFT) + val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT) + val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT) return DubinsPath(a1, a2, a3) } @@ -97,8 +101,8 @@ public class DubinsPath( val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = leftOuterTangent(c1, c2) - val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT) + val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT) + val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT) return DubinsPath(a1, s, a3) } @@ -106,8 +110,8 @@ public class DubinsPath( val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = rightOuterTangent(c1, c2) - val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT) + val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT) + val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT) return DubinsPath(a1, s, a3) } @@ -117,8 +121,8 @@ public class DubinsPath( val s = rightInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT) + val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT) + val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT) return DubinsPath(a1, s, a3) } @@ -128,8 +132,8 @@ public class DubinsPath( val s = leftInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT) + val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT) + val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT) return DubinsPath(a1, s, a3) } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt similarity index 51% rename from kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt rename to kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt index 547cb99fc..18384d349 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt @@ -5,10 +5,10 @@ package space.kscience.kmath.trajectory.dubins +import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Straight -import space.kscience.kmath.trajectory.segments.components.Circle -import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.Pose2D +import space.kscience.kmath.trajectory.segments.StraightSegment import kotlin.math.PI import kotlin.math.acos import kotlin.math.cos @@ -18,18 +18,19 @@ private enum class SIDE { LEFT, RIGHT } -internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first -internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second -internal fun Pose2D.getTangentCircles(radius: Double): Pair { +internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first +internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second +internal fun Pose2D.getTangentCircles(radius: Double): Pair { val dX = radius * cos(theta) val dY = radius * sin(theta) - return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius) + return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius) } -internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) -internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) -private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { - val centers = Straight(a.center, b.center) +internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT) +internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT) + +private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment { + val centers = StraightSegment(a.center, b.center) val p1 = when (side) { SIDE.LEFT -> Vector2D( a.center.x - a.radius * cos(centers.theta), @@ -40,16 +41,20 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { a.center.y - a.radius * sin(centers.theta) ) } - return Straight( + return StraightSegment( p1, Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) ) } -internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) -internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) -private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? { - val centers = Straight(base.center, direction.center) +internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = + innerTangent(base, direction, SIDE.LEFT) + +internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = + innerTangent(base, direction, SIDE.RIGHT) + +private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? { + val centers = StraightSegment(base.center, direction.center) if (centers.length < base.radius * 2) return null val angle = theta( when (side) { @@ -61,7 +66,7 @@ private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? val dY = base.radius * cos(angle) val p1 = Vector2D(base.center.x + dX, base.center.y + dY) val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) - return Straight(p1, p2) + return StraightSegment(p1, p2) } -internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) +internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt deleted file mode 100644 index 1c02dd952..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt +++ /dev/null @@ -1,59 +0,0 @@ -package space.kscience.kmath.trajectory.segments - -import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.dubins.theta -import space.kscience.kmath.trajectory.segments.components.Circle -import space.kscience.kmath.trajectory.segments.components.Pose2D -import kotlin.math.PI - -public data class Arc( - public val circle: Circle, - public val start: Pose2D, - public val end: Pose2D -) : Segment { - - internal companion object { - fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc { - val s1 = Straight(center, start) - val s2 = Straight(center, end) - val pose1 = calculatePose(start, s1.theta, direction) - val pose2 = calculatePose(end, s2.theta, direction) - return Arc(Circle(center, s1.length), pose1, pose2) - } - - private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D = - Pose2D.of( - vector, - when (direction) { - Direction.LEFT -> theta(theta - PI / 2) - Direction.RIGHT -> theta(theta + PI / 2) - } - ) - } - - internal enum class Direction { - LEFT, RIGHT - } - - override val length: Double - get() { - val angle: Double = - theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta) - val proportion = angle / (2 * PI) - return circle.circumference * proportion - } - - internal val direction: Direction - get() = if (start.y < circle.center.y) { - if (start.theta > PI) Direction.RIGHT else Direction.LEFT - } else if (start.y > circle.center.y) { - if (start.theta < PI) Direction.RIGHT else Direction.LEFT - } else { - if (start.theta == 0.0) { - if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT - } else { - if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT - } - } - -} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt new file mode 100644 index 000000000..3df34c107 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt @@ -0,0 +1,21 @@ +/* + * Copyright 2018-2021 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.segments + +import space.kscience.kmath.geometry.Vector2D + +/** + * A [Vector2D] with view direction + */ +public data class Pose2D( + override val x: Double, + override val y: Double, + public val theta: Double +) : Vector2D { + public companion object { + public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta) + } +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt deleted file mode 100644 index 8a1d086fc..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt +++ /dev/null @@ -1,5 +0,0 @@ -package space.kscience.kmath.trajectory.segments - -public interface Segment { - public val length: Double -} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt deleted file mode 100644 index 444025d83..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt +++ /dev/null @@ -1,18 +0,0 @@ -package space.kscience.kmath.trajectory.segments - -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.dubins.theta -import kotlin.math.PI -import kotlin.math.atan2 - -public data class Straight( - internal val start: Vector2D, - internal val end: Vector2D -) : Segment { - override val length: Double - get() = start.distanceTo(end) - - internal val theta: Double - get() = theta(atan2(end.x - start.x, end.y - start.y)) -} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt new file mode 100644 index 000000000..453eef47f --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt @@ -0,0 +1,89 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.circumference +import space.kscience.kmath.trajectory.dubins.theta +import kotlin.math.PI +import kotlin.math.atan2 + +public interface Trajectory { + public val length: Double +} + +/** + * Straight path segment. The order of start and end defines the direction + */ +public data class StraightSegment( + internal val start: Vector2D, + internal val end: Vector2D, +) : Trajectory { + override val length: Double get() = start.distanceTo(end) + + internal val theta: Double get() = theta(atan2(end.x - start.x, end.y - start.y)) +} + +/** + * An arc segment + */ +public data class ArcSegment( + public val circle: Circle2D, + public val start: Pose2D, + public val end: Pose2D, +) : Trajectory { + + public enum class Direction { + LEFT, RIGHT + } + + override val length: Double by lazy { + val angle: Double = theta( + if (direction == Direction.LEFT) { + start.theta - end.theta + } else { + end.theta - start.theta + } + ) + val proportion = angle / (2 * PI) + circle.circumference * proportion + } + + internal val direction: Direction by lazy { + if (start.y < circle.center.y) { + if (start.theta > PI) Direction.RIGHT else Direction.LEFT + } else if (start.y > circle.center.y) { + if (start.theta < PI) Direction.RIGHT else Direction.LEFT + } else { + if (start.theta == 0.0) { + if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT + } else { + if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT + } + } + } + + public companion object { + public fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): ArcSegment { + fun calculatePose( + vector: Vector2D, + theta: Double, + direction: Direction, + ): Pose2D = Pose2D.of( + vector, + when (direction) { + Direction.LEFT -> theta(theta - PI / 2) + Direction.RIGHT -> theta(theta + PI / 2) + } + ) + + val s1 = StraightSegment(center, start) + val s2 = StraightSegment(center, end) + val pose1 = calculatePose(start, s1.theta, direction) + val pose2 = calculatePose(end, s2.theta, direction) + return ArcSegment(Circle2D(center, s1.length), pose1, pose2) + } + } + +} + diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt deleted file mode 100644 index 946dd8c6e..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt +++ /dev/null @@ -1,11 +0,0 @@ -package space.kscience.kmath.trajectory.segments.components - -import space.kscience.kmath.geometry.Vector2D -import kotlin.math.PI - -public open class Circle( - internal val center: Vector2D, - internal val radius: Double -) { - internal val circumference = radius * 2 * PI -} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt deleted file mode 100644 index c49da3187..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt +++ /dev/null @@ -1,13 +0,0 @@ -package space.kscience.kmath.trajectory.segments.components - -import space.kscience.kmath.geometry.Vector2D - -public data class Pose2D( - override val x: Double, - override val y: Double, - public val theta: Double -) : Vector2D { - internal companion object { - internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta) - } -} diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt index 92b2f1df9..4f8fda826 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt @@ -1,8 +1,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Straight -import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.Pose2D +import space.kscience.kmath.trajectory.segments.StraightSegment import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin @@ -14,12 +14,12 @@ fun Double.radiansToDegrees() = this * 180 / PI fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta) -fun Straight.inverse() = Straight(end, start) -fun Straight.shift(shift: Int, width: Double): Straight { +fun StraightSegment.inverse() = StraightSegment(end, start) +fun StraightSegment.shift(shift: Int, width: Double): StraightSegment { val dX = width * sin(inverse().theta) val dY = width * sin(theta) - return Straight( + return StraightSegment( Vector2D(start.x - dX * shift, start.y - dY * shift), Vector2D(end.x - dX * shift, end.y - dY * shift) ) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt index 47e6ac2ef..9069f6dd2 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -10,9 +10,9 @@ import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.equalFloat import space.kscience.kmath.trajectory.equalsFloat import space.kscience.kmath.trajectory.inverse -import space.kscience.kmath.trajectory.segments.Arc -import space.kscience.kmath.trajectory.segments.Straight -import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.ArcSegment +import space.kscience.kmath.trajectory.segments.Pose2D +import space.kscience.kmath.trajectory.segments.StraightSegment import space.kscience.kmath.trajectory.shift import kotlin.test.Test import kotlin.test.assertNotNull @@ -23,7 +23,7 @@ class DubinsTests { @Test fun dubinsTest() { - val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) val lineP1 = straight.shift(1, 10.0).inverse() val start = Pose2D.of(straight.end, straight.theta) @@ -52,12 +52,12 @@ class DubinsTests { assertTrue(end.equalsFloat(path.c.end)) // Not working, theta double precision inaccuracy - if (path.b is Arc) { - val b = path.b as Arc + if (path.b is ArcSegment) { + val b = path.b as ArcSegment assertTrue(path.a.end.equalsFloat(b.start)) assertTrue(path.c.start.equalsFloat(b.end)) - } else if (path.b is Straight) { - val b = path.b as Straight + } else if (path.b is StraightSegment) { + val b = path.b as StraightSegment assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta))) assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta))) } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt index 5b4ae6d7a..aa8778315 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -1,8 +1,9 @@ package space.kscience.kmath.trajectory.segments +import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.circumference import space.kscience.kmath.trajectory.radiansToDegrees -import space.kscience.kmath.trajectory.segments.components.Circle import kotlin.test.Test import kotlin.test.assertEquals @@ -10,8 +11,8 @@ class ArcTests { @Test fun arcTest() { - val circle = Circle(Vector2D(0.0, 0.0), 2.0) - val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) + val circle = Circle2D(Vector2D(0.0, 0.0), 2.0) + val arc = ArcSegment.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), ArcSegment.Direction.RIGHT) assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(0.0, arc.start.theta.radiansToDegrees()) assertEquals(90.0, arc.end.theta.radiansToDegrees()) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt similarity index 74% rename from kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt rename to kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt index 6f28885e0..5170c1db5 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt @@ -3,9 +3,11 @@ * 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.segments.components +package space.kscience.kmath.trajectory.segments +import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.circumference import space.kscience.kmath.trajectory.maxFloatDelta import kotlin.test.Test import kotlin.test.assertEquals @@ -17,7 +19,7 @@ class CircleTests { val center = Vector2D(0.0, 0.0) val radius = 2.0 val expectedCircumference = 12.56637 - val circle = Circle(center, radius) + val circle = Circle2D(center, radius) assertEquals(expectedCircumference, circle.circumference, maxFloatDelta) } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt index e8184e178..afc473d79 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -12,21 +12,21 @@ class LineTests { @Test fun lineTest() { - val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length) assertEquals(45.0, straight.theta.radiansToDegrees()) } @Test fun lineAngleTest() { - val zero = Vector2D(0.0, 0.0) - val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + //val zero = Vector2D(0.0, 0.0) + val north = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) assertEquals(0.0, north.theta.radiansToDegrees()) - val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + val east = StraightSegment(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) assertEquals(90.0, east.theta.radiansToDegrees()) - val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + val south = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) assertEquals(180.0, south.theta.radiansToDegrees()) - val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + val west = StraightSegment(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) assertEquals(270.0, west.theta.radiansToDegrees()) } }