Add trajectory-kt
This commit is contained in:
parent
dcee205b7c
commit
3ab875d87c
13
README.md
13
README.md
@ -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
|
||||||
|
|||||||
@ -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")
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -26,7 +26,8 @@ kotlin {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
compose.desktop {
|
compose {
|
||||||
|
desktop {
|
||||||
application {
|
application {
|
||||||
mainClass = "MainKt"
|
mainClass = "MainKt"
|
||||||
nativeDistributions {
|
nativeDistributions {
|
||||||
@ -36,3 +37,4 @@ compose.desktop {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|||||||
@ -26,7 +26,8 @@ kotlin {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
compose.desktop {
|
compose{
|
||||||
|
desktop {
|
||||||
application {
|
application {
|
||||||
mainClass = "MainKt"
|
mainClass = "MainKt"
|
||||||
nativeDistributions {
|
nativeDistributions {
|
||||||
@ -36,3 +37,4 @@ compose.desktop {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|||||||
@ -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,7 +26,8 @@ kotlin {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
compose.desktop {
|
compose{
|
||||||
|
desktop {
|
||||||
application {
|
application {
|
||||||
mainClass = "MainKt"
|
mainClass = "MainKt"
|
||||||
nativeDistributions {
|
nativeDistributions {
|
||||||
@ -37,3 +37,4 @@ compose.desktop {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|||||||
@ -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
|
||||||
@ -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")
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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()}")
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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")
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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,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")
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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()
|
||||||
}
|
}
|
||||||
@ -26,3 +17,14 @@ kscience{
|
|||||||
protobuf()
|
protobuf()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
kotlin {
|
||||||
|
sourceSets {
|
||||||
|
commonMain {
|
||||||
|
dependencies {
|
||||||
|
api(projects.trajectoryKt)
|
||||||
|
api(compose.foundation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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
|
||||||
@ -61,8 +61,7 @@ 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()
|
||||||
|
|
||||||
@ -136,7 +135,6 @@ public suspend fun PointerInputScope.detectClicks(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Consumes all pointer events until nothing is pressed and then returns. This method assumes
|
* Consumes all pointer events until nothing is pressed and then returns. This method assumes
|
||||||
|
|||||||
@ -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")
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@ -3,20 +3,16 @@ plugins {
|
|||||||
`maven-publish`
|
`maven-publish`
|
||||||
}
|
}
|
||||||
|
|
||||||
kotlin {
|
|
||||||
sourceSets {
|
kscience{
|
||||||
commonMain {
|
jvm()
|
||||||
|
js()
|
||||||
|
useSerialization {
|
||||||
|
json()
|
||||||
|
}
|
||||||
dependencies{
|
dependencies{
|
||||||
api(projects.mapsKtCore)
|
api(projects.mapsKtCore)
|
||||||
api(projects.mapsKtFeatures)
|
api(projects.mapsKtFeatures)
|
||||||
api(spclibs.kotlinx.serialization.json)
|
api(spclibs.kotlinx.serialization.json)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
kscience{
|
|
||||||
useSerialization {
|
|
||||||
json()
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -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")
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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
|
||||||
}
|
//}
|
||||||
@ -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
41
trajectory-kt/README.md
Normal 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
26
trajectory-kt/build.gradle.kts
Normal 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
16
trajectory-kt/docs/README-TEMPLATE.md
Normal 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
|
||||||
@ -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))
|
||||||
|
|
||||||
@ -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)
|
||||||
@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -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())
|
||||||
|
|
||||||
@ -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)))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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))
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
)
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user