kscience/maps-kt
8
0
Fork 0
Code Issues Pull Requests 3 Packages Projects Releases 1 Wiki Activity

Add trajectory-kt

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-06 10:03:38 +03:00
parent dcee205b7c
commit 3ab875d87c
32 changed files with 1641 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
README.md
View File

@ -15,7 +15,7 @@ This repository is a work-in-progress implementation of Map-with-markers compone
### [maps-kt-compose](maps-kt-compose) ### [maps-kt-compose](maps-kt-compose)
> Compose-multiplaform implementation for web-mercator tiled maps > Compose-multiplaform implementation for web-mercator tiled maps
> >
> **Maturity**: DEVELOPMENT > **Maturity**: EXPERIMENTAL
> >
> **Features:** > **Features:**
> - [osm](maps-kt-compose/#) : OpenStreetMap tile provider. > - [osm](maps-kt-compose/#) : OpenStreetMap tile provider.
@ -47,17 +47,22 @@ This repository is a work-in-progress implementation of Map-with-markers compone
> >
> **Maturity**: EXPERIMENTAL > **Maturity**: EXPERIMENTAL
### [maps](demo/maps) ### [trajectory-kt](trajectory-kt)
> Path and trajectory optimization (to be moved to a separate project)
>
> **Maturity**: DEPRECATED
### [demo/maps](demo/maps)
> >
> >
> **Maturity**: EXPERIMENTAL > **Maturity**: EXPERIMENTAL
### [polygon-editor](demo/polygon-editor) ### [demo/polygon-editor](demo/polygon-editor)
> >
> >
> **Maturity**: EXPERIMENTAL > **Maturity**: EXPERIMENTAL
### [scheme](demo/scheme) ### [demo/scheme](demo/scheme)
> >
> >
> **Maturity**: EXPERIMENTAL > **Maturity**: EXPERIMENTAL

5
build.gradle.kts
View File

@ -6,14 +6,15 @@ plugins {
id("space.kscience.gradle.project") id("space.kscience.gradle.project")
} }
val kmathVersion: String by extra("0.3.1-dev-10") val kmathVersion: String by extra("0.3.1-dev-11")
allprojects { allprojects {
group = "center.sciprog" group = "center.sciprog"
version = "0.2.2-dev-9" version = "0.2.2-dev-10"
repositories { repositories {
mavenLocal() mavenLocal()
maven("https://repo.kotlin.link")
maven("https://maven.pkg.jetbrains.space/spc/p/sci/dev") maven("https://maven.pkg.jetbrains.space/spc/p/sci/dev")
} }
} }

16
demo/maps/build.gradle.kts
View File

@ -26,13 +26,15 @@ kotlin {
} }
} }
compose.desktop { compose {
application { desktop {
mainClass = "MainKt" application {
nativeDistributions { mainClass = "MainKt"
targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) nativeDistributions {
packageName = "maps-compose-demo" targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb)
packageVersion = "1.0.0" packageName = "maps-compose-demo"
packageVersion = "1.0.0"
}
} }
} }
} }

16
demo/polygon-editor/build.gradle.kts
View File

@ -26,13 +26,15 @@ kotlin {
} }
} }
compose.desktop { compose{
application { desktop {
mainClass = "MainKt" application {
nativeDistributions { mainClass = "MainKt"
targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) nativeDistributions {
packageName = "polygon-editor-demo" targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb)
packageVersion = "1.0.0" packageName = "polygon-editor-demo"
packageVersion = "1.0.0"
}
} }
} }
} }

17
demo/scheme/build.gradle.kts
View File

@ -1,4 +1,3 @@
import org.jetbrains.compose.compose
import org.jetbrains.compose.desktop.application.dsl.TargetFormat import org.jetbrains.compose.desktop.application.dsl.TargetFormat
plugins { plugins {
@ -27,13 +26,15 @@ kotlin {
} }
} }
compose.desktop { compose{
application { desktop {
mainClass = "MainKt" application {
nativeDistributions { mainClass = "MainKt"
targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) nativeDistributions {
packageName = "scheme-compose-demo" targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb)
packageVersion = "1.0.0" packageName = "scheme-compose-demo"
packageVersion = "1.0.0"
}
} }
} }
} }

4
gradle.properties
View File

@ -1,10 +1,10 @@
kotlin.code.style=official kotlin.code.style=official
compose.version=1.3.0 compose.version=1.4.0-rc01
agp.version=7.3.1 agp.version=7.3.1
android.useAndroidX=true android.useAndroidX=true
org.jetbrains.compose.experimental.jscanvas.enabled=true org.jetbrains.compose.experimental.jscanvas.enabled=true
org.gradle.jvmargs=-Xmx4096m org.gradle.jvmargs=-Xmx4096m
toolsVersion=0.13.4-kotlin-1.8.0 toolsVersion=0.14.3-kotlin-1.8.10

6
maps-kt-compose/README.md
View File

@ -7,7 +7,7 @@ The core interfaces of KMath.
## Artifact: ## Artifact:
The Maven coordinates of this project are `center.sciprog:maps-kt-compose:0.2.1-dev-2`. The Maven coordinates of this project are `center.sciprog:maps-kt-compose:0.2.2-dev-10`.
**Gradle Groovy:** **Gradle Groovy:**
```groovy ```groovy
@ -17,7 +17,7 @@ repositories {
} }
dependencies { dependencies {
implementation 'center.sciprog:maps-kt-compose:0.2.1-dev-2' implementation 'center.sciprog:maps-kt-compose:0.2.2-dev-10'
} }
``` ```
**Gradle Kotlin DSL:** **Gradle Kotlin DSL:**
@ -28,6 +28,6 @@ repositories {
} }
dependencies { dependencies {
implementation("center.sciprog:maps-kt-compose:0.2.1-dev-2") implementation("center.sciprog:maps-kt-compose:0.2.2-dev-10")
} }
``` ```

4
maps-kt-compose/build.gradle.kts
View File

@ -27,10 +27,10 @@ kotlin {
implementation(compose.desktop.currentOs) implementation(compose.desktop.currentOs)
implementation(spclibs.kotlinx.coroutines.test) implementation(spclibs.kotlinx.coroutines.test)
implementation("ch.qos.logback:logback-classic:1.2.11") implementation(spclibs.logback.classic)
implementation(kotlin("test-junit5")) implementation(kotlin("test-junit5"))
implementation("org.junit.jupiter:junit-jupiter:5.8.2") implementation("org.junit.jupiter:junit-jupiter:${spclibs.versions.junit.get()}")
} }
} }
} }

6
maps-kt-core/README.md
View File

@ -9,7 +9,7 @@ The core interfaces of KMath.
## Artifact: ## Artifact:
The Maven coordinates of this project are `center.sciprog:maps-kt-core:0.2.1-dev-2`. The Maven coordinates of this project are `center.sciprog:maps-kt-core:0.2.2-dev-10`.
**Gradle Groovy:** **Gradle Groovy:**
```groovy ```groovy
@ -19,7 +19,7 @@ repositories {
} }
dependencies { dependencies {
implementation 'center.sciprog:maps-kt-core:0.2.1-dev-2' implementation 'center.sciprog:maps-kt-core:0.2.2-dev-10'
} }
``` ```
**Gradle Kotlin DSL:** **Gradle Kotlin DSL:**
@ -30,6 +30,6 @@ repositories {
} }
dependencies { dependencies {
implementation("center.sciprog:maps-kt-core:0.2.1-dev-2") implementation("center.sciprog:maps-kt-core:0.2.2-dev-10")
} }
``` ```

4
maps-kt-core/build.gradle.kts
View File

@ -6,10 +6,12 @@ plugins {
val kmathVersion: String by rootProject.extra val kmathVersion: String by rootProject.extra
kscience{ kscience{
jvm()
js()
useSerialization() useSerialization()
dependencies{ dependencies{
api("space.kscience:kmath-trajectory:$kmathVersion") api(projects.trajectoryKt)
} }
} }

6
maps-kt-features/README.md
View File

@ -6,7 +6,7 @@
## Artifact: ## Artifact:
The Maven coordinates of this project are `center.sciprog:maps-kt-features:0.2.1-dev-2`. The Maven coordinates of this project are `center.sciprog:maps-kt-features:0.2.2-dev-10`.
**Gradle Groovy:** **Gradle Groovy:**
```groovy ```groovy
@ -16,7 +16,7 @@ repositories {
} }
dependencies { dependencies {
implementation 'center.sciprog:maps-kt-features:0.2.1-dev-2' implementation 'center.sciprog:maps-kt-features:0.2.2-dev-10'
} }
``` ```
**Gradle Kotlin DSL:** **Gradle Kotlin DSL:**
@ -27,6 +27,6 @@ repositories {
} }
dependencies { dependencies {
implementation("center.sciprog:maps-kt-features:0.2.1-dev-2") implementation("center.sciprog:maps-kt-features:0.2.2-dev-10")
} }
``` ```

24
maps-kt-features/build.gradle.kts
View File

@ -6,18 +6,9 @@ plugins {
val kmathVersion: String by rootProject.extra val kmathVersion: String by rootProject.extra
kotlin {
sourceSets {
commonMain {
dependencies {
api("space.kscience:kmath-trajectory:$kmathVersion")
api(compose.foundation)
}
}
}
}
kscience{ kscience{
jvm()
js()
useSerialization{ useSerialization{
json() json()
} }
@ -25,4 +16,15 @@ kscience{
useSerialization(sourceSet = space.kscience.gradle.DependencySourceSet.TEST){ useSerialization(sourceSet = space.kscience.gradle.DependencySourceSet.TEST){
protobuf() protobuf()
} }
}
kotlin {
sourceSets {
commonMain {
dependencies {
api(projects.trajectoryKt)
api(compose.foundation)
}
}
}
} }

126
maps-kt-features/src/jvmMain/kotlin/center/sciprog/maps/compose/clickGestures.kt
View File

@ -2,8 +2,8 @@ package center.sciprog.maps.compose
import androidx.compose.foundation.gestures.GestureCancellationException import androidx.compose.foundation.gestures.GestureCancellationException
import androidx.compose.foundation.gestures.PressGestureScope import androidx.compose.foundation.gestures.PressGestureScope
import androidx.compose.foundation.gestures.awaitEachGesture
import androidx.compose.foundation.gestures.detectTapGestures import androidx.compose.foundation.gestures.detectTapGestures
import androidx.compose.foundation.gestures.forEachGesture
import androidx.compose.ui.geometry.Offset import androidx.compose.ui.geometry.Offset
import androidx.compose.ui.input.pointer.* import androidx.compose.ui.input.pointer.*
import androidx.compose.ui.platform.ViewConfiguration import androidx.compose.ui.platform.ViewConfiguration
@ -16,13 +16,13 @@ import kotlinx.coroutines.sync.Mutex
* Clone of tap gestures for mouse * Clone of tap gestures for mouse
*/ */
private val NoPressGesture: suspend PressGestureScope.(event: PointerEvent) -> Unit = { } private val NoPressGesture: suspend PressGestureScope.(event: PointerEvent) -> Unit = { }
internal fun PointerEvent.consume() = changes.forEach { it.consume() } internal fun PointerEvent.consume() = changes.forEach { it.consume() }
internal val PointerEvent.firstChange get() = changes.first() internal val PointerEvent.firstChange get() = changes.first()
public val PointerEvent.position: Offset get() = firstChange.position public val PointerEvent.position: Offset get() = firstChange.position
/** /**
@ -61,76 +61,74 @@ public suspend fun PointerInputScope.detectClicks(
// cancel/up events as we're only require down events // cancel/up events as we're only require down events
val pressScope = PressGestureScopeImpl(this@detectClicks) val pressScope = PressGestureScopeImpl(this@detectClicks)
forEachGesture { awaitEachGesture {
awaitPointerEventScope { val down = awaitFirstDownEvent()
val down = awaitFirstDownEvent() down.consume()
down.consume()
pressScope.reset() pressScope.reset()
if (onPress !== NoPressGesture) launch { if (onPress !== NoPressGesture) launch {
pressScope.onPress(down) pressScope.onPress(down)
}
val longPressTimeout = onLongClick?.let {
viewConfiguration.longPressTimeoutMillis
} ?: (Long.MAX_VALUE / 2)
var upOrCancel: PointerEvent? = null
try {
// wait for first tap up or long press
upOrCancel = withTimeout(longPressTimeout) {
waitForUpOrCancellation()
} }
val longPressTimeout = onLongClick?.let { if (upOrCancel == null) {
viewConfiguration.longPressTimeoutMillis pressScope.cancel() // tap-up was canceled
} ?: (Long.MAX_VALUE / 2) } else {
var upOrCancel: PointerEvent? = null upOrCancel.consume()
try {
// wait for first tap up or long press
upOrCancel = withTimeout(longPressTimeout) {
waitForUpOrCancellation()
}
if (upOrCancel == null) {
pressScope.cancel() // tap-up was canceled
} else {
upOrCancel.consume()
pressScope.release()
}
} catch (_: PointerEventTimeoutCancellationException) {
onLongClick?.invoke(this, down)
consumeUntilUp()
pressScope.release() pressScope.release()
} }
} catch (_: PointerEventTimeoutCancellationException) {
onLongClick?.invoke(this, down)
consumeUntilUp()
pressScope.release()
}
if (upOrCancel != null) { if (upOrCancel != null) {
// tap was successful. // tap was successful.
if (onDoubleClick == null) { if (onDoubleClick == null) {
onClick?.invoke(this, down) // no need to check for double-tap. onClick?.invoke(this, down) // no need to check for double-tap.
} else {
// check for second tap
val secondDown = awaitSecondDown(upOrCancel.firstChange)
if (secondDown == null) {
onClick?.invoke(this, down) // no valid second tap started
} else { } else {
// check for second tap // Second tap down detected
val secondDown = awaitSecondDown(upOrCancel.firstChange) pressScope.reset()
if (onPress !== NoPressGesture) {
launch { pressScope.onPress(secondDown) }
}
if (secondDown == null) { try {
onClick?.invoke(this, down) // no valid second tap started // Might have a long second press as the second tap
} else { withTimeout(longPressTimeout) {
// Second tap down detected val secondUp = waitForUpOrCancellation()
pressScope.reset() if (secondUp != null) {
if (onPress !== NoPressGesture) { secondUp.consume()
launch { pressScope.onPress(secondDown) } pressScope.release()
} onDoubleClick(secondDown)
} else {
try { pressScope.cancel()
// Might have a long second press as the second tap onClick?.invoke(this, down)
withTimeout(longPressTimeout) {
val secondUp = waitForUpOrCancellation()
if (secondUp != null) {
secondUp.consume()
pressScope.release()
onDoubleClick(secondDown)
} else {
pressScope.cancel()
onClick?.invoke(this, down)
}
} }
} catch (e: PointerEventTimeoutCancellationException) {
// The first tap was valid, but the second tap is a long press.
// notify for the first tap
onClick?.invoke(this, down)
// notify for the long press
onLongClick?.invoke(this, secondDown)
consumeUntilUp()
pressScope.release()
} }
} catch (e: PointerEventTimeoutCancellationException) {
// The first tap was valid, but the second tap is a long press.
// notify for the first tap
onClick?.invoke(this, down)
// notify for the long press
onLongClick?.invoke(this, secondDown)
consumeUntilUp()
pressScope.release()
} }
} }
} }

6
maps-kt-geojson/README.md
View File

@ -6,7 +6,7 @@
## Artifact: ## Artifact:
The Maven coordinates of this project are `center.sciprog:maps-kt-geojson:0.2.1-dev-2`. The Maven coordinates of this project are `center.sciprog:maps-kt-geojson:0.2.2-dev-10`.
**Gradle Groovy:** **Gradle Groovy:**
```groovy ```groovy
@ -16,7 +16,7 @@ repositories {
} }
dependencies { dependencies {
implementation 'center.sciprog:maps-kt-geojson:0.2.1-dev-2' implementation 'center.sciprog:maps-kt-geojson:0.2.2-dev-10'
} }
``` ```
**Gradle Kotlin DSL:** **Gradle Kotlin DSL:**
@ -27,6 +27,6 @@ repositories {
} }
dependencies { dependencies {
implementation("center.sciprog:maps-kt-geojson:0.2.1-dev-2") implementation("center.sciprog:maps-kt-geojson:0.2.2-dev-10")
} }
``` ```

18
maps-kt-geojson/build.gradle.kts
View File

@ -3,20 +3,16 @@ plugins {
`maven-publish` `maven-publish`
} }
kotlin {
sourceSets {
commonMain {
dependencies {
api(projects.mapsKtCore)
api(projects.mapsKtFeatures)
api(spclibs.kotlinx.serialization.json)
}
}
}
}
kscience{ kscience{
jvm()
js()
useSerialization { useSerialization {
json() json()
} }
dependencies{
api(projects.mapsKtCore)
api(projects.mapsKtFeatures)
api(spclibs.kotlinx.serialization.json)
}
} }

6
maps-kt-scheme/README.md
View File

@ -6,7 +6,7 @@
## Artifact: ## Artifact:
The Maven coordinates of this project are `center.sciprog:maps-kt-scheme:0.2.1-dev-2`. The Maven coordinates of this project are `center.sciprog:maps-kt-scheme:0.2.2-dev-10`.
**Gradle Groovy:** **Gradle Groovy:**
```groovy ```groovy
@ -16,7 +16,7 @@ repositories {
} }
dependencies { dependencies {
implementation 'center.sciprog:maps-kt-scheme:0.2.1-dev-2' implementation 'center.sciprog:maps-kt-scheme:0.2.2-dev-10'
} }
``` ```
**Gradle Kotlin DSL:** **Gradle Kotlin DSL:**
@ -27,6 +27,6 @@ repositories {
} }
dependencies { dependencies {
implementation("center.sciprog:maps-kt-scheme:0.2.1-dev-2") implementation("center.sciprog:maps-kt-scheme:0.2.2-dev-10")
} }
``` ```

15
maps-kt-scheme/build.gradle.kts
View File

@ -1,6 +1,3 @@
import space.kscience.gradle.KScienceVersions.JVM_TARGET
plugins { plugins {
kotlin("multiplatform") kotlin("multiplatform")
id("org.jetbrains.compose") id("org.jetbrains.compose")
@ -8,11 +5,7 @@ plugins {
} }
kotlin { kotlin {
jvm { jvm()
compilations.all {
kotlinOptions.jvmTarget = JVM_TARGET.toString()
}
}
sourceSets { sourceSets {
commonMain { commonMain {
dependencies { dependencies {
@ -30,6 +23,6 @@ kotlin {
} }
} }
java { //java {
targetCompatibility = JVM_TARGET // targetCompatibility = JVM_TARGET
} //}

1
settings.gradle.kts
View File

@ -46,6 +46,7 @@ dependencyResolutionManagement {
include( include(
":trajectory-kt",
":maps-kt-core", ":maps-kt-core",
":maps-kt-geojson", ":maps-kt-geojson",
":maps-kt-features", ":maps-kt-features",

41
trajectory-kt/README.md Normal file
View File

@ -0,0 +1,41 @@
# Trajectory-kt
## Artifact:
The Maven coordinates of this project are `space.kscience:trajectory-kt:0.2.2-dev-10`.
**Gradle Groovy:**
```groovy
repositories {
maven { url 'https://repo.kotlin.link' }
mavenCentral()
}
dependencies {
implementation 'space.kscience:trajectory-kt:0.2.2-dev-10'
}
```
**Gradle Kotlin DSL:**
```kotlin
repositories {
maven("https://repo.kotlin.link")
mavenCentral()
}
dependencies {
implementation("space.kscience:trajectory-kt:0.2.2-dev-10")
}
```
## Authors
### Erik Schouten
https://github.com/ESchouten
Email: erik-schouten@hotmail.nl
### Artem Degtyarev
https://github.com/artdegt

26
trajectory-kt/build.gradle.kts Normal file
View File

@ -0,0 +1,26 @@
plugins {
id("space.kscience.gradle.mpp")
`maven-publish`
}
group = "space.kscience"
val kmathVersion: String by rootProject.extra
kscience{
jvm()
js()
native()
useContextReceivers()
useSerialization()
dependencies {
api("space.kscience:kmath-geometry:$kmathVersion")
}
}
readme {
description = "Path and trajectory optimization (to be moved to a separate project)"
maturity = space.kscience.gradle.Maturity.DEPRECATED
propertyByTemplate("artifact", rootProject.file("docs/templates/ARTIFACT-TEMPLATE.md"))
}

16
trajectory-kt/docs/README-TEMPLATE.md Normal file
View File

@ -0,0 +1,16 @@
# Trajectory-kt
${features}
${artifact}
## Authors
### Erik Schouten
https://github.com/ESchouten
Email: erik-schouten@hotmail.nl
### Artem Degtyarev
https://github.com/artdegt

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

@ -0,0 +1,258 @@
/*
* Copyright 2018-2022 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
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.trajectory.Trajectory2D.*
import kotlin.math.acos
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
val dX = radius * cos(bearing)
val dY = radius * sin(bearing)
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
}
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): StraightTrajectory2D =
with(Euclidean2DSpace) {
val centers = StraightTrajectory2D(from.center, to.center)
val p1 = when (direction) {
L -> vector(
from.center.x - from.radius * cos(centers.bearing),
from.center.y + from.radius * sin(centers.bearing)
)
R -> vector(
from.center.x + from.radius * cos(centers.bearing),
from.center.y - from.radius * sin(centers.bearing)
)
}
return StraightTrajectory2D(
p1,
vector(p1.x + (centers.end.x - centers.begin.x), p1.y + (centers.end.y - centers.begin.y))
)
}
private fun innerTangent(
from: Circle2D,
to: Circle2D,
direction: Direction,
): StraightTrajectory2D? =
with(Euclidean2DSpace) {
val centers = StraightTrajectory2D(from.center, to.center)
if (centers.length < from.radius * 2) return null
val angle = when (direction) {
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
R -> centers.bearing - acos(from.radius * 2 / centers.length).radians
}.normalized()
val dX = from.radius * sin(angle)
val dY = from.radius * cos(angle)
val p1 = vector(from.center.x + dX, from.center.y + dY)
val p2 = vector(to.center.x - dX, to.center.y - dY)
return StraightTrajectory2D(p1, p2)
}
@Suppress("DuplicatedCode")
public object DubinsPath {
public data class Type(
public val first: Direction,
public val second: Trajectory2D.Type,
public val third: Direction,
) {
public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
override fun toString(): String = "${first}${second}${third}"
public companion object {
public val RLR: Type = Type(R, L, R)
public val LRL: Type = Type(L, R, L)
public val RSR: Type = Type(R, S, R)
public val LSL: Type = Type(L, S, L)
public val RSL: Type = Type(R, S, L)
public val LSR: Type = Type(L, S, R)
}
}
/**
* Return Dubins trajectory type or null if trajectory is not a Dubins path
*/
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type? {
if (trajectory2D.segments.size != 3) return null
val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
val b = trajectory2D.segments[1]
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
return Type(
a.direction,
if (b is CircleTrajectory2D) b.direction else S,
c.direction
)
}
public fun all(
start: DubinsPose2D,
end: DubinsPose2D,
turningRadius: Double,
): List<CompositeTrajectory2D> = 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: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
all(start, end, turningRadius).minBy { it.length }
public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
with(Euclidean2DSpace) {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val centers = StraightTrajectory2D(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null
val firstVariant = run {
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
var dX = turningRadius * sin(theta)
var dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
val secondVariant = run {
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
var dX = turningRadius * sin(theta)
var dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, R)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
CompositeTrajectory2D(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
with(Euclidean2DSpace) {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val centers = StraightTrajectory2D(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null
val firstVariant = run {
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
var dX = turningRadius * sin(theta)
var dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
val secondVariant = run {
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
var dX = turningRadius * sin(theta)
var dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory2D.of(c1.center, start, p1, L)
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
CompositeTrajectory2D(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = outerTangent(c1, c2, L)
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = outerTangent(c1, c2, R)
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = innerTangent(c1, c2, R)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L)
return CompositeTrajectory2D(a1, s, a3)
}
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = innerTangent(c1, c2, L)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
return CompositeTrajectory2D(a1, s, a3)
}
}
public typealias PathTypes = List<Type>
public fun interface MaxCurvature {
public fun compute(startPoint: PhaseVector2D): Double
}
public fun DubinsPath.shortest(
start: PhaseVector2D,
end: PhaseVector2D,
maxCurvature: MaxCurvature,
): CompositeTrajectory2D = shortest(start, end, maxCurvature.compute(start))

75
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt Normal file
View File

@ -0,0 +1,75 @@
/*
* Copyright 2018-2022 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.
*/
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
package space.kscience.kmath.trajectory
import kotlinx.serialization.KSerializer
import kotlinx.serialization.SerialName
import kotlinx.serialization.Serializable
import kotlinx.serialization.UseSerializers
import kotlinx.serialization.descriptors.SerialDescriptor
import kotlinx.serialization.encoding.Decoder
import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.*
import kotlin.math.atan2
/**
* Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
*/
@Serializable(DubinsPose2DSerializer::class)
public interface DubinsPose2D : DoubleVector2D {
public val coordinates: DoubleVector2D
public val bearing: Angle
public companion object {
public fun bearingToVector(bearing: Angle): Vector2D<Double> =
Euclidean2DSpace.vector(cos(bearing), sin(bearing))
public fun vectorToBearing(vector2D: DoubleVector2D): Angle {
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
return atan2(vector2D.y, vector2D.x).radians
}
public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
DubinsPose2D(point, vectorToBearing(direction))
}
}
@Serializable
public class PhaseVector2D(
override val coordinates: DoubleVector2D,
public val velocity: DoubleVector2D,
) : DubinsPose2D, DoubleVector2D by coordinates {
override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
}
@Serializable
@SerialName("DubinsPose2D")
private class DubinsPose2DImpl(
override val coordinates: DoubleVector2D,
override val bearing: Angle,
) : DubinsPose2D, DoubleVector2D by coordinates {
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
}
public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
private val proxySerializer = DubinsPose2DImpl.serializer()
override val descriptor: SerialDescriptor
get() = proxySerializer.descriptor
override fun deserialize(decoder: Decoder): DubinsPose2D {
return decoder.decodeSerializableValue(proxySerializer)
}
override fun serialize(encoder: Encoder, value: DubinsPose2D) {
val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing)
encoder.encodeSerializableValue(proxySerializer, pose)
}
}
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)

614
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt Normal file
View File

@ -0,0 +1,614 @@
/*
* Copyright 2018-2023 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
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Euclidean2DSpace.minus
import space.kscience.kmath.geometry.Euclidean2DSpace.norm
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
import space.kscience.kmath.geometry.Euclidean2DSpace.times
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.*
internal data class Tangent(
val startCircle: Circle2D,
val endCircle: Circle2D,
val startObstacle: Obstacle,
val endObstacle: Obstacle,
val lineSegment: LineSegment2D,
val startDirection: Trajectory2D.Direction,
val endDirection: Trajectory2D.Direction = startDirection,
) : LineSegment2D by lineSegment
private class TangentPath(val tangents: List<Tangent>) {
fun last() = tangents.last()
}
private fun TangentPath(vararg tangents: Tangent) = TangentPath(listOf(*tangents))
/**
* Create inner and outer tangents between two circles.
* This method returns a map of segments using [DubinsPath] connection type notation.
*/
internal fun Circle2D.tangentsToCircle(
other: Circle2D,
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
//return empty map for concentric circles
if (center.equalsVector(other.center)) return emptyMap()
// A line connecting centers
val line = LineSegment(center, other.center)
// Distance between centers
val distance = line.begin.distanceTo(line.end)
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
var angle2: Double
val routes = mapOf(
DubinsPath.Type.RSR to Pair(radius, other.radius),
DubinsPath.Type.RSL to Pair(radius, -other.radius),
DubinsPath.Type.LSR to Pair(-radius, other.radius),
DubinsPath.Type.LSL to Pair(-radius, -other.radius)
)
return buildMap {
for ((route, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
val r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (distance * distance >= r * r) {
val l = sqrt(distance * distance - r * r)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = vector(-cos(angle2), sin(angle2))
put(
route,
LineSegment(
center + w * r1,
other.center + w * r2
)
)
} else {
throw Exception("Circles should not intersect")
}
}
}
}
private fun dubinsTangentsToCircles(
firstCircle: Circle2D,
secondCircle: Circle2D,
firstObstacle: Obstacle,
secondObstacle: Obstacle,
): Map<DubinsPath.Type, Tangent> = with(Euclidean2DSpace) {
val line = LineSegment(firstCircle.center, secondCircle.center)
val distance = line.begin.distanceTo(line.end)
val angle1 = atan2(
secondCircle.center.x - firstCircle.center.x,
secondCircle.center.y - firstCircle.center.y
)
var r: Double
var angle2: Double
val routes = mapOf(
DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
)
return buildMap {
for ((route: DubinsPath.Type, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (distance * distance >= r * r) {
val l = sqrt(distance * distance - r * r)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = vector(-cos(angle2), sin(angle2))
put(
route,
Tangent(
startCircle = Circle2D(firstCircle.center, firstCircle.radius),
endCircle = secondCircle,
startObstacle = firstObstacle,
endObstacle = secondObstacle,
lineSegment = LineSegment(
firstCircle.center + w * r1,
secondCircle.center + w * r2
),
startDirection = route.first,
endDirection = route.third
)
)
} else {
throw Exception("Circles should not intersect")
}
}
}
}
internal class Obstacle(
public val circles: List<Circle2D>,
) {
internal val tangents: List<Tangent> = boundaryTangents().first
public val boundaryRoute: DubinsPath.Type = boundaryTangents().second
public val center: Vector2D<Double> = vector(
circles.sumOf { it.center.x } / circles.size,
circles.sumOf { it.center.y } / circles.size
)
private fun boundaryTangents(): Pair<List<Tangent>, DubinsPath.Type> {
// outer tangents for a polygon circles can be either lsl or rsr
fun Circle2D.dubinsTangentsToCircles(
other: Circle2D,
): Map<DubinsPath.Type, Tangent> = with(Euclidean2DSpace) {
val line = LineSegment(center, other.center)
val d = line.begin.distanceTo(line.end)
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
var r: Double
var angle2: Double
val routes = mapOf(
DubinsPath.Type.RSR to Pair(radius, other.radius),
DubinsPath.Type.LSL to Pair(-radius, -other.radius)
)
return buildMap {
for ((routeType, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (d * d >= r * r) {
val l = (d * d - r * r).pow(0.5)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = vector(-cos(angle2), sin(angle2))
put(
routeType, Tangent(
Circle2D(center, radius),
other,
this@Obstacle,
this@Obstacle,
LineSegment(
center + w * r1,
other.center + w * r2
),
startDirection = routeType.first,
endDirection = routeType.third
)
)
} else {
throw Exception("Circles should not intersect")
}
}
}
}
val firstCircles = circles
val secondCircles = circles.slice(1..circles.lastIndex) +
circles[0]
val lslTangents = firstCircles.zip(secondCircles)
{ a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!! }
val rsrTangents = firstCircles.zip(secondCircles)
{ a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.RSR]!! }
val center = vector(
circles.sumOf { it.center.x } / circles.size,
circles.sumOf { it.center.y } / circles.size
)
val lslToCenter = lslTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
lslTangents.sumOf { it.lineSegment.end.distanceTo(center) }
val rsrToCenter = rsrTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
rsrTangents.sumOf { it.lineSegment.end.distanceTo(center) }
return if (rsrToCenter >= lslToCenter) {
Pair(rsrTangents, DubinsPath.Type.RSR)
} else {
Pair(lslTangents, DubinsPath.Type.LSL)
}
}
internal fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent {
if (direction == boundaryRoute.first) {
for (i in circles.indices) {
if (circles[i] == circle) {
return tangents[i]
}
}
} else {
for (i in circles.indices) {
if (circles[i] == circle) {
if (i > 0) {
return Tangent(
circles[i],
circles[i - 1],
this,
this,
LineSegment(
tangents[i - 1].lineSegment.end,
tangents[i - 1].lineSegment.begin
),
direction
)
} else {
return Tangent(
circles[0],
circles.last(),
this,
this,
LineSegment(
tangents.last().lineSegment.end,
tangents.last().lineSegment.begin
),
direction
)
}
}
}
}
error("next tangent not found")
}
override fun equals(other: Any?): Boolean {
if (other == null || other !is Obstacle) return false
return circles == other.circles
}
override fun hashCode(): Int {
return circles.hashCode()
}
}
internal fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
return v1.x * v2.y - v1.y * v2.x
}
return if (crossProduct(other.begin - begin, other.end - begin).sign ==
crossProduct(other.begin - end, other.end - end).sign
) {
false
} else {
crossProduct(begin - other.begin, end - other.begin).sign != crossProduct(
begin - other.end,
end - other.end
).sign
}
}
private fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
val a = (begin.x - end.x).pow(2.0) + (begin.y - end.y).pow(2.0)
val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
(begin.y - end.y) * (end.y - circle.center.y))
val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
circle.radius.pow(2.0)
val d = b.pow(2.0) - 4 * a * c
if (d < 1e-6) {
return false
} else {
val t1 = (-b - d.pow(0.5)) * 0.5 / a
val t2 = (-b + d.pow(0.5)) * 0.5 / a
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
return true
}
}
return false
}
private fun Tangent.intersectObstacle(obstacle: Obstacle): Boolean {
for (tangent in obstacle.tangents) {
if (lineSegment.intersectSegment(tangent.lineSegment)) {
return true
}
}
for (circle in obstacle.circles) {
if (lineSegment.intersectCircle(circle)) {
return true
}
}
return false
}
private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, Tangent> = buildMap {
for (circle1 in first.circles) {
for (circle2 in second.circles) {
for (tangent in dubinsTangentsToCircles(circle1, circle2, first, second)) {
if (!(tangent.value.intersectObstacle(first))
and !(tangent.value.intersectObstacle(second))
) {
put(
tangent.key,
tangent.value
)
}
}
}
}
}
private fun arcLength(
circle: Circle2D,
point1: DoubleVector2D,
point2: DoubleVector2D,
direction: Trajectory2D.Direction,
): Double {
val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
var angle = 0.0
when (direction) {
Trajectory2D.L -> {
angle = if (phi2 >= phi1) {
phi2 - phi1
} else {
2 * PI + phi2 - phi1
}
}
Trajectory2D.R -> {
angle = if (phi2 >= phi1) {
2 * PI - (phi2 - phi1)
} else {
-(phi2 - phi1)
}
}
}
return circle.radius * angle
}
private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
return Pair(
r * vector(v.y / norm(v), -v.x / norm(v)),
r * vector(-v.y / norm(v), v.x / norm(v))
)
}
private fun constructTangentCircles(
point: DoubleVector2D,
direction: DoubleVector2D,
r: Double,
): Map<Trajectory2D.Type, Circle2D> {
val center1 = point + normalVectors(direction, r).first
val center2 = point + normalVectors(direction, r).second
val p1 = center1 - point
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
mapOf(
Trajectory2D.L to Circle2D(center1, r),
Trajectory2D.R to Circle2D(center2, r)
)
} else {
mapOf(
Trajectory2D.L to Circle2D(center2, r),
Trajectory2D.R to Circle2D(center1, r)
)
}
}
private fun sortedObstacles(
currentObstacle: Obstacle,
obstacles: List<Obstacle>,
): List<Obstacle> {
return obstacles.sortedBy { norm(it.center - currentObstacle.center) }
}
private fun tangentsAlongTheObstacle(
initialCircle: Circle2D,
direction: Trajectory2D.Direction,
finalCircle: Circle2D,
obstacle: Obstacle,
): List<Tangent> {
val dubinsTangents = mutableListOf<Tangent>()
var tangent = obstacle.nextTangent(initialCircle, direction)
dubinsTangents.add(tangent)
while (tangent.endCircle != finalCircle) {
tangent = obstacle.nextTangent(tangent.endCircle, direction)
dubinsTangents.add(tangent)
}
return dubinsTangents
}
private fun allFinished(
paths: List<TangentPath>,
finalObstacle: Obstacle,
): Boolean {
for (path in paths) {
if (path.last().endObstacle != finalObstacle) {
return false
}
}
return true
}
private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
buildList {
tangents.zipWithNext().forEach { (left, right) ->
add(left.lineSegment.toTrajectory())
add(
CircleTrajectory2D.of(
right.startCircle.center,
left.lineSegment.end,
right.lineSegment.begin,
right.startDirection
)
)
}
add(tangents.last().lineSegment.toTrajectory())
}
)
internal fun findAllPaths(
start: DubinsPose2D,
startingRadius: Double,
finish: DubinsPose2D,
finalRadius: Double,
obstacles: List<Obstacle>,
): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing))
val initialCircles = constructTangentCircles(
start,
start.direction(),
startingRadius
)
val finalCircles = constructTangentCircles(
finish,
finish.direction(),
finalRadius
)
val trajectories = mutableListOf<CompositeTrajectory2D>()
for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
val finalCircle = finalCircles[j]!!
val finalObstacle = Obstacle(listOf(finalCircle))
var currentPaths: List<TangentPath> = listOf(
TangentPath(
Tangent(
initialCircles[i]!!,
initialCircles[i]!!,
Obstacle(listOf(initialCircles[i]!!)),
Obstacle(listOf(initialCircles[i]!!)),
LineSegment(start, start),
i
)
)
)
while (!allFinished(currentPaths, finalObstacle)) {
val newPaths = mutableListOf<TangentPath>()
for (tangentPath: TangentPath in currentPaths) {
val currentCircle = tangentPath.last().endCircle
val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val currentObstacle = tangentPath.last().endObstacle
var nextObstacle: Obstacle? = null
if (currentObstacle != finalObstacle) {
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
currentDirection,
Trajectory2D.S,
j
)]
for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
if (tangentToFinal!!.intersectObstacle(obstacle)) {
nextObstacle = obstacle
break
}
}
if (nextObstacle == null) {
nextObstacle = finalObstacle
}
val nextTangents: Map<DubinsPath.Type, Tangent> = outerTangents(currentObstacle, nextObstacle)
.filter { (key, tangent) ->
obstacles.none { obstacle -> tangent.intersectObstacle(obstacle) } &&
key.first == currentDirection &&
(nextObstacle != finalObstacle || key.third == j)
}
var tangentsAlong: List<Tangent>
for (tangent in nextTangents.values) {
if (tangent.startCircle == tangentPath.last().endCircle) {
val lengthMaxPossible = arcLength(
tangent.startCircle,
tangentPath.last().lineSegment.end,
tangent.startObstacle.nextTangent(
tangent.startCircle,
currentDirection
).lineSegment.begin,
currentDirection
)
val lengthCalculated = arcLength(
tangent.startCircle,
tangentPath.last().lineSegment.end,
tangent.lineSegment.begin,
currentDirection
)
tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
tangentsAlongTheObstacle(
currentCircle,
currentDirection,
tangent.startCircle,
currentObstacle
)
} else {
emptyList()
}
} else {
tangentsAlong = tangentsAlongTheObstacle(
currentCircle,
currentDirection,
tangent.startCircle,
currentObstacle
)
}
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
}
} else {
newPaths.add(tangentPath)
}
}
currentPaths = newPaths
}
trajectories += currentPaths.map { tangentPath ->
val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
val end = finalCircles[j]!!
TangentPath(
tangentPath.tangents +
Tangent(
end,
end,
Obstacle(end),
Obstacle(end),
LineSegment(finish, finish),
startDirection = lastDirection,
endDirection = j
)
)
}.map { it.toTrajectory() }
}
}
return trajectories
}
public object Obstacles {
public fun allPathsAvoiding(
start: DubinsPose2D,
finish: DubinsPose2D,
trajectoryRadius: Double,
obstaclePolygons: List<Polygon<Double>>,
): List<CompositeTrajectory2D> {
val obstacles: List<Obstacle> = obstaclePolygons.map { polygon ->
Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) })
}
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles)
}
}

131
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt Normal file
View File

@ -0,0 +1,131 @@
/*
* Copyright 2018-2022 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.
*/
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
package space.kscience.kmath.trajectory
import kotlinx.serialization.SerialName
import kotlinx.serialization.Serializable
import kotlinx.serialization.UseSerializers
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import kotlin.math.atan2
@Serializable
public sealed interface Trajectory2D {
public val length: Double
public sealed interface Type
public sealed interface Direction: Type
public object R : Direction {
override fun toString(): String = "R"
}
public object S : Type {
override fun toString(): String = "L"
}
public object L : Direction {
override fun toString(): String = "L"
}
}
/**
* Straight path segment. The order of start and end defines the direction
*/
@Serializable
@SerialName("straight")
public data class StraightTrajectory2D(
override val begin: DoubleVector2D,
override val end: DoubleVector2D,
) : Trajectory2D, LineSegment2D {
override val length: Double get() = begin.distanceTo(end)
public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized()
}
/**
* An arc segment
*/
@Serializable
@SerialName("arc")
public data class CircleTrajectory2D(
public val circle: Circle2D,
public val start: DubinsPose2D,
public val end: DubinsPose2D,
) : Trajectory2D {
/**
* Arc length in radians
*/
val arcLength: Angle
get() = if (direction == Trajectory2D.L) {
start.bearing - end.bearing
} else {
end.bearing - start.bearing
}.normalized()
override val length: Double by lazy {
circle.radius * arcLength.radians
}
public val direction: Trajectory2D.Direction by lazy {
if (start.y < circle.center.y) {
if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
} else if (start.y > circle.center.y) {
if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
} else {
if (start.bearing == Angle.zero) {
if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
} else {
if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
}
}
}
public companion object {
public fun of(
center: DoubleVector2D,
start: DoubleVector2D,
end: DoubleVector2D,
direction: Trajectory2D.Direction,
): CircleTrajectory2D {
fun calculatePose(
vector: DoubleVector2D,
theta: Angle,
direction: Trajectory2D.Direction,
): DubinsPose2D = DubinsPose2D(
vector,
when (direction) {
Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
}
)
val s1 = StraightTrajectory2D(center, start)
val s2 = StraightTrajectory2D(center, end)
val pose1 = calculatePose(start, s1.bearing, direction)
val pose2 = calculatePose(end, s2.bearing, direction)
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
if (trajectory.direction != direction) error("Trajectory direction mismatch")
return trajectory
}
}
}
@Serializable
@SerialName("composite")
public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
override val length: Double get() = segments.sumOf { it.length }
}
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
CompositeTrajectory2D(segments.toList())

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

@ -0,0 +1,61 @@
/*
* Copyright 2018-2023 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
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.equalsFloat
import kotlin.test.Test
import kotlin.test.assertNotNull
import kotlin.test.assertTrue
class DubinsTests {
@Test
fun dubinsTest() = with(Euclidean2DSpace){
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = DubinsPose2D(straight.end, straight.bearing)
val end = DubinsPose2D(lineP1.begin, lineP1.bearing)
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 -> DubinsPath.trajectoryTypeOf(p) == it.key }
assertNotNull(path, "Path ${it.key} not found")
println("${it.key}: ${path.length}")
assertTrue(it.value.equalsFloat(path.length))
val a = path.segments[0] as CircleTrajectory2D
val b = path.segments[1]
val c = path.segments[2] as CircleTrajectory2D
assertTrue(start.equalsFloat(a.start))
assertTrue(end.equalsFloat(c.end))
// Not working, theta double precision inaccuracy
if (b is CircleTrajectory2D) {
assertTrue(a.end.equalsFloat(b.start))
assertTrue(c.start.equalsFloat(b.end))
} else if (b is StraightTrajectory2D) {
assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing)))
assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
}
}
}
}

88
trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt Normal file
View File

@ -0,0 +1,88 @@
/*
* Copyright 2018-2023 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
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import kotlin.test.Test
import kotlin.test.assertEquals
class ObstacleTest {
@Test
fun firstPath() {
val startPoint = vector(-5.0, -1.0)
val startDirection = vector(1.0, 1.0)
val startRadius = 0.5
val finalPoint = vector(20.0, 4.0)
val finalDirection = vector(1.0, -1.0)
val finalRadius = 0.5
val obstacles = listOf(
Obstacle(
listOf(
Circle2D(vector(7.0, 1.0), 5.0)
)
)
)
val outputTangents = findAllPaths(
DubinsPose2D.of(startPoint, startDirection),
startRadius,
DubinsPose2D.of(finalPoint, finalDirection),
finalRadius,
obstacles
)
val length = outputTangents.minOf { it.length }
assertEquals(27.2113183, length, 1e-6)
}
@Test
fun secondPath() {
val startPoint = vector(-5.0, -1.0)
val startDirection = vector(1.0, 1.0)
val startRadius = 0.5
val finalPoint = vector(20.0, 4.0)
val finalDirection = vector(1.0, -1.0)
val finalRadius = 0.5
val obstacles = listOf(
Obstacle(
listOf(
Circle2D(vector(1.0, 6.5), 0.5),
Circle2D(vector(2.0, 1.0), 0.5),
Circle2D(vector(6.0, 0.0), 0.5),
Circle2D(vector(5.0, 5.0), 0.5)
)
), Obstacle(
listOf(
Circle2D(vector(10.0, 1.0), 0.5),
Circle2D(vector(16.0, 0.0), 0.5),
Circle2D(vector(14.0, 6.0), 0.5),
Circle2D(vector(9.0, 4.0), 0.5)
)
)
)
val paths = findAllPaths(
DubinsPose2D.of(startPoint, startDirection),
startRadius,
DubinsPose2D.of(finalPoint, finalDirection),
finalRadius,
obstacles
)
val length = paths.minOf { it.length }
assertEquals(28.9678224, length, 1e-6)
}
@Test
fun equalObstacles() {
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
assertEquals(circle1, circle2)
val obstacle1 = Obstacle(listOf(circle1))
val obstacle2 = Obstacle(listOf(circle2))
assertEquals(obstacle1, obstacle2)
}
}

63
trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt Normal file
View File

@ -0,0 +1,63 @@
/*
* Copyright 2018-2023 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
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.LineSegment
import space.kscience.kmath.geometry.equalsLine
import kotlin.test.Test
import kotlin.test.assertEquals
import kotlin.test.assertTrue
class TangentTest {
@Test
fun tangents() {
val c1 = Circle2D(vector(0.0, 0.0), 1.0)
val c2 = Circle2D(vector(4.0, 0.0), 1.0)
val routes = listOf(
DubinsPath.Type.RSR,
DubinsPath.Type.RSL,
DubinsPath.Type.LSR,
DubinsPath.Type.LSL
)
val segments = listOf(
LineSegment(
begin = vector(0.0, 1.0),
end = vector(4.0, 1.0)
),
LineSegment(
begin = vector(0.5, 0.8660254),
end = vector(3.5, -0.8660254)
),
LineSegment(
begin = vector(0.5, -0.8660254),
end = vector(3.5, 0.8660254)
),
LineSegment(
begin = vector(0.0, -1.0),
end = vector(4.0, -1.0)
)
)
val tangentMap = c1.tangentsToCircle(c2)
val tangentMapKeys = tangentMap.keys.toList()
val tangentMapValues = tangentMap.values.toList()
assertEquals(routes, tangentMapKeys)
for (i in segments.indices) {
assertTrue(segments[i].equalsLine(Euclidean2DSpace, tangentMapValues[i]))
}
}
@Test
fun concentric(){
val c1 = Circle2D(vector(0.0, 0.0), 10.0)
val c2 = Circle2D(vector(0.0, 0.0), 1.0)
assertEquals(emptyMap(), c1.tangentsToCircle(c2))
}
}

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

@ -0,0 +1,27 @@
/*
* Copyright 2018-2022 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
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.equalsFloat
import space.kscience.kmath.geometry.radians
import space.kscience.kmath.geometry.sin
fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians)
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)
fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) {
val dX = width * sin(inverse().bearing)
val dY = width * sin(bearing)
return StraightTrajectory2D(
vector(begin.x - dX * shift, begin.y - dY * shift),
vector(end.x - dX * shift, end.y - dY * shift)
)
}

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

@ -0,0 +1,32 @@
/*
* Copyright 2018-2022 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
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.geometry.degrees
import space.kscience.kmath.trajectory.CircleTrajectory2D
import space.kscience.kmath.trajectory.Trajectory2D
import kotlin.test.Test
import kotlin.test.assertEquals
class ArcTests {
@Test
fun arcTest() = with(Euclidean2DSpace){
val circle = Circle2D(vector(0.0, 0.0), 2.0)
val arc = CircleTrajectory2D.of(
circle.center,
vector(-2.0, 0.0),
vector(0.0, 2.0),
Trajectory2D.R
)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.start.bearing.degrees)
assertEquals(90.0, arc.end.bearing.degrees)
}
}

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

@ -0,0 +1,24 @@
/*
* Copyright 2018-2022 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
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.circumference
import kotlin.test.Test
import kotlin.test.assertEquals
class CircleTests {
@Test
fun arcTest() {
val center = Euclidean2DSpace.vector(0.0, 0.0)
val radius = 2.0
val expectedCircumference = 12.56637
val circle = Circle2D(center, radius)
assertEquals(expectedCircumference, circle.circumference, 1e-4)
}
}

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

@ -0,0 +1,37 @@
/*
* Copyright 2018-2022 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
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.degrees
import space.kscience.kmath.trajectory.StraightTrajectory2D
import kotlin.math.pow
import kotlin.math.sqrt
import kotlin.test.Test
import kotlin.test.assertEquals
class LineTests {
@Test
fun lineTest() = with(Euclidean2DSpace){
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
assertEquals(45.0, straight.bearing.degrees)
}
@Test
fun lineAngleTest() = with(Euclidean2DSpace){
//val zero = Vector2D(0.0, 0.0)
val north = StraightTrajectory2D(zero, vector(0.0, 2.0))
assertEquals(0.0, north.bearing.degrees)
val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
assertEquals(90.0, east.bearing.degrees)
val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
assertEquals(180.0, south.bearing.degrees)
val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
assertEquals(270.0, west.bearing.degrees)
}
}