teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Dubins path

Browse Source
This commit is contained in:
Erik Schouten 2022-07-15 18:13:50 +02:00
parent a1267d84ac
commit 32769d6906
14 changed files with 502 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
kmath-trajectory/README.md Normal file
View File

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

15
kmath-trajectory/build.gradle.kts Normal file
View File

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

30
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt Normal file
View File

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

171
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt Normal file
View File

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

43
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt Normal file
View File

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

23
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt Normal file
View File

@ -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)

5
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt Normal file
View File

@ -0,0 +1,5 @@
package space.kscience.kmath.trajectory.segments
public interface Segment {
public val length: Double
}

11
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt Normal file
View File

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

18
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt Normal file
View File

@ -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)"
}
}

27
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt Normal file
View File

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

68
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt Normal file
View File

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

24
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt Normal file
View File

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

33
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt Normal file
View File

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

3
settings.gradle.kts
View File

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