From 86fce7ec68d10dda664ddd936652eacd387c83b8 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 15:47:05 +0200 Subject: [PATCH] Arc contains circle, circle direction is computed from poses, factory function can create Arc based on Vector points and provided direction --- .../kmath/trajectory/dubins/DubinsPath.kt | 28 ++++---- .../kscience/kmath/trajectory/segments/Arc.kt | 64 +++++++++++-------- .../kmath/trajectory/dubins/DubinsTests.kt | 12 ++-- .../kmath/trajectory/segments/ArcTests.kt | 6 +- 4 files changed, 61 insertions(+), 49 deletions(-) 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 ad67fa11d..ee4f38662 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 @@ -65,9 +65,9 @@ public class DubinsPath( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) - val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) - val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) + 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) return DubinsPath(a1, a2, a3) } @@ -87,9 +87,9 @@ public class DubinsPath( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) - val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) + 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) return DubinsPath(a1, a2, a3) } @@ -97,8 +97,8 @@ public class DubinsPath( val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = leftOuterTangent(c1, c2) - val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT) return DubinsPath(a1, s, a3) } @@ -106,8 +106,8 @@ public class DubinsPath( val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = rightOuterTangent(c1, c2) - val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT) return DubinsPath(a1, s, a3) } @@ -117,8 +117,8 @@ public class DubinsPath( val s = rightInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT) return DubinsPath(a1, s, a3) } @@ -128,8 +128,8 @@ public class DubinsPath( val s = leftInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) - val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT) return DubinsPath(a1, s, a3) } } 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 index fe101f359..966e1e4e2 100644 --- 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 @@ -1,44 +1,56 @@ 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 space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Pose2D import kotlin.math.PI -public class Arc( - center: Vector2D, - a: Vector2D, - b: Vector2D, - internal val direction: Direction -) : Circle(center, center.distanceTo(a)), Segment { +public data class Arc( + public val circle: Circle, + public val start: Pose2D, + public val end: Pose2D +) : Segment { - private val s1 = Straight(center, a) - private val s2 = Straight(center, b) + 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) + } - internal val pose1 = calculatePose(a, s1.theta) - internal val pose2 = calculatePose(b, s2.theta) - private val angle = calculateAngle() - override val length: Double = calculateLength() + 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) + } + ) + } - public enum class Direction { + internal enum class Direction { LEFT, RIGHT } - private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta) - - private fun calculateLength(): Double { + 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 circumference * proportion + return circle.circumference * proportion + } + + internal val direction: Direction = 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 + } } - private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = - Pose2D.of( - vector, - when (direction) { - Direction.LEFT -> theta(theta - PI / 2) - Direction.RIGHT -> theta(theta + PI / 2) - } - ) } 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 a5288e40d..47e6ac2ef 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 @@ -48,18 +48,18 @@ class DubinsTests { println("${it.key}: ${path.length}") assertTrue(it.value.equalFloat(path.length)) - assertTrue(start.equalsFloat(path.a.pose1)) - assertTrue(end.equalsFloat(path.c.pose2)) + assertTrue(start.equalsFloat(path.a.start)) + assertTrue(end.equalsFloat(path.c.end)) // Not working, theta double precision inaccuracy if (path.b is Arc) { val b = path.b as Arc - assertTrue(path.a.pose2.equalsFloat(b.pose1)) - assertTrue(path.c.pose1.equalsFloat(b.pose2)) + 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 - assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta))) - assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta))) + 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 a59643c0c..5b4ae6d7a 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 @@ -11,9 +11,9 @@ class ArcTests { @Test fun arcTest() { val circle = Circle(Vector2D(0.0, 0.0), 2.0) - val arc = Arc(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) + val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) assertEquals(circle.circumference / 4, arc.length, 1.0) - assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) - assertEquals(90.0, arc.pose2.theta.radiansToDegrees()) + assertEquals(0.0, arc.start.theta.radiansToDegrees()) + assertEquals(90.0, arc.end.theta.radiansToDegrees()) } }