Merge pull request #495 from ESchouten/trajectory

Trajectory: Dubins path

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

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")
+}
+```

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
+}

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt

@@ -0,0 +1,136 @@
+/*
+ * 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.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,
+    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
+    }
+
+    public companion object {
+        public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List<DubinsPath> =
+            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.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)
+        }
+
+        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.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)
+        }
+
+        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.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)
+        }
+
+        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.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)
+        }
+
+        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.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)
+        }
+
+        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.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)
+        }
+    }
+}

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<Circle, Circle> {
+    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)

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt

@@ -0,0 +1,59 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.dubins.theta
+import space.kscience.kmath.trajectory.segments.components.Circle
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.PI
+
+public data class Arc(
+    public val circle: Circle,
+    public val start: Pose2D,
+    public val end: Pose2D
+) : Segment {
+
+    internal companion object {
+        fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
+            val s1 = Straight(center, start)
+            val s2 = Straight(center, end)
+            val pose1 = calculatePose(start, s1.theta, direction)
+            val pose2 = calculatePose(end, s2.theta, direction)
+            return Arc(Circle(center, s1.length), pose1, pose2)
+        }
+
+        private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
+            Pose2D.of(
+                vector,
+                when (direction) {
+                    Direction.LEFT -> theta(theta - PI / 2)
+                    Direction.RIGHT -> theta(theta + PI / 2)
+                }
+            )
+    }
+
+    internal enum class Direction {
+        LEFT, RIGHT
+    }
+
+    override val length: Double
+        get() {
+            val angle: Double =
+                theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
+            val proportion = angle / (2 * PI)
+            return circle.circumference * proportion
+        }
+
+    internal val direction: Direction
+        get() = if (start.y < circle.center.y) {
+            if (start.theta > PI) Direction.RIGHT else Direction.LEFT
+        } else if (start.y > circle.center.y) {
+            if (start.theta < PI) Direction.RIGHT else Direction.LEFT
+        } else {
+            if (start.theta == 0.0) {
+                if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
+            } else {
+                if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
+            }
+        }
+
+}

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
+}

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))
+}

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
+}

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt

@@ -0,0 +1,13 @@
+package space.kscience.kmath.trajectory.segments.components
+
+import space.kscience.kmath.geometry.Vector2D
+
+public data class Pose2D(
+    override val x: Double,
+    override val y: Double,
+    public val theta: Double
+) : Vector2D {
+    internal companion object {
+        internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
+    }
+}

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt

@@ -0,0 +1,26 @@
+package space.kscience.kmath.trajectory
+
+import space.kscience.kmath.geometry.Vector2D
+import space.kscience.kmath.trajectory.segments.Straight
+import space.kscience.kmath.trajectory.segments.components.Pose2D
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.sin
+
+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 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 Straight(
+        Vector2D(start.x - dX * shift, start.y - dY * shift),
+        Vector2D(end.x - dX * shift, end.y - dY * shift)
+    )
+}

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt

@@ -0,0 +1,66 @@
+/*
+ * 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.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
+import kotlin.test.Test
+import kotlin.test.assertNotNull
+import kotlin.test.assertTrue
+
+
+class DubinsTests {
+
+    @Test
+    fun dubinsTest() {
+        val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
+        val lineP1 = straight.shift(1, 10.0).inverse()
+
+        val start = Pose2D.of(straight.end, straight.theta)
+        val end = Pose2D.of(lineP1.start, lineP1.theta)
+        val radius = 2.0
+        val dubins = DubinsPath.all(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.find { p -> p.type === 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.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.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.end.equalsFloat(Pose2D.of(b.start, b.theta)))
+                assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
+            }
+        }
+    }
+}

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt

@@ -0,0 +1,19 @@
+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 circle = Circle(Vector2D(0.0, 0.0), 2.0)
+        val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
+        assertEquals(circle.circumference / 4, arc.length, 1.0)
+        assertEquals(0.0, arc.start.theta.radiansToDegrees())
+        assertEquals(90.0, arc.end.theta.radiansToDegrees())
+    }
+}

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt

@@ -0,0 +1,32 @@
+package space.kscience.kmath.trajectory.segments
+
+import space.kscience.kmath.geometry.Euclidean2DSpace
+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 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 = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
+        assertEquals(0.0, north.theta.radiansToDegrees())
+        val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
+        assertEquals(90.0, east.theta.radiansToDegrees())
+        val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
+        assertEquals(180.0, south.theta.radiansToDegrees())
+        val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
+        assertEquals(270.0, west.theta.radiansToDegrees())
+    }
+}

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)
+    }
+}

settings.gradle.kts

@@ -44,6 +44,7 @@ include(
     ":kmath-jupyter",
     ":kmath-symja",
     ":kmath-jafama",
+    ":kmath-trajectory",
     ":examples",
     ":benchmarks",
-)
+)