Dubins path

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

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

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

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)

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

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

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

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

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

settings.gradle.kts

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