From 32769d690683d8b1975f1d3380c2f6f3ffe49972 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 18:13:50 +0200 Subject: [PATCH 01/11] Dubins path --- kmath-trajectory/README.md | 32 ++++ kmath-trajectory/build.gradle.kts | 15 ++ .../kmath/trajectory/dubins/DubinsPath.kt | 30 +++ .../trajectory/dubins/DubinsPathFactory.kt | 171 ++++++++++++++++++ .../kscience/kmath/trajectory/segments/Arc.kt | 43 +++++ .../kmath/trajectory/segments/Line.kt | 23 +++ .../kmath/trajectory/segments/Segment.kt | 5 + .../trajectory/segments/components/Circle.kt | 11 ++ .../trajectory/segments/components/Pose2D.kt | 18 ++ .../space/kscience/kmath/trajectory/Math.kt | 27 +++ .../kmath/trajectory/dubins/DubinsTests.kt | 68 +++++++ .../kmath/trajectory/segments/ArcTests.kt | 24 +++ .../kmath/trajectory/segments/LineTests.kt | 33 ++++ settings.gradle.kts | 3 +- 14 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 kmath-trajectory/README.md create mode 100644 kmath-trajectory/build.gradle.kts create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt create 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/Line.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt diff --git a/kmath-trajectory/README.md b/kmath-trajectory/README.md new file mode 100644 index 000000000..cb2b6989f --- /dev/null +++ b/kmath-trajectory/README.md @@ -0,0 +1,32 @@ +# Module kmath-trajectory + + + +## Usage + +## Artifact: + +The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.0`. + +**Gradle Groovy:** +```groovy +repositories { + maven { url 'https://repo.kotlin.link' } + mavenCentral() +} + +dependencies { + implementation 'space.kscience:kmath-trajectory:0.3.0' +} +``` +**Gradle Kotlin DSL:** +```kotlin +repositories { + maven("https://repo.kotlin.link") + mavenCentral() +} + +dependencies { + implementation("space.kscience:kmath-trajectory:0.3.0") +} +``` diff --git a/kmath-trajectory/build.gradle.kts b/kmath-trajectory/build.gradle.kts new file mode 100644 index 000000000..502867ee3 --- /dev/null +++ b/kmath-trajectory/build.gradle.kts @@ -0,0 +1,15 @@ +plugins { + kotlin("multiplatform") + id("ru.mipt.npm.gradle.common") + id("ru.mipt.npm.gradle.native") +} + +kotlin.sourceSets.commonMain { + dependencies { + api(projects.kmath.kmathGeometry) + } +} + +readme { + maturity = ru.mipt.npm.gradle.Maturity.PROTOTYPE +} 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 new file mode 100644 index 000000000..005d7fd60 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt @@ -0,0 +1,30 @@ +/* + * 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.dubins + +import space.kscience.kmath.trajectory.segments.Arc +import space.kscience.kmath.trajectory.segments.Segment + +public class DubinsPath( + public val a: Arc, + public val b: Segment, + public val c: Arc, +) { + + public val type: TYPE = TYPE.valueOf( + arrayOf( + a.direction.name[0], + if (b is Arc) b.direction.name[0] else 'S', + c.direction.name[0] + ).toCharArray().concatToString() + ) + + public val length: Double = a.length + b.length + c.length + + public enum class TYPE { + RLR, LRL, RSR, LSL, RSL, LSR + } +} 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 new file mode 100644 index 000000000..98ed8ed32 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt @@ -0,0 +1,171 @@ +/* + * 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.dubins + +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.Arc +import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.segments.components.Circle +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.length +import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin + +public class DubinsPathFactory( + private val base: Pose2D, + private val direction: Pose2D, + private val turningRadius: Double +) { + + public val all: List get() = listOfNotNull(rlr, lrl, rsr, lsl, rsl, lsr) + public val shortest: DubinsPath get() = all.minByOrNull { it.length }!! + public operator fun get(type: DubinsPath.TYPE): DubinsPath? = all.find { it.type == type } + + public val rlr: DubinsPath? get () { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val centers = Line2D(c1.center, c2.center) + return if (centers.length < turningRadius * 4) { + var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + 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 + 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) + DubinsPath(a1, a2, a3) + } else { + null + } + } + + private val lrl: DubinsPath? get () { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val centers = Line2D(c1.center, c2.center) + return if (centers.length < turningRadius * 4) { + var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + 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 + 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) + DubinsPath(a1, a2, a3) + } else { + null + } + } + + public val rsr: DubinsPath? get () { + 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) + return DubinsPath(a1, LineSegment(l), a3) + } + + public val lsl: DubinsPath + get () { + 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) + return DubinsPath(a1, LineSegment(l), a3) + } + + public val rsl: DubinsPath? get () { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val l = rightInnerTangent(c1, c2) + return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != 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) + DubinsPath(a1, LineSegment(l), a3) + } else { + null + } + } + + public val lsr: DubinsPath? get () { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val l = leftInnerTangent(c1, c2) + return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != 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) + DubinsPath(a1, LineSegment(l), a3) + } else { + null + } + } +} + +private enum class SIDE { + LEFT, RIGHT +} + +private fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first +private fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second +private 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) +} + +private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) +private fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) +private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { + val centers = Line2D(a.center, b.center) + val p1 = when (side) { + SIDE.LEFT -> Vector2D( + a.center.x - a.radius * cos(centers.theta), + a.center.y + a.radius * sin(centers.theta) + ) + SIDE.RIGHT -> Vector2D( + a.center.x + a.radius * cos(centers.theta), + a.center.y - a.radius * sin(centers.theta) + ) + } + return Line2D(p1, Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y))) +} + +private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) +private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) +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 dX = base.radius * sin(angle) + 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) + Line2D(p1, p2) + } else { + null + } +} 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 new file mode 100644 index 000000000..a7b2fe259 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt @@ -0,0 +1,43 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +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, + radius: Double, + a: Vector2D, + b: Vector2D, + internal val direction: Direction +) : Circle(center, radius), Segment { + + private val l1 = Line2D(center, a) + private val l2 = Line2D(center, b) + + internal val pose1 = calculatePose(a, l1.theta) + internal val pose2 = calculatePose(b, l2.theta) + private val angle = calculateAngle() + override val length: Double = calculateLength() + + public enum class Direction { + LEFT, RIGHT + } + + private fun calculateAngle() = + (if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta).theta + + private fun calculateLength(): Double { + val proportion = angle / (2 * PI) + return circumference * proportion + } + + private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = + if (direction == Direction.LEFT) { + Pose2D(vector.x, vector.y, (theta - PI / 2).theta) + } else { + Pose2D(vector.x, vector.y, (theta + PI / 2).theta) + } +} 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 new file mode 100644 index 000000000..f63372016 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt @@ -0,0 +1,23 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.operations.DoubleField.pow +import kotlin.math.PI +import kotlin.math.atan2 +import kotlin.math.sqrt + +public class LineSegment( + internal val line: Line2D +) : Segment { + override val length: Double + get() = line.length +} + +internal val Line2D.theta: Double + get() = atan2(direction.x - base.x, direction.y - base.y).theta + +internal val Line2D.length: Double + get() = sqrt((direction.x - base.x).pow(2) + (direction.y - base.y).pow(2)) + +internal val Double.theta: Double + get() = (this + (2 * PI)) % (2 * PI) 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 new file mode 100644 index 000000000..8a1d086fc --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt @@ -0,0 +1,5 @@ +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/components/Circle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt new file mode 100644 index 000000000..946dd8c6e --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt @@ -0,0 +1,11 @@ +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 new file mode 100644 index 000000000..6bcc3d308 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt @@ -0,0 +1,18 @@ +package space.kscience.kmath.trajectory.segments.components + +import space.kscience.kmath.geometry.Vector2D +import kotlin.math.cos +import kotlin.math.sin + +public 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 new file mode 100644 index 000000000..9258e6b4a --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt @@ -0,0 +1,27 @@ +package space.kscience.kmath.trajectory + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.PI +import kotlin.math.abs +import kotlin.math.sin + +private const val maxFloatDelta = 0.000001 + +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 Line2D.inverse() = Line2D(direction, base) +fun Line2D.shift(shift: Int, width: Double): Line2D { + val dX = width * sin(inverse().theta) + val dY = width * sin(theta) + + return Line2D( + Vector2D(base.x - dX * shift, base.y - dY * shift), + Vector2D(direction.x - dX * shift, direction.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 new file mode 100644 index 000000000..583e7a4e0 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -0,0 +1,68 @@ +/* + * 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.dubins + +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.Arc +import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.equalFloat +import space.kscience.kmath.trajectory.equalsFloat +import space.kscience.kmath.trajectory.inverse +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.theta +import space.kscience.kmath.trajectory.shift +import kotlin.test.Test +import kotlin.test.assertNotNull +import kotlin.test.assertTrue + + +class DubinsTests { + + @Test + fun dubinsTest() { + val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val lineP1 = line.shift(1, 10.0).inverse() + + val start = Pose2D(line.direction, line.theta) + val end = Pose2D(lineP1.base, lineP1.theta) + val radius = 2.0 + val dubins = DubinsPathFactory(start, end, radius) + + val absoluteDistance = start.distanceTo(end) + println("Absolute distance: $absoluteDistance") + + val expectedLengths = mapOf( + DubinsPath.TYPE.RLR to 13.067681939031397, + DubinsPath.TYPE.RSR to 12.28318530717957, + DubinsPath.TYPE.LSL to 32.84955592153878, + DubinsPath.TYPE.RSL to 23.37758938854081, + DubinsPath.TYPE.LSR to 23.37758938854081 + ) + + expectedLengths.forEach { + val path = dubins[it.key] + assertNotNull(path, "Path ${it.key} not found") + println("${it.key}: ${path.length}") + assertTrue(it.value.equalFloat(path.length)) + + assertTrue(start.equalsFloat(path.a.pose1)) + assertTrue(end.equalsFloat(path.c.pose2)) + + // 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)) + } else if (path.b is LineSegment) { + val b = (path.b as LineSegment).line + assertTrue(path.a.pose2.equalsFloat(Pose2D(b.base, b.theta))) + assertTrue(path.c.pose1.equalsFloat(Pose2D(b.direction, 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 new file mode 100644 index 000000000..73b3a1d87 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -0,0 +1,24 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.radiansToDegrees +import space.kscience.kmath.trajectory.segments.components.Circle +import kotlin.test.Test +import kotlin.test.assertEquals + +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) + 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/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt new file mode 100644 index 000000000..30f5ef6d8 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -0,0 +1,33 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.radiansToDegrees +import kotlin.math.pow +import kotlin.math.sqrt +import kotlin.test.Test +import kotlin.test.assertEquals + +class LineTests { + + @Test + fun lineTest() { + val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), line.length) + assertEquals(45.0, line.theta.radiansToDegrees()) + } + + @Test + fun lineAngleTest() { + val zero = Vector2D(0.0, 0.0) + val north = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + assertEquals(0.0, north.theta.radiansToDegrees()) + val east = Line2D(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + assertEquals(90.0, east.theta.radiansToDegrees()) + val south = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + assertEquals(180.0, south.theta.radiansToDegrees()) + val west = Line2D(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + assertEquals(270.0, west.theta.radiansToDegrees()) + } +} diff --git a/settings.gradle.kts b/settings.gradle.kts index e3c621e9a..b564972c7 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -44,6 +44,7 @@ include( ":kmath-jupyter", ":kmath-symja", ":kmath-jafama", + ":kmath-trajectory", ":examples", ":benchmarks", -) \ No newline at end of file +) From cdb116fa2009c36a2aaad9942032b35f83d4f8de Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 18:55:37 +0200 Subject: [PATCH 02/11] Cleanup --- .../trajectory/dubins/DubinsPathFactory.kt | 94 +++++++++---------- 1 file changed, 43 insertions(+), 51 deletions(-) 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 98ed8ed32..56875ac5b 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 @@ -32,51 +32,47 @@ public class DubinsPathFactory( val c1 = base.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius) val centers = Line2D(c1.center, c2.center) - return if (centers.length < turningRadius * 4) { - var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta - 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 - 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) - DubinsPath(a1, a2, a3) - } else { - null - } + if (centers.length > turningRadius * 4) return null + + var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + 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 + 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) + return DubinsPath(a1, a2, a3) } private val lrl: DubinsPath? get () { val c1 = base.getLeftCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius) val centers = Line2D(c1.center, c2.center) - return if (centers.length < turningRadius * 4) { - var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta - 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 - 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) - DubinsPath(a1, a2, a3) - } else { - null - } + if (centers.length > turningRadius * 4) return null + + var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + 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 + 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) + return DubinsPath(a1, a2, a3) } - public val rsr: DubinsPath? get () { + public val rsr: DubinsPath get () { val c1 = base.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius) val l = leftOuterTangent(c1, c2) @@ -99,26 +95,22 @@ public class DubinsPathFactory( val c1 = base.getRightCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius) val l = rightInnerTangent(c1, c2) - return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != 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) - DubinsPath(a1, LineSegment(l), a3) - } else { - null - } + 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) + return DubinsPath(a1, LineSegment(l), a3) } public val lsr: DubinsPath? get () { val c1 = base.getLeftCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius) val l = leftInnerTangent(c1, c2) - return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != 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) - DubinsPath(a1, LineSegment(l), a3) - } else { - null - } + 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) + return DubinsPath(a1, LineSegment(l), a3) } } From ada1141738328101da29f48af4ba71bc8fca2d30 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 18:57:10 +0200 Subject: [PATCH 03/11] Use Line distancTo function --- .../kotlin/space/kscience/kmath/trajectory/segments/Line.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 f63372016..b3e93b5ed 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 @@ -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.operations.DoubleField.pow import kotlin.math.PI @@ -17,7 +18,7 @@ internal val Line2D.theta: Double get() = atan2(direction.x - base.x, direction.y - base.y).theta internal val Line2D.length: Double - get() = sqrt((direction.x - base.x).pow(2) + (direction.y - base.y).pow(2)) + get() = base.distanceTo(direction) internal val Double.theta: Double get() = (this + (2 * PI)) % (2 * PI) From fa6d741869099923c120e709e11c521c85d425fc Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 22:12:36 +0200 Subject: [PATCH 04/11] 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) + } +} From 4f88982734acfdfa5f771c3c9c3705801ab00781 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 22:13:50 +0200 Subject: [PATCH 05/11] Formatting --- .../trajectory/dubins/DubinsPathFactory.kt | 158 +++++++++--------- 1 file changed, 83 insertions(+), 75 deletions(-) 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 818735a96..91287b952 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 @@ -28,90 +28,95 @@ public class DubinsPathFactory( public val shortest: DubinsPath get() = all.minByOrNull { it.length }!! public operator fun get(type: DubinsPath.TYPE): DubinsPath? = all.find { it.type == type } - public val rlr: DubinsPath? get () { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) - if (centers.length > turningRadius * 4) return null + public val rlr: DubinsPath? + get() { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val centers = Line2D(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 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(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) - } + 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 = 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, 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) + } - private val lrl: DubinsPath? get () { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) - if (centers.length > turningRadius * 4) return null + private val lrl: DubinsPath? + get() { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val centers = Line2D(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 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(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) - } + 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 = 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, 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) + } - public val rsr: DubinsPath get () { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftOuterTangent(c1, c2) - 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) - } + public val rsr: DubinsPath + get() { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val l = leftOuterTangent(c1, c2) + 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) + } public val lsl: DubinsPath - get () { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightOuterTangent(c1, c2) - 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) - } + get() { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val l = rightOuterTangent(c1, c2) + 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) + } - public val rsl: DubinsPath? get () { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + public val rsl: DubinsPath? + get() { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val l = rightInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null - 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) - } + 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) + } - public val lsr: DubinsPath? get () { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + public val lsr: DubinsPath? + get() { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val l = leftInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null - 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) - } + 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) + } } private enum class SIDE { @@ -140,7 +145,10 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { a.center.y - a.radius * sin(centers.theta) ) } - return Line2D(p1, Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y))) + return Line2D( + p1, + Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y)) + ) } private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) From 7de157ce24da9346f323c3226ce93d0355d996bf Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 14:21:12 +0200 Subject: [PATCH 06/11] Re-introduce line/straight segment, rename components to start/end --- .../trajectory/dubins/DubinsPathFactory.kt | 124 +++++++++--------- .../kscience/kmath/trajectory/segments/Arc.kt | 12 +- .../kmath/trajectory/segments/Line.kt | 23 ---- .../kmath/trajectory/segments/Straight.kt | 18 +++ .../space/kscience/kmath/trajectory/Math.kt | 13 +- .../kmath/trajectory/dubins/DubinsTests.kt | 20 ++- .../kmath/trajectory/segments/LineTests.kt | 15 +-- 7 files changed, 106 insertions(+), 119 deletions(-) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.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 91287b952..04c639576 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 @@ -6,21 +6,18 @@ package space.kscience.kmath.trajectory.dubins 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.Arc -import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.segments.* import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Pose2D -import space.kscience.kmath.trajectory.segments.length -import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.PI import kotlin.math.acos import kotlin.math.cos import kotlin.math.sin public class DubinsPathFactory( - private val base: Pose2D, - private val direction: Pose2D, + private val start: Pose2D, + private val end: Pose2D, private val turningRadius: Double ) { @@ -30,9 +27,9 @@ public class DubinsPathFactory( public val rlr: DubinsPath? get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val centers = Straight(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) @@ -45,17 +42,17 @@ public class DubinsPathFactory( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, base, p1, Arc.Direction.RIGHT) + 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, direction, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) return DubinsPath(a1, a2, a3) } private val lrl: DubinsPath? get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val centers = Straight(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) @@ -68,54 +65,54 @@ public class DubinsPathFactory( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, base, p1, Arc.Direction.LEFT) + 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, direction, Arc.Direction.LEFT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) return DubinsPath(a1, a2, a3) } public val rsr: DubinsPath get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftOuterTangent(c1, c2) - 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) + 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) + return DubinsPath(a1, s, a3) } public val lsl: DubinsPath get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightOuterTangent(c1, c2) - 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) + 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) + return DubinsPath(a1, s, a3) } public val rsl: DubinsPath? get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val s = rightInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - 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) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + return DubinsPath(a1, s, a3) } public val lsr: DubinsPath? get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val s = leftInnerTangent(c1, c2) + if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null - 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) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + return DubinsPath(a1, s, a3) } } @@ -133,8 +130,8 @@ private fun Pose2D.getTangentCircles(radius: Double): Pair { private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) private fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) -private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { - val centers = Line2D(a.center, b.center) +private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { + val centers = Straight(a.center, b.center) val p1 = when (side) { SIDE.LEFT -> Vector2D( a.center.x - a.radius * cos(centers.theta), @@ -145,29 +142,28 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { a.center.y - a.radius * sin(centers.theta) ) } - return Line2D( + return Straight( p1, - Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y)) + Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) ) } private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) -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 = 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) - val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) - Line2D(p1, p2) - } else { - null - } +private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? { + val centers = Straight(base.center, direction.center) + if (centers.length < base.radius * 2) return null + 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) + val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) + return Straight(p1, p2) } + +internal fun theta(theta: 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 index b8a81a070..b5e091db7 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,8 +1,8 @@ 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.dubins.theta import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Pose2D import kotlin.math.PI @@ -14,11 +14,11 @@ public class Arc( internal val direction: Direction ) : Circle(center, center.distanceTo(a)), Segment { - private val l1 = Line2D(center, a) - private val l2 = Line2D(center, b) + private val s1 = Straight(center, a) + private val s2 = Straight(center, b) - internal val pose1 = calculatePose(a, l1.theta) - internal val pose2 = calculatePose(b, l2.theta) + internal val pose1 = calculatePose(a, s1.theta) + internal val pose2 = calculatePose(b, s2.theta) private val angle = calculateAngle() override val length: Double = calculateLength() @@ -26,7 +26,7 @@ public class Arc( LEFT, RIGHT } - private fun calculateAngle() = theta(if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta) + private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta) private fun calculateLength(): Double { val proportion = angle / (2 * PI) 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 deleted file mode 100644 index 0e23b27f1..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt +++ /dev/null @@ -1,23 +0,0 @@ -package space.kscience.kmath.trajectory.segments - -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Line2D -import space.kscience.kmath.operations.DoubleField.pow -import kotlin.math.PI -import kotlin.math.atan2 -import kotlin.math.sqrt - -public data class LineSegment( - internal val line: Line2D -) : Segment { - override val length: Double - get() = line.length -} - -internal val Line2D.theta: Double - get() = theta(atan2(direction.x - base.x, direction.y - base.y)) - -internal val Line2D.length: Double - get() = base.distanceTo(direction) - -internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) 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 new file mode 100644 index 000000000..444025d83 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt @@ -0,0 +1,18 @@ +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/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt index f52bb56f2..92b2f1df9 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,9 +1,8 @@ package space.kscience.kmath.trajectory -import space.kscience.kmath.geometry.Line2D 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.theta import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin @@ -15,13 +14,13 @@ 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 Line2D.inverse() = Line2D(direction, base) -fun Line2D.shift(shift: Int, width: Double): Line2D { +fun Straight.inverse() = Straight(end, start) +fun Straight.shift(shift: Int, width: Double): Straight { val dX = width * sin(inverse().theta) val dY = width * sin(theta) - return Line2D( - Vector2D(base.x - dX * shift, base.y - dY * shift), - Vector2D(direction.x - dX * shift, direction.y - dY * shift) + return Straight( + 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 583e7a4e0..efe35e5d8 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 @@ -6,15 +6,13 @@ package space.kscience.kmath.trajectory.dubins 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.Arc -import space.kscience.kmath.trajectory.segments.LineSegment import space.kscience.kmath.trajectory.equalFloat import space.kscience.kmath.trajectory.equalsFloat import space.kscience.kmath.trajectory.inverse +import space.kscience.kmath.trajectory.segments.Straight import space.kscience.kmath.trajectory.segments.components.Pose2D -import space.kscience.kmath.trajectory.segments.theta import space.kscience.kmath.trajectory.shift import kotlin.test.Test import kotlin.test.assertNotNull @@ -25,11 +23,11 @@ class DubinsTests { @Test fun dubinsTest() { - val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) - val lineP1 = line.shift(1, 10.0).inverse() + val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val lineP1 = straight.shift(1, 10.0).inverse() - val start = Pose2D(line.direction, line.theta) - val end = Pose2D(lineP1.base, lineP1.theta) + val start = Pose2D(straight.end, straight.theta) + val end = Pose2D(lineP1.start, lineP1.theta) val radius = 2.0 val dubins = DubinsPathFactory(start, end, radius) @@ -58,10 +56,10 @@ class DubinsTests { val b = path.b as Arc assertTrue(path.a.pose2.equalsFloat(b.pose1)) assertTrue(path.c.pose1.equalsFloat(b.pose2)) - } else if (path.b is LineSegment) { - val b = (path.b as LineSegment).line - assertTrue(path.a.pose2.equalsFloat(Pose2D(b.base, b.theta))) - assertTrue(path.c.pose1.equalsFloat(Pose2D(b.direction, b.theta))) + } else if (path.b is Straight) { + val b = path.b as Straight + assertTrue(path.a.pose2.equalsFloat(Pose2D(b.start, b.theta))) + assertTrue(path.c.pose1.equalsFloat(Pose2D(b.end, b.theta))) } } } 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 30f5ef6d8..e8184e178 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 @@ -1,7 +1,6 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.math.pow @@ -13,21 +12,21 @@ class LineTests { @Test fun lineTest() { - val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) - assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), line.length) - assertEquals(45.0, line.theta.radiansToDegrees()) + val straight = Straight(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 = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) assertEquals(0.0, north.theta.radiansToDegrees()) - val east = Line2D(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) assertEquals(90.0, east.theta.radiansToDegrees()) - val south = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) assertEquals(180.0, south.theta.radiansToDegrees()) - val west = Line2D(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) assertEquals(270.0, west.theta.radiansToDegrees()) } } From 3260c3d17146226f465aa98a5fc6f8a7cb4262cd Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 14:39:43 +0200 Subject: [PATCH 07/11] Pose2D facrtory function --- .../space/kscience/kmath/trajectory/segments/Arc.kt | 12 +++++++----- .../kmath/trajectory/segments/components/Pose2D.kt | 6 +++--- .../kscience/kmath/trajectory/dubins/DubinsTests.kt | 10 +++++----- 3 files changed, 15 insertions(+), 13 deletions(-) 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 b5e091db7..fe101f359 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 @@ -34,9 +34,11 @@ public class Arc( } private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = - if (direction == Direction.LEFT) { - Pose2D(vector.x, vector.y, theta(theta - PI / 2)) - } else { - Pose2D(vector.x, vector.y, theta(theta + PI / 2)) - } + Pose2D.of( + vector, + when (direction) { + Direction.LEFT -> theta(theta - PI / 2) + Direction.RIGHT -> theta(theta + PI / 2) + } + ) } 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 d00dfbd96..c49da3187 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 @@ -1,13 +1,13 @@ package space.kscience.kmath.trajectory.segments.components import space.kscience.kmath.geometry.Vector2D -import kotlin.math.cos -import kotlin.math.sin 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) + 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/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt index efe35e5d8..025aab2b7 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 @@ -7,10 +7,10 @@ package space.kscience.kmath.trajectory.dubins 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.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.shift @@ -26,8 +26,8 @@ class DubinsTests { val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) val lineP1 = straight.shift(1, 10.0).inverse() - val start = Pose2D(straight.end, straight.theta) - val end = Pose2D(lineP1.start, lineP1.theta) + val start = Pose2D.of(straight.end, straight.theta) + val end = Pose2D.of(lineP1.start, lineP1.theta) val radius = 2.0 val dubins = DubinsPathFactory(start, end, radius) @@ -58,8 +58,8 @@ class DubinsTests { assertTrue(path.c.pose1.equalsFloat(b.pose2)) } else if (path.b is Straight) { val b = path.b as Straight - assertTrue(path.a.pose2.equalsFloat(Pose2D(b.start, b.theta))) - assertTrue(path.c.pose1.equalsFloat(Pose2D(b.end, b.theta))) + assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta))) + assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta))) } } } From 8faa312424f37bf6aa4b6db3c33e166709c346d2 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 14:56:21 +0200 Subject: [PATCH 08/11] Dubins factory functions --- .../kmath/trajectory/dubins/DubinsPath.kt | 106 +++++++++++ .../trajectory/dubins/DubinsPathFactory.kt | 169 ------------------ .../trajectory/dubins/TrajectoryFunctions.kt | 67 +++++++ .../kmath/trajectory/dubins/DubinsTests.kt | 4 +- 4 files changed, 175 insertions(+), 171 deletions(-) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt 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 005d7fd60..ad67fa11d 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,8 +5,16 @@ package space.kscience.kmath.trajectory.dubins +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 kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin public class DubinsPath( public val a: Arc, @@ -27,4 +35,102 @@ public class DubinsPath( 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 shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath = + all(start, end, turningRadius).minByOrNull { 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) + 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 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(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) + 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) + 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 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(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) + return DubinsPath(a1, a2, a3) + } + + public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): 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) + return DubinsPath(a1, s, a3) + } + + public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): 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) + return DubinsPath(a1, s, a3) + } + + public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + 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) + return DubinsPath(a1, s, a3) + } + + public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + 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) + return DubinsPath(a1, s, a3) + } + } } 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 deleted file mode 100644 index 04c639576..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt +++ /dev/null @@ -1,169 +0,0 @@ -/* - * 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.dubins - -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.* -import space.kscience.kmath.trajectory.segments.components.Circle -import space.kscience.kmath.trajectory.segments.components.Pose2D -import kotlin.math.PI -import kotlin.math.acos -import kotlin.math.cos -import kotlin.math.sin - -public class DubinsPathFactory( - private val start: Pose2D, - private val end: Pose2D, - private val turningRadius: Double -) { - - public val all: List get() = listOfNotNull(rlr, lrl, rsr, lsl, rsl, lsr) - public val shortest: DubinsPath get() = all.minByOrNull { it.length }!! - public operator fun get(type: DubinsPath.TYPE): DubinsPath? = all.find { it.type == type } - - public val rlr: DubinsPath? - get() { - val c1 = start.getRightCircle(turningRadius) - val c2 = end.getRightCircle(turningRadius) - val centers = Straight(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 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(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) - return DubinsPath(a1, a2, a3) - } - - private val lrl: DubinsPath? - get() { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - val centers = Straight(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 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(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) - return DubinsPath(a1, a2, a3) - } - - public val rsr: DubinsPath - get() { - 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) - return DubinsPath(a1, s, a3) - } - - public val lsl: DubinsPath - get() { - 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) - return DubinsPath(a1, s, a3) - } - - public val rsl: DubinsPath? - get() { - val c1 = start.getRightCircle(turningRadius) - val c2 = end.getLeftCircle(turningRadius) - 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) - return DubinsPath(a1, s, a3) - } - - public val lsr: DubinsPath? - get() { - val c1 = start.getLeftCircle(turningRadius) - val c2 = end.getRightCircle(turningRadius) - 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) - return DubinsPath(a1, s, a3) - } -} - -private enum class SIDE { - LEFT, RIGHT -} - -private fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first -private fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second -private 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) -} - -private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) -private 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) - val p1 = when (side) { - SIDE.LEFT -> Vector2D( - a.center.x - a.radius * cos(centers.theta), - a.center.y + a.radius * sin(centers.theta) - ) - SIDE.RIGHT -> Vector2D( - a.center.x + a.radius * cos(centers.theta), - a.center.y - a.radius * sin(centers.theta) - ) - } - return Straight( - p1, - Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) - ) -} - -private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) -private 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) - if (centers.length < base.radius * 2) return null - 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) - val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) - return Straight(p1, p2) -} - -internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) 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 new file mode 100644 index 000000000..547cb99fc --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt @@ -0,0 +1,67 @@ +/* + * 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.dubins + +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 kotlin.math.PI +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin + +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 { + 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) +} + +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) + val p1 = when (side) { + SIDE.LEFT -> Vector2D( + a.center.x - a.radius * cos(centers.theta), + a.center.y + a.radius * sin(centers.theta) + ) + SIDE.RIGHT -> Vector2D( + a.center.x + a.radius * cos(centers.theta), + a.center.y - a.radius * sin(centers.theta) + ) + } + return Straight( + 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) + if (centers.length < base.radius * 2) return null + 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) + val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) + return Straight(p1, p2) +} + +internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) 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 025aab2b7..a5288e40d 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 @@ -29,7 +29,7 @@ class DubinsTests { val start = Pose2D.of(straight.end, straight.theta) val end = Pose2D.of(lineP1.start, lineP1.theta) val radius = 2.0 - val dubins = DubinsPathFactory(start, end, radius) + val dubins = DubinsPath.all(start, end, radius) val absoluteDistance = start.distanceTo(end) println("Absolute distance: $absoluteDistance") @@ -43,7 +43,7 @@ class DubinsTests { ) expectedLengths.forEach { - val path = dubins[it.key] + val path = dubins.find { p -> p.type === it.key } assertNotNull(path, "Path ${it.key} not found") println("${it.key}: ${path.length}") assertTrue(it.value.equalFloat(path.length)) From 86fce7ec68d10dda664ddd936652eacd387c83b8 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 15:47:05 +0200 Subject: [PATCH 09/11] 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()) } } From 429eefa3f7750c4d8e6dfccabb1206c77988ec38 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 15:48:08 +0200 Subject: [PATCH 10/11] Arc direction as computed property --- .../kscience/kmath/trajectory/segments/Arc.kt | 35 ++++++++++--------- 1 file changed, 19 insertions(+), 16 deletions(-) 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 966e1e4e2..1c02dd952 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 @@ -35,22 +35,25 @@ public data class Arc( 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 = 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 + 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 + } } - } } From f2cbbeba2017af8dd54795e9c4b0ef82682b7715 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Sun, 17 Jul 2022 15:56:24 +0200 Subject: [PATCH 11/11] Author details --- docs/templates/README-TEMPLATE.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/templates/README-TEMPLATE.md b/docs/templates/README-TEMPLATE.md index 4ffa9e75f..c466324b8 100644 --- a/docs/templates/README-TEMPLATE.md +++ b/docs/templates/README-TEMPLATE.md @@ -103,3 +103,10 @@ The project requires a lot of additional work. The most important thing we need required the most. Feel free to create feature requests. We are also welcome to code contributions, especially in issues marked with [waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label. + +## Author +Erik Schouten + +Github: ESchouten + +Email: erik-schouten@hotmail.nl