kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

Merge pull request #495 from ESchouten/trajectory

Browse Source 
Trajectory: Dubins path
This commit is contained in:
TatianaMy 2022-07-24 11:29:24 +03:00 committed by GitHub
commit ad30c68eba
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 531 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
docs/templates/README-TEMPLATE.md vendored
View File

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

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
}

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

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

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

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

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

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

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
}

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

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

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
}

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

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

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

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

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

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

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

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

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

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

23
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt Normal file
View File

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

1
settings.gradle.kts
View File

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