From 3ab875d87c74b79dafcbc5779000e466151762ba Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Thu, 6 Apr 2023 10:03:38 +0300 Subject: [PATCH] Add trajectory-kt --- README.md | 13 +- build.gradle.kts | 5 +- demo/maps/build.gradle.kts | 16 +- demo/polygon-editor/build.gradle.kts | 16 +- demo/scheme/build.gradle.kts | 17 +- gradle.properties | 4 +- maps-kt-compose/README.md | 6 +- maps-kt-compose/build.gradle.kts | 4 +- maps-kt-core/README.md | 6 +- maps-kt-core/build.gradle.kts | 4 +- maps-kt-features/README.md | 6 +- maps-kt-features/build.gradle.kts | 24 +- .../sciprog/maps/compose/clickGestures.kt | 126 ++-- maps-kt-geojson/README.md | 6 +- maps-kt-geojson/build.gradle.kts | 18 +- maps-kt-scheme/README.md | 6 +- maps-kt-scheme/build.gradle.kts | 15 +- settings.gradle.kts | 1 + trajectory-kt/README.md | 41 ++ trajectory-kt/build.gradle.kts | 26 + trajectory-kt/docs/README-TEMPLATE.md | 16 + .../kscience/kmath/trajectory/DubinsPath.kt | 258 ++++++++ .../kscience/kmath/trajectory/DubinsPose2D.kt | 75 +++ .../kscience/kmath/trajectory/Obstacle.kt | 614 ++++++++++++++++++ .../kscience/kmath/trajectory/Trajectory2D.kt | 131 ++++ .../kscience/kmath/trajectory/DubinsTests.kt | 61 ++ .../kscience/kmath/trajectory/ObstacleTest.kt | 88 +++ .../kscience/kmath/trajectory/TangentTest.kt | 63 ++ .../space/kscience/kmath/trajectory/math.kt | 27 + .../kmath/trajectory/segments/ArcTests.kt | 32 + .../kmath/trajectory/segments/CircleTests.kt | 24 + .../kmath/trajectory/segments/LineTests.kt | 37 ++ 32 files changed, 1641 insertions(+), 145 deletions(-) create mode 100644 trajectory-kt/README.md create mode 100644 trajectory-kt/build.gradle.kts create mode 100644 trajectory-kt/docs/README-TEMPLATE.md create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt diff --git a/README.md b/README.md index bcbf181..89e04af 100644 --- a/README.md +++ b/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) > Compose-multiplaform implementation for web-mercator tiled maps > -> **Maturity**: DEVELOPMENT +> **Maturity**: EXPERIMENTAL > > **Features:** > - [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 -### [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 -### [polygon-editor](demo/polygon-editor) +### [demo/polygon-editor](demo/polygon-editor) > > > **Maturity**: EXPERIMENTAL -### [scheme](demo/scheme) +### [demo/scheme](demo/scheme) > > > **Maturity**: EXPERIMENTAL diff --git a/build.gradle.kts b/build.gradle.kts index 9de57d8..fc6c014 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -6,14 +6,15 @@ plugins { 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 { group = "center.sciprog" - version = "0.2.2-dev-9" + version = "0.2.2-dev-10" repositories { mavenLocal() + maven("https://repo.kotlin.link") maven("https://maven.pkg.jetbrains.space/spc/p/sci/dev") } } diff --git a/demo/maps/build.gradle.kts b/demo/maps/build.gradle.kts index 4df75a0..02c651f 100644 --- a/demo/maps/build.gradle.kts +++ b/demo/maps/build.gradle.kts @@ -26,13 +26,15 @@ kotlin { } } -compose.desktop { - application { - mainClass = "MainKt" - nativeDistributions { - targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) - packageName = "maps-compose-demo" - packageVersion = "1.0.0" +compose { + desktop { + application { + mainClass = "MainKt" + nativeDistributions { + targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) + packageName = "maps-compose-demo" + packageVersion = "1.0.0" + } } } } diff --git a/demo/polygon-editor/build.gradle.kts b/demo/polygon-editor/build.gradle.kts index f140cf5..66e621f 100644 --- a/demo/polygon-editor/build.gradle.kts +++ b/demo/polygon-editor/build.gradle.kts @@ -26,13 +26,15 @@ kotlin { } } -compose.desktop { - application { - mainClass = "MainKt" - nativeDistributions { - targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) - packageName = "polygon-editor-demo" - packageVersion = "1.0.0" +compose{ + desktop { + application { + mainClass = "MainKt" + nativeDistributions { + targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) + packageName = "polygon-editor-demo" + packageVersion = "1.0.0" + } } } } diff --git a/demo/scheme/build.gradle.kts b/demo/scheme/build.gradle.kts index 2e64e6d..e8c8e15 100644 --- a/demo/scheme/build.gradle.kts +++ b/demo/scheme/build.gradle.kts @@ -1,4 +1,3 @@ -import org.jetbrains.compose.compose import org.jetbrains.compose.desktop.application.dsl.TargetFormat plugins { @@ -27,13 +26,15 @@ kotlin { } } -compose.desktop { - application { - mainClass = "MainKt" - nativeDistributions { - targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) - packageName = "scheme-compose-demo" - packageVersion = "1.0.0" +compose{ + desktop { + application { + mainClass = "MainKt" + nativeDistributions { + targetFormats(TargetFormat.Dmg, TargetFormat.Msi, TargetFormat.Deb) + packageName = "scheme-compose-demo" + packageVersion = "1.0.0" + } } } } diff --git a/gradle.properties b/gradle.properties index 794b690..c12acfb 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1,10 +1,10 @@ kotlin.code.style=official -compose.version=1.3.0 +compose.version=1.4.0-rc01 agp.version=7.3.1 android.useAndroidX=true org.jetbrains.compose.experimental.jscanvas.enabled=true org.gradle.jvmargs=-Xmx4096m -toolsVersion=0.13.4-kotlin-1.8.0 \ No newline at end of file +toolsVersion=0.14.3-kotlin-1.8.10 \ No newline at end of file diff --git a/maps-kt-compose/README.md b/maps-kt-compose/README.md index a838973..25aa653 100644 --- a/maps-kt-compose/README.md +++ b/maps-kt-compose/README.md @@ -7,7 +7,7 @@ The core interfaces of KMath. ## 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:** ```groovy @@ -17,7 +17,7 @@ repositories { } 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:** @@ -28,6 +28,6 @@ repositories { } dependencies { - implementation("center.sciprog:maps-kt-compose:0.2.1-dev-2") + implementation("center.sciprog:maps-kt-compose:0.2.2-dev-10") } ``` diff --git a/maps-kt-compose/build.gradle.kts b/maps-kt-compose/build.gradle.kts index 76d5267..0d821a7 100644 --- a/maps-kt-compose/build.gradle.kts +++ b/maps-kt-compose/build.gradle.kts @@ -27,10 +27,10 @@ kotlin { implementation(compose.desktop.currentOs) implementation(spclibs.kotlinx.coroutines.test) - implementation("ch.qos.logback:logback-classic:1.2.11") + implementation(spclibs.logback.classic) implementation(kotlin("test-junit5")) - implementation("org.junit.jupiter:junit-jupiter:5.8.2") + implementation("org.junit.jupiter:junit-jupiter:${spclibs.versions.junit.get()}") } } } diff --git a/maps-kt-core/README.md b/maps-kt-core/README.md index a8e8b7a..9b8cccf 100644 --- a/maps-kt-core/README.md +++ b/maps-kt-core/README.md @@ -9,7 +9,7 @@ The core interfaces of KMath. ## 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:** ```groovy @@ -19,7 +19,7 @@ repositories { } 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:** @@ -30,6 +30,6 @@ repositories { } dependencies { - implementation("center.sciprog:maps-kt-core:0.2.1-dev-2") + implementation("center.sciprog:maps-kt-core:0.2.2-dev-10") } ``` diff --git a/maps-kt-core/build.gradle.kts b/maps-kt-core/build.gradle.kts index 933d68e..a7f9a0e 100644 --- a/maps-kt-core/build.gradle.kts +++ b/maps-kt-core/build.gradle.kts @@ -6,10 +6,12 @@ plugins { val kmathVersion: String by rootProject.extra kscience{ + jvm() + js() useSerialization() dependencies{ - api("space.kscience:kmath-trajectory:$kmathVersion") + api(projects.trajectoryKt) } } diff --git a/maps-kt-features/README.md b/maps-kt-features/README.md index afed054..2553f37 100644 --- a/maps-kt-features/README.md +++ b/maps-kt-features/README.md @@ -6,7 +6,7 @@ ## 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:** ```groovy @@ -16,7 +16,7 @@ repositories { } 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:** @@ -27,6 +27,6 @@ repositories { } dependencies { - implementation("center.sciprog:maps-kt-features:0.2.1-dev-2") + implementation("center.sciprog:maps-kt-features:0.2.2-dev-10") } ``` diff --git a/maps-kt-features/build.gradle.kts b/maps-kt-features/build.gradle.kts index 0ca6d61..d761986 100644 --- a/maps-kt-features/build.gradle.kts +++ b/maps-kt-features/build.gradle.kts @@ -6,18 +6,9 @@ plugins { val kmathVersion: String by rootProject.extra -kotlin { - sourceSets { - commonMain { - dependencies { - api("space.kscience:kmath-trajectory:$kmathVersion") - api(compose.foundation) - } - } - } -} - kscience{ + jvm() + js() useSerialization{ json() } @@ -25,4 +16,15 @@ kscience{ useSerialization(sourceSet = space.kscience.gradle.DependencySourceSet.TEST){ protobuf() } +} + +kotlin { + sourceSets { + commonMain { + dependencies { + api(projects.trajectoryKt) + api(compose.foundation) + } + } + } } \ No newline at end of file diff --git a/maps-kt-features/src/jvmMain/kotlin/center/sciprog/maps/compose/clickGestures.kt b/maps-kt-features/src/jvmMain/kotlin/center/sciprog/maps/compose/clickGestures.kt index 9e1af37..2939a0d 100644 --- a/maps-kt-features/src/jvmMain/kotlin/center/sciprog/maps/compose/clickGestures.kt +++ b/maps-kt-features/src/jvmMain/kotlin/center/sciprog/maps/compose/clickGestures.kt @@ -2,8 +2,8 @@ package center.sciprog.maps.compose import androidx.compose.foundation.gestures.GestureCancellationException import androidx.compose.foundation.gestures.PressGestureScope +import androidx.compose.foundation.gestures.awaitEachGesture import androidx.compose.foundation.gestures.detectTapGestures -import androidx.compose.foundation.gestures.forEachGesture import androidx.compose.ui.geometry.Offset import androidx.compose.ui.input.pointer.* import androidx.compose.ui.platform.ViewConfiguration @@ -16,13 +16,13 @@ import kotlinx.coroutines.sync.Mutex * 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 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 val pressScope = PressGestureScopeImpl(this@detectClicks) - forEachGesture { - awaitPointerEventScope { - val down = awaitFirstDownEvent() - down.consume() + awaitEachGesture { + val down = awaitFirstDownEvent() + down.consume() - pressScope.reset() - if (onPress !== NoPressGesture) launch { - pressScope.onPress(down) + pressScope.reset() + if (onPress !== NoPressGesture) launch { + 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 { - viewConfiguration.longPressTimeoutMillis - } ?: (Long.MAX_VALUE / 2) - var upOrCancel: PointerEvent? = null - 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() + if (upOrCancel == null) { + pressScope.cancel() // tap-up was canceled + } else { + upOrCancel.consume() pressScope.release() } + } catch (_: PointerEventTimeoutCancellationException) { + onLongClick?.invoke(this, down) + consumeUntilUp() + pressScope.release() + } - if (upOrCancel != null) { - // tap was successful. - if (onDoubleClick == null) { - onClick?.invoke(this, down) // no need to check for double-tap. + if (upOrCancel != null) { + // tap was successful. + if (onDoubleClick == null) { + 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 { - // check for second tap - val secondDown = awaitSecondDown(upOrCancel.firstChange) + // Second tap down detected + pressScope.reset() + if (onPress !== NoPressGesture) { + launch { pressScope.onPress(secondDown) } + } - if (secondDown == null) { - onClick?.invoke(this, down) // no valid second tap started - } else { - // Second tap down detected - pressScope.reset() - if (onPress !== NoPressGesture) { - launch { pressScope.onPress(secondDown) } - } - - try { - // Might have a long second press as the second tap - withTimeout(longPressTimeout) { - val secondUp = waitForUpOrCancellation() - if (secondUp != null) { - secondUp.consume() - pressScope.release() - onDoubleClick(secondDown) - } else { - pressScope.cancel() - onClick?.invoke(this, down) - } + try { + // Might have a long second press as the second tap + 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() } } } diff --git a/maps-kt-geojson/README.md b/maps-kt-geojson/README.md index 41c72ba..d7b97aa 100644 --- a/maps-kt-geojson/README.md +++ b/maps-kt-geojson/README.md @@ -6,7 +6,7 @@ ## 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:** ```groovy @@ -16,7 +16,7 @@ repositories { } 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:** @@ -27,6 +27,6 @@ repositories { } dependencies { - implementation("center.sciprog:maps-kt-geojson:0.2.1-dev-2") + implementation("center.sciprog:maps-kt-geojson:0.2.2-dev-10") } ``` diff --git a/maps-kt-geojson/build.gradle.kts b/maps-kt-geojson/build.gradle.kts index bdb2863..2203034 100644 --- a/maps-kt-geojson/build.gradle.kts +++ b/maps-kt-geojson/build.gradle.kts @@ -3,20 +3,16 @@ plugins { `maven-publish` } -kotlin { - sourceSets { - commonMain { - dependencies { - api(projects.mapsKtCore) - api(projects.mapsKtFeatures) - api(spclibs.kotlinx.serialization.json) - } - } - } -} kscience{ + jvm() + js() useSerialization { json() } + dependencies{ + api(projects.mapsKtCore) + api(projects.mapsKtFeatures) + api(spclibs.kotlinx.serialization.json) + } } \ No newline at end of file diff --git a/maps-kt-scheme/README.md b/maps-kt-scheme/README.md index e661110..87967d4 100644 --- a/maps-kt-scheme/README.md +++ b/maps-kt-scheme/README.md @@ -6,7 +6,7 @@ ## 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:** ```groovy @@ -16,7 +16,7 @@ repositories { } 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:** @@ -27,6 +27,6 @@ repositories { } dependencies { - implementation("center.sciprog:maps-kt-scheme:0.2.1-dev-2") + implementation("center.sciprog:maps-kt-scheme:0.2.2-dev-10") } ``` diff --git a/maps-kt-scheme/build.gradle.kts b/maps-kt-scheme/build.gradle.kts index 59d1edb..49ca678 100644 --- a/maps-kt-scheme/build.gradle.kts +++ b/maps-kt-scheme/build.gradle.kts @@ -1,6 +1,3 @@ -import space.kscience.gradle.KScienceVersions.JVM_TARGET - - plugins { kotlin("multiplatform") id("org.jetbrains.compose") @@ -8,11 +5,7 @@ plugins { } kotlin { - jvm { - compilations.all { - kotlinOptions.jvmTarget = JVM_TARGET.toString() - } - } + jvm() sourceSets { commonMain { dependencies { @@ -30,6 +23,6 @@ kotlin { } } -java { - targetCompatibility = JVM_TARGET -} \ No newline at end of file +//java { +// targetCompatibility = JVM_TARGET +//} \ No newline at end of file diff --git a/settings.gradle.kts b/settings.gradle.kts index af40e60..232def8 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -46,6 +46,7 @@ dependencyResolutionManagement { include( + ":trajectory-kt", ":maps-kt-core", ":maps-kt-geojson", ":maps-kt-features", diff --git a/trajectory-kt/README.md b/trajectory-kt/README.md new file mode 100644 index 0000000..5296ec7 --- /dev/null +++ b/trajectory-kt/README.md @@ -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 \ No newline at end of file diff --git a/trajectory-kt/build.gradle.kts b/trajectory-kt/build.gradle.kts new file mode 100644 index 0000000..71d240a --- /dev/null +++ b/trajectory-kt/build.gradle.kts @@ -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")) +} diff --git a/trajectory-kt/docs/README-TEMPLATE.md b/trajectory-kt/docs/README-TEMPLATE.md new file mode 100644 index 0000000..be25d0d --- /dev/null +++ b/trajectory-kt/docs/README-TEMPLATE.md @@ -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 \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt new file mode 100644 index 0000000..a1563b2 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -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 = 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 = 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 = 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 + +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)) + diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt new file mode 100644 index 0000000..078e158 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt @@ -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 = + 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 { + 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) \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt new file mode 100644 index 0000000..32061ef --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -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) { + 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 = 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 = 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, +) { + internal val tangents: List = boundaryTangents().first + public val boundaryRoute: DubinsPath.Type = boundaryTangents().second + + public val center: Vector2D = vector( + circles.sumOf { it.center.x } / circles.size, + circles.sumOf { it.center.y } / circles.size + ) + + private fun boundaryTangents(): Pair, DubinsPath.Type> { + // outer tangents for a polygon circles can be either lsl or rsr + + fun Circle2D.dubinsTangentsToCircles( + other: Circle2D, + ): Map = 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 = 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 { + 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 { + 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, +): List { + return obstacles.sortedBy { norm(it.center - currentObstacle.center) } +} + +private fun tangentsAlongTheObstacle( + initialCircle: Circle2D, + direction: Trajectory2D.Direction, + finalCircle: Circle2D, + obstacle: Obstacle, +): List { + val dubinsTangents = mutableListOf() + 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, + 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, +): List { + 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() + 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 = 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() + 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 = outerTangents(currentObstacle, nextObstacle) + .filter { (key, tangent) -> + obstacles.none { obstacle -> tangent.intersectObstacle(obstacle) } && + key.first == currentDirection && + (nextObstacle != finalObstacle || key.third == j) + } + + var tangentsAlong: List + 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>, + ): List { + val obstacles: List = obstaclePolygons.map { polygon -> + Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) }) + } + return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles) + } +} + + + + + diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt new file mode 100644 index 0000000..59a8e61 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -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 { + override val length: Double get() = segments.sumOf { it.length } +} + +public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = + CompositeTrajectory2D(segments.toList()) + diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt new file mode 100644 index 0000000..80f7173 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt @@ -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))) + } + } + } +} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt new file mode 100644 index 0000000..1a8c3a4 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt @@ -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) + } +} \ No newline at end of file diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt new file mode 100644 index 0000000..6fc00fb --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt @@ -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)) + } +} \ No newline at end of file diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt new file mode 100644 index 0000000..8b8ccf9 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt @@ -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) + ) +} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt new file mode 100644 index 0000000..b3825b9 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -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) + } +} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt new file mode 100644 index 0000000..c3fca06 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt @@ -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) + } +} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt new file mode 100644 index 0000000..54deb21 --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -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) + } +}