From fa6d741869099923c120e709e11c521c85d425fc Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 22:12:36 +0200 Subject: [PATCH] Small improvement in test classes, theta function --- .../trajectory/dubins/DubinsPathFactory.kt | 46 ++++++++++--------- .../kscience/kmath/trajectory/segments/Arc.kt | 11 ++--- .../kmath/trajectory/segments/Line.kt | 7 ++- .../trajectory/segments/components/Pose2D.kt | 7 +-- .../space/kscience/kmath/trajectory/Math.kt | 2 +- .../kmath/trajectory/segments/ArcTests.kt | 11 ++--- .../segments/components/CircleTests.kt | 23 ++++++++++ 7 files changed, 60 insertions(+), 47 deletions(-) create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt index 56875ac5b..818735a96 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt @@ -34,19 +34,19 @@ public class DubinsPathFactory( val centers = Line2D(c1.center, c2.center) if (centers.length > turningRadius * 4) return null - var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + 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 p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) - theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + 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(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT) - val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT) - val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT) + val a1 = Arc(c1.center, base, p1, Arc.Direction.RIGHT) + val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) + val a3 = Arc(c2.center, p2, direction, Arc.Direction.RIGHT) return DubinsPath(a1, a2, a3) } @@ -56,19 +56,19 @@ public class DubinsPathFactory( val centers = Line2D(c1.center, c2.center) if (centers.length > turningRadius * 4) return null - var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + 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 p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) - theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + 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(c1.center, turningRadius, base, p1, Arc.Direction.LEFT) - val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT) + val a1 = Arc(c1.center, base, p1, Arc.Direction.LEFT) + val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, p2, direction, Arc.Direction.LEFT) return DubinsPath(a1, a2, a3) } @@ -76,8 +76,8 @@ public class DubinsPathFactory( val c1 = base.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius) val l = leftOuterTangent(c1, c2) - val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) + val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT) return DubinsPath(a1, LineSegment(l), a3) } @@ -86,8 +86,8 @@ public class DubinsPathFactory( val c1 = base.getLeftCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius) val l = rightOuterTangent(c1, c2) - val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) - val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) + val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT) + val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT) return DubinsPath(a1, LineSegment(l), a3) } @@ -97,8 +97,8 @@ public class DubinsPathFactory( val l = rightInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null - val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) + val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT) return DubinsPath(a1, LineSegment(l), a3) } @@ -108,8 +108,8 @@ public class DubinsPathFactory( val l = leftInnerTangent(c1, c2) if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null - val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) - val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) + val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT) + val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT) return DubinsPath(a1, LineSegment(l), a3) } } @@ -148,10 +148,12 @@ private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(ba private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? { val centers = Line2D(base.center, direction.center) return if (centers.length > base.radius * 2) { - val angle = when (side) { - SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) - SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) - }.theta + val angle = theta( + when (side) { + SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) + SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) + } + ) val dX = base.radius * sin(angle) val dY = base.radius * cos(angle) val p1 = Vector2D(base.center.x + dX, base.center.y + dY) 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 a7b2fe259..b8a81a070 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,5 +1,6 @@ package space.kscience.kmath.trajectory.segments +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.segments.components.Circle @@ -8,11 +9,10 @@ import kotlin.math.PI public class Arc( center: Vector2D, - radius: Double, a: Vector2D, b: Vector2D, internal val direction: Direction -) : Circle(center, radius), Segment { +) : Circle(center, center.distanceTo(a)), Segment { private val l1 = Line2D(center, a) private val l2 = Line2D(center, b) @@ -26,8 +26,7 @@ public class Arc( LEFT, RIGHT } - private fun calculateAngle() = - (if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta).theta + private fun calculateAngle() = theta(if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta) private fun calculateLength(): Double { val proportion = angle / (2 * PI) @@ -36,8 +35,8 @@ public class Arc( private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = if (direction == Direction.LEFT) { - Pose2D(vector.x, vector.y, (theta - PI / 2).theta) + Pose2D(vector.x, vector.y, theta(theta - PI / 2)) } else { - Pose2D(vector.x, vector.y, (theta + PI / 2).theta) + Pose2D(vector.x, vector.y, theta(theta + PI / 2)) } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt index b3e93b5ed..0e23b27f1 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt @@ -7,7 +7,7 @@ import kotlin.math.PI import kotlin.math.atan2 import kotlin.math.sqrt -public class LineSegment( +public data class LineSegment( internal val line: Line2D ) : Segment { override val length: Double @@ -15,10 +15,9 @@ public class LineSegment( } internal val Line2D.theta: Double - get() = atan2(direction.x - base.x, direction.y - base.y).theta + get() = theta(atan2(direction.x - base.x, direction.y - base.y)) internal val Line2D.length: Double get() = base.distanceTo(direction) -internal val Double.theta: Double - get() = (this + (2 * PI)) % (2 * PI) +internal fun theta(theta: Double) = (theta + (2 * PI)) % (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 index 6bcc3d308..d00dfbd96 100644 --- 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 @@ -4,15 +4,10 @@ import space.kscience.kmath.geometry.Vector2D import kotlin.math.cos import kotlin.math.sin -public class Pose2D( +public data class Pose2D( override val x: Double, override val y: Double, public val theta: Double ) : Vector2D { - internal constructor(vector: Vector2D, theta: Double) : this(vector.x, vector.y, theta) - - override fun toString(): String { - return "Pose2D(x=$x, y=$y, theta=$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 9258e6b4a..f52bb56f2 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 @@ -8,7 +8,7 @@ import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin -private const val maxFloatDelta = 0.000001 +const val maxFloatDelta = 0.000001 fun Double.radiansToDegrees() = this * 180 / PI 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 73b3a1d87..a59643c0c 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 @@ -10,14 +10,9 @@ class ArcTests { @Test fun arcTest() { - val center = Vector2D(0.0, 0.0) - val radius = 2.0 - val expectedCircumference = 12.56637 - val circle = Circle(center, radius) - assertEquals(expectedCircumference, circle.circumference, 1.0) - - val arc = Arc(center, radius, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) - assertEquals(expectedCircumference / 4, arc.length, 1.0) + 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) + assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) assertEquals(90.0, arc.pose2.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/components/CircleTests.kt new file mode 100644 index 000000000..6f28885e0 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt @@ -0,0 +1,23 @@ +/* + * 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.components + +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.maxFloatDelta +import kotlin.test.Test +import kotlin.test.assertEquals + +class CircleTests { + + @Test + fun arcTest() { + val center = Vector2D(0.0, 0.0) + val radius = 2.0 + val expectedCircumference = 12.56637 + val circle = Circle(center, radius) + assertEquals(expectedCircumference, circle.circumference, maxFloatDelta) + } +}