From 7cc6a4be400bb7195dba6df50259a2c48cee849f Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Wed, 5 Apr 2023 15:26:09 +0300 Subject: [PATCH] remove trajectory --- CHANGELOG.md | 1 + build.gradle.kts | 2 +- gradle.properties | 2 +- kmath-trajectory/README.md | 34 - kmath-trajectory/build.gradle.kts | 21 - kmath-trajectory/docs/README-TEMPLATE.md | 13 - .../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 | 64 -- .../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 -- settings.gradle.kts | 1 - 18 files changed, 3 insertions(+), 1482 deletions(-) delete mode 100644 kmath-trajectory/README.md delete mode 100644 kmath-trajectory/build.gradle.kts delete mode 100644 kmath-trajectory/docs/README-TEMPLATE.md delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt delete mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt diff --git a/CHANGELOG.md b/CHANGELOG.md index c5fa3f372..24b592430 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ ### Deprecated ### Removed +- Trajectory moved to https://github.com/SciProgCentre/maps-kt - Polynomials moved to https://github.com/SciProgCentre/kmath-polynomial ### Fixed diff --git a/build.gradle.kts b/build.gradle.kts index 9b1101a22..cd8dfb4a0 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -15,7 +15,7 @@ allprojects { } group = "space.kscience" - version = "0.3.1-dev-10" + version = "0.3.1-dev-11" } subprojects { diff --git a/gradle.properties b/gradle.properties index 262bcabfb..e33106c0c 100644 --- a/gradle.properties +++ b/gradle.properties @@ -9,7 +9,7 @@ kotlin.native.ignoreDisabledTargets=true org.gradle.configureondemand=true org.gradle.jvmargs=-Xmx4096m -toolsVersion=0.14.5-kotlin-1.8.20-RC +toolsVersion=0.14.6-kotlin-1.8.20 org.gradle.parallel=true diff --git a/kmath-trajectory/README.md b/kmath-trajectory/README.md deleted file mode 100644 index ac2930b04..000000000 --- a/kmath-trajectory/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# kmath-trajectory - - - - -## Artifact: - -The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.1-dev-1`. - -**Gradle Groovy:** -```groovy -repositories { - maven { url 'https://repo.kotlin.link' } - mavenCentral() -} - -dependencies { - implementation 'space.kscience:kmath-trajectory:0.3.1-dev-1' -} -``` -**Gradle Kotlin DSL:** -```kotlin -repositories { - maven("https://repo.kotlin.link") - mavenCentral() -} - -dependencies { - implementation("space.kscience:kmath-trajectory:0.3.1-dev-1") -} -``` - -## Contributors -Erik Schouten (github: @ESchouten, email: erik-schouten@hotmail.nl) diff --git a/kmath-trajectory/build.gradle.kts b/kmath-trajectory/build.gradle.kts deleted file mode 100644 index 32b87bb06..000000000 --- a/kmath-trajectory/build.gradle.kts +++ /dev/null @@ -1,21 +0,0 @@ -plugins { - id("space.kscience.gradle.mpp") -} - -kscience{ - jvm() - js() - native() - - useContextReceivers() - useSerialization() - dependencies { - api(projects.kmath.kmathGeometry) - } -} - -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/kmath-trajectory/docs/README-TEMPLATE.md b/kmath-trajectory/docs/README-TEMPLATE.md deleted file mode 100644 index eb8e4a0c0..000000000 --- a/kmath-trajectory/docs/README-TEMPLATE.md +++ /dev/null @@ -1,13 +0,0 @@ -# kmath-trajectory - - -${features} - -${artifact} - -## Author -Erik Schouten - -Github: ESchouten - -Email: erik-schouten@hotmail.nl diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt deleted file mode 100644 index a1563b29c..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ /dev/null @@ -1,258 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt deleted file mode 100644 index 078e158ea..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt deleted file mode 100644 index 32061efea..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ /dev/null @@ -1,614 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt deleted file mode 100644 index 59a8e613a..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ /dev/null @@ -1,131 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt deleted file mode 100644 index 80f7173f1..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt deleted file mode 100644 index 1a8c3a474..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt deleted file mode 100644 index 2ae89038c..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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.assertFailsWith -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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt deleted file mode 100644 index 8b8ccf95e..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt +++ /dev/null @@ -1,27 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt deleted file mode 100644 index b3825b93b..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt deleted file mode 100644 index c3fca06ec..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt +++ /dev/null @@ -1,24 +0,0 @@ -/* - * 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/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt deleted file mode 100644 index 54deb2193..000000000 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ /dev/null @@ -1,37 +0,0 @@ -/* - * 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) - } -} diff --git a/settings.gradle.kts b/settings.gradle.kts index 7d92dc36e..f158f3444 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -44,7 +44,6 @@ include( ":kmath-jupyter", ":kmath-symja", ":kmath-jafama", - ":kmath-trajectory", ":examples", ":benchmarks", )