From 2c13386646a6f3f38523e9546eccf38343251163 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Wed, 1 Mar 2023 10:40:54 +0300 Subject: [PATCH 01/30] search for shortest path algorithm --- .../space/kscience/kmath/geometry/Line.kt | 1 + .../kmath/trajectory/DubinsObstacle.kt | 413 ++++++++++++++++++ .../kscience/kmath/trajectory/DubinsPath.kt | 110 +++++ .../kscience/kmath/trajectory/tangent.kt | 66 ++- 4 files changed, 583 insertions(+), 7 deletions(-) create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt index ab322ddca..e593150f1 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt @@ -6,6 +6,7 @@ package space.kscience.kmath.geometry import kotlinx.serialization.Serializable +import space.kscience.kmath.operations.DoubleField.pow /** * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized, diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt new file mode 100644 index 000000000..56aa88e4a --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -0,0 +1,413 @@ +/* + * 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.plus +import space.kscience.kmath.geometry.Euclidean2DSpace.times +import space.kscience.kmath.geometry.Euclidean2DSpace.vector +import space.kscience.kmath.geometry.Euclidean2DSpace.norm +import space.kscience.kmath.operations.DoubleField.pow +import kotlin.math.* + +public fun LineSegment2D.length(): Double { + return ((end.y - begin.y).pow(2.0) + (end.x - begin.x).pow(2.0)).pow(0.5) +} +public class DubinsObstacle( + public val circles: List +) { + public val tangents: List = boundaryTangents().first + public val boundaryRoute: DubinsPath.Type = boundaryTangents().second + public val center: Vector2D = + vector(this.circles.sumOf{it.center.x} / this.circles.size, + this.circles.sumOf{it.center.y} / this.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.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 + 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(route, DubinsTangent(Circle2D(center, radius), + other, + this@DubinsObstacle, + this@DubinsObstacle, + LineSegment2D( + center + w * r1, + other.center + w * r2 + ), + DubinsPath.toSimpleTypes(route)) + ) + } else { + throw Exception("Circles should not intersect") + } + } + } + } + val firstCircles = this.circles.slice(-this.circles.size..-1) + val secondCircles = this.circles.slice(-this.circles.size+1..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( + this.circles.sumOf { it.center.x } / this.circles.size, + this.circles.sumOf { it.center.y } / this.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) + } + } + + public fun nextTangent(circle: Circle2D, route: DubinsPath.Type): DubinsTangent { + if (route == this.boundaryRoute) { + for (i in this.circles.indices) { + if (this.circles[i] == circle) { + return this.tangents[i] + } + } + } + else { + for (i in this.circles.indices) { + if (this.circles[i] == circle) { + return DubinsTangent(this.circles[i], + this.circles[i-1], + this, + this, + LineSegment2D(this.tangents[i-1].lineSegment.end, + this.tangents[i-1].lineSegment.begin), + DubinsPath.toSimpleTypes(route)) + } + } + } + + error("next tangent not found") + } + + public fun equals(other: DubinsObstacle): Boolean { + return this.circles == other.circles + } +} + +public data class DubinsTangent(val startCircle: Circle2D, + val endCircle: Circle2D, + val startObstacle: DubinsObstacle, + val endObstacle: DubinsObstacle, + val lineSegment: LineSegment2D, + val route: PathTypes) + +public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { + fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { + return v1.x * v2.y - v1.y * v2.x + } + if (crossProduct(other.begin - this.begin, other.end - this.begin).sign == + crossProduct(other.begin - this.end, other.end - this.end).sign) { + return false + } + if (crossProduct(this.begin - other.begin, this.end - other.begin).sign == + crossProduct(this.begin - other.end, this.end - other.end).sign) { + return false + } + return true +} + +public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { + val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0) + val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) + + (this.begin.y - this.end.y) * (this.end.y - circle.center.y)) + val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) - + 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 +} + +public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { + for (tangent in obstacle.tangents) { + if (this.lineSegment.intersectSegment(tangent.lineSegment)) { + return true + } + } + for (circle in obstacle.circles) { + if (this.lineSegment.intersectCircle(circle)) { + return true + } + } + return false +} + +public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { + return 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 + ) + } + } + } + } + }.toMutableMap() +} + +public fun arcLength(circle: Circle2D, + point1: DoubleVector2D, + point2: DoubleVector2D, + route: DubinsPath.SimpleType): 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 (route) { + DubinsPath.SimpleType.L -> { + angle = if (phi2 >= phi1) { + phi2 - phi1 + } else { + 2 * PI + phi2 - phi1 + } + } + DubinsPath.SimpleType.R -> { + angle = if (phi2 >= phi1) { + 2 * PI - (phi2 - phi1) + } else { + -(phi2 - phi1) + } + } + DubinsPath.SimpleType.S -> { + error("L or R route is expected") + } + } + return circle.radius * angle +} + +public 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)) + ) +} + +public 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 + val p2 = center2 - point + return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) { + mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r), + DubinsPath.SimpleType.R to Circle2D(center2, r)) + } + else { + mapOf(DubinsPath.SimpleType.L to Circle2D(center2, r), + DubinsPath.SimpleType.R to Circle2D(center1, r)) + } +} + +public fun sortedObstacles(currentObstacle: DubinsObstacle, + obstacles: List): List { + return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed() +} + +public fun tangentsAlongTheObstacle(initialCircle: Circle2D, + initialRoute: DubinsPath.Type, + finalCircle: Circle2D, + obstacle: DubinsObstacle): MutableList { + val dubinsTangents = mutableListOf() + var tangent = obstacle.nextTangent(initialCircle, initialRoute) + dubinsTangents.add(tangent) + while (tangent.endCircle != finalCircle) { + tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) + dubinsTangents.add(tangent) + } + return dubinsTangents +} + +public fun allFinished(paths: List>, + finalObstacle: DubinsObstacle): Boolean { + for (path in paths) { + if (path[-1].endObstacle != finalObstacle) { + return false + } + } + return true +} + +public fun pathLength(path: List): Double { + val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)} + val arcsLength = buildList{ + for (i in 1..path.size) { + add(arcLength(path[i].startCircle, + path[i-1].lineSegment.end, + path[i].lineSegment.begin, + path[i].route[0])) + } + }.sum() + return tangentsLength + arcsLength +} + +public fun shortestPath(path: List>): List> { + return path.sortedBy { pathLength(it) } +} + +public typealias Path = List +public fun findAllPaths( + startingPoint: DoubleVector2D, + startingDirection: DoubleVector2D, + startingRadius: Double, + finalPoint: DoubleVector2D, + finalDirection: DoubleVector2D, + finalRadius: Double, + obstacles: List +) { + val initialCircles = constructTangentCircles( + startingPoint, + startingDirection, + startingRadius) + val finalCircles = constructTangentCircles( + finalPoint, + finalDirection, + finalRadius) + var outputTangents = mutableMapOf>() + for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { + for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { + val finalCircle = finalCircles[j]!! + val finalObstacle = DubinsObstacle(listOf(finalCircle)) + outputTangents[listOf(i, + DubinsPath.SimpleType.S, + j)] = listOf( + listOf(DubinsTangent( + initialCircles[i]!!, + initialCircles[i]!!, + DubinsObstacle(listOf(initialCircles[i]!!)), + DubinsObstacle(listOf(initialCircles[i]!!)), + LineSegment2D(startingPoint, startingPoint), + listOf(i, DubinsPath.SimpleType.S, i) + ))) + var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) + while (!allFinished(outputTangents[listOf(i, + DubinsPath.SimpleType.S, + j)]!!, finalObstacle)) { + var newOutputTangents = listOf() + for (line in outputTangents[listOf(i, + DubinsPath.SimpleType.S, + j)]!!) { + var currentCircle = line[-1].endCircle + var currentDirection = line[-1].route[-1] + var currentObstacle = line[-1].endObstacle + var nextObstacle = DubinsObstacle(listOf()) + if (currentObstacle != finalObstacle) { + var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( + currentDirection, + DubinsPath.SimpleType.S, + j) + )] + for (obstacle in sortedObstacles(currentObstacle, obstacles)) { + if (tangentToFinal!!.intersectObstacle(obstacle)) { + nextObstacle = obstacle + break + } + } + if (nextObstacle == DubinsObstacle(listOf())) { + nextObstacle = finalObstacle + } + var nextTangents = outerTangents(currentObstacle, nextObstacle) +// for (pathType in listOf( +// listOf(DubinsPath.SimpleType.L, +// DubinsPath.SimpleType.S, +// DubinsPath.SimpleType.L), +// listOf(DubinsPath.SimpleType.L, +// DubinsPath.SimpleType.S, +// DubinsPath.SimpleType.R), +// listOf(DubinsPath.SimpleType.R, +// DubinsPath.SimpleType.S, +// DubinsPath.SimpleType.L), +// listOf(DubinsPath.SimpleType.R, +// DubinsPath.SimpleType.S, +// DubinsPath.SimpleType.R) +// )) { + for (pathType in nextTangents.keys) { + for (obstacle in obstacles) { + // in Python code here try/except was used, but seems unneeded + if (nextTangents[pathType]!!.intersectObstacle(obstacle)) { + nextTangents.remove(pathType) + } + + } + } + if (nextObstacle == finalObstacle) { + nextTangents = + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and + (DubinsPath.toSimpleTypes(it.key)[0] == j)} + as MutableMap + } + else { + nextTangents = + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} + as MutableMap + } + TODO("rewrite fragment from Python") + } + } + } + } + } +} + + + + + + + + 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 index 568ef691a..5654d10ae 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -77,10 +77,118 @@ private fun innerTangent( @Suppress("DuplicatedCode") public object DubinsPath { +// public class ArcType(private val type: Type){ +// public val first: SimpleType +// get() { +// if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) { +// return SimpleType.R +// } +// else if (type in listOf(Type.LSL, Type.LSR, Type.LRL)) { +// return SimpleType.L +// } +// error("Wrong DubinsPath.Type") +// } +// +// public val last: SimpleType +// get() { +// if (type in listOf(Type.RSR, Type.LSR, Type.RLR)) { +// return SimpleType.R +// } +// else if (type in listOf(Type.LSL, Type.RSL, Type.LRL)) { +// return SimpleType.L +// } +// error("Wrong DubinsPath.Type") +// } +// public val intermediate: SimpleType +// get() { +// if (type == Type.RLR) { +// return SimpleType.L +// } +// else if (type == Type.LRL) { +// return SimpleType.R +// } +// error("This DubinsPath.Type doesn't contain intermediate arc") +// } +// } + + public enum class SimpleType { + R, S, L + } + public enum class Type { RLR, LRL, RSR, LSL, RSL, LSR } + public fun toSimpleTypes(type: Type): List { + when (type) { + Type.RLR -> { + return listOf(SimpleType.R, SimpleType.L, SimpleType.R) + } + Type.LRL -> { + return listOf(SimpleType.L, SimpleType.R, SimpleType.L) + } + Type.RSR -> { + return listOf(SimpleType.R, SimpleType.S, SimpleType.R) + } + Type.LSL -> { + return listOf(SimpleType.L, SimpleType.S, SimpleType.L) + } + Type.RSL -> { + return listOf(SimpleType.R, SimpleType.S, SimpleType.L) + } + Type.LSR -> { + return listOf(SimpleType.L, SimpleType.S, SimpleType.R) + } + else -> error("This type doesn't exist") + } + } + + public fun toType(types: List): Type { + when (types) { + listOf(SimpleType.R, SimpleType.L, SimpleType.R) -> { + return Type.RLR + } + listOf(SimpleType.L, SimpleType.R, SimpleType.L) -> { + return Type.LRL + } + listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> { + return Type.RSR + } + listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> { + return Type.LSL + } + listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> { + return Type.RSL + } + listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> { + return Type.LSR + } + else -> error("This type doesn't exist") + } + } + +// public class PathTypes(private val inputTypes: List) { +// public val type: Type +// get() { +// when (this.inputTypes) { +// listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> { +// return Type.RSR +// } +// listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> { +// return Type.RSL +// } +// listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> { +// return Type.LSR +// } +// listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> { +// return Type.LSL +// } +// else -> error("Wrong list of SimpleTypes") +// } +// } +// public val chain: List = this.inputTypes +// } + /** * Return Dubins trajectory type or null if trajectory is not a Dubins path */ @@ -243,6 +351,8 @@ public object DubinsPath { } } +public typealias PathTypes = List + public fun interface MaxCurvature { public fun compute(startPoint: PhaseVector2D): Double } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index 945b140c2..2d935aa00 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -5,10 +5,10 @@ package space.kscience.kmath.trajectory -import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.DoubleVector2D -import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.LineSegment +import space.kscience.kmath.geometry.* +import space.kscience.kmath.geometry.Euclidean2DSpace.plus +import space.kscience.kmath.geometry.Euclidean2DSpace.times +import space.kscience.kmath.operations.DoubleField.pow import kotlin.math.* /** @@ -17,7 +17,7 @@ import kotlin.math.* */ public fun Circle2D.tangentsToCircle( other: Circle2D, -): Map> = with(Euclidean2DSpace) { +): 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) @@ -48,14 +48,66 @@ public fun Circle2D.tangentsToCircle( val w = vector(-cos(angle2), sin(angle2)) put( route, - LineSegment( + LineSegment2D( center + w * r1, other.center + w * r2 ) ) } else { - throw Exception("Circles should not be") + throw Exception("Circles should not intersect") + } + } + } +} + +public fun dubinsTangentsToCircles( + firstCircle: Circle2D, + secondCircle: Circle2D, + firstObstacle: DubinsObstacle, + secondObstacle: DubinsObstacle +): Map = with(Euclidean2DSpace) { + val line = LineSegment(firstCircle.center, secondCircle.center) + val d = 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, 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 = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), + secondCircle, + firstObstacle, + secondObstacle, + LineSegment2D( + firstCircle.center + w * r1, + secondCircle.center + w * r2 + ), + DubinsPath.toSimpleTypes(route)) + ) + } else { + throw Exception("Circles should not intersect") } } } From 61d43ae5faa05f8c9d90d6519897a7f0ebd6b77a Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Sat, 4 Mar 2023 21:31:06 +0300 Subject: [PATCH 02/30] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 144 ++++++++++++++---- .../kscience/kmath/trajectory/tangent.kt | 82 ++++++++-- .../kscience/kmath/trajectory/DubinsTest.kt | 39 +++++ 3 files changed, 225 insertions(+), 40 deletions(-) create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index 56aa88e4a..e1c967e2a 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -276,7 +276,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D, public fun allFinished(paths: List>, finalObstacle: DubinsObstacle): Boolean { for (path in paths) { - if (path[-1].endObstacle != finalObstacle) { + if (path.last().endObstacle != finalObstacle) { return false } } @@ -296,8 +296,8 @@ public fun pathLength(path: List): Double { return tangentsLength + arcsLength } -public fun shortestPath(path: List>): List> { - return path.sortedBy { pathLength(it) } +public fun shortestPath(path: List>): List { + return path.sortedBy { pathLength(it) }[0] } public typealias Path = List @@ -309,7 +309,7 @@ public fun findAllPaths( finalDirection: DoubleVector2D, finalRadius: Double, obstacles: List -) { +): List> { val initialCircles = constructTangentCircles( startingPoint, startingDirection, @@ -318,15 +318,15 @@ public fun findAllPaths( finalPoint, finalDirection, finalRadius) - var outputTangents = mutableMapOf>() + var outputTangents = mutableMapOf>>() for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { val finalCircle = finalCircles[j]!! val finalObstacle = DubinsObstacle(listOf(finalCircle)) outputTangents[listOf(i, DubinsPath.SimpleType.S, - j)] = listOf( - listOf(DubinsTangent( + j)] = mutableListOf( + mutableListOf(DubinsTangent( initialCircles[i]!!, initialCircles[i]!!, DubinsObstacle(listOf(initialCircles[i]!!)), @@ -338,13 +338,13 @@ public fun findAllPaths( while (!allFinished(outputTangents[listOf(i, DubinsPath.SimpleType.S, j)]!!, finalObstacle)) { - var newOutputTangents = listOf() + var newOutputTangents = mutableListOf>() for (line in outputTangents[listOf(i, DubinsPath.SimpleType.S, j)]!!) { - var currentCircle = line[-1].endCircle - var currentDirection = line[-1].route[-1] - var currentObstacle = line[-1].endObstacle + var currentCircle = line.last().endCircle + var currentDirection = line.last().route.last() + var currentObstacle = line.last().endObstacle var nextObstacle = DubinsObstacle(listOf()) if (currentObstacle != finalObstacle) { var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( @@ -362,20 +362,7 @@ public fun findAllPaths( nextObstacle = finalObstacle } var nextTangents = outerTangents(currentObstacle, nextObstacle) -// for (pathType in listOf( -// listOf(DubinsPath.SimpleType.L, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.L), -// listOf(DubinsPath.SimpleType.L, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.R), -// listOf(DubinsPath.SimpleType.R, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.L), -// listOf(DubinsPath.SimpleType.R, -// DubinsPath.SimpleType.S, -// DubinsPath.SimpleType.R) -// )) { + for (pathType in nextTangents.keys) { for (obstacle in obstacles) { // in Python code here try/except was used, but seems unneeded @@ -396,12 +383,117 @@ public fun findAllPaths( nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} as MutableMap } - TODO("rewrite fragment from Python") + val tangentsAlong = mutableListOf() + for (tangent in nextTangents.values) { + if (tangent.startCircle == line.last().endCircle) { + val lengthMaxPossible = arcLength( + tangent.startCircle, + line.last().lineSegment.end, + tangent.startObstacle.nextTangent( + tangent.startCircle, + DubinsPath.toType(listOf(currentDirection, DubinsPath.SimpleType.S, currentDirection)), + ).lineSegment.begin, + currentDirection + ) + val lengthCalculated = arcLength( + tangent.startCircle, + line.last().lineSegment.end, + tangent.lineSegment.begin, + currentDirection) + if (lengthCalculated > lengthMaxPossible) { + val tangentsAlong = tangentsAlongTheObstacle( + currentCircle, + DubinsPath.toType(listOf( + currentDirection, + DubinsPath.SimpleType.S, + currentDirection)), + tangent.startCircle, + currentObstacle + ) + } + else { + val tangentsAlong = mutableListOf() + } + } + else { + val tangentsAlong = tangentsAlongTheObstacle( + currentCircle, + DubinsPath.toType(listOf( + currentDirection, + DubinsPath.SimpleType.S, + currentDirection)), + tangent.startCircle, + currentObstacle + ) + } + newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList()) + } + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents + } + else { + // minor changes from Python code + newOutputTangents.add(line) + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents } } } + for (lineId in outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!!.indices) { + val lastDirection = outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!![lineId].last().route[2] + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )]!![lineId].add(DubinsTangent( + finalCircles[j]!!, + finalCircles[j]!!, + DubinsObstacle( + listOf(finalCircles[j]!!) + ), + DubinsObstacle( + listOf(finalCircles[j]!!) + ), + LineSegment2D(finalPoint, finalPoint), + listOf( + lastDirection, + DubinsPath.SimpleType.S, + j + ) + )) + } } } + return outputTangents[listOf( + DubinsPath.SimpleType.L, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.L, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.R + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.R, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L + )]!! + outputTangents[listOf( + DubinsPath.SimpleType.R, + DubinsPath.SimpleType.S, + DubinsPath.SimpleType.L)]!! } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index d3165e162..1a58d64cc 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -39,22 +39,76 @@ public fun Circle2D.tangentsToCircle( } else { r1.absoluteValue + r2.absoluteValue } - if (distance <= r) TODO("Intersecting circles are not supported yet") - 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 + 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 = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + put( + route, + LineSegment( + center + w * r1, + other.center + w * r2 + ) ) - ) + } else { + throw Exception("Circles should not intersect") + } + } + } +} +public fun dubinsTangentsToCircles( + firstCircle: Circle2D, + secondCircle: Circle2D, + firstObstacle: DubinsObstacle, + secondObstacle: DubinsObstacle +): 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, 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 = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), + secondCircle, + firstObstacle, + secondObstacle, + LineSegment2D( + firstCircle.center + w * r1, + secondCircle.center + w * r2 + ), + DubinsPath.toSimpleTypes(route)) + ) + } else { + throw Exception("Circles should not intersect") + } } } } \ No newline at end of file diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt new file mode 100644 index 000000000..3eefbdaeb --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -0,0 +1,39 @@ +/* + * 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.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace.vector +import kotlin.test.Test +import kotlin.test.assertTrue + +class DubinsTest { + @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(DubinsObstacle(listOf( + Circle2D(vector(7.0, 1.0), 5.0)))) + + val outputTangents = findAllPaths( + startPoint, + startDirection, + startRadius, + finalPoint, + finalDirection, + finalRadius, + obstacles) + val length = pathLength(shortestPath(outputTangents)) + TODO("fix negative indices in boundaryTangents and accomplish test") + assertTrue(false) + } +} \ No newline at end of file From 1b6a41c728150bb9374625b06045c06f8073c524 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Thu, 9 Mar 2023 08:39:20 +0300 Subject: [PATCH 03/30] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 30 ++++++++--------- .../kscience/kmath/trajectory/tangent.kt | 2 +- .../kscience/kmath/trajectory/DubinsTest.kt | 33 +++++++++++++++++++ 3 files changed, 49 insertions(+), 16 deletions(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index e1c967e2a..d3ab13f26 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -39,8 +39,8 @@ public class DubinsObstacle( 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.RSL to Pair(radius, -other.radius), +// DubinsPath.Type.LSR to Pair(-radius, other.radius), DubinsPath.Type.LSL to Pair(-radius, -other.radius) ) return buildMap { @@ -52,7 +52,7 @@ public class DubinsObstacle( } else { r1.absoluteValue + r2.absoluteValue } - if (d * d > r * r) { + 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) @@ -76,8 +76,11 @@ public class DubinsObstacle( } } } - val firstCircles = this.circles.slice(-this.circles.size..-1) - val secondCircles = this.circles.slice(-this.circles.size+1..0) +// val firstCircles = this.circles.slice(-this.circles.size..-1) +// val secondCircles = this.circles.slice(-this.circles.size+1..0) + val firstCircles = this.circles + val secondCircles = this.circles.slice(1..this.circles.lastIndex) + + this.circles[0] val lslTangents = firstCircles.zip(secondCircles) {a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!} val rsrTangents = firstCircles.zip(secondCircles) @@ -345,7 +348,7 @@ public fun findAllPaths( var currentCircle = line.last().endCircle var currentDirection = line.last().route.last() var currentObstacle = line.last().endObstacle - var nextObstacle = DubinsObstacle(listOf()) + var nextObstacle: DubinsObstacle? = null if (currentObstacle != finalObstacle) { var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( currentDirection, @@ -358,7 +361,7 @@ public fun findAllPaths( break } } - if (nextObstacle == DubinsObstacle(listOf())) { + if (nextObstacle == null) { nextObstacle = finalObstacle } var nextTangents = outerTangents(currentObstacle, nextObstacle) @@ -372,16 +375,13 @@ public fun findAllPaths( } } - if (nextObstacle == finalObstacle) { - nextTangents = - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and + nextTangents = if (nextObstacle == finalObstacle) { + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and (DubinsPath.toSimpleTypes(it.key)[0] == j)} as MutableMap - } - else { - nextTangents = - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} - as MutableMap + } else { + nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} + as MutableMap } val tangentsAlong = mutableListOf() for (tangent in nextTangents.values) { diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt index 1a58d64cc..fb1c3927b 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt @@ -95,7 +95,7 @@ public fun dubinsTangentsToCircles( } else { angle1 - r2.sign * atan2(r.absoluteValue, l) } - val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + val w = vector(-cos(angle2), sin(angle2)) put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), secondCircle, firstObstacle, diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index 3eefbdaeb..a0697c59c 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -36,4 +36,37 @@ class DubinsTest { TODO("fix negative indices in boundaryTangents and accomplish test") assertTrue(false) } + @Test + fun outerTangentsTest1() { + // works incorrectly + val circles1 = listOf( + Circle2D(vector(0.0, 0.0), 1.0)) + val circles2 = listOf( + Circle2D(vector(5.0, 5.0), 1.0) + ) + println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) + assertTrue(false) + } + @Test + fun outerTangentsTest2() { + // works incorrectly + val circles1 = listOf( + Circle2D(vector(0.0, 0.0), 1.0), + Circle2D(vector( 2.0, 0.0), 1.0)) + val circles2 = listOf( + Circle2D(vector(5.0, 5.0), 1.0), + Circle2D(vector(7.0, 5.0), 1.0) + ) + println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) + + for (circle1 in circles1) { + for (circle2 in circles2) { + for (tangent in dubinsTangentsToCircles(circle1, circle2, + DubinsObstacle(circles1), DubinsObstacle(circles2))) { + println(tangent) + } + } + } + assertTrue(false) + } } \ No newline at end of file From 2bce369c5dcdae8b4ba16f498fd19d31807b29df Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Thu, 9 Mar 2023 16:03:48 +0300 Subject: [PATCH 04/30] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 38 ++++++++++++------- .../kscience/kmath/trajectory/DubinsTest.kt | 37 +++++++++++++++++- 2 files changed, 60 insertions(+), 15 deletions(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index d3ab13f26..6b247e55b 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -111,13 +111,24 @@ public class DubinsObstacle( else { for (i in this.circles.indices) { if (this.circles[i] == circle) { - return DubinsTangent(this.circles[i], - this.circles[i-1], - this, - this, - LineSegment2D(this.tangents[i-1].lineSegment.end, - this.tangents[i-1].lineSegment.begin), - DubinsPath.toSimpleTypes(route)) + if (i > 0) { + return DubinsTangent(this.circles[i], + this.circles[i-1], + this, + this, + LineSegment2D(this.tangents[i-1].lineSegment.end, + this.tangents[i-1].lineSegment.begin), + DubinsPath.toSimpleTypes(route)) + } + else { + return DubinsTangent(this.circles[0], + this.circles.last(), + this, + this, + LineSegment2D(this.tangents.last().lineSegment.end, + this.tangents.last().lineSegment.begin), + DubinsPath.toSimpleTypes(route)) + } } } } @@ -156,7 +167,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0) val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) + (this.begin.y - this.end.y) * (this.end.y - circle.center.y)) - val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) - + val c = (this.end.x - circle.center.x).pow(2.0) + (this.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) { @@ -289,7 +300,7 @@ public fun allFinished(paths: List>, public fun pathLength(path: List): Double { val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)} val arcsLength = buildList{ - for (i in 1..path.size) { + for (i in 1..path.lastIndex) { add(arcLength(path[i].startCircle, path[i-1].lineSegment.end, path[i].lineSegment.begin, @@ -366,13 +377,14 @@ public fun findAllPaths( } var nextTangents = outerTangents(currentObstacle, nextObstacle) - for (pathType in nextTangents.keys) { + for (pathType in DubinsPath.Type.values()) { for (obstacle in obstacles) { // in Python code here try/except was used, but seems unneeded - if (nextTangents[pathType]!!.intersectObstacle(obstacle)) { - nextTangents.remove(pathType) + if (nextTangents.containsKey(pathType)) { + if (nextTangents[pathType]!!.intersectObstacle(obstacle)) { + nextTangents.remove(pathType) + } } - } } nextTangents = if (nextObstacle == finalObstacle) { diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index a0697c59c..16d77c84d 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -33,8 +33,41 @@ class DubinsTest { finalRadius, obstacles) val length = pathLength(shortestPath(outputTangents)) - TODO("fix negative indices in boundaryTangents and accomplish test") - assertTrue(false) + println(length) + } + + @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( + DubinsObstacle(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) + )), DubinsObstacle(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 outputTangents = findAllPaths( + startPoint, + startDirection, + startRadius, + finalPoint, + finalDirection, + finalRadius, + obstacles) + val length = pathLength(shortestPath(outputTangents)) + println(length) } @Test fun outerTangentsTest1() { From 4871baf0e5f4d45bf3c7d31916d4b84be187b689 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Fri, 10 Mar 2023 12:01:08 +0300 Subject: [PATCH 05/30] Add vector product to Euclidean3DSpace --- gradle.properties | 2 +- .../kmath/geometry/Euclidean3DSpace.kt | 47 ++++++++++++++++++- .../kmath/geometry/Euclidean3DSpaceTest.kt | 46 ++++++++++++------ 3 files changed, 78 insertions(+), 17 deletions(-) diff --git a/gradle.properties b/gradle.properties index c3f070c2d..048c9c9f3 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.2-kotlin-1.8.10 +toolsVersion=0.14.3-kotlin-1.8.20-RC org.gradle.parallel=true diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index cc641a3f1..f1cf0bad2 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -78,8 +78,11 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< } } + public fun vector(x: Double, y: Double, z: Double): DoubleVector3D = + Vector3DImpl(x, y, z) + public fun vector(x: Number, y: Number, z: Number): DoubleVector3D = - Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble()) + vector(x.toDouble(), y.toDouble(), z.toDouble()) override val zero: DoubleVector3D by lazy { vector(0.0, 0.0, 0.0) } @@ -100,6 +103,48 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< override fun DoubleVector3D.dot(other: DoubleVector3D): Double = x * other.x + y * other.y + z * other.z + private fun leviCivita(i: Int, j: Int, k: Int): Int = when { + // even permutation + i == 0 && j == 1 && k == 2 -> 1 + i == 1 && j == 2 && k == 0 -> 1 + i == 2 && j == 0 && k == 1 -> 1 + // odd permutations + i == 2 && j == 1 && k == 0 -> -1 + i == 0 && j == 2 && k == 1 -> -1 + i == 1 && j == 0 && k == 2 -> -1 + + else -> 0 + } + + /** + * Compute vector product of [first] and [second]. The basis assumed to be right-handed if [rightBasis] is true and + * left-handed otherwise + */ + public fun vectorProduct( + first: DoubleVector3D, + second: DoubleVector3D, + rightBasis: Boolean = true, + ): DoubleVector3D { + var x = 0.0 + var y = 0.0 + var z = 0.0 + + for (j in (0..2)) { + for (k in (0..2)) { + x += leviCivita(0, j, k) * first[j] * second[k] + y += leviCivita(1, j, k) * first[j] * second[k] + z += leviCivita(2, j, k) * first[j] * second[k] + } + } + + return vector(x, y, z) * (if (rightBasis) 1 else -1) + } + + /** + * Vector product with right basis + */ + public infix fun DoubleVector3D.cross(other: DoubleVector3D): Vector3D = vectorProduct(this, other) + public val xAxis: DoubleVector3D = vector(1.0, 0.0, 0.0) public val yAxis: DoubleVector3D = vector(0.0, 1.0, 0.0) public val zAxis: DoubleVector3D = vector(0.0, 0.0, 1.0) diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt index 6d9a169eb..291b0ad47 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt @@ -57,23 +57,39 @@ internal class Euclidean3DSpaceTest { } @Test - fun add() { - with(Euclidean3DSpace) { - assertVectorEquals( - vector(1.0, -2.0, 0.001), - vector(1.0, -2.0, 0.001) + zero - ) - assertVectorEquals( - vector(8.0, -3.0, 3.001), - vector(1.0, 2.0, 3.0) + vector(7.0, -5.0, 0.001) - ) - } + fun add() = with(Euclidean3DSpace) { + assertVectorEquals( + vector(1.0, -2.0, 0.001), + vector(1.0, -2.0, 0.001) + zero + ) + assertVectorEquals( + vector(8.0, -3.0, 3.001), + vector(1.0, 2.0, 3.0) + vector(7.0, -5.0, 0.001) + ) } @Test - fun multiply() { - with(Euclidean3DSpace) { - assertVectorEquals(vector(2.0, -4.0, 0.0), vector(1.0, -2.0, 0.0) * 2) - } + fun multiply() = with(Euclidean3DSpace) { + assertVectorEquals(vector(2.0, -4.0, 0.0), vector(1.0, -2.0, 0.0) * 2) } + + @Test + fun vectorProduct() = with(Euclidean3DSpace) { + assertVectorEquals(zAxis, vectorProduct(xAxis, yAxis)) + assertVectorEquals(zAxis, xAxis cross yAxis) + assertVectorEquals(-zAxis, vectorProduct(yAxis, xAxis)) + assertVectorEquals(zAxis, vectorProduct(yAxis, xAxis, rightBasis = false)) + } + + @Test + fun doubleVectorProduct() = with(Euclidean3DSpace) { + val a = vector(1, 2, -3) + val b = vector(-1, 0, 1) + val c = vector(4, 5, 6) + + val res = a cross (b cross c) + val expected = b * (a dot c) - c * (a dot b) + assertVectorEquals(expected, res) + } + } From a3963ac4f50b7042cf21c51c8b5f97c36877249e Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Fri, 10 Mar 2023 21:40:14 +0300 Subject: [PATCH 06/30] Refactor series naming and docs --- .../kscience/kmath/series/SeriesAlgebra.kt | 79 +++++++++++-------- .../kscience/kmath/series/seriesExtensions.kt | 32 ++++---- 2 files changed, 62 insertions(+), 49 deletions(-) diff --git a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt index 3cd2212f6..45e392693 100644 --- a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt +++ b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt @@ -6,6 +6,7 @@ import space.kscience.kmath.operations.RingOps import space.kscience.kmath.stat.StatisticalAlgebra import space.kscience.kmath.structures.Buffer import space.kscience.kmath.structures.BufferView +import space.kscience.kmath.structures.getOrNull import kotlin.math.max import kotlin.math.min @@ -33,8 +34,6 @@ public interface Series : Buffer { public val position: Int } -public val Series.absoluteIndices: IntRange get() = position until position + size - /** * A [BufferView] with index offset (both positive and negative) and possible size change */ @@ -60,54 +59,68 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( private val labelResolver: (Int) -> L, ) : RingOps>, StatisticalAlgebra { - public val Buffer.indices: IntRange + /** + * A range of valid offset indices. In general, does not start with zero. + */ + public val Buffer.offsetIndices: IntRange get() = if (this is Series) { - absoluteIndices + position until position + size } else { 0 until size } /** - * Get the value by absolute index in the series algebra or return null if index is out of range + * Get the value by absolute offset in the series algebra or return null if index is out of range */ - public fun Buffer.getAbsoluteOrNull(index: Int): T? = when { - index !in indices -> null - this is Series -> origin[index - position] - else -> get(index) + public fun Buffer.getByOffsetOrNull(index: Int): T? = when { + index !in offsetIndices -> null + this is Series -> origin.getOrNull(index - position) + else -> getOrNull(index) } /** * Get the value by absolute index in the series algebra or throw [IndexOutOfBoundsException] if index is out of range */ - public fun Buffer.getAbsolute(index: Int): T = - getAbsoluteOrNull(index) ?: throw IndexOutOfBoundsException("Index $index is not in $indices") + public fun Buffer.getByOffset(index: Int): T = + getByOffsetOrNull(index) ?: throw IndexOutOfBoundsException("Index $index is not in $offsetIndices") /** - * Create an offset series with index starting point at [index] + * Zero-copy move [Buffer] or [Series] to given [position] ignoring series offset if it is present. */ - public fun Buffer.moveTo(index: Int): Series = if (this is Series) { - SeriesImpl(origin, index, size) + public fun Buffer.moveTo(position: Int): Series = if (this is Series) { + SeriesImpl(origin, position, size) } else { - SeriesImpl(this, index, size) + SeriesImpl(this, position, size) } - public val Buffer.offset: Int get() = if (this is Series) position else 0 + /** + * Zero-copy move [Buffer] or [Series] by given [offset]. If it is [Series], sum intrinsic series position and the [offset]. + */ + public fun Buffer.moveBy(offset: Int): Series = if (this is Series) { + SeriesImpl(origin, position + offset, size) + } else { + SeriesImpl(this, offset, size) + } /** - * Build a new series + * An offset of the buffer start relative to [SeriesAlgebra] zero offset */ - public fun series(size: Int, fromIndex: Int = 0, block: A.(label: L) -> T): Series { + public val Buffer.startOffset: Int get() = if (this is Series) position else 0 + + /** + * Build a new series positioned at [startOffset]. + */ + public fun series(size: Int, startOffset: Int = 0, block: A.(label: L) -> T): Series { return elementAlgebra.bufferFactory(size) { - val index = it + fromIndex + val index = it + startOffset elementAlgebra.block(labelResolver(index)) - }.moveTo(fromIndex) + }.moveTo(startOffset) } /** * Get a label buffer for given buffer. */ - public val Buffer.labels: List get() = indices.map(labelResolver) - + public val Buffer.labels: List get() = offsetIndices.map(labelResolver) /** * Try to resolve element by label and return null if element with a given label is not found @@ -115,7 +128,7 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( public operator fun Buffer.get(label: L): T? { val index = labels.indexOf(label) if (index == -1) return null - return getAbsolute(index + offset) + return getByOffset(index + startOffset) } /** @@ -123,9 +136,9 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( */ public inline fun Buffer.map(crossinline transform: A.(T) -> T): Series { val buf = elementAlgebra.bufferFactory(size) { - elementAlgebra.transform(getAbsolute(it)) + elementAlgebra.transform(getByOffset(it)) } - return buf.moveTo(indices.first) + return buf.moveTo(offsetIndices.first) } /** @@ -134,22 +147,22 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( public inline fun Buffer.mapWithLabel(crossinline transform: A.(arg: T, label: L) -> T): Series { val labels = labels val buf = elementAlgebra.bufferFactory(size) { - elementAlgebra.transform(getAbsolute(it), labels[it]) + elementAlgebra.transform(getByOffset(it), labels[it]) } - return buf.moveTo(indices.first) + return buf.moveTo(offsetIndices.first) } public inline fun Buffer.fold(initial: R, operation: A.(acc: R, T) -> R): R { var accumulator = initial - for (index in this.indices) accumulator = elementAlgebra.operation(accumulator, getAbsolute(index)) + for (index in this.offsetIndices) accumulator = elementAlgebra.operation(accumulator, getByOffset(index)) return accumulator } public inline fun Buffer.foldWithLabel(initial: R, operation: A.(acc: R, arg: T, label: L) -> R): R { val labels = labels var accumulator = initial - for (index in this.indices) accumulator = - elementAlgebra.operation(accumulator, getAbsolute(index), labels[index]) + for (index in this.offsetIndices) accumulator = + elementAlgebra.operation(accumulator, getByOffset(index), labels[index]) return accumulator } @@ -160,11 +173,11 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( other: Buffer, crossinline operation: A.(left: T, right: T) -> T, ): Series { - val newRange = indices.intersect(other.indices) + val newRange = offsetIndices.intersect(other.offsetIndices) return elementAlgebra.bufferFactory(newRange.size) { elementAlgebra.operation( - getAbsolute(it), - other.getAbsolute(it) + getByOffset(it), + other.getByOffset(it) ) }.moveTo(newRange.first) } diff --git a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/seriesExtensions.kt b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/seriesExtensions.kt index 882fa2c46..fa5e0addd 100644 --- a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/seriesExtensions.kt +++ b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/seriesExtensions.kt @@ -12,32 +12,32 @@ import space.kscience.kmath.structures.Buffer public fun SeriesAlgebra.sin( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.sin(arg).moveTo(arg.offset) + bufferAlgebra.sin(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.cos( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.cos(arg).moveTo(arg.offset) + bufferAlgebra.cos(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.tan( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.tan(arg).moveTo(arg.offset) + bufferAlgebra.tan(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.asin( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.asin(arg).moveTo(arg.offset) + bufferAlgebra.asin(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.acos( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.acos(arg).moveTo(arg.offset) + bufferAlgebra.acos(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.atan( arg: Buffer, ): Series where BA : BufferAlgebra, BA : TrigonometricOperations> = - bufferAlgebra.atan(arg).moveTo(arg.offset) + bufferAlgebra.atan(arg).moveTo(arg.startOffset) //exponential @@ -45,42 +45,42 @@ public fun SeriesAlgebra.atan( public fun SeriesAlgebra.exp( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.exp(arg).moveTo(arg.offset) + bufferAlgebra.exp(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.ln( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.ln(arg).moveTo(arg.offset) + bufferAlgebra.ln(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.sinh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.sinh(arg).moveTo(arg.offset) + bufferAlgebra.sinh(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.cosh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.cosh(arg).moveTo(arg.offset) + bufferAlgebra.cosh(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.tanh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.tanh(arg).moveTo(arg.offset) + bufferAlgebra.tanh(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.asinh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.asinh(arg).moveTo(arg.offset) + bufferAlgebra.asinh(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.acosh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.acosh(arg).moveTo(arg.offset) + bufferAlgebra.acosh(arg).moveTo(arg.startOffset) public fun SeriesAlgebra.atanh( arg: Buffer, ): Series where BA : BufferAlgebra, BA : ExponentialOperations> = - bufferAlgebra.atanh(arg).moveTo(arg.offset) + bufferAlgebra.atanh(arg).moveTo(arg.startOffset) //power @@ -89,9 +89,9 @@ public fun SeriesAlgebra.power( arg: Buffer, pow: Number, ): Series where BA : BufferAlgebra, BA : PowerOperations> = - bufferAlgebra.power(arg, pow).moveTo(arg.offset) + bufferAlgebra.power(arg, pow).moveTo(arg.startOffset) public fun SeriesAlgebra.sqrt( arg: Buffer, ): Series where BA : BufferAlgebra, BA : PowerOperations> = - bufferAlgebra.sqrt(arg).moveTo(arg.offset) \ No newline at end of file + bufferAlgebra.sqrt(arg).moveTo(arg.startOffset) \ No newline at end of file From 72c70302974a1cc568f32a6c6242d7180c77a9fd Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Fri, 10 Mar 2023 22:50:41 +0300 Subject: [PATCH 07/30] Add time series example stub --- examples/build.gradle.kts | 9 ++-- .../kscience/kmath/stat/DateTimeSeries.kt | 19 ++++++++ .../kmath/series/MonotonicSeriesAlgebra.kt | 47 +++++++++++++++++++ .../kscience/kmath/series/SeriesAlgebra.kt | 16 +++++-- 4 files changed, 82 insertions(+), 9 deletions(-) create mode 100644 examples/src/main/kotlin/space/kscience/kmath/stat/DateTimeSeries.kt create mode 100644 kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/MonotonicSeriesAlgebra.kt diff --git a/examples/build.gradle.kts b/examples/build.gradle.kts index da645f012..f7b7794f3 100644 --- a/examples/build.gradle.kts +++ b/examples/build.gradle.kts @@ -1,3 +1,5 @@ +import org.jetbrains.kotlin.gradle.tasks.KotlinJvmCompile + plugins { kotlin("jvm") } @@ -33,6 +35,8 @@ dependencies { implementation(project(":kmath-multik")) implementation("org.jetbrains.kotlinx:multik-default:$multikVersion") + //datetime + implementation("org.jetbrains.kotlinx:kotlinx-datetime:0.4.0") implementation("org.nd4j:nd4j-native:1.0.0-beta7") @@ -46,9 +50,6 @@ dependencies { // } else implementation("org.nd4j:nd4j-native-platform:1.0.0-beta7") - // multik implementation - implementation("org.jetbrains.kotlinx:multik-default:0.1.0") - implementation("org.slf4j:slf4j-simple:1.7.32") // plotting implementation("space.kscience:plotlykt-server:0.5.0") @@ -62,7 +63,7 @@ kotlin.sourceSets.all { } } -tasks.withType { +tasks.withType { kotlinOptions { jvmTarget = "11" freeCompilerArgs = freeCompilerArgs + "-Xjvm-default=all" + "-Xopt-in=kotlin.RequiresOptIn" + "-Xlambdas=indy" diff --git a/examples/src/main/kotlin/space/kscience/kmath/stat/DateTimeSeries.kt b/examples/src/main/kotlin/space/kscience/kmath/stat/DateTimeSeries.kt new file mode 100644 index 000000000..9836db6ea --- /dev/null +++ b/examples/src/main/kotlin/space/kscience/kmath/stat/DateTimeSeries.kt @@ -0,0 +1,19 @@ +/* + * 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.stat + +import kotlinx.datetime.Instant +import space.kscience.kmath.operations.algebra +import space.kscience.kmath.operations.bufferAlgebra +import space.kscience.kmath.series.MonotonicSeriesAlgebra +import space.kscience.kmath.series.SeriesAlgebra +import kotlin.time.Duration + +fun SeriesAlgebra.Companion.time(zero: Instant, step: Duration) = MonotonicSeriesAlgebra( + bufferAlgebra = Double.algebra.bufferAlgebra, + offsetToLabel = { zero + step * it }, + labelToOffset = { (it - zero) / step } +) \ No newline at end of file diff --git a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/MonotonicSeriesAlgebra.kt b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/MonotonicSeriesAlgebra.kt new file mode 100644 index 000000000..2bc363934 --- /dev/null +++ b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/MonotonicSeriesAlgebra.kt @@ -0,0 +1,47 @@ +/* + * 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.series + +import space.kscience.kmath.operations.BufferAlgebra +import space.kscience.kmath.operations.Ring +import space.kscience.kmath.structures.Buffer +import kotlin.math.ceil +import kotlin.math.floor + +/** + * A [SeriesAlgebra] with reverse label to index transformation. + * + * @param [labelToOffset] returns floating point number that is used for index resolution. + */ +public class MonotonicSeriesAlgebra, out BA : BufferAlgebra, L : Comparable>( + bufferAlgebra: BA, + offsetToLabel: (Int) -> L, + private val labelToOffset: (L) -> Double, +) : SeriesAlgebra(bufferAlgebra, offsetToLabel) { + + public val Buffer.labelRange: ClosedRange get() = offsetToLabel(startOffset)..offsetToLabel(startOffset + size) + + /** + * An offset of the given [label] rounded down + */ + public fun floorOffset(label: L): Int = floor(labelToOffset(label)).toInt() + + /** + * An offset of the given [label] rounded up + */ + public fun ceilOffset(label: L): Int = ceil(labelToOffset(label)).toInt() + + /** + * Get value by label (rounded down) or return null if the value is outside series boundaries. + */ + public fun Buffer.getByLabelOrNull(label: L): T? = getByOffsetOrNull(floorOffset(label)) + + /** + * Get value by label (rounded down) or throw [IndexOutOfBoundsException] if the value is outside series boundaries. + */ + public fun Buffer.getByLabel(label: L): T = getByLabelOrNull(label) + ?: throw IndexOutOfBoundsException("Label $label is not in $labelRange") +} \ No newline at end of file diff --git a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt index 45e392693..1847e33b6 100644 --- a/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt +++ b/kmath-stat/src/commonMain/kotlin/space/kscience/kmath/series/SeriesAlgebra.kt @@ -24,7 +24,9 @@ internal operator fun IntRange.contains(other: IntRange): Boolean = (other.first //TODO add permutation sort //TODO check rank statistics - +/** + * A [Buffer] with an offset relative to the [SeriesAlgebra] zero. + */ public interface Series : Buffer { public val origin: Buffer @@ -54,9 +56,9 @@ private class SeriesImpl( /** * A scope to operation on series */ -public class SeriesAlgebra, out BA : BufferAlgebra, L>( +public open class SeriesAlgebra, out BA : BufferAlgebra, L>( override val bufferAlgebra: BA, - private val labelResolver: (Int) -> L, + public val offsetToLabel: (Int) -> L, ) : RingOps>, StatisticalAlgebra { /** @@ -107,20 +109,22 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( */ public val Buffer.startOffset: Int get() = if (this is Series) position else 0 + public val Buffer.startLabel: L get() = offsetToLabel(startOffset) + /** * Build a new series positioned at [startOffset]. */ public fun series(size: Int, startOffset: Int = 0, block: A.(label: L) -> T): Series { return elementAlgebra.bufferFactory(size) { val index = it + startOffset - elementAlgebra.block(labelResolver(index)) + elementAlgebra.block(offsetToLabel(index)) }.moveTo(startOffset) } /** * Get a label buffer for given buffer. */ - public val Buffer.labels: List get() = offsetIndices.map(labelResolver) + public val Buffer.labels: List get() = offsetIndices.map(offsetToLabel) /** * Try to resolve element by label and return null if element with a given label is not found @@ -187,6 +191,8 @@ public class SeriesAlgebra, out BA : BufferAlgebra, L>( override fun add(left: Buffer, right: Buffer): Series = left.zip(right) { l, r -> l + r } override fun multiply(left: Buffer, right: Buffer): Buffer = left.zip(right) { l, r -> l * r } + + public companion object } public fun , BA : BufferAlgebra, L> BA.seriesAlgebra(labels: Iterable): SeriesAlgebra { From 4c1ffdb6d9db3ea37ff159ad95958184bb8fd517 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Tue, 14 Mar 2023 13:50:42 +0300 Subject: [PATCH 08/30] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 24 +++++++----- .../kscience/kmath/trajectory/DubinsTest.kt | 39 ++++++++++++++++++- 2 files changed, 52 insertions(+), 11 deletions(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index 6b247e55b..3aea139ec 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -136,7 +136,8 @@ public class DubinsObstacle( error("next tangent not found") } - public fun equals(other: DubinsObstacle): Boolean { + override fun equals(other: Any?): Boolean { + if (other == null || other !is DubinsObstacle) return false return this.circles == other.circles } } @@ -258,7 +259,7 @@ public fun constructTangentCircles(point: DoubleVector2D, val center2 = point + normalVectors(direction, r).second val p1 = center1 - point val p2 = center2 - point - return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) { + return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI/2, -3*PI/2)) { mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r), DubinsPath.SimpleType.R to Circle2D(center2, r)) } @@ -270,7 +271,7 @@ public fun constructTangentCircles(point: DoubleVector2D, public fun sortedObstacles(currentObstacle: DubinsObstacle, obstacles: List): List { - return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed() + return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() } public fun tangentsAlongTheObstacle(initialCircle: Circle2D, @@ -389,13 +390,13 @@ public fun findAllPaths( } nextTangents = if (nextObstacle == finalObstacle) { nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and - (DubinsPath.toSimpleTypes(it.key)[0] == j)} + (DubinsPath.toSimpleTypes(it.key)[2] == j)} as MutableMap } else { nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} as MutableMap } - val tangentsAlong = mutableListOf() + var tangentsAlong = mutableListOf() for (tangent in nextTangents.values) { if (tangent.startCircle == line.last().endCircle) { val lengthMaxPossible = arcLength( @@ -413,7 +414,7 @@ public fun findAllPaths( tangent.lineSegment.begin, currentDirection) if (lengthCalculated > lengthMaxPossible) { - val tangentsAlong = tangentsAlongTheObstacle( + tangentsAlong = tangentsAlongTheObstacle( currentCircle, DubinsPath.toType(listOf( currentDirection, @@ -424,11 +425,11 @@ public fun findAllPaths( ) } else { - val tangentsAlong = mutableListOf() + tangentsAlong = mutableListOf() } } else { - val tangentsAlong = tangentsAlongTheObstacle( + tangentsAlong = tangentsAlongTheObstacle( currentCircle, DubinsPath.toType(listOf( currentDirection, @@ -456,6 +457,11 @@ public fun findAllPaths( )] = newOutputTangents } } + outputTangents[listOf( + i, + DubinsPath.SimpleType.S, + j + )] = newOutputTangents } for (lineId in outputTangents[listOf( i, @@ -505,7 +511,7 @@ public fun findAllPaths( )]!! + outputTangents[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, - DubinsPath.SimpleType.L)]!! + DubinsPath.SimpleType.R)]!! } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index 16d77c84d..b26b21737 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -7,6 +7,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.DoubleVector2D +import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.kmath.geometry.Euclidean2DSpace.minus import space.kscience.kmath.geometry.Euclidean2DSpace.vector import kotlin.test.Test import kotlin.test.assertTrue @@ -68,6 +70,18 @@ class DubinsTest { obstacles) val length = pathLength(shortestPath(outputTangents)) println(length) + for (path in outputTangents) { + println(pathLength(path)) + println(path.size) + for (tangent in path) { +// println(tangent.route) +// println(tangent.startCircle) +// println(tangent.endCircle) +// println(Euclidean2DSpace.norm(tangent.lineSegment.end - tangent.lineSegment.begin)) + } + println() + println() + } } @Test fun outerTangentsTest1() { @@ -78,7 +92,6 @@ class DubinsTest { Circle2D(vector(5.0, 5.0), 1.0) ) println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) - assertTrue(false) } @Test fun outerTangentsTest2() { @@ -100,6 +113,28 @@ class DubinsTest { } } } - assertTrue(false) + } + @Test + fun tangentsTest() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + val obstacle1 = DubinsObstacle(listOf(circle1)) + val obstacle2 = DubinsObstacle(listOf(circle2)) + val tangent = dubinsTangentsToCircles(circle1, circle2, obstacle1, obstacle2) + println(tangent) + } + @Test + fun equalCircles() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + println(circle1 == circle2) + } + @Test + fun equalObstacles() { + val circle1 = Circle2D(vector(1.0, 6.5), 0.5) + val circle2 = Circle2D(vector(1.0, 6.5), 0.5) + val obstacle1 = DubinsObstacle(listOf(circle1)) + val obstacle2 = DubinsObstacle(listOf(circle2)) + println(obstacle1 == obstacle2) } } \ No newline at end of file From 28b85b0f53ba5d6e5e82777ffa8d57a770860baa Mon Sep 17 00:00:00 2001 From: Gleb Minaev <43728100+lounres@users.noreply.github.com> Date: Tue, 14 Mar 2023 20:13:34 +0300 Subject: [PATCH 09/30] Remove the choice of left-handed product. Refactor `vectorProduct`. Remove `leviChivita` function. --- .../kmath/geometry/Euclidean3DSpace.kt | 34 +++---------------- 1 file changed, 5 insertions(+), 29 deletions(-) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index f1cf0bad2..38c252bc0 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -103,45 +103,21 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< override fun DoubleVector3D.dot(other: DoubleVector3D): Double = x * other.x + y * other.y + z * other.z - private fun leviCivita(i: Int, j: Int, k: Int): Int = when { - // even permutation - i == 0 && j == 1 && k == 2 -> 1 - i == 1 && j == 2 && k == 0 -> 1 - i == 2 && j == 0 && k == 1 -> 1 - // odd permutations - i == 2 && j == 1 && k == 0 -> -1 - i == 0 && j == 2 && k == 1 -> -1 - i == 1 && j == 0 && k == 2 -> -1 - - else -> 0 - } - /** - * Compute vector product of [first] and [second]. The basis assumed to be right-handed if [rightBasis] is true and - * left-handed otherwise + * Compute vector product of [first] and [second]. The basis assumed to be right-handed. */ public fun vectorProduct( first: DoubleVector3D, second: DoubleVector3D, - rightBasis: Boolean = true, ): DoubleVector3D { - var x = 0.0 - var y = 0.0 - var z = 0.0 + val (x1, y1, z1) = first + val (x2, y2, z2) = second - for (j in (0..2)) { - for (k in (0..2)) { - x += leviCivita(0, j, k) * first[j] * second[k] - y += leviCivita(1, j, k) * first[j] * second[k] - z += leviCivita(2, j, k) * first[j] * second[k] - } - } - - return vector(x, y, z) * (if (rightBasis) 1 else -1) + return vector(y1 * z2 - y2 * z2, z1 * x2 - z2 * x2, x1 * y2 - x2 * y2) } /** - * Vector product with right basis + * Vector product with a right basis */ public infix fun DoubleVector3D.cross(other: DoubleVector3D): Vector3D = vectorProduct(this, other) From cd2ade881add23d4ac9b3f4b4d5ea594b6ad6d63 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Thu, 16 Mar 2023 09:33:17 +0300 Subject: [PATCH 10/30] Revert "Remove the choice of left-handed product. Refactor `vectorProduct`. Remove `leviChivita` function." This reverts commit 28b85b0f53ba5d6e5e82777ffa8d57a770860baa. --- .../kmath/geometry/Euclidean3DSpace.kt | 34 ++++++++++++++++--- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index 38c252bc0..f1cf0bad2 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -103,21 +103,45 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< override fun DoubleVector3D.dot(other: DoubleVector3D): Double = x * other.x + y * other.y + z * other.z + private fun leviCivita(i: Int, j: Int, k: Int): Int = when { + // even permutation + i == 0 && j == 1 && k == 2 -> 1 + i == 1 && j == 2 && k == 0 -> 1 + i == 2 && j == 0 && k == 1 -> 1 + // odd permutations + i == 2 && j == 1 && k == 0 -> -1 + i == 0 && j == 2 && k == 1 -> -1 + i == 1 && j == 0 && k == 2 -> -1 + + else -> 0 + } + /** - * Compute vector product of [first] and [second]. The basis assumed to be right-handed. + * Compute vector product of [first] and [second]. The basis assumed to be right-handed if [rightBasis] is true and + * left-handed otherwise */ public fun vectorProduct( first: DoubleVector3D, second: DoubleVector3D, + rightBasis: Boolean = true, ): DoubleVector3D { - val (x1, y1, z1) = first - val (x2, y2, z2) = second + var x = 0.0 + var y = 0.0 + var z = 0.0 - return vector(y1 * z2 - y2 * z2, z1 * x2 - z2 * x2, x1 * y2 - x2 * y2) + for (j in (0..2)) { + for (k in (0..2)) { + x += leviCivita(0, j, k) * first[j] * second[k] + y += leviCivita(1, j, k) * first[j] * second[k] + z += leviCivita(2, j, k) * first[j] * second[k] + } + } + + return vector(x, y, z) * (if (rightBasis) 1 else -1) } /** - * Vector product with a right basis + * Vector product with right basis */ public infix fun DoubleVector3D.cross(other: DoubleVector3D): Vector3D = vectorProduct(this, other) From ef336af87ddb6df9acc8029e9633234ae5fe6851 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Thu, 16 Mar 2023 09:37:03 +0300 Subject: [PATCH 11/30] Fix vector product --- .gitignore | 3 ++- CHANGELOG.md | 1 + examples/build.gradle.kts | 14 +++++----- gradle.properties | 2 +- .../space/kscience/kmath/nd/BufferND.kt | 26 +++++++++++++++++++ .../integration/GaussIntegratorRuleFactory.kt | 2 -- .../kmath/geometry/Euclidean3DSpace.kt | 6 ++--- .../kmath/geometry/Euclidean3DSpaceTest.kt | 1 - 8 files changed, 40 insertions(+), 15 deletions(-) diff --git a/.gitignore b/.gitignore index 34ddf3fd9..7713a9f96 100644 --- a/.gitignore +++ b/.gitignore @@ -18,4 +18,5 @@ out/ !/.idea/copyright/ !/.idea/scopes/ -/kotlin-js-store/yarn.lock +/gradle/yarn.lock + diff --git a/CHANGELOG.md b/CHANGELOG.md index 404366a03..c5fa3f372 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ ## [Unreleased] ### Added +- Generic builders for `BufferND` and `MutableBufferND` - `NamedMatrix` - matrix with symbol-based indexing - `Expression` with default arguments - Type-aliases for numbers like `Float64` diff --git a/examples/build.gradle.kts b/examples/build.gradle.kts index f7b7794f3..50708eaa9 100644 --- a/examples/build.gradle.kts +++ b/examples/build.gradle.kts @@ -55,17 +55,19 @@ dependencies { implementation("space.kscience:plotlykt-server:0.5.0") } -kotlin.sourceSets.all { - with(languageSettings) { - optIn("kotlin.contracts.ExperimentalContracts") - optIn("kotlin.ExperimentalUnsignedTypes") - optIn("space.kscience.kmath.misc.UnstableKMathAPI") +kotlin { + jvmToolchain(11) + sourceSets.all { + with(languageSettings) { + optIn("kotlin.contracts.ExperimentalContracts") + optIn("kotlin.ExperimentalUnsignedTypes") + optIn("space.kscience.kmath.misc.UnstableKMathAPI") + } } } tasks.withType { kotlinOptions { - jvmTarget = "11" freeCompilerArgs = freeCompilerArgs + "-Xjvm-default=all" + "-Xopt-in=kotlin.RequiresOptIn" + "-Xlambdas=indy" } } diff --git a/gradle.properties b/gradle.properties index 048c9c9f3..cded5934c 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.3-kotlin-1.8.20-RC +toolsVersion=0.14.4-kotlin-1.8.20-RC org.gradle.parallel=true diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/BufferND.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/BufferND.kt index 55e8bbcf8..a6bab8be1 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/BufferND.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/BufferND.kt @@ -7,7 +7,9 @@ package space.kscience.kmath.nd import space.kscience.kmath.misc.PerformancePitfall import space.kscience.kmath.structures.Buffer +import space.kscience.kmath.structures.BufferFactory import space.kscience.kmath.structures.MutableBuffer +import space.kscience.kmath.structures.MutableBufferFactory /** * Represents [StructureND] over [Buffer]. @@ -29,6 +31,18 @@ public open class BufferND( override fun toString(): String = StructureND.toString(this) } +/** + * Create a generic [BufferND] using provided [initializer] + */ +public fun BufferND( + shape: ShapeND, + bufferFactory: BufferFactory = BufferFactory.boxing(), + initializer: (IntArray) -> T, +): BufferND { + val strides = Strides(shape) + return BufferND(strides, bufferFactory(strides.linearSize) { initializer(strides.index(it)) }) +} + ///** // * Transform structure to a new structure using provided [BufferFactory] and optimizing if argument is [BufferND] // */ @@ -67,6 +81,18 @@ public open class MutableBufferND( } } +/** + * Create a generic [BufferND] using provided [initializer] + */ +public fun MutableBufferND( + shape: ShapeND, + bufferFactory: MutableBufferFactory = MutableBufferFactory.boxing(), + initializer: (IntArray) -> T, +): MutableBufferND { + val strides = Strides(shape) + return MutableBufferND(strides, bufferFactory(strides.linearSize) { initializer(strides.index(it)) }) +} + ///** // * Transform structure to a new structure using provided [MutableBufferFactory] and optimizing if argument is [MutableBufferND] // */ diff --git a/kmath-functions/src/commonMain/kotlin/space/kscience/kmath/integration/GaussIntegratorRuleFactory.kt b/kmath-functions/src/commonMain/kotlin/space/kscience/kmath/integration/GaussIntegratorRuleFactory.kt index fc76ea819..4ed4965c9 100644 --- a/kmath-functions/src/commonMain/kotlin/space/kscience/kmath/integration/GaussIntegratorRuleFactory.kt +++ b/kmath-functions/src/commonMain/kotlin/space/kscience/kmath/integration/GaussIntegratorRuleFactory.kt @@ -9,7 +9,6 @@ import space.kscience.kmath.operations.mapToBuffer import space.kscience.kmath.structures.Buffer import space.kscience.kmath.structures.DoubleBuffer import space.kscience.kmath.structures.asBuffer -import kotlin.jvm.Synchronized import kotlin.math.ulp import kotlin.native.concurrent.ThreadLocal @@ -57,7 +56,6 @@ public object GaussLegendreRuleFactory : GaussIntegratorRuleFactory { private val cache = HashMap, Buffer>>() - @Synchronized private fun getOrBuildRule(numPoints: Int): Pair, Buffer> = cache.getOrPut(numPoints) { buildRule(numPoints) } diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index f1cf0bad2..3059cefe6 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -117,13 +117,11 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< } /** - * Compute vector product of [first] and [second]. The basis assumed to be right-handed if [rightBasis] is true and - * left-handed otherwise + * Compute vector product of [first] and [second]. The basis assumed to be right-handed. */ public fun vectorProduct( first: DoubleVector3D, second: DoubleVector3D, - rightBasis: Boolean = true, ): DoubleVector3D { var x = 0.0 var y = 0.0 @@ -137,7 +135,7 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations< } } - return vector(x, y, z) * (if (rightBasis) 1 else -1) + return vector(x, y, z) } /** diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt index 291b0ad47..20e112ad1 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt @@ -78,7 +78,6 @@ internal class Euclidean3DSpaceTest { assertVectorEquals(zAxis, vectorProduct(xAxis, yAxis)) assertVectorEquals(zAxis, xAxis cross yAxis) assertVectorEquals(-zAxis, vectorProduct(yAxis, xAxis)) - assertVectorEquals(zAxis, vectorProduct(yAxis, xAxis, rightBasis = false)) } @Test From c36af3515e08adb3595a95a6470309eb149fc0c9 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 19 Mar 2023 18:39:27 +0300 Subject: [PATCH 12/30] Update trajectory description --- kmath-trajectory/build.gradle.kts | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kmath-trajectory/build.gradle.kts b/kmath-trajectory/build.gradle.kts index 689c3265b..32b87bb06 100644 --- a/kmath-trajectory/build.gradle.kts +++ b/kmath-trajectory/build.gradle.kts @@ -15,7 +15,7 @@ kscience{ } readme { - description = "Path and trajectory optimization" - maturity = space.kscience.gradle.Maturity.PROTOTYPE + 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")) } From 62c8610a9ebb45d5c450ef283ce21ec5e0d4de3c Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 19 Mar 2023 19:16:46 +0300 Subject: [PATCH 13/30] Update publishing CD --- .github/workflows/publish.yml | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 794881b09..8087195c2 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -15,7 +15,7 @@ jobs: runs-on: ${{matrix.os}} steps: - uses: actions/checkout@v3.0.0 - - uses: actions/setup-java@v3.0.0 + - uses: actions/setup-java@v3.10.0 with: java-version: 11 distribution: liberica @@ -26,26 +26,24 @@ jobs: key: ${{ runner.os }}-gradle-${{ hashFiles('*.gradle.kts') }} restore-keys: | ${{ runner.os }}-gradle- - - uses: gradle/wrapper-validation-action@v1.0.4 - name: Publish Windows Artifacts if: matrix.os == 'windows-latest' - uses: gradle/gradle-build-action@v2.1.5 + uses: gradle/gradle-build-action@v2.4.0 with: arguments: | - releaseAll + publishAllPublicationsToSpaceRepository -Ppublishing.enabled=true - -Ppublishing.sonatype=false -Ppublishing.space.user=${{ secrets.SPACE_APP_ID }} -Ppublishing.space.token=${{ secrets.SPACE_APP_SECRET }} - name: Publish Mac Artifacts if: matrix.os == 'macOS-latest' - uses: gradle/gradle-build-action@v2.1.5 + uses: gradle/gradle-build-action@v2.4.0 with: arguments: | - releaseMacosX64 - releaseIosArm64 - releaseIosX64 + publishMacosX64PublicationsToSpaceRepository + publishIosX64PublicationsToSpaceRepository + publishIosArm64PublicationsToSpaceRepository + publishIosSimulatorArm64PublicationsToSpaceRepository -Ppublishing.enabled=true - -Ppublishing.sonatype=false -Ppublishing.space.user=${{ secrets.SPACE_APP_ID }} -Ppublishing.space.token=${{ secrets.SPACE_APP_SECRET }} From c442eb7e94ff562688b171cfcdf7deb829b71d47 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 19 Mar 2023 19:41:31 +0300 Subject: [PATCH 14/30] Fix publish task names --- .github/workflows/publish.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 8087195c2..0ac8805e0 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -40,10 +40,10 @@ jobs: uses: gradle/gradle-build-action@v2.4.0 with: arguments: | - publishMacosX64PublicationsToSpaceRepository - publishIosX64PublicationsToSpaceRepository - publishIosArm64PublicationsToSpaceRepository - publishIosSimulatorArm64PublicationsToSpaceRepository + publishMacosX64PublicationToSpaceRepository + publishIosX64PublicationToSpaceRepository + publishIosArm64PublicationToSpaceRepository + publishIosSimulatorArm64PublicationToSpaceRepository -Ppublishing.enabled=true -Ppublishing.space.user=${{ secrets.SPACE_APP_ID }} -Ppublishing.space.token=${{ secrets.SPACE_APP_SECRET }} From 81213eb94340fc87ae6958d30e598d234439dc35 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Tue, 21 Mar 2023 12:04:27 +0300 Subject: [PATCH 15/30] search for shortest path algorithm --- .../kmath/trajectory/DubinsObstacle.kt | 87 +++++++++---------- .../kscience/kmath/trajectory/DubinsTest.kt | 50 +---------- 2 files changed, 45 insertions(+), 92 deletions(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index 3aea139ec..896803fba 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -39,8 +39,6 @@ public class DubinsObstacle( 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 { @@ -76,8 +74,7 @@ public class DubinsObstacle( } } } -// val firstCircles = this.circles.slice(-this.circles.size..-1) -// val secondCircles = this.circles.slice(-this.circles.size+1..0) + val firstCircles = this.circles val secondCircles = this.circles.slice(1..this.circles.lastIndex) + this.circles[0] @@ -149,7 +146,7 @@ public data class DubinsTangent(val startCircle: Circle2D, val lineSegment: LineSegment2D, val route: PathTypes) -public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { +private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { return v1.x * v2.y - v1.y * v2.x } @@ -164,7 +161,7 @@ public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { return true } -public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { +private fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0) val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) + (this.begin.y - this.end.y) * (this.end.y - circle.center.y)) @@ -184,7 +181,7 @@ public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { return false } -public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { +private fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { for (tangent in obstacle.tangents) { if (this.lineSegment.intersectSegment(tangent.lineSegment)) { return true @@ -198,7 +195,7 @@ public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { return false } -public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { +private fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { return buildMap { for (circle1 in first.circles) { for (circle2 in second.circles) { @@ -216,7 +213,7 @@ public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): Mutable }.toMutableMap() } -public fun arcLength(circle: Circle2D, +private fun arcLength(circle: Circle2D, point1: DoubleVector2D, point2: DoubleVector2D, route: DubinsPath.SimpleType): Double { @@ -245,14 +242,14 @@ public fun arcLength(circle: Circle2D, return circle.radius * angle } -public fun normalVectors(v: DoubleVector2D, r: Double): Pair { +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)) ) } -public fun constructTangentCircles(point: DoubleVector2D, +private fun constructTangentCircles(point: DoubleVector2D, direction: DoubleVector2D, r: Double): Map { val center1 = point + normalVectors(direction, r).first @@ -269,12 +266,12 @@ public fun constructTangentCircles(point: DoubleVector2D, } } -public fun sortedObstacles(currentObstacle: DubinsObstacle, +private fun sortedObstacles(currentObstacle: DubinsObstacle, obstacles: List): List { return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() } -public fun tangentsAlongTheObstacle(initialCircle: Circle2D, +private fun tangentsAlongTheObstacle(initialCircle: Circle2D, initialRoute: DubinsPath.Type, finalCircle: Circle2D, obstacle: DubinsObstacle): MutableList { @@ -288,7 +285,7 @@ public fun tangentsAlongTheObstacle(initialCircle: Circle2D, return dubinsTangents } -public fun allFinished(paths: List>, +private fun allFinished(paths: List>, finalObstacle: DubinsObstacle): Boolean { for (path in paths) { if (path.last().endObstacle != finalObstacle) { @@ -333,28 +330,28 @@ public fun findAllPaths( finalPoint, finalDirection, finalRadius) - var outputTangents = mutableMapOf>>() + var path = mutableMapOf>>() for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { val finalCircle = finalCircles[j]!! val finalObstacle = DubinsObstacle(listOf(finalCircle)) - outputTangents[listOf(i, - DubinsPath.SimpleType.S, - j)] = mutableListOf( - mutableListOf(DubinsTangent( - initialCircles[i]!!, - initialCircles[i]!!, - DubinsObstacle(listOf(initialCircles[i]!!)), - DubinsObstacle(listOf(initialCircles[i]!!)), - LineSegment2D(startingPoint, startingPoint), - listOf(i, DubinsPath.SimpleType.S, i) - ))) - var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) - while (!allFinished(outputTangents[listOf(i, + path[listOf(i, + DubinsPath.SimpleType.S, + j)] = mutableListOf( + mutableListOf(DubinsTangent( + initialCircles[i]!!, + initialCircles[i]!!, + DubinsObstacle(listOf(initialCircles[i]!!)), + DubinsObstacle(listOf(initialCircles[i]!!)), + LineSegment2D(startingPoint, startingPoint), + listOf(i, DubinsPath.SimpleType.S, i) + ))) + //var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) + while (!allFinished(path[listOf(i, DubinsPath.SimpleType.S, j)]!!, finalObstacle)) { - var newOutputTangents = mutableListOf>() - for (line in outputTangents[listOf(i, + var newPaths = mutableListOf>() + for (line in path[listOf(i, DubinsPath.SimpleType.S, j)]!!) { var currentCircle = line.last().endCircle @@ -439,41 +436,41 @@ public fun findAllPaths( currentObstacle ) } - newOutputTangents.add((line + tangentsAlong + listOf(tangent)).toMutableList()) + newPaths.add((line + tangentsAlong + listOf(tangent)).toMutableList()) } - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } else { // minor changes from Python code - newOutputTangents.add(line) - outputTangents[listOf( + newPaths.add(line) + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } } - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j - )] = newOutputTangents + )] = newPaths } - for (lineId in outputTangents[listOf( + for (lineId in path[listOf( i, DubinsPath.SimpleType.S, j )]!!.indices) { - val lastDirection = outputTangents[listOf( + val lastDirection = path[listOf( i, DubinsPath.SimpleType.S, j )]!![lineId].last().route[2] - outputTangents[listOf( + path[listOf( i, DubinsPath.SimpleType.S, j @@ -496,19 +493,19 @@ public fun findAllPaths( } } } - return outputTangents[listOf( + return path[listOf( DubinsPath.SimpleType.L, DubinsPath.SimpleType.S, DubinsPath.SimpleType.L - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.L, DubinsPath.SimpleType.S, DubinsPath.SimpleType.R - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, DubinsPath.SimpleType.L - )]!! + outputTangents[listOf( + )]!! + path[listOf( DubinsPath.SimpleType.R, DubinsPath.SimpleType.S, DubinsPath.SimpleType.R)]!! diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index b26b21737..a78a5317d 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -6,12 +6,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.DoubleVector2D -import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.Euclidean2DSpace.minus import space.kscience.kmath.geometry.Euclidean2DSpace.vector import kotlin.test.Test -import kotlin.test.assertTrue class DubinsTest { @Test @@ -60,7 +56,7 @@ class DubinsTest { Circle2D(vector(9.0, 4.0), 0.5) )) ) - val outputTangents = findAllPaths( + val paths = findAllPaths( startPoint, startDirection, startRadius, @@ -68,9 +64,9 @@ class DubinsTest { finalDirection, finalRadius, obstacles) - val length = pathLength(shortestPath(outputTangents)) + val length = pathLength(shortestPath(paths)) println(length) - for (path in outputTangents) { + for (path in paths) { println(pathLength(path)) println(path.size) for (tangent in path) { @@ -84,46 +80,6 @@ class DubinsTest { } } @Test - fun outerTangentsTest1() { - // works incorrectly - val circles1 = listOf( - Circle2D(vector(0.0, 0.0), 1.0)) - val circles2 = listOf( - Circle2D(vector(5.0, 5.0), 1.0) - ) - println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) - } - @Test - fun outerTangentsTest2() { - // works incorrectly - val circles1 = listOf( - Circle2D(vector(0.0, 0.0), 1.0), - Circle2D(vector( 2.0, 0.0), 1.0)) - val circles2 = listOf( - Circle2D(vector(5.0, 5.0), 1.0), - Circle2D(vector(7.0, 5.0), 1.0) - ) - println(outerTangents(DubinsObstacle(circles1), DubinsObstacle(circles2))) - - for (circle1 in circles1) { - for (circle2 in circles2) { - for (tangent in dubinsTangentsToCircles(circle1, circle2, - DubinsObstacle(circles1), DubinsObstacle(circles2))) { - println(tangent) - } - } - } - } - @Test - fun tangentsTest() { - val circle1 = Circle2D(vector(1.0, 6.5), 0.5) - val circle2 = Circle2D(vector(1.0, 6.5), 0.5) - val obstacle1 = DubinsObstacle(listOf(circle1)) - val obstacle2 = DubinsObstacle(listOf(circle2)) - val tangent = dubinsTangentsToCircles(circle1, circle2, obstacle1, obstacle2) - println(tangent) - } - @Test fun equalCircles() { val circle1 = Circle2D(vector(1.0, 6.5), 0.5) val circle2 = Circle2D(vector(1.0, 6.5), 0.5) From 56bba749c026863a02ed6eb461d85d29d56e81e6 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Wed, 22 Mar 2023 10:54:24 +0300 Subject: [PATCH 16/30] Update publishing --- .github/workflows/publish.yml | 4 ++-- gradle.properties | 2 +- .../kotlin/space/kscience/kmath/coroutines/coroutinesExtra.kt | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 0ac8805e0..471388364 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -32,7 +32,7 @@ jobs: with: arguments: | publishAllPublicationsToSpaceRepository - -Ppublishing.enabled=true + -Ppublishing.targets=all -Ppublishing.space.user=${{ secrets.SPACE_APP_ID }} -Ppublishing.space.token=${{ secrets.SPACE_APP_SECRET }} - name: Publish Mac Artifacts @@ -44,6 +44,6 @@ jobs: publishIosX64PublicationToSpaceRepository publishIosArm64PublicationToSpaceRepository publishIosSimulatorArm64PublicationToSpaceRepository - -Ppublishing.enabled=true + -Ppublishing.targets=all -Ppublishing.space.user=${{ secrets.SPACE_APP_ID }} -Ppublishing.space.token=${{ secrets.SPACE_APP_SECRET }} diff --git a/gradle.properties b/gradle.properties index cded5934c..262bcabfb 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.4-kotlin-1.8.20-RC +toolsVersion=0.14.5-kotlin-1.8.20-RC org.gradle.parallel=true diff --git a/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/coroutines/coroutinesExtra.kt b/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/coroutines/coroutinesExtra.kt index 2fd4f0057..48be93b87 100644 --- a/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/coroutines/coroutinesExtra.kt +++ b/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/coroutines/coroutinesExtra.kt @@ -8,6 +8,7 @@ package space.kscience.kmath.coroutines import kotlinx.coroutines.* +import kotlinx.coroutines.channels.ReceiveChannel import kotlinx.coroutines.channels.produce import kotlinx.coroutines.flow.* @@ -57,7 +58,7 @@ public suspend fun AsyncFlow.collect(concurrency: Int, collector: FlowCol coroutineScope { //Starting up to N deferred coroutines ahead of time - val channel = produce(capacity = concurrency - 1) { + val channel: ReceiveChannel> = produce(capacity = concurrency - 1) { deferredFlow.collect { value -> value.start(this@coroutineScope) send(value) From d87eefcaa359f92a9a35c34784e196f767aa2836 Mon Sep 17 00:00:00 2001 From: SPC-code <112205870+SPC-code@users.noreply.github.com> Date: Wed, 22 Mar 2023 18:04:16 +0300 Subject: [PATCH 17/30] Add macOsArm64 to publish.yml --- .github/workflows/publish.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 471388364..ab9243f15 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -41,6 +41,7 @@ jobs: with: arguments: | publishMacosX64PublicationToSpaceRepository + publishMacosArm64PublicationToSpaceRepository publishIosX64PublicationToSpaceRepository publishIosArm64PublicationToSpaceRepository publishIosSimulatorArm64PublicationToSpaceRepository From ea5305c8d878e910128543f44453fe7e20294164 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Fri, 24 Mar 2023 10:28:02 +0300 Subject: [PATCH 18/30] search for shortest path algorithm --- .../kscience/kmath/trajectory/DubinsTest.kt | 18 ++++-------------- .../kscience/kmath/trajectory/TangentTest.kt | 15 +-------------- 2 files changed, 5 insertions(+), 28 deletions(-) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index a78a5317d..fdc3bd3c7 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -7,7 +7,9 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.vector +import space.kscience.kmath.geometry.equalsFloat import kotlin.test.Test +import kotlin.test.assertTrue class DubinsTest { @Test @@ -31,7 +33,7 @@ class DubinsTest { finalRadius, obstacles) val length = pathLength(shortestPath(outputTangents)) - println(length) + assertTrue(length.equalsFloat(27.2113183)) } @Test @@ -65,19 +67,7 @@ class DubinsTest { finalRadius, obstacles) val length = pathLength(shortestPath(paths)) - println(length) - for (path in paths) { - println(pathLength(path)) - println(path.size) - for (tangent in path) { -// println(tangent.route) -// println(tangent.startCircle) -// println(tangent.endCircle) -// println(Euclidean2DSpace.norm(tangent.lineSegment.end - tangent.lineSegment.begin)) - } - println() - println() - } + assertTrue(length.equalsFloat(28.9678224)) } @Test fun equalCircles() { 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 index 6d4493124..2ae89038c 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/TangentTest.kt @@ -12,6 +12,7 @@ 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 { @@ -60,18 +61,4 @@ class TangentTest { val c2 = Circle2D(vector(0.0, 0.0), 1.0) assertEquals(emptyMap(), c1.tangentsToCircle(c2)) } -// -// @Test -// fun nonExistingTangents() { -// assertFailsWith { -// val c1 = Circle2D(vector(0.0, 0.0), 1.0) -// val c2 = Circle2D(vector(2.0, 0.0), 1.0) -// c1.tangentsToCircle(c2) -// } -// assertFailsWith { -// val c1 = Circle2D(vector(0.0, 0.0), 1.0) -// val c2 = Circle2D(vector(0.5, 0.0), 1.0) -// c1.tangentsToCircle(c2) -// } -// } } \ No newline at end of file From 24c39c97cd4b9186a8445dedffff4c7fcc3e5ca1 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Fri, 24 Mar 2023 10:30:13 +0300 Subject: [PATCH 19/30] search for shortest path algorithm --- .../kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt index 896803fba..ee0b58a15 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt @@ -255,7 +255,6 @@ private fun constructTangentCircles(point: DoubleVector2D, val center1 = point + normalVectors(direction, r).first val center2 = point + normalVectors(direction, r).second val p1 = center1 - point - val p2 = center2 - point return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI/2, -3*PI/2)) { mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r), DubinsPath.SimpleType.R to Circle2D(center2, r)) From 11dd4088d977f5c2ecacfe264e77bf432a44a8e3 Mon Sep 17 00:00:00 2001 From: Artyom Degtyarev Date: Fri, 24 Mar 2023 10:39:51 +0300 Subject: [PATCH 20/30] search for shortest path algorithm --- .../kotlin/space/kscience/kmath/trajectory/DubinsTest.kt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt index fdc3bd3c7..07e9861c7 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt @@ -9,6 +9,7 @@ import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.vector import space.kscience.kmath.geometry.equalsFloat import kotlin.test.Test +import kotlin.test.assertEquals import kotlin.test.assertTrue class DubinsTest { @@ -33,7 +34,7 @@ class DubinsTest { finalRadius, obstacles) val length = pathLength(shortestPath(outputTangents)) - assertTrue(length.equalsFloat(27.2113183)) + assertEquals(length, 27.2113183, 1e-6) } @Test @@ -67,7 +68,7 @@ class DubinsTest { finalRadius, obstacles) val length = pathLength(shortestPath(paths)) - assertTrue(length.equalsFloat(28.9678224)) + assertEquals(length,28.9678224, 1e-6) } @Test fun equalCircles() { From f809e40791f73ab33bb05d3a23947fe8af310f98 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 11:42:58 +0300 Subject: [PATCH 21/30] Disentangle obstacle code phase 1 --- .../kmath/trajectory/DubinsObstacle.kt | 519 --------------- .../kscience/kmath/trajectory/DubinsPath.kt | 241 ++----- .../kscience/kmath/trajectory/Obstacle.kt | 619 ++++++++++++++++++ .../kscience/kmath/trajectory/Trajectory2D.kt | 34 +- .../kscience/kmath/trajectory/tangent.kt | 114 ---- .../trajectory/{dubins => }/DubinsTests.kt | 7 +- .../{DubinsTest.kt => ObstacleTest.kt} | 65 +- .../kmath/trajectory/segments/ArcTests.kt | 3 +- 8 files changed, 748 insertions(+), 854 deletions(-) delete mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt create 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/tangent.kt rename kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/{dubins => }/DubinsTests.kt (93%) rename kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/{DubinsTest.kt => ObstacleTest.kt} (55%) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt deleted file mode 100644 index ee0b58a15..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsObstacle.kt +++ /dev/null @@ -1,519 +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.plus -import space.kscience.kmath.geometry.Euclidean2DSpace.times -import space.kscience.kmath.geometry.Euclidean2DSpace.vector -import space.kscience.kmath.geometry.Euclidean2DSpace.norm -import space.kscience.kmath.operations.DoubleField.pow -import kotlin.math.* - -public fun LineSegment2D.length(): Double { - return ((end.y - begin.y).pow(2.0) + (end.x - begin.x).pow(2.0)).pow(0.5) -} -public class DubinsObstacle( - public val circles: List -) { - public val tangents: List = boundaryTangents().first - public val boundaryRoute: DubinsPath.Type = boundaryTangents().second - public val center: Vector2D = - vector(this.circles.sumOf{it.center.x} / this.circles.size, - this.circles.sumOf{it.center.y} / this.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 ((route, 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(route, DubinsTangent(Circle2D(center, radius), - other, - this@DubinsObstacle, - this@DubinsObstacle, - LineSegment2D( - center + w * r1, - other.center + w * r2 - ), - DubinsPath.toSimpleTypes(route)) - ) - } else { - throw Exception("Circles should not intersect") - } - } - } - } - - val firstCircles = this.circles - val secondCircles = this.circles.slice(1..this.circles.lastIndex) + - this.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( - this.circles.sumOf { it.center.x } / this.circles.size, - this.circles.sumOf { it.center.y } / this.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) - } - } - - public fun nextTangent(circle: Circle2D, route: DubinsPath.Type): DubinsTangent { - if (route == this.boundaryRoute) { - for (i in this.circles.indices) { - if (this.circles[i] == circle) { - return this.tangents[i] - } - } - } - else { - for (i in this.circles.indices) { - if (this.circles[i] == circle) { - if (i > 0) { - return DubinsTangent(this.circles[i], - this.circles[i-1], - this, - this, - LineSegment2D(this.tangents[i-1].lineSegment.end, - this.tangents[i-1].lineSegment.begin), - DubinsPath.toSimpleTypes(route)) - } - else { - return DubinsTangent(this.circles[0], - this.circles.last(), - this, - this, - LineSegment2D(this.tangents.last().lineSegment.end, - this.tangents.last().lineSegment.begin), - DubinsPath.toSimpleTypes(route)) - } - } - } - } - - error("next tangent not found") - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is DubinsObstacle) return false - return this.circles == other.circles - } -} - -public data class DubinsTangent(val startCircle: Circle2D, - val endCircle: Circle2D, - val startObstacle: DubinsObstacle, - val endObstacle: DubinsObstacle, - val lineSegment: LineSegment2D, - val route: PathTypes) - -private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { - fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { - return v1.x * v2.y - v1.y * v2.x - } - if (crossProduct(other.begin - this.begin, other.end - this.begin).sign == - crossProduct(other.begin - this.end, other.end - this.end).sign) { - return false - } - if (crossProduct(this.begin - other.begin, this.end - other.begin).sign == - crossProduct(this.begin - other.end, this.end - other.end).sign) { - return false - } - return true -} - -private fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean { - val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0) - val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) + - (this.begin.y - this.end.y) * (this.end.y - circle.center.y)) - val c = (this.end.x - circle.center.x).pow(2.0) + (this.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 DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean { - for (tangent in obstacle.tangents) { - if (this.lineSegment.intersectSegment(tangent.lineSegment)) { - return true - } - } - for (circle in obstacle.circles) { - if (this.lineSegment.intersectCircle(circle)) { - return true - } - } - return false -} - -private fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap { - return 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 - ) - } - } - } - } - }.toMutableMap() -} - -private fun arcLength(circle: Circle2D, - point1: DoubleVector2D, - point2: DoubleVector2D, - route: DubinsPath.SimpleType): 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 (route) { - DubinsPath.SimpleType.L -> { - angle = if (phi2 >= phi1) { - phi2 - phi1 - } else { - 2 * PI + phi2 - phi1 - } - } - DubinsPath.SimpleType.R -> { - angle = if (phi2 >= phi1) { - 2 * PI - (phi2 - phi1) - } else { - -(phi2 - phi1) - } - } - DubinsPath.SimpleType.S -> { - error("L or R route is expected") - } - } - 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(DubinsPath.SimpleType.L to Circle2D(center1, r), - DubinsPath.SimpleType.R to Circle2D(center2, r)) - } - else { - mapOf(DubinsPath.SimpleType.L to Circle2D(center2, r), - DubinsPath.SimpleType.R to Circle2D(center1, r)) - } -} - -private fun sortedObstacles(currentObstacle: DubinsObstacle, - obstacles: List): List { - return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed() -} - -private fun tangentsAlongTheObstacle(initialCircle: Circle2D, - initialRoute: DubinsPath.Type, - finalCircle: Circle2D, - obstacle: DubinsObstacle): MutableList { - val dubinsTangents = mutableListOf() - var tangent = obstacle.nextTangent(initialCircle, initialRoute) - dubinsTangents.add(tangent) - while (tangent.endCircle != finalCircle) { - tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) - dubinsTangents.add(tangent) - } - return dubinsTangents -} - -private fun allFinished(paths: List>, - finalObstacle: DubinsObstacle): Boolean { - for (path in paths) { - if (path.last().endObstacle != finalObstacle) { - return false - } - } - return true -} - -public fun pathLength(path: List): Double { - val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)} - val arcsLength = buildList{ - for (i in 1..path.lastIndex) { - add(arcLength(path[i].startCircle, - path[i-1].lineSegment.end, - path[i].lineSegment.begin, - path[i].route[0])) - } - }.sum() - return tangentsLength + arcsLength -} - -public fun shortestPath(path: List>): List { - return path.sortedBy { pathLength(it) }[0] -} - -public typealias Path = List -public fun findAllPaths( - startingPoint: DoubleVector2D, - startingDirection: DoubleVector2D, - startingRadius: Double, - finalPoint: DoubleVector2D, - finalDirection: DoubleVector2D, - finalRadius: Double, - obstacles: List -): List> { - val initialCircles = constructTangentCircles( - startingPoint, - startingDirection, - startingRadius) - val finalCircles = constructTangentCircles( - finalPoint, - finalDirection, - finalRadius) - var path = mutableMapOf>>() - for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { - for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) { - val finalCircle = finalCircles[j]!! - val finalObstacle = DubinsObstacle(listOf(finalCircle)) - path[listOf(i, - DubinsPath.SimpleType.S, - j)] = mutableListOf( - mutableListOf(DubinsTangent( - initialCircles[i]!!, - initialCircles[i]!!, - DubinsObstacle(listOf(initialCircles[i]!!)), - DubinsObstacle(listOf(initialCircles[i]!!)), - LineSegment2D(startingPoint, startingPoint), - listOf(i, DubinsPath.SimpleType.S, i) - ))) - //var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!)) - while (!allFinished(path[listOf(i, - DubinsPath.SimpleType.S, - j)]!!, finalObstacle)) { - var newPaths = mutableListOf>() - for (line in path[listOf(i, - DubinsPath.SimpleType.S, - j)]!!) { - var currentCircle = line.last().endCircle - var currentDirection = line.last().route.last() - var currentObstacle = line.last().endObstacle - var nextObstacle: DubinsObstacle? = null - if (currentObstacle != finalObstacle) { - var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf( - currentDirection, - DubinsPath.SimpleType.S, - j) - )] - for (obstacle in sortedObstacles(currentObstacle, obstacles)) { - if (tangentToFinal!!.intersectObstacle(obstacle)) { - nextObstacle = obstacle - break - } - } - if (nextObstacle == null) { - nextObstacle = finalObstacle - } - var nextTangents = outerTangents(currentObstacle, nextObstacle) - - for (pathType in DubinsPath.Type.values()) { - for (obstacle in obstacles) { - // in Python code here try/except was used, but seems unneeded - if (nextTangents.containsKey(pathType)) { - if (nextTangents[pathType]!!.intersectObstacle(obstacle)) { - nextTangents.remove(pathType) - } - } - } - } - nextTangents = if (nextObstacle == finalObstacle) { - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and - (DubinsPath.toSimpleTypes(it.key)[2] == j)} - as MutableMap - } else { - nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)} - as MutableMap - } - var tangentsAlong = mutableListOf() - for (tangent in nextTangents.values) { - if (tangent.startCircle == line.last().endCircle) { - val lengthMaxPossible = arcLength( - tangent.startCircle, - line.last().lineSegment.end, - tangent.startObstacle.nextTangent( - tangent.startCircle, - DubinsPath.toType(listOf(currentDirection, DubinsPath.SimpleType.S, currentDirection)), - ).lineSegment.begin, - currentDirection - ) - val lengthCalculated = arcLength( - tangent.startCircle, - line.last().lineSegment.end, - tangent.lineSegment.begin, - currentDirection) - if (lengthCalculated > lengthMaxPossible) { - tangentsAlong = tangentsAlongTheObstacle( - currentCircle, - DubinsPath.toType(listOf( - currentDirection, - DubinsPath.SimpleType.S, - currentDirection)), - tangent.startCircle, - currentObstacle - ) - } - else { - tangentsAlong = mutableListOf() - } - } - else { - tangentsAlong = tangentsAlongTheObstacle( - currentCircle, - DubinsPath.toType(listOf( - currentDirection, - DubinsPath.SimpleType.S, - currentDirection)), - tangent.startCircle, - currentObstacle - ) - } - newPaths.add((line + tangentsAlong + listOf(tangent)).toMutableList()) - } - path[listOf( - i, - DubinsPath.SimpleType.S, - j - )] = newPaths - } - else { - // minor changes from Python code - newPaths.add(line) - path[listOf( - i, - DubinsPath.SimpleType.S, - j - )] = newPaths - } - } - path[listOf( - i, - DubinsPath.SimpleType.S, - j - )] = newPaths - } - for (lineId in path[listOf( - i, - DubinsPath.SimpleType.S, - j - )]!!.indices) { - val lastDirection = path[listOf( - i, - DubinsPath.SimpleType.S, - j - )]!![lineId].last().route[2] - path[listOf( - i, - DubinsPath.SimpleType.S, - j - )]!![lineId].add(DubinsTangent( - finalCircles[j]!!, - finalCircles[j]!!, - DubinsObstacle( - listOf(finalCircles[j]!!) - ), - DubinsObstacle( - listOf(finalCircles[j]!!) - ), - LineSegment2D(finalPoint, finalPoint), - listOf( - lastDirection, - DubinsPath.SimpleType.S, - j - ) - )) - } - } - } - return path[listOf( - DubinsPath.SimpleType.L, - DubinsPath.SimpleType.S, - DubinsPath.SimpleType.L - )]!! + path[listOf( - DubinsPath.SimpleType.L, - DubinsPath.SimpleType.S, - DubinsPath.SimpleType.R - )]!! + path[listOf( - DubinsPath.SimpleType.R, - DubinsPath.SimpleType.S, - DubinsPath.SimpleType.L - )]!! + path[listOf( - DubinsPath.SimpleType.R, - DubinsPath.SimpleType.S, - DubinsPath.SimpleType.R)]!! -} - - - - - - - - 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 index 5654d10ae..272cf9e5b 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -7,6 +7,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.* import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.trajectory.Trajectory2D.Type +import space.kscience.kmath.trajectory.Trajectory2D.Type.* import kotlin.math.acos internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first @@ -19,27 +21,21 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair vector( - a.center.x - a.radius * cos(centers.bearing), - a.center.y + a.radius * sin(centers.bearing) + 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) ) - CircleTrajectory2D.Direction.RIGHT -> vector( - a.center.x + a.radius * cos(centers.bearing), - a.center.y - a.radius * sin(centers.bearing) + R -> vector( + from.center.x + from.radius * cos(centers.bearing), + from.center.y - from.radius * sin(centers.bearing) ) + + else -> error("S trajectory type not allowed") } return StraightTrajectory2D( p1, @@ -47,29 +43,25 @@ private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory2D.Dire ) } -internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = - innerTangent(base, direction, CircleTrajectory2D.Direction.LEFT) - -internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? = - innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT) private fun innerTangent( - base: Circle2D, - direction: Circle2D, - side: CircleTrajectory2D.Direction, + from: Circle2D, + to: Circle2D, + direction: Type, ): StraightTrajectory2D? = with(Euclidean2DSpace) { - val centers = StraightTrajectory2D(base.center, direction.center) - if (centers.length < base.radius * 2) return null - val angle = when (side) { - CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians - CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians + 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 + else -> error("S trajectory type not allowed") }.normalized() - val dX = base.radius * sin(angle) - val dY = base.radius * cos(angle) - val p1 = vector(base.center.x + dX, base.center.y + dY) - val p2 = vector(direction.center.x - dX, direction.center.y - dY) + 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) } @@ -77,118 +69,25 @@ private fun innerTangent( @Suppress("DuplicatedCode") public object DubinsPath { -// public class ArcType(private val type: Type){ -// public val first: SimpleType -// get() { -// if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) { -// return SimpleType.R -// } -// else if (type in listOf(Type.LSL, Type.LSR, Type.LRL)) { -// return SimpleType.L -// } -// error("Wrong DubinsPath.Type") -// } -// -// public val last: SimpleType -// get() { -// if (type in listOf(Type.RSR, Type.LSR, Type.RLR)) { -// return SimpleType.R -// } -// else if (type in listOf(Type.LSL, Type.RSL, Type.LRL)) { -// return SimpleType.L -// } -// error("Wrong DubinsPath.Type") -// } -// public val intermediate: SimpleType -// get() { -// if (type == Type.RLR) { -// return SimpleType.L -// } -// else if (type == Type.LRL) { -// return SimpleType.R -// } -// error("This DubinsPath.Type doesn't contain intermediate arc") -// } -// } + public data class Type( + public val first: Trajectory2D.Type, + public val second: Trajectory2D.Type, + public val third: Trajectory2D.Type, + ) { + public fun toList(): List = listOf(first, second, third) - public enum class SimpleType { - R, S, L - } + override fun toString(): String = "${first.name}${second.name}${third.name}" - public enum class Type { - RLR, LRL, RSR, LSL, RSL, LSR - } - - public fun toSimpleTypes(type: Type): List { - when (type) { - Type.RLR -> { - return listOf(SimpleType.R, SimpleType.L, SimpleType.R) - } - Type.LRL -> { - return listOf(SimpleType.L, SimpleType.R, SimpleType.L) - } - Type.RSR -> { - return listOf(SimpleType.R, SimpleType.S, SimpleType.R) - } - Type.LSL -> { - return listOf(SimpleType.L, SimpleType.S, SimpleType.L) - } - Type.RSL -> { - return listOf(SimpleType.R, SimpleType.S, SimpleType.L) - } - Type.LSR -> { - return listOf(SimpleType.L, SimpleType.S, SimpleType.R) - } - else -> error("This type doesn't exist") + 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) } } - public fun toType(types: List): Type { - when (types) { - listOf(SimpleType.R, SimpleType.L, SimpleType.R) -> { - return Type.RLR - } - listOf(SimpleType.L, SimpleType.R, SimpleType.L) -> { - return Type.LRL - } - listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> { - return Type.RSR - } - listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> { - return Type.LSL - } - listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> { - return Type.RSL - } - listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> { - return Type.LSR - } - else -> error("This type doesn't exist") - } - } - -// public class PathTypes(private val inputTypes: List) { -// public val type: Type -// get() { -// when (this.inputTypes) { -// listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> { -// return Type.RSR -// } -// listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> { -// return Type.RSL -// } -// listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> { -// return Type.LSR -// } -// listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> { -// return Type.LSL -// } -// else -> error("Wrong list of SimpleTypes") -// } -// } -// public val chain: List = this.inputTypes -// } - /** * Return Dubins trajectory type or null if trajectory is not a Dubins path */ @@ -197,12 +96,10 @@ public object DubinsPath { 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.valueOf( - arrayOf( - a.direction.name[0], - if (b is CircleTrajectory2D) b.direction.name[0] else 'S', - c.direction.name[0] - ).toCharArray().concatToString() + return Type( + a.direction, + if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S, + c.direction ) } @@ -240,9 +137,9 @@ public object DubinsPath { 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, CircleTrajectory2D.Direction.RIGHT) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) CompositeTrajectory2D(a1, a2, a3) } @@ -257,9 +154,9 @@ public object DubinsPath { 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, CircleTrajectory2D.Direction.RIGHT) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) CompositeTrajectory2D(a1, a2, a3) } @@ -284,9 +181,9 @@ public object DubinsPath { 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, CircleTrajectory2D.Direction.LEFT) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) CompositeTrajectory2D(a1, a2, a3) } @@ -301,9 +198,9 @@ public object DubinsPath { 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, CircleTrajectory2D.Direction.LEFT) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L) + val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) + val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) CompositeTrajectory2D(a1, a2, a3) } @@ -313,45 +210,45 @@ public object DubinsPath { public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) - val s = leftOuterTangent(c1, c2) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT) + val s = outerTangent(c1, c2, L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.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 = rightOuterTangent(c1, c2) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT) + val s = outerTangent(c1, c2, R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.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 = rightInnerTangent(c1, c2) + 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.start, CircleTrajectory2D.Direction.RIGHT) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.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 = leftInnerTangent(c1, c2) + 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.start, CircleTrajectory2D.Direction.LEFT) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R) return CompositeTrajectory2D(a1, s, a3) } } -public typealias PathTypes = List +public typealias PathTypes = List public fun interface MaxCurvature { public fun compute(startPoint: PhaseVector2D): Double 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 new file mode 100644 index 000000000..d5d04e1a4 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -0,0 +1,619 @@ +/* + * 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 trajectoryType: List, +) + +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 = Euclidean2DSpace.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 = LineSegment2D( + firstCircle.center + w * r1, + secondCircle.center + w * r2 + ), + trajectoryType = route.toList() + ) + ) + } else { + throw Exception("Circles should not intersect") + } + } + } +} + +public 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, + LineSegment2D( + center + w * r1, + other.center + w * r2 + ), + routeType.toList() + ) + ) + } 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) + } + } + + 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() + } +} + +private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent { + if (routeType == boundaryRoute) { + 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, + LineSegment2D( + tangents[i - 1].lineSegment.end, + tangents[i - 1].lineSegment.begin + ), + routeType.toList() + ) + } else { + return Tangent( + circles[0], + circles.last(), + this, + this, + LineSegment2D( + tangents.last().lineSegment.end, + tangents.last().lineSegment.begin + ), + routeType.toList() + ) + } + } + } + } + + error("next tangent not found") +} + +public 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, + route: Trajectory2D.Type, +): 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 (route) { + Trajectory2D.Type.L -> { + angle = if (phi2 >= phi1) { + phi2 - phi1 + } else { + 2 * PI + phi2 - phi1 + } + } + + Trajectory2D.Type.R -> { + angle = if (phi2 >= phi1) { + 2 * PI - (phi2 - phi1) + } else { + -(phi2 - phi1) + } + } + + Trajectory2D.Type.S -> { + error("L or R route is expected") + } + } + 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.Type.L to Circle2D(center1, r), + Trajectory2D.Type.R to Circle2D(center2, r) + ) + } else { + mapOf( + Trajectory2D.Type.L to Circle2D(center2, r), + Trajectory2D.Type.R to Circle2D(center1, r) + ) + } +} + +private fun sortedObstacles( + currentObstacle: Obstacle, + obstacles: List, +): List { + return obstacles.sortedBy { norm(it.center - currentObstacle.center) }//.reversed() +} + +private fun tangentsAlongTheObstacle( + initialCircle: Circle2D, + initialRoute: DubinsPath.Type, + finalCircle: Circle2D, + obstacle: Obstacle, +): List { + val dubinsTangents = mutableListOf() + var tangent = obstacle.nextTangent(initialCircle, initialRoute) + dubinsTangents.add(tangent) + while (tangent.endCircle != finalCircle) { + tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) + 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.trajectoryType.first() + ) + ) + } + + add(tangents.last().lineSegment.toTrajectory()) + } +) + +internal fun findAllPaths( + startingPoint: DoubleVector2D, + startingDirection: DoubleVector2D, + startingRadius: Double, + finalPoint: DoubleVector2D, + finalDirection: DoubleVector2D, + finalRadius: Double, + obstacles: List, +): List { + val initialCircles = constructTangentCircles( + startingPoint, + startingDirection, + startingRadius + ) + val finalCircles = constructTangentCircles( + finalPoint, + finalDirection, + finalRadius + ) + val trajectories = mutableListOf() + for (i in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) { + for (j in listOf(Trajectory2D.Type.L, Trajectory2D.Type.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]!!)), + LineSegment2D(startingPoint, startingPoint), + listOf(i, Trajectory2D.Type.S, i) + ) + ) + ) + while (!allFinished(currentPaths, finalObstacle)) { + val newPaths = mutableListOf() + for (tangentPath: TangentPath in currentPaths) { + val currentCircle = tangentPath.last().endCircle + val currentDirection = tangentPath.last().trajectoryType.last() + val currentObstacle = tangentPath.last().endObstacle + var nextObstacle: Obstacle? = null + if (currentObstacle != finalObstacle) { + val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( + currentDirection, + Trajectory2D.Type.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, + DubinsPath.Type( + currentDirection, + Trajectory2D.Type.S, + currentDirection + ), + ).lineSegment.begin, + currentDirection + ) + val lengthCalculated = arcLength( + tangent.startCircle, + tangentPath.last().lineSegment.end, + tangent.lineSegment.begin, + currentDirection + ) + tangentsAlong = if (lengthCalculated > lengthMaxPossible) { + tangentsAlongTheObstacle( + currentCircle, + DubinsPath.Type( + currentDirection, + Trajectory2D.Type.S, + currentDirection + ), + tangent.startCircle, + currentObstacle + ) + } else { + emptyList() + } + } else { + tangentsAlong = tangentsAlongTheObstacle( + currentCircle, + DubinsPath.Type( + currentDirection, + Trajectory2D.Type.S, + currentDirection + ), + tangent.startCircle, + currentObstacle + ) + } + newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) + } + } else { + // minor changes from Python code + newPaths.add(tangentPath) + } + } + currentPaths = newPaths + } + + trajectories += currentPaths.map { tangentPath -> + val lastDirection: Trajectory2D.Type = tangentPath.last().trajectoryType[2] + val end = finalCircles[j]!! + TangentPath( + tangentPath.tangents + + Tangent( + end, + end, + Obstacle(end), + Obstacle(end), + LineSegment2D(finalPoint, finalPoint), + listOf( + lastDirection, + Trajectory2D.Type.S, + j + ) + ) + ) + }.map { it.toTrajectory() } + } + } + return trajectories +} + + + + + + + + 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 index e5f60e025..7bf17fc26 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -16,6 +16,12 @@ import kotlin.math.atan2 @Serializable public sealed interface Trajectory2D { public val length: Double + + public enum class Type { + R, + S, + L + } } /** @@ -27,11 +33,14 @@ public data class StraightTrajectory2D( public val start: DoubleVector2D, public val end: DoubleVector2D, ) : Trajectory2D { + override val length: Double get() = start.distanceTo(end) public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized() } +public fun StraightTrajectory2D.toSegment(): LineSegment> = LineSegment2D(start, end) + /** * An arc segment */ @@ -43,15 +52,11 @@ public data class CircleTrajectory2D( public val end: DubinsPose2D, ) : Trajectory2D { - public enum class Direction { - LEFT, RIGHT - } - /** * Arc length in radians */ val arcLength: Angle - get() = if (direction == Direction.LEFT) { + get() = if (direction == Trajectory2D.Type.L) { start.bearing - end.bearing } else { end.bearing - start.bearing @@ -62,16 +67,16 @@ public data class CircleTrajectory2D( circle.radius * arcLength.radians } - public val direction: Direction by lazy { + public val direction: Trajectory2D.Type by lazy { if (start.y < circle.center.y) { - if (start.bearing > Angle.pi) Direction.RIGHT else Direction.LEFT + if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L } else if (start.y > circle.center.y) { - if (start.bearing < Angle.pi) Direction.RIGHT else Direction.LEFT + if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L } else { if (start.bearing == Angle.zero) { - if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT + if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L } else { - if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT + if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L } } } @@ -81,17 +86,18 @@ public data class CircleTrajectory2D( center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, - direction: Direction, + direction: Trajectory2D.Type, ): CircleTrajectory2D { fun calculatePose( vector: DoubleVector2D, theta: Angle, - direction: Direction, + direction: Trajectory2D.Type, ): DubinsPose2D = DubinsPose2D( vector, when (direction) { - Direction.LEFT -> (theta - Angle.piDiv2).normalized() - Direction.RIGHT -> (theta + Angle.piDiv2).normalized() + Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized() + Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized() + else -> error("S trajectory type is not allowed in circle constructor") } ) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt deleted file mode 100644 index fb1c3927b..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt +++ /dev/null @@ -1,114 +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 kotlin.math.* - -/** - * Create inner and outer tangents between two circles. - * This method returns a map of segments using [DubinsPath] connection type notation. - */ -public fun Circle2D.tangentsToCircle( - other: Circle2D, -): Map> = with(Euclidean2DSpace) { - //return empty map for concentric circles - if(center.equalsVector(other.center)) return@tangentsToCircle 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 = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) - put( - route, - LineSegment( - center + w * r1, - other.center + w * r2 - ) - ) - } else { - throw Exception("Circles should not intersect") - } - } - } -} - -public fun dubinsTangentsToCircles( - firstCircle: Circle2D, - secondCircle: Circle2D, - firstObstacle: DubinsObstacle, - secondObstacle: DubinsObstacle -): 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, 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, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius), - secondCircle, - firstObstacle, - secondObstacle, - LineSegment2D( - firstCircle.center + w * r1, - secondCircle.center + w * r2 - ), - DubinsPath.toSimpleTypes(route)) - ) - } else { - throw Exception("Circles should not intersect") - } - } - } -} \ No newline at end of file diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt similarity index 93% rename from kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt rename to kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt index 481ea4786..f5bfb884e 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt @@ -1,13 +1,12 @@ /* - * Copyright 2018-2022 KMath contributors. + * 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.dubins +package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.equalsFloat -import space.kscience.kmath.trajectory.* import kotlin.test.Test import kotlin.test.assertNotNull import kotlin.test.assertTrue @@ -37,7 +36,7 @@ class DubinsTests { ) expectedLengths.forEach { - val path = dubins.find { p -> DubinsPath.trajectoryTypeOf(p) === it.key } + 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)) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt similarity index 55% rename from kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt rename to kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt index 07e9861c7..446e0a4d3 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt @@ -7,12 +7,10 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.vector -import space.kscience.kmath.geometry.equalsFloat import kotlin.test.Test import kotlin.test.assertEquals -import kotlin.test.assertTrue -class DubinsTest { +class ObstacleTest { @Test fun firstPath() { val startPoint = vector(-5.0, -1.0) @@ -22,8 +20,13 @@ class DubinsTest { val finalDirection = vector(1.0, -1.0) val finalRadius = 0.5 - val obstacles = listOf(DubinsObstacle(listOf( - Circle2D(vector(7.0, 1.0), 5.0)))) + val obstacles = listOf( + Obstacle( + listOf( + Circle2D(vector(7.0, 1.0), 5.0) + ) + ) + ) val outputTangents = findAllPaths( startPoint, @@ -32,8 +35,9 @@ class DubinsTest { finalPoint, finalDirection, finalRadius, - obstacles) - val length = pathLength(shortestPath(outputTangents)) + obstacles + ) + val length = outputTangents.minOf { it.length } assertEquals(length, 27.2113183, 1e-6) } @@ -47,17 +51,21 @@ class DubinsTest { val finalRadius = 0.5 val obstacles = listOf( - DubinsObstacle(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) - )), DubinsObstacle(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) - )) + 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( startPoint, @@ -66,22 +74,19 @@ class DubinsTest { finalPoint, finalDirection, finalRadius, - obstacles) - val length = pathLength(shortestPath(paths)) - assertEquals(length,28.9678224, 1e-6) - } - @Test - fun equalCircles() { - val circle1 = Circle2D(vector(1.0, 6.5), 0.5) - val circle2 = Circle2D(vector(1.0, 6.5), 0.5) - println(circle1 == circle2) + obstacles + ) + val length = paths.minOf { it.length } + assertEquals(length, 28.9678224, 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) - val obstacle1 = DubinsObstacle(listOf(circle1)) - val obstacle2 = DubinsObstacle(listOf(circle2)) - println(obstacle1 == obstacle2) + 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/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt index 7594aa046..f149004bb 100644 --- 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 @@ -10,6 +10,7 @@ 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 @@ -22,7 +23,7 @@ class ArcTests { circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), - CircleTrajectory2D.Direction.RIGHT + Trajectory2D.Type.R ) assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(0.0, arc.start.bearing.degrees) From 109e050f03abc0b585a024dd8abd743aa434e8d1 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 15:16:33 +0300 Subject: [PATCH 22/30] Hieraechy for trajectory types --- .../space/kscience/kmath/misc/Featured.kt | 2 +- .../space/kscience/kmath/misc/collections.kt | 22 +++++++ .../kscience/kmath/trajectory/DubinsPath.kt | 58 +++++++++---------- .../kscience/kmath/trajectory/Trajectory2D.kt | 40 ++++++++----- .../kscience/kmath/trajectory/ObstacleTest.kt | 4 +- .../kmath/trajectory/segments/ArcTests.kt | 2 +- 6 files changed, 78 insertions(+), 50 deletions(-) create mode 100644 kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt index a752a8339..bdda674dc 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/Featured.kt @@ -9,7 +9,7 @@ import kotlin.jvm.JvmInline import kotlin.reflect.KClass /** - * A entity that contains a set of features defined by their types + * An entity that contains a set of features defined by their types */ public interface Featured { public fun getFeature(type: FeatureKey): T? diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt new file mode 100644 index 000000000..90cc5bbfa --- /dev/null +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/misc/collections.kt @@ -0,0 +1,22 @@ +/* + * 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.misc + +/** + * The same as [zipWithNext], but includes link between last and first element + */ +public inline fun List.zipWithNextCircular(transform: (a: T, b: T) -> R): List { + if (isEmpty()) return emptyList() + return indices.map { i -> + if (i == size - 1) { + transform(last(), first()) + } else { + transform(get(i), get(i + 1)) + } + } +} + +public inline fun List.zipWithNextCircular(): List> = zipWithNextCircular { l, r -> l to r } \ No newline at end of file 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 index 272cf9e5b..87ea52a69 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -7,8 +7,7 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.* import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.trajectory.Trajectory2D.Type -import space.kscience.kmath.trajectory.Trajectory2D.Type.* +import space.kscience.kmath.trajectory.Trajectory2D.* import kotlin.math.acos internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first @@ -21,7 +20,7 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair error("S trajectory type not allowed") } return StraightTrajectory2D( p1, @@ -47,7 +44,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): Straigh private fun innerTangent( from: Circle2D, to: Circle2D, - direction: Type, + direction: Direction, ): StraightTrajectory2D? = with(Euclidean2DSpace) { val centers = StraightTrajectory2D(from.center, to.center) @@ -55,7 +52,6 @@ private fun innerTangent( val angle = when (direction) { L -> centers.bearing + acos(from.radius * 2 / centers.length).radians R -> centers.bearing - acos(from.radius * 2 / centers.length).radians - else -> error("S trajectory type not allowed") }.normalized() val dX = from.radius * sin(angle) @@ -70,13 +66,13 @@ private fun innerTangent( public object DubinsPath { public data class Type( - public val first: Trajectory2D.Type, + public val first: Direction, public val second: Trajectory2D.Type, - public val third: Trajectory2D.Type, + public val third: Direction, ) { public fun toList(): List = listOf(first, second, third) - override fun toString(): String = "${first.name}${second.name}${third.name}" + override fun toString(): String = "${first}${second}${third}" public companion object { public val RLR: Type = Type(R, L, R) @@ -98,7 +94,7 @@ public object DubinsPath { val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null return Type( a.direction, - if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S, + if (b is CircleTrajectory2D) b.direction else S, c.direction ) } @@ -137,9 +133,9 @@ public object DubinsPath { 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, Trajectory2D.Type.R) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) + 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) } @@ -154,9 +150,9 @@ public object DubinsPath { 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, Trajectory2D.Type.R) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R) + 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) } @@ -181,9 +177,9 @@ public object DubinsPath { 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, Trajectory2D.Type.L) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) + 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) } @@ -198,9 +194,9 @@ public object DubinsPath { 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, Trajectory2D.Type.L) - val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L) + 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) } @@ -211,8 +207,8 @@ public object DubinsPath { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = outerTangent(c1, c2, L) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } @@ -220,8 +216,8 @@ public object DubinsPath { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = outerTangent(c1, c2, R) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -231,8 +227,8 @@ public object DubinsPath { 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.start, Trajectory2D.Type.R) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -242,8 +238,8 @@ public object DubinsPath { 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.start, Trajectory2D.Type.L) - val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } } 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 index 7bf17fc26..d6974b105 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -17,10 +17,21 @@ import kotlin.math.atan2 public sealed interface Trajectory2D { public val length: Double - public enum class Type { - R, - S, - L + + 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" } } @@ -56,7 +67,7 @@ public data class CircleTrajectory2D( * Arc length in radians */ val arcLength: Angle - get() = if (direction == Trajectory2D.Type.L) { + get() = if (direction == Trajectory2D.L) { start.bearing - end.bearing } else { end.bearing - start.bearing @@ -67,16 +78,16 @@ public data class CircleTrajectory2D( circle.radius * arcLength.radians } - public val direction: Trajectory2D.Type by lazy { + public val direction: Trajectory2D.Direction by lazy { if (start.y < circle.center.y) { - if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L } else if (start.y > circle.center.y) { - if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L } else { if (start.bearing == Angle.zero) { - if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L } else { - if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L + if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L } } } @@ -86,18 +97,17 @@ public data class CircleTrajectory2D( center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, - direction: Trajectory2D.Type, + direction: Trajectory2D.Direction, ): CircleTrajectory2D { fun calculatePose( vector: DoubleVector2D, theta: Angle, - direction: Trajectory2D.Type, + direction: Trajectory2D.Direction, ): DubinsPose2D = DubinsPose2D( vector, when (direction) { - Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized() - Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized() - else -> error("S trajectory type is not allowed in circle constructor") + Trajectory2D.L -> (theta - Angle.piDiv2).normalized() + Trajectory2D.R -> (theta + Angle.piDiv2).normalized() } ) 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 index 446e0a4d3..150b370d0 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt @@ -38,7 +38,7 @@ class ObstacleTest { obstacles ) val length = outputTangents.minOf { it.length } - assertEquals(length, 27.2113183, 1e-6) + assertEquals(27.2113183, length, 1e-6) } @Test @@ -77,7 +77,7 @@ class ObstacleTest { obstacles ) val length = paths.minOf { it.length } - assertEquals(length, 28.9678224, 1e-6) + assertEquals(28.9678224, length, 1e-6) } @Test 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 index f149004bb..b3825b93b 100644 --- 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 @@ -23,7 +23,7 @@ class ArcTests { circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), - Trajectory2D.Type.R + Trajectory2D.R ) assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(0.0, arc.start.bearing.degrees) From fd35d7c6143a5f544788bfda005cc399dc3cb245 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 15:28:02 +0300 Subject: [PATCH 23/30] [WIP] refactoring directions --- .../kscience/kmath/trajectory/Obstacle.kt | 38 +++++++++---------- 1 file changed, 17 insertions(+), 21 deletions(-) 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 index d5d04e1a4..b4ae04342 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -350,13 +350,13 @@ private fun arcLength( circle: Circle2D, point1: DoubleVector2D, point2: DoubleVector2D, - route: Trajectory2D.Type, + 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 (route) { - Trajectory2D.Type.L -> { + when (direction) { + Trajectory2D.L -> { angle = if (phi2 >= phi1) { phi2 - phi1 } else { @@ -364,17 +364,13 @@ private fun arcLength( } } - Trajectory2D.Type.R -> { + Trajectory2D.R -> { angle = if (phi2 >= phi1) { 2 * PI - (phi2 - phi1) } else { -(phi2 - phi1) } } - - Trajectory2D.Type.S -> { - error("L or R route is expected") - } } return circle.radius * angle } @@ -396,13 +392,13 @@ private fun constructTangentCircles( 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.Type.L to Circle2D(center1, r), - Trajectory2D.Type.R to Circle2D(center2, r) + Trajectory2D.L to Circle2D(center1, r), + Trajectory2D.R to Circle2D(center2, r) ) } else { mapOf( - Trajectory2D.Type.L to Circle2D(center2, r), - Trajectory2D.Type.R to Circle2D(center1, r) + Trajectory2D.L to Circle2D(center2, r), + Trajectory2D.R to Circle2D(center1, r) ) } } @@ -483,8 +479,8 @@ internal fun findAllPaths( finalRadius ) val trajectories = mutableListOf() - for (i in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) { - for (j in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) { + 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( @@ -495,7 +491,7 @@ internal fun findAllPaths( Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)), LineSegment2D(startingPoint, startingPoint), - listOf(i, Trajectory2D.Type.S, i) + listOf(i, Trajectory2D.S, i) ) ) ) @@ -503,13 +499,13 @@ internal fun findAllPaths( val newPaths = mutableListOf() for (tangentPath: TangentPath in currentPaths) { val currentCircle = tangentPath.last().endCircle - val currentDirection = tangentPath.last().trajectoryType.last() + val currentDirection: Trajectory2D.Direction = tangentPath.last().trajectoryType.last() val currentObstacle = tangentPath.last().endObstacle var nextObstacle: Obstacle? = null if (currentObstacle != finalObstacle) { val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( currentDirection, - Trajectory2D.Type.S, + Trajectory2D.S, j )] for (obstacle in sortedObstacles(currentObstacle, obstacles)) { @@ -538,7 +534,7 @@ internal fun findAllPaths( tangent.startCircle, DubinsPath.Type( currentDirection, - Trajectory2D.Type.S, + Trajectory2D.S, currentDirection ), ).lineSegment.begin, @@ -555,7 +551,7 @@ internal fun findAllPaths( currentCircle, DubinsPath.Type( currentDirection, - Trajectory2D.Type.S, + Trajectory2D.S, currentDirection ), tangent.startCircle, @@ -569,7 +565,7 @@ internal fun findAllPaths( currentCircle, DubinsPath.Type( currentDirection, - Trajectory2D.Type.S, + Trajectory2D.S, currentDirection ), tangent.startCircle, @@ -599,7 +595,7 @@ internal fun findAllPaths( LineSegment2D(finalPoint, finalPoint), listOf( lastDirection, - Trajectory2D.Type.S, + Trajectory2D.S, j ) ) From 1e46ffbd98809c43b14c5b8f4df65f9315d040c7 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 16:50:30 +0300 Subject: [PATCH 24/30] refactoring directions --- .../kscience/kmath/trajectory/Obstacle.kt | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) 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 index b4ae04342..c4ff34295 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -21,7 +21,8 @@ internal data class Tangent( val startObstacle: Obstacle, val endObstacle: Obstacle, val lineSegment: LineSegment2D, - val trajectoryType: List, + val startDirection: Trajectory2D.Direction, + val endDirection: Trajectory2D.Direction = startDirection ) private class TangentPath(val tangents: List) { @@ -131,7 +132,8 @@ private fun dubinsTangentsToCircles( firstCircle.center + w * r1, secondCircle.center + w * r2 ), - trajectoryType = route.toList() + startDirection = route.first, + endDirection = route.third ) ) } else { @@ -194,7 +196,8 @@ public class Obstacle( center + w * r1, other.center + w * r2 ), - routeType.toList() + startDirection = routeType.first, + endDirection = routeType.third ) ) } else { @@ -256,7 +259,8 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): tangents[i - 1].lineSegment.end, tangents[i - 1].lineSegment.begin ), - routeType.toList() + startDirection = routeType.first, + endDirection = routeType.third ) } else { return Tangent( @@ -268,7 +272,8 @@ private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): tangents.last().lineSegment.end, tangents.last().lineSegment.begin ), - routeType.toList() + startDirection = routeType.first, + endDirection = routeType.third ) } } @@ -450,7 +455,7 @@ private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTraject right.startCircle.center, left.lineSegment.end, right.lineSegment.begin, - right.trajectoryType.first() + right.startDirection ) ) } @@ -491,7 +496,7 @@ internal fun findAllPaths( Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)), LineSegment2D(startingPoint, startingPoint), - listOf(i, Trajectory2D.S, i) + i ) ) ) @@ -499,7 +504,7 @@ internal fun findAllPaths( val newPaths = mutableListOf() for (tangentPath: TangentPath in currentPaths) { val currentCircle = tangentPath.last().endCircle - val currentDirection: Trajectory2D.Direction = tangentPath.last().trajectoryType.last() + val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection val currentObstacle = tangentPath.last().endObstacle var nextObstacle: Obstacle? = null if (currentObstacle != finalObstacle) { @@ -583,7 +588,7 @@ internal fun findAllPaths( } trajectories += currentPaths.map { tangentPath -> - val lastDirection: Trajectory2D.Type = tangentPath.last().trajectoryType[2] + val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection val end = finalCircles[j]!! TangentPath( tangentPath.tangents + @@ -593,11 +598,8 @@ internal fun findAllPaths( Obstacle(end), Obstacle(end), LineSegment2D(finalPoint, finalPoint), - listOf( - lastDirection, - Trajectory2D.S, - j - ) + startDirection = lastDirection, + endDirection = j ) ) }.map { it.toTrajectory() } From f5201b6be0c2cf784e2a192193d2f7130caac857 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 17:42:40 +0300 Subject: [PATCH 25/30] refactoring directions --- .../kotlin/space/kscience/kmath/trajectory/Obstacle.kt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index c4ff34295..553ca0388 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -69,7 +69,7 @@ internal fun Circle2D.tangentsToCircle( } else { angle1 - r2.sign * atan2(r.absoluteValue, l) } - val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2)) + val w = vector(-cos(angle2), sin(angle2)) put( route, LineSegment( @@ -412,7 +412,7 @@ private fun sortedObstacles( currentObstacle: Obstacle, obstacles: List, ): List { - return obstacles.sortedBy { norm(it.center - currentObstacle.center) }//.reversed() + return obstacles.sortedBy { norm(it.center - currentObstacle.center) } } private fun tangentsAlongTheObstacle( @@ -580,7 +580,6 @@ internal fun findAllPaths( newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) } } else { - // minor changes from Python code newPaths.add(tangentPath) } } From 639a255aaf96c3b41a458774036b3f48efd86765 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 18:50:17 +0300 Subject: [PATCH 26/30] refactoring directions --- .../kscience/kmath/trajectory/DubinsPose2D.kt | 18 +++++++++++++-- .../kscience/kmath/trajectory/Obstacle.kt | 22 +++++++++---------- .../kscience/kmath/trajectory/ObstacleTest.kt | 12 ++++------ 3 files changed, 31 insertions(+), 21 deletions(-) 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 index 8362d0cb5..078e158ea 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt @@ -3,6 +3,7 @@ * 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 @@ -22,6 +23,19 @@ import kotlin.math.atan2 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 @@ -37,12 +51,12 @@ public class PhaseVector2D( private class DubinsPose2DImpl( override val coordinates: DoubleVector2D, override val bearing: Angle, -) : DubinsPose2D, DoubleVector2D by coordinates{ +) : DubinsPose2D, DoubleVector2D by coordinates { override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)" } -public object DubinsPose2DSerializer: KSerializer{ +public object DubinsPose2DSerializer : KSerializer { private val proxySerializer = DubinsPose2DImpl.serializer() override val descriptor: SerialDescriptor 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 index 553ca0388..a00fe24d4 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -22,7 +22,7 @@ internal data class Tangent( val endObstacle: Obstacle, val lineSegment: LineSegment2D, val startDirection: Trajectory2D.Direction, - val endDirection: Trajectory2D.Direction = startDirection + val endDirection: Trajectory2D.Direction = startDirection, ) private class TangentPath(val tangents: List) { @@ -465,22 +465,22 @@ private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTraject ) internal fun findAllPaths( - startingPoint: DoubleVector2D, - startingDirection: DoubleVector2D, + start: DubinsPose2D, startingRadius: Double, - finalPoint: DoubleVector2D, - finalDirection: DoubleVector2D, + finish: DubinsPose2D, finalRadius: Double, obstacles: List, ): List { + fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing)) + val initialCircles = constructTangentCircles( - startingPoint, - startingDirection, + start, + start.direction(), startingRadius ) val finalCircles = constructTangentCircles( - finalPoint, - finalDirection, + finish, + finish.direction(), finalRadius ) val trajectories = mutableListOf() @@ -495,7 +495,7 @@ internal fun findAllPaths( initialCircles[i]!!, Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)), - LineSegment2D(startingPoint, startingPoint), + LineSegment2D(start, start), i ) ) @@ -596,7 +596,7 @@ internal fun findAllPaths( end, Obstacle(end), Obstacle(end), - LineSegment2D(finalPoint, finalPoint), + LineSegment2D(finish, finish), startDirection = lastDirection, endDirection = j ) 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 index 150b370d0..1a8c3a474 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/ObstacleTest.kt @@ -29,11 +29,9 @@ class ObstacleTest { ) val outputTangents = findAllPaths( - startPoint, - startDirection, + DubinsPose2D.of(startPoint, startDirection), startRadius, - finalPoint, - finalDirection, + DubinsPose2D.of(finalPoint, finalDirection), finalRadius, obstacles ) @@ -68,11 +66,9 @@ class ObstacleTest { ) ) val paths = findAllPaths( - startPoint, - startDirection, + DubinsPose2D.of(startPoint, startDirection), startRadius, - finalPoint, - finalDirection, + DubinsPose2D.of(finalPoint, finalDirection), finalRadius, obstacles ) From 025cb5806030665550f0767bc7de8ecec7ad3c7a Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 19:02:24 +0300 Subject: [PATCH 27/30] refactoring directions --- .../kscience/kmath/trajectory/Obstacle.kt | 112 ++++++++---------- 1 file changed, 49 insertions(+), 63 deletions(-) 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 index a00fe24d4..f2768bbca 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -229,6 +229,48 @@ public class Obstacle( } } + 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, + LineSegment2D( + tangents[i - 1].lineSegment.end, + tangents[i - 1].lineSegment.begin + ), + direction + ) + } else { + return Tangent( + circles[0], + circles.last(), + this, + this, + LineSegment2D( + 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 @@ -239,50 +281,6 @@ public class Obstacle( } } -private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent { - if (routeType == boundaryRoute) { - 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, - LineSegment2D( - tangents[i - 1].lineSegment.end, - tangents[i - 1].lineSegment.begin - ), - startDirection = routeType.first, - endDirection = routeType.third - ) - } else { - return Tangent( - circles[0], - circles.last(), - this, - this, - LineSegment2D( - tangents.last().lineSegment.end, - tangents.last().lineSegment.begin - ), - startDirection = routeType.first, - endDirection = routeType.third - ) - } - } - } - } - - error("next tangent not found") -} - public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { @@ -417,15 +415,15 @@ private fun sortedObstacles( private fun tangentsAlongTheObstacle( initialCircle: Circle2D, - initialRoute: DubinsPath.Type, + direction: Trajectory2D.Direction, finalCircle: Circle2D, obstacle: Obstacle, ): List { val dubinsTangents = mutableListOf() - var tangent = obstacle.nextTangent(initialCircle, initialRoute) + var tangent = obstacle.nextTangent(initialCircle, direction) dubinsTangents.add(tangent) while (tangent.endCircle != finalCircle) { - tangent = obstacle.nextTangent(tangent.endCircle, initialRoute) + tangent = obstacle.nextTangent(tangent.endCircle, direction) dubinsTangents.add(tangent) } return dubinsTangents @@ -471,7 +469,7 @@ internal fun findAllPaths( finalRadius: Double, obstacles: List, ): List { - fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing)) + fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing)) val initialCircles = constructTangentCircles( start, @@ -537,11 +535,7 @@ internal fun findAllPaths( tangentPath.last().lineSegment.end, tangent.startObstacle.nextTangent( tangent.startCircle, - DubinsPath.Type( - currentDirection, - Trajectory2D.S, - currentDirection - ), + currentDirection ).lineSegment.begin, currentDirection ) @@ -554,11 +548,7 @@ internal fun findAllPaths( tangentsAlong = if (lengthCalculated > lengthMaxPossible) { tangentsAlongTheObstacle( currentCircle, - DubinsPath.Type( - currentDirection, - Trajectory2D.S, - currentDirection - ), + currentDirection, tangent.startCircle, currentObstacle ) @@ -568,11 +558,7 @@ internal fun findAllPaths( } else { tangentsAlong = tangentsAlongTheObstacle( currentCircle, - DubinsPath.Type( - currentDirection, - Trajectory2D.S, - currentDirection - ), + currentDirection, tangent.startCircle, currentObstacle ) From a0e2ef1afc08765240f0e00ac39205a25bdc92e8 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 4 Apr 2023 19:33:43 +0300 Subject: [PATCH 28/30] refactor lines and segments --- .../space/kscience/kmath/geometry/Line.kt | 27 ++++++++++++++++--- .../kscience/kmath/geometry/projections.kt | 2 +- .../kscience/kmath/trajectory/Obstacle.kt | 14 +++++----- .../kscience/kmath/trajectory/Trajectory2D.kt | 2 +- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt index e593150f1..a7f6ae35d 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt @@ -5,15 +5,23 @@ package space.kscience.kmath.geometry +import kotlinx.serialization.SerialName import kotlinx.serialization.Serializable -import space.kscience.kmath.operations.DoubleField.pow /** - * A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized, + * A line formed by [start] vector of start and a [direction] vector. Direction vector is not necessarily normalized, * but its length does not affect line properties */ +public interface Line { + public val start: V + public val direction: V +} + @Serializable -public data class Line(val base: V, val direction: V) +@SerialName("Line") +private data class LineImpl(override val start: V, override val direction: V): Line + +public fun Line(base: V, direction: V): Line = LineImpl(base, direction) public typealias Line2D = Line public typealias Line3D = Line @@ -21,8 +29,19 @@ public typealias Line3D = Line /** * A directed line segment between [begin] and [end] */ +public interface LineSegment { + public val begin: V + public val end: V +} + +/** + * Basic implementation for [LineSegment] + */ @Serializable -public data class LineSegment(val begin: V, val end: V) +@SerialName("LineSegment") +private data class LineSegmentImpl(override val begin: V, override val end: V) : LineSegment + +public fun LineSegment(begin: V, end: V): LineSegment = LineSegmentImpl(begin, end) public fun LineSegment.line(algebra: GeometrySpace): Line = with(algebra) { Line(begin, end - begin) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/projections.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/projections.kt index 6950abbdc..c5c3487a1 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/projections.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/projections.kt @@ -13,7 +13,7 @@ package space.kscience.kmath.geometry * @param line line to which vector should be projected */ public fun GeometrySpace.projectToLine(vector: V, line: Line): V = with(line) { - base + (direction dot (vector - base)) / (direction dot direction) * direction + start + (direction dot (vector - start)) / (direction dot direction) * direction } /** 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 index f2768bbca..3819afb54 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -23,7 +23,7 @@ internal data class Tangent( 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() @@ -128,7 +128,7 @@ private fun dubinsTangentsToCircles( endCircle = secondCircle, startObstacle = firstObstacle, endObstacle = secondObstacle, - lineSegment = LineSegment2D( + lineSegment = LineSegment( firstCircle.center + w * r1, secondCircle.center + w * r2 ), @@ -192,7 +192,7 @@ public class Obstacle( other, this@Obstacle, this@Obstacle, - LineSegment2D( + LineSegment( center + w * r1, other.center + w * r2 ), @@ -245,7 +245,7 @@ public class Obstacle( circles[i - 1], this, this, - LineSegment2D( + LineSegment( tangents[i - 1].lineSegment.end, tangents[i - 1].lineSegment.begin ), @@ -257,7 +257,7 @@ public class Obstacle( circles.last(), this, this, - LineSegment2D( + LineSegment( tangents.last().lineSegment.end, tangents.last().lineSegment.begin ), @@ -493,7 +493,7 @@ internal fun findAllPaths( initialCircles[i]!!, Obstacle(listOf(initialCircles[i]!!)), Obstacle(listOf(initialCircles[i]!!)), - LineSegment2D(start, start), + LineSegment(start, start), i ) ) @@ -582,7 +582,7 @@ internal fun findAllPaths( end, Obstacle(end), Obstacle(end), - LineSegment2D(finish, finish), + LineSegment(finish, finish), startDirection = lastDirection, endDirection = j ) 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 index d6974b105..8df0de237 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -50,7 +50,7 @@ public data class StraightTrajectory2D( public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized() } -public fun StraightTrajectory2D.toSegment(): LineSegment> = LineSegment2D(start, end) +public fun StraightTrajectory2D.toSegment(): LineSegment> = LineSegment(start, end) /** * An arc segment From 00ce7d5a48e4736ec6d80a480327a2c550787425 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Wed, 5 Apr 2023 13:30:13 +0300 Subject: [PATCH 29/30] Obstacle avoidance finished --- .../space/kscience/kmath/geometry/Polygon.kt | 14 +++++++++++++ .../kmath/geometry/ProjectionOntoLineTest.kt | 4 ++-- .../kscience/kmath/trajectory/DubinsPath.kt | 10 +++++----- .../kscience/kmath/trajectory/Obstacle.kt | 20 +++++++++++++++---- .../kscience/kmath/trajectory/Trajectory2D.kt | 12 +++++------ .../kscience/kmath/trajectory/DubinsTests.kt | 4 ++-- .../space/kscience/kmath/trajectory/math.kt | 4 ++-- 7 files changed, 46 insertions(+), 22 deletions(-) create mode 100644 kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt new file mode 100644 index 000000000..20f4a031e --- /dev/null +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt @@ -0,0 +1,14 @@ +/* + * 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.geometry + + +/** + * A closed polygon in 2D space + */ +public interface Polygon { + public val points: List> +} \ No newline at end of file diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt index cdb8ea870..7c6c105cf 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt @@ -63,7 +63,7 @@ internal class ProjectionOntoLineTest { @Test fun projectionOntoLine3d() = with(Euclidean3DSpace) { - val line = Line3D( + val line = Line( base = vector(1.0, 3.5, 0.07), direction = vector(2.0, -0.0037, 11.1111) ) @@ -77,7 +77,7 @@ internal class ProjectionOntoLineTest { val result = projectToLine(v, line) // assert that result is on the line - assertTrue(isCollinear(result - line.base, line.direction)) + assertTrue(isCollinear(result - line.start, line.direction)) // assert that PV vector is orthogonal to direction vector assertTrue(isOrthogonal(v - result, line.direction)) } 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 index 87ea52a69..a1563b29c 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -36,7 +36,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): St } return StraightTrajectory2D( p1, - vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) + vector(p1.x + (centers.end.x - centers.begin.x), p1.y + (centers.end.y - centers.begin.y)) ) } @@ -207,7 +207,7 @@ public object DubinsPath { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = outerTangent(c1, c2, L) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + 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) } @@ -216,7 +216,7 @@ public object DubinsPath { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = outerTangent(c1, c2, R) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + 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) } @@ -227,7 +227,7 @@ public object DubinsPath { 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.start, R) + 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) } @@ -238,7 +238,7 @@ public object DubinsPath { 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.start, L) + 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) } 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 index 3819afb54..32061efea 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -23,7 +23,7 @@ internal data class Tangent( val lineSegment: LineSegment2D, val startDirection: Trajectory2D.Direction, val endDirection: Trajectory2D.Direction = startDirection, -): LineSegment2D by lineSegment +) : LineSegment2D by lineSegment private class TangentPath(val tangents: List) { fun last() = tangents.last() @@ -143,7 +143,7 @@ private fun dubinsTangentsToCircles( } } -public class Obstacle( +internal class Obstacle( public val circles: List, ) { internal val tangents: List = boundaryTangents().first @@ -281,7 +281,7 @@ public class Obstacle( } } -public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) +internal fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { @@ -594,7 +594,19 @@ internal fun findAllPaths( } - +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 index 8df0de237..59a8e613a 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -41,17 +41,15 @@ public sealed interface Trajectory2D { @Serializable @SerialName("straight") public data class StraightTrajectory2D( - public val start: DoubleVector2D, - public val end: DoubleVector2D, -) : Trajectory2D { + override val begin: DoubleVector2D, + override val end: DoubleVector2D, +) : Trajectory2D, LineSegment2D { - override val length: Double get() = start.distanceTo(end) + override val length: Double get() = begin.distanceTo(end) - public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized() + public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized() } -public fun StraightTrajectory2D.toSegment(): LineSegment> = LineSegment(start, end) - /** * An arc segment */ 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 index f5bfb884e..80f7173f1 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt @@ -20,7 +20,7 @@ class DubinsTests { val lineP1 = straight.shift(1, 10.0).inverse() val start = DubinsPose2D(straight.end, straight.bearing) - val end = DubinsPose2D(lineP1.start, lineP1.bearing) + val end = DubinsPose2D(lineP1.begin, lineP1.bearing) val radius = 2.0 val dubins = DubinsPath.all(start, end, radius) @@ -53,7 +53,7 @@ class DubinsTests { assertTrue(a.end.equalsFloat(b.start)) assertTrue(c.start.equalsFloat(b.end)) } else if (b is StraightTrajectory2D) { - assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing))) + 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/math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt index 24685f528..8b8ccf95e 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt @@ -14,14 +14,14 @@ 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, start) +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(start.x - dX * shift, start.y - dY * shift), + vector(begin.x - dX * shift, begin.y - dY * shift), vector(end.x - dX * shift, end.y - dY * shift) ) } From 7cc6a4be400bb7195dba6df50259a2c48cee849f Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Wed, 5 Apr 2023 15:26:09 +0300 Subject: [PATCH 30/30] 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", )