From 8bc1987acfa5456bd1cb2721b25c4d04e76b954d Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 30 Apr 2023 21:20:10 +0300 Subject: [PATCH] [WIP] full rewrite of obstacle avoidance --- .../kmath/geometry/geometryExtensions.kt | 49 ++ .../kmath/geometry/polygonExtensions.kt | 10 + .../space/kscience/trajectory/DubinsPath.kt | 2 + .../space/kscience/trajectory/DubinsPose2D.kt | 15 +- .../space/kscience/trajectory/Obstacle.kt | 155 ++--- .../kscience/trajectory/ObstacleShell.kt | 389 +++++++++++++ .../space/kscience/trajectory/Obstacles.kt | 363 ++++++++++++ .../space/kscience/trajectory/Trajectory2D.kt | 74 ++- .../kscience/trajectory/circumvention.kt | 120 ++++ .../kscience/trajectory/obstacleInternal.kt | 536 ------------------ .../segments => kmath/geometry}/ArcTests.kt | 24 +- .../kscience/kmath/geometry/CircleTests.kt | 135 +++++ .../segments => kmath/geometry}/LineTests.kt | 4 +- .../space/kscience/trajectory/DubinsTests.kt | 12 +- .../space/kscience/trajectory/ObstacleTest.kt | 75 +-- .../trajectory/segments/CircleTests.kt | 24 - .../trajectory/{math.kt => testutils.kt} | 8 +- 17 files changed, 1291 insertions(+), 704 deletions(-) create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/ObstacleShell.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt delete mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt rename trajectory-kt/src/commonTest/kotlin/space/kscience/{trajectory/segments => kmath/geometry}/ArcTests.kt (52%) create mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt rename trajectory-kt/src/commonTest/kotlin/space/kscience/{trajectory/segments => kmath/geometry}/LineTests.kt (89%) delete mode 100644 trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/CircleTests.kt rename trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/{math.kt => testutils.kt} (71%) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt new file mode 100644 index 0000000..2ece6f0 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt @@ -0,0 +1,49 @@ +package space.kscience.kmath.geometry + +import space.kscience.kmath.operations.DoubleField.pow +import kotlin.math.sign + +public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D = + Circle2D(vector(x, y), radius = radius.toDouble()) + +public fun Euclidean2DSpace.segment(begin: DoubleVector2D, end: DoubleVector2D): LineSegment2D = + LineSegment(begin, end) + +public fun Euclidean2DSpace.segment(x1: Number, y1: Number, x2: Number, y2: Number): LineSegment2D = + LineSegment(vector(x1, y1), vector(x2, y2)) + +public fun Euclidean2DSpace.intersectsOrInside(circle1: Circle2D, circle2: Circle2D): Boolean { + val distance = norm(circle2.center - circle1.center) + return distance <= circle1.radius + circle2.radius +} + +/** + * https://mathworld.wolfram.com/Circle-LineIntersection.html + */ +public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D): Boolean { + val begin = segment.begin + val end = segment.end + val d = begin.distanceTo(end) + val det = (begin.x - circle.center.x) * (end.y - circle.center.y) - + (end.x - circle.center.x) * (begin.y - circle.center.y) + + val incidence = circle.radius.pow(2) * d.pow(2) - det.pow(2) + + return incidence >= 0 +} + +public fun Euclidean2DSpace.intersects(circle: Circle2D, segment: LineSegment2D): Boolean = + intersects(segment, circle) + +public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSegment2D): Boolean { + infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x + infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign + + return with(Euclidean2DSpace) { + (segment2.begin - segment1.begin) crossSign (segment2.end - segment1.begin) != + (segment2.begin - segment1.end) crossSign (segment2.end - segment1.end) && + (segment1.begin - segment2.begin) crossSign (segment1.end - segment2.begin) != + (segment1.begin - segment2.end) crossSign (segment1.end - segment2.end) + } +} + diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt new file mode 100644 index 0000000..10059f7 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt @@ -0,0 +1,10 @@ +package space.kscience.kmath.geometry + +import space.kscience.kmath.misc.zipWithNextCircular + +public fun Euclidean2DSpace.polygon(points: List): Polygon = object : Polygon { + override val points: List> get() = points +} + +public fun Euclidean2DSpace.intersects(polygon: Polygon, segment: LineSegment2D): Boolean = + polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, segment) } \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt index 086c841..9650e73 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPath.kt @@ -74,6 +74,8 @@ public object DubinsPath { override fun toString(): String = "${first}${second}${third}" + public val last: Direction get() = third + public companion object { public val RLR: Type = Type(R, L, R) public val LRL: Type = Type(L, R, L) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt index 36c6c1c..0bc476f 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/DubinsPose2D.kt @@ -24,6 +24,11 @@ public interface DubinsPose2D : DoubleVector2D { public val coordinates: DoubleVector2D public val bearing: Angle + /** + * Reverse the direction of this pose to the opposite, keeping other parameters the same + */ + public fun reversed(): DubinsPose2D + public companion object { public fun bearingToVector(bearing: Angle): Vector2D = Euclidean2DSpace.vector(cos(bearing), sin(bearing)) @@ -35,12 +40,15 @@ public interface DubinsPose2D : DoubleVector2D { } } + @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 + + override fun reversed(): DubinsPose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) } } @Serializable @@ -50,7 +58,9 @@ private class DubinsPose2DImpl( override val bearing: Angle, ) : DubinsPose2D, DoubleVector2D by coordinates { - override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)" + override fun reversed(): DubinsPose2D = DubinsPose2DImpl(coordinates, bearing.plus(Angle.pi).normalized()) + + override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)" } public object DubinsPose2DSerializer : KSerializer { @@ -69,7 +79,8 @@ public object DubinsPose2DSerializer : KSerializer { } } -public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, bearing) +public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = + DubinsPose2DImpl(coordinate, bearing) public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D = DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction)) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt index d066356..e84c465 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt @@ -5,96 +5,109 @@ package space.kscience.trajectory +import space.kscience.kmath.geometry.Angle import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.LineSegment2D -import space.kscience.kmath.geometry.Polygon +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.misc.zipWithNextCircular public interface Obstacle { public val circles: List public val center: Vector2D - public fun intersects(segment: LineSegment2D): Boolean + public val circumvention: CompositeTrajectory2D + + /** + * Check if obstacle has intersection with given [Trajectory2D] + */ + public fun intersects(trajectory: Trajectory2D): Boolean = + Euclidean2DSpace.trajectoryIntersects(circumvention, trajectory) - public fun intersects(circle: Circle2D): Boolean public companion object { - public fun avoidObstacles( - start: DubinsPose2D, - finish: DubinsPose2D, - trajectoryRadius: Double, - vararg obstacles: Obstacle, - ): List { - val obstacleShells: List = obstacles.map { polygon -> - ObstacleShell(polygon.circles) - } - return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) - } - public fun avoidPolygons( - start: DubinsPose2D, - finish: DubinsPose2D, - trajectoryRadius: Double, - vararg obstacles: Polygon, - ): List { - val obstacleShells: List = obstacles.map { polygon -> - ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) }) - } - return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) - } - - public fun avoidObstacles( - start: DubinsPose2D, - finish: DubinsPose2D, - trajectoryRadius: Double, - obstacles: Collection, - ): List { - val obstacleShells: List = obstacles.map { polygon -> - ObstacleShell(polygon.circles) - } - return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) - } - - public fun avoidPolygons( - start: DubinsPose2D, - finish: DubinsPose2D, - trajectoryRadius: Double, - obstacles: Collection>, - ): List { - val obstacleShells: List = obstacles.map { polygon -> - ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) }) - } - return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) - } } } -public fun Obstacle.intersectsTrajectory(trajectory: Trajectory2D): Boolean = when (trajectory) { - is CircleTrajectory2D -> intersects(trajectory.circle) - is StraightTrajectory2D -> intersects(trajectory) - is CompositeTrajectory2D -> trajectory.segments.any { intersectsTrajectory(it) } +private class ObstacleImpl(override val circles: List) : Obstacle { + override val center: Vector2D by lazy { + Euclidean2DSpace.vector( + circles.sumOf { it.center.x } / circles.size, + circles.sumOf { it.center.y } / circles.size + ) + } + + override val circumvention: CompositeTrajectory2D by lazy { + with(Euclidean2DSpace) { + /** + * A closed right-handed circuit minimal path circumvention of an obstacle. + * @return null if number of distinct circles in the obstacle is less than + */ + require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" } + + if (circles.size == 1) { + // a circumvention consisting of a single circle, starting on top + val circle = circles.first() + val top = vector(circle.center.x + circle.radius, circle.center.y) + val startEnd = DubinsPose2D( + top, + Angle.piDiv2 + ) + return@lazy CompositeTrajectory2D( + CircleTrajectory2D(circle, startEnd, startEnd) + ) + } + + //TODO use convex hull + //distinct and sorted in right-handed direction + val circles = circles.distinct().sortedBy { + (it.center - center).bearing + } + + val tangents = circles.zipWithNextCircular { a: Circle2D, b: Circle2D -> + tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] + ?: error("Can't find right handed circumvention") + } + + val trajectory: List = buildList { + for (i in 0 until tangents.lastIndex) { + add(tangents[i]) + add(CircleTrajectory2D(circles[i + 1], tangents[i].endPose, tangents[i + 1].beginPose)) + } + add(tangents.last()) + add(CircleTrajectory2D(circles[0], tangents.last().endPose, tangents.first().beginPose)) + } + + return@lazy CompositeTrajectory2D(trajectory) + + } + } + + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (other == null || this::class != other::class) return false + + other as ObstacleImpl + + return circles == other.circles + } + + override fun hashCode(): Int { + return circles.hashCode() + } + + override fun toString(): String { + return "Obstacle(circles=$circles)" + } + + } -public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles)) +public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles)) public fun Obstacle(points: List>, radius: Double): Obstacle = - ObstacleShell(points.map { Circle2D(it, radius) }) - -//public fun Trajectory2D.intersects( -// polygon: Polygon, -// radius: Double, -//): Boolean { -// val obstacle = Obstacle(polygon.points.map { point -> Circle2D(point, radius) }) -// return when (this) { -// is CircleTrajectory2D -> { -// val nearestCircle = obstacle.circles.minBy { it.center.distanceTo(circle.center) } -// -// } -// is StraightTrajectory2D -> obstacle.intersects(this) -// is CompositeTrajectory2D -> segments.any { it.intersects(polygon, radius) } -// } -//} + ObstacleImpl(points.map { Circle2D(it, radius) }) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/ObstacleShell.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/ObstacleShell.kt new file mode 100644 index 0000000..83be837 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/ObstacleShell.kt @@ -0,0 +1,389 @@ +package space.kscience.trajectory + + +// +// +//private class LR(val l: T, val r: T) { +// operator fun get(direction: Trajectory2D.Direction) = when (direction) { +// Trajectory2D.L -> l +// Trajectory2D.R -> r +// } +//} +// +//private class TangentPath(val tangents: List) { +// fun last() = tangents.last() +//} +// +//private fun TangentPath(vararg tangents: ObstacleTangent) = TangentPath(listOf(*tangents)) +// +//private fun Circle2D.isInside(other: Circle2D): Boolean { +// return center.distanceTo(other.center) + radius <= other.radius +//} +// +// +//internal class ObstacleShell( +// nodes: List, +//) : Obstacle { +// override val circles: List +// override val center: Vector2D +// private val shell: List +// private val shellDirection: Trajectory2D.Direction +// +// init { +// this.center = Euclidean2DSpace.vector( +// nodes.sumOf { it.center.x } / nodes.size, +// nodes.sumOf { it.center.y } / nodes.size +// ) +// +//// this.circles = nodes.filter { node -> +//// //filter nodes inside other nodes +//// nodes.none{ node !== it && node.isInside(it) } +//// } +// +// this.circles = nodes.distinct() +// +// if (nodes.size < 2) { +// shell = emptyList() +// shellDirection = Trajectory2D.R +// } else { +// +// //ignore cases when one circle is inside another one +// val lslTangents = circles.zipWithNextCircular { a, b -> +// tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL] ?: error("Intersecting circles") +// } +// +// val rsrTangents = circles.zipWithNextCircular { a, b -> +// tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] ?: error("Intersecting circles") +// } +// +// +// val lslToCenter = lslTangents.sumOf { it.begin.distanceTo(center) } + +// lslTangents.sumOf { it.end.distanceTo(center) } +// val rsrToCenter = rsrTangents.sumOf { it.begin.distanceTo(center) } + +// rsrTangents.sumOf { it.end.distanceTo(center) } +// +// if (rsrToCenter >= lslToCenter) { +// this.shell = rsrTangents +// this.shellDirection = Trajectory2D.R +// } else { +// this.shell = lslTangents +// this.shellDirection = Trajectory2D.L +// } +// } +// } +// +// constructor(obstacle: Obstacle) : this(obstacle.circles) +// +// /** +// * Check if segment has any intersections with this obstacle +// */ +// override fun intersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) { +// shell.any { tangent -> intersects(segment, tangent) } +// || circles.any { circle -> intersects(segment, circle) } +// } +// +// internal fun innerIntersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) { +// intersects(polygon(circles.map { it.center }), segment) +// } +// +// override fun intersects(circle: Circle2D): Boolean = with(Euclidean2DSpace) { +// shell.any { tangent -> intersects(tangent, circle) } +// || circles.any { c2 -> intersectsOrInside(circle, c2) } +// } +// +// /** +// * Tangent to next obstacle node in given direction +// */ +// fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent { +// if (circleIndex == -1) error("Circle does not belong to this tangent") +// +// val nextCircleIndex = if (direction == this.shellDirection) { +// if (circleIndex == circles.lastIndex) 0 else circleIndex + 1 +// } else { +// if (circleIndex == 0) circles.lastIndex else circleIndex - 1 +// } +// +// return ObstacleTangent( +// LineSegment( +// shell[nextCircleIndex].end, +// shell[nextCircleIndex].begin +// ), +// ObstacleConnection(this, circleIndex, direction), +// ObstacleConnection(this, nextCircleIndex, direction), +// ) +// } +// +// /** +// * All tangents in given direction +// */ +// internal fun tangentsAlong( +// initialCircleIndex: Int, +// finalCircleIndex: Int, +// direction: Trajectory2D.Direction, +// ): List { +// return buildList { +// var currentIndex = initialCircleIndex +// do { +// val tangent = nextTangent(currentIndex, direction) +// add(tangent) +// currentIndex = tangent.endNode.nodeIndex +// } while (currentIndex != finalCircleIndex) +// } +// } +// +// override fun equals(other: Any?): Boolean { +// if (other == null || other !is ObstacleShell) return false +// return circles == other.circles +// } +// +// override fun hashCode(): Int { +// return circles.hashCode() +// } +//} +// +//internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleShell(listOf(*circles)) +// +// +//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 * Euclidean2DSpace.vector(v.y / Euclidean2DSpace.norm(v), -v.x / Euclidean2DSpace.norm(v)), +// r * Euclidean2DSpace.vector(-v.y / Euclidean2DSpace.norm(v), v.x / Euclidean2DSpace.norm(v)) +// ) +//} +// +// +//private fun constructTangentCircles( +// point: DoubleVector2D, +// direction: DoubleVector2D, +// r: Double, +//): LR { +// 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)) { +// LR( +// Circle2D(center1, r), +// Circle2D(center2, r) +// ) +// } else { +// LR( +// Circle2D(center2, r), +// Circle2D(center1, r) +// ) +// } +//} +// +//private fun sortedObstacles( +// currentObstacle: Obstacle, +// obstacles: List, +//): List { +// return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) } +//} +// +///** +// * Check if all proposed paths have ended at [finalObstacle] +// */ +//private fun allFinished( +// paths: List, +// finalObstacle: Obstacle, +//): Boolean = paths.all { it.last().endNode.obstacle === finalObstacle } +// +//private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end) +// +// +//private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D( +// buildList { +// tangents.zipWithNext().forEach { (left, right: ObstacleTangent) -> +// 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() = Euclidean2DSpace.vector(cos(bearing), sin(bearing)) +// +// // two circles for the initial point +// val initialCircles = constructTangentCircles( +// start, +// start.direction(), +// startingRadius +// ) +// +// //two circles for the final point +// val finalCircles = constructTangentCircles( +// finish, +// finish.direction(), +// finalRadius +// ) +// +// //all valid trajectories +// val trajectories = mutableListOf() +// +// for (i in listOf(Trajectory2D.L, Trajectory2D.R)) { +// for (j in listOf(Trajectory2D.L, Trajectory2D.R)) { +// //Using obstacle to minimize code bloat +// val initialObstacle = ObstacleShell(initialCircles[i]) +// val finalObstacle = ObstacleShell(finalCircles[j]) +// +// var currentPaths: List = listOf( +// TangentPath( +// //We need only the direction of the final segment from this +// ObstacleTangent( +// LineSegment(start, start), +// ObstacleConnection(initialObstacle, 0, i), +// ObstacleConnection(initialObstacle, 0, i), +// ) +// ) +// ) +// while (!allFinished(currentPaths, finalObstacle)) { +// // paths after next obstacle iteration +// val newPaths = mutableListOf() +// // for each path propagate it one obstacle further +// for (tangentPath: TangentPath in currentPaths) { +// val currentNode = tangentPath.last().endNode +// val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection +// val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle) +// +// // If path is finished, ignore it +// // TODO avoid returning to ignored obstacle on the next cycle +// if (currentObstacle == finalObstacle) { +// newPaths.add(tangentPath) +// } else { +// val tangentToFinal: ObstacleTangent = +// outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( +// currentDirection, +// Trajectory2D.S, +// j +// )] ?: break +// +// // searching for the nearest obstacle that intersects with the direct path +// val nextObstacle = obstacles.filter { obstacle -> +// obstacle.intersects(tangentToFinal) +// }.minByOrNull { currentObstacle.center.distanceTo(it.center) } ?: finalObstacle +// +// //TODO add break check for end of path +// +// // All valid tangents from current obstacle to the next one +// val nextTangents: Collection = outerTangents( +// currentObstacle, +// nextObstacle +// ).filter { (key, tangent) -> +//// obstacles.none { obstacle -> +//// obstacle === currentObstacle +//// || obstacle === nextObstacle +//// || obstacle.innerIntersects(tangent) +//// } && // does not intersect other obstacles +// key.first == currentDirection && // initial direction is the same as end of previous segment direction +// (nextObstacle != finalObstacle || key.third == j) // if it is the last, it should be the same as the one we are searching for +// }.values +// +// for (tangent in nextTangents) { +// val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) { +// //if the previous segment last circle is the same as first circle of the next segment +// +// //If obstacle consists of single circle, do not walk around +// if (currentObstacle.circles.size < 2) { +// emptyList() +// } else { +// val lengthMaxPossible = arcLength( +// tangent.startCircle, +// tangentPath.last().lineSegment.end, +// currentObstacle.nextTangent( +// tangent.beginNode.nodeIndex, +// currentDirection +// ).lineSegment.begin, +// currentDirection +// ) +// +// val lengthCalculated = arcLength( +// tangent.startCircle, +// tangentPath.last().lineSegment.end, +// tangent.lineSegment.begin, +// currentDirection +// ) +// // ensure that path does not go inside the obstacle +// if (lengthCalculated > lengthMaxPossible) { +// currentObstacle.tangentsAlong( +// currentNode.nodeIndex, +// tangent.beginNode.nodeIndex, +// currentDirection, +// ) +// } else { +// emptyList() +// } +// } +// } else { +// currentObstacle.tangentsAlong( +// currentNode.nodeIndex, +// tangent.beginNode.nodeIndex, +// currentDirection, +// ) +// } +// newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) +// } +// } +// } +// currentPaths = newPaths +// } +// +// trajectories += currentPaths.map { tangentPath -> +//// val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection +// val end = Obstacle(finalCircles[j]) +// TangentPath( +// tangentPath.tangents + +// ObstacleTangent( +// LineSegment(finish, finish), +// ObstacleConnection(end, 0, j), +// ObstacleConnection(end, 0, j) +// ) +// ) +// }.map { it.toTrajectory() } +// } +// } +// return trajectories +//} diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt new file mode 100644 index 0000000..fae7a7d --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt @@ -0,0 +1,363 @@ +package space.kscience.trajectory + +import space.kscience.kmath.geometry.* +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import kotlin.collections.component1 +import kotlin.collections.component2 +import kotlin.math.PI +import kotlin.math.atan2 + + +public class Obstacles(public val obstacles: List) { + + private inner class ObstacleConnection( + val obstacleIndex: Int, + val nodeIndex: Int, + val direction: Trajectory2D.Direction, + ) { + val obstacle: Obstacle get() = obstacles[obstacleIndex] + val circle: Circle2D get() = obstacle.circles[nodeIndex] + } + + private inner class ObstacleTangent( + val tangentTrajectory: StraightTrajectory2D, + val from: ObstacleConnection?, + val to: ObstacleConnection?, + ) { + /** + * If false this tangent intersects another obstacle + */ + val isValid by lazy { + obstacles.indices.none { + it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory) + } + } + } + + + /** + * All tangents between two obstacles + * + * In general generates 4 paths. + * TODO check intersections. + */ + private fun tangentsBetween( + firstIndex: Int, + secondIndex: Int, + ): Map = with(Euclidean2DSpace) { + val first = obstacles[firstIndex] + val second = obstacles[secondIndex] + val firstPolygon = polygon(first.circles.map { it.center }) + val secondPolygon = polygon(second.circles.map { it.center }) + buildMap { + for (firstCircleIndex in first.circles.indices) { + val firstCircle = first.circles[firstCircleIndex] + for (secondCircleIndex in second.circles.indices) { + val secondCircle = second.circles[secondCircleIndex] + for ((pathType, segment) in tangentsBetweenCircles( + firstCircle, + secondCircle + )) { + if (!intersects(firstPolygon, segment) && !intersects(secondPolygon, segment)) { + put( + pathType, + ObstacleTangent( + segment, + ObstacleConnection(firstIndex, firstCircleIndex, pathType.first), + ObstacleConnection(secondIndex, secondCircleIndex, pathType.third) + ) + ) + } + } + } + } + } + } + + + private fun tangentsFromCircle( + circle: Circle2D, + direction: Trajectory2D.Direction, + obstacleIndex: Int, + ): Map = with(Euclidean2DSpace) { + val obstacle = obstacles[obstacleIndex] + val polygon = polygon(obstacle.circles.map { it.center }) + buildMap { + for (circleIndex in obstacle.circles.indices) { + val obstacleCircle = obstacle.circles[circleIndex] + for ((pathType, segment) in tangentsBetweenCircles( + circle, + obstacleCircle + )) { + if (pathType.first == direction && !intersects(polygon, segment)) { + put( + pathType, + ObstacleTangent( + segment, + null, + ObstacleConnection(obstacleIndex, circleIndex, pathType.third) + ) + ) + } + } + } + } + } + + private fun tangentToCircle( + obstacleIndex: Int, + obstacleDirection: Trajectory2D.Direction, + circle: Circle2D, + direction: Trajectory2D.Direction, + ): ObstacleTangent? = with(Euclidean2DSpace) { + val obstacle = obstacles[obstacleIndex] + val polygon = polygon(obstacle.circles.map { it.center }) + for (circleIndex in obstacle.circles.indices) { + val obstacleCircle = obstacle.circles[circleIndex] + tangentsBetweenCircles( + obstacleCircle, + circle + ).get(DubinsPath.Type(obstacleDirection, Trajectory2D.S, direction))?.takeIf { + !intersects(polygon, it) + }?.let { + return ObstacleTangent( + it, + ObstacleConnection(obstacleIndex, circleIndex, obstacleDirection), + null, + ) + } + } + return null + } + + + private val tangentsCache = hashMapOf, Map>() + + private fun getAllTangents(i: Int, j: Int): Map = + tangentsCache.getOrPut(i to j) { + tangentsBetween(i, j) + } + + + /** + * Circumvention trajectory alongside obstacle. Replacing first and last arcs with appropriate cuts + */ + private fun trajectoryBetween(tangent1: ObstacleTangent, tangent2: ObstacleTangent): CompositeTrajectory2D { + require(tangent1.to != null) + require(tangent2.from != null) + + require(tangent1.to.obstacleIndex == tangent2.from.obstacleIndex) + require(tangent1.to.direction == tangent2.from.direction) + + val circumvention = tangent1.to.obstacle.circumvention( + tangent1.to.direction, + tangent1.to.nodeIndex, + tangent2.from.nodeIndex + ).segments.toMutableList() + + //cutting first and last arcs to accommodate connection points + val first = circumvention.first() as CircleTrajectory2D + val last = circumvention.last() as CircleTrajectory2D + circumvention[0] = CircleTrajectory2D( + first.circle, + tangent1.tangentTrajectory.endPose, + first.end + ) + circumvention[circumvention.lastIndex] = CircleTrajectory2D( + last.circle, + last.begin, + tangent2.tangentTrajectory.beginPose + ) + return CompositeTrajectory2D(circumvention) + } + + private inner class TangentPath(val tangents: List) { + val isFinished get() = tangents.last().to == null + + fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D( + buildList { + add(tangents.first().tangentTrajectory) + tangents.zipWithNext().forEach { (l, r) -> + addAll(trajectoryBetween(l, r).segments) + add(r.tangentTrajectory) + } + } + ) + } + + public fun allTrajectoriesAvoiding( + startCircle: Circle2D, + startDirection: Trajectory2D.Direction, + endCircle: Circle2D, + endDirection: Trajectory2D.Direction, + ): Collection { + val directTangent: StraightTrajectory2D = tangentsBetweenCircles(startCircle, endCircle).get( + DubinsPath.Type(startDirection, Trajectory2D.S, endDirection) + ) ?: return emptySet() + + //fast return if no obstacles intersect direct path + if (obstacles.none { it.intersects(directTangent) }) return listOf(directTangent) + + /** + * Continue current tangent to final point or to the next obstacle + */ + fun TangentPath.nextSteps(): Collection { + val connection = tangents.last().to + require(connection != null) + + //indices of obstacles that are not on previous path + val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex } + + //a tangent to end point, null if tangent could not be constructed + val tangentToEnd: ObstacleTangent = tangentToCircle( + connection.obstacleIndex, + connection.direction, + endCircle, + endDirection + ) ?: return emptySet() + + if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf( + TangentPath(tangents + tangentToEnd) + ) + + // tangents to other obstacles + return remainingObstacleIndices.sortedWith( + compareByDescending { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles + .thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest + ).firstNotNullOf { nextObstacleIndex -> + //all tangents from cache + getAllTangents(connection.obstacleIndex, nextObstacleIndex).filter { + //filtered by direction + it.key.first == connection.direction + }.values.takeIf { it.isNotEmpty() } // skip if empty + }.map { + TangentPath(tangents + it) + } + } + + + //find nearest obstacle that has valid tangents to + val tangentsToFirstObstacle: Collection = obstacles.indices.sortedWith( + compareByDescending { obstacles[it].intersects(directTangent) } //take intersecting obstacles + .thenBy { startCircle.center.distanceTo(obstacles[it].center) } //then nearest + ).firstNotNullOf { obstacleIndex -> + tangentsFromCircle(startCircle, startDirection, obstacleIndex).values + .filter { it.isValid }.takeIf { it.isNotEmpty() } + } + + var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) } + + while (!paths.all { it.isFinished }) { + paths = paths.flatMap { it.nextSteps() } + } + return paths.map { it.toTrajectory() } + } + + + public companion object { + private data class LR(val l: T, val r: T) { + operator fun get(direction: Trajectory2D.Direction) = when (direction) { + Trajectory2D.L -> l + Trajectory2D.R -> r + } + } + + private fun normalVectors(v: DoubleVector2D, r: Double): Pair = + with(Euclidean2DSpace) { + Pair( + r * vector(v.y / norm(v), -v.x / norm(v)), + r * vector(-v.y / norm(v), v.x / norm(v)) + ) + } + + private fun constructTangentCircles( + pose: DubinsPose2D, + r: Double, + ): LR = with(Euclidean2DSpace) { + val direction = DubinsPose2D.bearingToVector(pose.bearing) + //TODO optimize to use bearing + val center1 = pose + normalVectors(direction, r).first + val center2 = pose + normalVectors(direction, r).second + val p1 = center1 - pose + return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) { + LR( + Circle2D(center1, r), + Circle2D(center2, r) + ) + } else { + LR( + Circle2D(center2, r), + Circle2D(center1, r) + ) + } + } + + public fun avoidObstacles( + start: DubinsPose2D, + finish: DubinsPose2D, + startingRadius: Double, + obstacleList: List, + finalRadius: Double = startingRadius, + ): List { + val obstacles = Obstacles(obstacleList) + val initialCircles = constructTangentCircles( + start, + startingRadius + ) + + //two circles for the final point + val finalCircles = constructTangentCircles( + finish, + finalRadius + ) + val lr = listOf(Trajectory2D.L, Trajectory2D.R) + return buildList { + lr.forEach { beginDirection -> + lr.forEach { endDirection -> + addAll( + obstacles.allTrajectoriesAvoiding( + initialCircles[beginDirection], + beginDirection, + finalCircles[endDirection], + endDirection + ) + ) + } + } + } + } + + public fun avoidObstacles( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + vararg obstacles: Obstacle, + ): List = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList()) + + public fun avoidPolygons( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + vararg polygons: Polygon, + ): List { + val obstacles: List = polygons.map { polygon -> + Obstacle(polygon.points, trajectoryRadius) + } + return avoidObstacles(start, finish, trajectoryRadius, obstacles) + } + + + public fun avoidPolygons( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + polygons: Collection>, + ): List { + val obstacles: List = polygons.map { polygon -> + Obstacle(polygon.points, trajectoryRadius) + } + return avoidObstacles(start, finish, trajectoryRadius, obstacles) + } + } + +} \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt index 5eb84a6..5dcfdcb 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Trajectory2D.kt @@ -11,16 +11,24 @@ import kotlinx.serialization.Serializable import kotlinx.serialization.UseSerializers import space.kscience.kmath.geometry.* import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Euclidean2DSpace.minus import kotlin.math.atan2 @Serializable public sealed interface Trajectory2D { public val length: Double + public val beginPose: DubinsPose2D + public val endPose: DubinsPose2D + + /** + * Produce a trajectory with reversed order of points + */ + public fun reversed(): Trajectory2D public sealed interface Type - public sealed interface Direction: Type + public sealed interface Direction : Type public object R : Direction { override fun toString(): String = "R" @@ -35,6 +43,9 @@ public sealed interface Trajectory2D { } } + +public val DoubleVector2D.bearing: Angle get() = (atan2(x, y).radians).normalized() + /** * Straight path segment. The order of start and end defines the direction */ @@ -47,49 +58,60 @@ public data class StraightTrajectory2D( override val length: Double get() = begin.distanceTo(end) - public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized() + public val bearing: Angle get() = (end - begin).bearing + + override val beginPose: DubinsPose2D get() = DubinsPose2D(begin, bearing) + override val endPose: DubinsPose2D get() = DubinsPose2D(end, bearing) + + override fun reversed(): StraightTrajectory2D = StraightTrajectory2D(end, begin) } +public fun StraightTrajectory2D(segment: LineSegment2D): StraightTrajectory2D = + StraightTrajectory2D(segment.begin, segment.end) + /** * An arc segment */ @Serializable @SerialName("arc") -public data class CircleTrajectory2D( +public data class CircleTrajectory2D ( public val circle: Circle2D, - public val start: DubinsPose2D, + public val begin: DubinsPose2D, public val end: DubinsPose2D, ) : Trajectory2D { + override val beginPose: DubinsPose2D get() = begin + override val endPose: DubinsPose2D get() = end + /** * Arc length in radians */ - val arcLength: Angle + val arcAngle: Angle get() = if (direction == Trajectory2D.L) { - start.bearing - end.bearing + begin.bearing - end.bearing } else { - end.bearing - start.bearing + end.bearing - begin.bearing }.normalized() override val length: Double by lazy { - circle.radius * arcLength.radians + circle.radius * arcAngle.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 + when { + begin.y < circle.center.y -> if (begin.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L + begin.y > circle.center.y -> if (begin.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L + else -> if (begin.bearing == Angle.zero) { + if (begin.x < circle.center.x) Trajectory2D.R else Trajectory2D.L } else { - if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L + if (begin.x > circle.center.x) Trajectory2D.R else Trajectory2D.L } } } + override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, end.reversed(), begin.reversed()) + public companion object { public fun of( center: DoubleVector2D, @@ -124,8 +146,28 @@ public data class CircleTrajectory2D( @SerialName("composite") public class CompositeTrajectory2D(public val segments: List) : Trajectory2D { override val length: Double get() = segments.sumOf { it.length } + + override val beginPose: DubinsPose2D get() = segments.first().beginPose + override val endPose: DubinsPose2D get() = segments.last().endPose + + override fun reversed(): CompositeTrajectory2D = CompositeTrajectory2D(segments.map { it.reversed() }.reversed()) } public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList()) +public fun Euclidean2DSpace.trajectoryIntersects(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) { + is CircleTrajectory2D -> when (b) { + is CircleTrajectory2D -> intersectsOrInside(a.circle, b.circle) + is StraightTrajectory2D -> intersects(a.circle, b) + is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) } + } + + is StraightTrajectory2D -> when (b) { + is CircleTrajectory2D -> intersects(a, b.circle) + is StraightTrajectory2D -> intersects(a, b) + is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) } + } + + is CompositeTrajectory2D -> a.segments.any { trajectoryIntersects(it, b) } +} diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt new file mode 100644 index 0000000..e5f044d --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt @@ -0,0 +1,120 @@ +package space.kscience.trajectory + +import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.trajectory.DubinsPath.Type +import kotlin.math.* + + +/** + * Create inner and outer tangents between two circles. + * This method returns a map of segments using [DubinsPath] connection type notation. + */ +internal fun tangentsBetweenCircles( + first: Circle2D, + second: Circle2D, +): Map = with(Euclidean2DSpace) { + // Distance between centers + val distanceBetweenCenters: Double = first.center.distanceTo(second.center) + + // return empty map if one circle is inside another + val minRadius = min(first.radius, second.radius) + val maxRadius = max(first.radius, second.radius) + + val listOfTangents = when { + // one circle inside another, no tangents + distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap() + // circles intersect, only outer tangents + distanceBetweenCenters - minRadius <= maxRadius -> listOf(Type.RSR, Type.LSL) + // no intersections all tangents + else -> listOf(Type.RSR, Type.RSL, Type.LSR, Type.LSL) + } + + val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y) + + return listOfTangents.associateWith { route -> + val r1 = when (route.first) { + Trajectory2D.L -> -first.radius + Trajectory2D.R -> first.radius + } + val r2 = when (route.third) { + Trajectory2D.L -> -second.radius + Trajectory2D.R -> second.radius + } + val r = if (r1.sign == r2.sign) { + r1.absoluteValue - r2.absoluteValue + } else { + r1.absoluteValue + r2.absoluteValue + } + + val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r) + val 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)) + + StraightTrajectory2D( + first.center + w * r1, + second.center + w * r2 + ) + } +} + +/** + * Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex] + */ +public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: Int): CompositeTrajectory2D { + require(fromIndex in circles.indices) { "$fromIndex is not in ${circles.indices}" } + val startCircle = circles[fromIndex] + val segments = buildList { + val reserve = mutableListOf() + + val sourceSegments = when (direction) { + Trajectory2D.L -> circumvention.reversed().segments + Trajectory2D.R -> circumvention.segments + } + + var i = 0 + while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) { + //put all segments before target circle on the reserve + reserve.add(sourceSegments[i]) + i++ + } + + while (i < sourceSegments.size) { + // put required segments on result list + add(sourceSegments[i]) + i++ + } + //add remaining segments from reserve + addAll(reserve) + check(i == sourceSegments.size) + } + return CompositeTrajectory2D(segments) +} + +/** + * Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex] + * and ending (including) at obstacle node with given [toIndex] + */ +public fun Obstacle.circumvention( + direction: Trajectory2D.Direction, + fromIndex: Int, + toIndex: Int, +): CompositeTrajectory2D { + require(toIndex in circles.indices) { "$toIndex is not in ${circles.indices}" } + val toCircle = circles[toIndex] + val fullCircumvention = circumvention(direction, fromIndex).segments + return CompositeTrajectory2D( + buildList { + var i = 0 + do { + val segment = fullCircumvention[i] + add(segment) + i++ + } while ((segment as? CircleTrajectory2D)?.circle != toCircle) + } + ) +} \ No newline at end of file diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt deleted file mode 100644 index 3b6f4b8..0000000 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt +++ /dev/null @@ -1,536 +0,0 @@ -package space.kscience.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.misc.zipWithNextCircular -import space.kscience.kmath.operations.DoubleField.pow -import kotlin.math.* - - -internal data class ObstacleConnection( - val obstacle: Obstacle, - val nodeIndex: Int, - val direction: Trajectory2D.Direction, -) { - val circle get() = obstacle.circles[nodeIndex] -} - -internal data class ObstacleTangent( - val lineSegment: LineSegment2D, - val beginNode: ObstacleConnection, - val endNode: ObstacleConnection, -) : LineSegment2D by lineSegment { - val startCircle get() = beginNode.circle - val startDirection get() = beginNode.direction - val endCircle get() = endNode.circle - val endDirection get() = endNode.direction -} - - -private class LR(val l: T, val r: T) { - operator fun get(direction: Trajectory2D.Direction) = when (direction) { - Trajectory2D.L -> l - Trajectory2D.R -> r - } -} - -private class TangentPath(val tangents: List) { - fun last() = tangents.last() -} - -private fun TangentPath(vararg tangents: ObstacleTangent) = 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 tangentsBetweenCircles( - first: Circle2D, - second: Circle2D, -): Map = with(Euclidean2DSpace) { - // Distance between centers - val distanceBetweenCenters: Double = first.center.distanceTo(second.center) - - // return empty map if one circle is inside another - val minRadius = min(first.radius, second.radius) - val maxRadius = max(first.radius, second.radius) - - val listOfTangents = when { - // one circle inside another, no tangents - distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap() - // circles intersect, only outer tangents - distanceBetweenCenters - minRadius <= maxRadius -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.LSL) - // no intersections all tangents - else -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.RSL, DubinsPath.Type.LSR, DubinsPath.Type.LSL) - } - - val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y) - - return listOfTangents.associateWith { route -> - val r1 = when (route.first) { - Trajectory2D.L -> -first.radius - Trajectory2D.R -> first.radius - } - val r2 = when (route.third) { - Trajectory2D.L -> -second.radius - Trajectory2D.R -> second.radius - } - val r = if (r1.sign == r2.sign) { - r1.absoluteValue - r2.absoluteValue - } else { - r1.absoluteValue + r2.absoluteValue - } - - val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r) - val 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)) - - LineSegment( - first.center + w * r1, - second.center + w * r2 - ) - } -} - -private fun Circle2D.isInside(other: Circle2D): Boolean { - return center.distanceTo(other.center) + radius <= other.radius -} - - -internal class ObstacleShell( - nodes: List, -) : Obstacle { - override val circles: List - override val center: Vector2D - private val shell: List - private val shellDirection: Trajectory2D.Direction - - init { - this.center = Euclidean2DSpace.vector( - nodes.sumOf { it.center.x } / nodes.size, - nodes.sumOf { it.center.y } / nodes.size - ) - -// this.circles = nodes.filter { node -> -// //filter nodes inside other nodes -// nodes.none{ node !== it && node.isInside(it) } -// } - - this.circles = nodes.distinct() - - if (nodes.size < 2) { - shell = emptyList() - shellDirection = Trajectory2D.R - } else { - - //ignore cases when one circle is inside another one - val lslTangents = circles.zipWithNextCircular { a, b -> - tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL] ?: error("Intersecting circles") - } - - val rsrTangents = circles.zipWithNextCircular { a, b -> - tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] ?: error("Intersecting circles") - } - - - val lslToCenter = lslTangents.sumOf { it.begin.distanceTo(center) } + - lslTangents.sumOf { it.end.distanceTo(center) } - val rsrToCenter = rsrTangents.sumOf { it.begin.distanceTo(center) } + - rsrTangents.sumOf { it.end.distanceTo(center) } - - if (rsrToCenter >= lslToCenter) { - this.shell = rsrTangents - this.shellDirection = Trajectory2D.R - } else { - this.shell = lslTangents - this.shellDirection = Trajectory2D.L - } - } - } - - constructor(obstacle: Obstacle) : this(obstacle.circles) - - /** - * Check if segment has any intersections with this obstacle - */ - override fun intersects(segment: LineSegment2D): Boolean = - shell.any { tangent -> segment.intersectsSegment(tangent) } - || circles.any { circle -> segment.intersectsCircle(circle) } - - override fun intersects(circle: Circle2D): Boolean = - shell.any { tangent -> tangent.intersectsCircle(circle) } - - /** - * Tangent to next obstacle node in given direction - */ - fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent { - if (circleIndex == -1) error("Circle does not belong to this tangent") - - val nextCircleIndex = if (direction == this.shellDirection) { - if (circleIndex == circles.lastIndex) 0 else circleIndex + 1 - } else { - if (circleIndex == 0) circles.lastIndex else circleIndex - 1 - } - - return ObstacleTangent( - LineSegment( - shell[nextCircleIndex].end, - shell[nextCircleIndex].begin - ), - ObstacleConnection(this, circleIndex, direction), - ObstacleConnection(this, nextCircleIndex, direction), - ) - } - - /** - * All tangents in given direction - */ - internal fun tangentsAlong( - initialCircleIndex: Int, - finalCircleIndex: Int, - direction: Trajectory2D.Direction, - ): List { - return buildList { - var currentIndex = initialCircleIndex - do { - val tangent = nextTangent(currentIndex, direction) - add(tangent) - currentIndex = tangent.endNode.nodeIndex - } while (currentIndex != finalCircleIndex) - } - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is ObstacleShell) return false - return circles == other.circles - } - - override fun hashCode(): Int { - return circles.hashCode() - } -} - -internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleShell(listOf(*circles)) - - -private fun LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean { - infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x - infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign - - return with(Euclidean2DSpace) { - (other.begin - begin) crossSign (other.end - begin) != - (other.begin - end) crossSign (other.end - end) && - (begin - other.begin) crossSign (end - other.begin) != - (begin - other.end) crossSign (end - other.end) - } -} - -private fun LineSegment2D.intersectsCircle(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 aNormalized = a / (a * a + b * b + c * c) - val bNormalized = b / (a * a + b * b + c * c) - val cNormalized = c / (a * a + b * b + c * c) - - val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized - if (d < 1e-6) { - return false - } else { - val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized - val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized - if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) { - return true - } - } - return false -} - - -/** - * All tangents between two obstacles - * - * In general generates 4 paths. - * TODO check intersections. - */ -private fun outerTangents(first: Obstacle, second: Obstacle): Map = buildMap { - - for (firstCircleIndex in first.circles.indices) { - for (secondCircleIndex in second.circles.indices) { - for ((pathType, segment) in tangentsBetweenCircles( - first.circles[firstCircleIndex], - second.circles[secondCircleIndex] - )) { - val tangent = ObstacleTangent( - segment, - ObstacleConnection(first, firstCircleIndex, pathType.first), - ObstacleConnection(second, secondCircleIndex, pathType.third) - ) - - if (!(first.intersects(tangent)) && !(second.intersects(tangent))) { - put( - pathType, - tangent - ) - } - } - } - } -} - -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 * Euclidean2DSpace.vector(v.y / Euclidean2DSpace.norm(v), -v.x / Euclidean2DSpace.norm(v)), - r * Euclidean2DSpace.vector(-v.y / Euclidean2DSpace.norm(v), v.x / Euclidean2DSpace.norm(v)) - ) -} - - -private fun constructTangentCircles( - point: DoubleVector2D, - direction: DoubleVector2D, - r: Double, -): LR { - 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)) { - LR( - Circle2D(center1, r), - Circle2D(center2, r) - ) - } else { - LR( - Circle2D(center2, r), - Circle2D(center1, r) - ) - } -} - -private fun sortedObstacles( - currentObstacle: Obstacle, - obstacles: List, -): List { - return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) } -} - -/** - * Check if all proposed paths have ended at [finalObstacle] - */ -private fun allFinished( - paths: List, - finalObstacle: Obstacle, -): Boolean { - for (path in paths) { - if (path.last().endNode.obstacle !== finalObstacle) { - return false - } - } - return true -} - -private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end) - - -private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D( - buildList { - tangents.zipWithNext().forEach { (left, right: ObstacleTangent) -> - 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() = Euclidean2DSpace.vector(cos(bearing), sin(bearing)) - - // two circles for the initial point - val initialCircles = constructTangentCircles( - start, - start.direction(), - startingRadius - ) - - //two circles for the final point - val finalCircles = constructTangentCircles( - finish, - finish.direction(), - finalRadius - ) - - //all valid trajectories - val trajectories = mutableListOf() - - for (i in listOf(Trajectory2D.L, Trajectory2D.R)) { - for (j in listOf(Trajectory2D.L, Trajectory2D.R)) { - //Using obstacle to minimize code bloat - val initialObstacle = ObstacleShell(initialCircles[i]) - val finalObstacle = ObstacleShell(finalCircles[j]) - - var currentPaths: List = listOf( - TangentPath( - //We need only the direction of the final segment from this - ObstacleTangent( - LineSegment(start, start), - ObstacleConnection(initialObstacle, 0, i), - ObstacleConnection(initialObstacle, 0, i), - ) - ) - ) - while (!allFinished(currentPaths, finalObstacle)) { - // paths after next obstacle iteration - val newPaths = mutableListOf() - // for each path propagate it one obstacle further - for (tangentPath: TangentPath in currentPaths) { - val currentNode = tangentPath.last().endNode - val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection - val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle) - - // If path is finished, ignore it - // TODO avoid returning to ignored obstacle on the next cycle - if (currentObstacle == finalObstacle) { - newPaths.add(tangentPath) - } else { - val tangentToFinal: ObstacleTangent = - outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( - currentDirection, - Trajectory2D.S, - j - )] ?: break - - // searching for the nearest obstacle that intersects with the direct path - val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle -> - obstacle.intersects(tangentToFinal) - } ?: finalObstacle - - //TODO add break check for end of path - - // All valid tangents from current obstacle to the next one - val nextTangents: Collection = outerTangents( - currentObstacle, - nextObstacle - ).filter { (key, tangent) -> - obstacles.none { obstacle -> obstacle.intersects(tangent) } && // does not intersect other obstacles - key.first == currentDirection && // initial direction is the same as end of previous segment direction - (nextObstacle != finalObstacle || key.third == j) // if it is the last, it should be the same as the one we are searching for - }.values - - for (tangent in nextTangents) { - val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) { - //if the previous segment last circle is the same as first circle of the next segment - - //If obstacle consists of single circle, do not walk around - if (currentObstacle.circles.size < 2) { - emptyList() - } else { - val lengthMaxPossible = arcLength( - tangent.startCircle, - tangentPath.last().lineSegment.end, - currentObstacle.nextTangent( - tangent.beginNode.nodeIndex, - currentDirection - ).lineSegment.begin, - currentDirection - ) - - val lengthCalculated = arcLength( - tangent.startCircle, - tangentPath.last().lineSegment.end, - tangent.lineSegment.begin, - currentDirection - ) - // ensure that path does not go inside the obstacle - if (lengthCalculated > lengthMaxPossible) { - currentObstacle.tangentsAlong( - currentNode.nodeIndex, - tangent.beginNode.nodeIndex, - currentDirection, - ) - } else { - emptyList() - } - } - } else { - currentObstacle.tangentsAlong( - currentNode.nodeIndex, - tangent.beginNode.nodeIndex, - currentDirection, - ) - } - newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) - } - } - } - currentPaths = newPaths - } - - trajectories += currentPaths.map { tangentPath -> -// val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection - val end = Obstacle(finalCircles[j]) - TangentPath( - tangentPath.tangents + - ObstacleTangent( - LineSegment(finish, finish), - ObstacleConnection(end, 0, j), - ObstacleConnection(end, 0, j) - ) - ) - }.map { it.toTrajectory() } - } - } - return trajectories -} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/ArcTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt similarity index 52% rename from trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/ArcTests.kt rename to trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt index ff4aef9..625ddb7 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/ArcTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt @@ -3,21 +3,19 @@ * 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.trajectory.segments +package space.kscience.kmath.geometry -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.trajectory.CircleTrajectory2D +import space.kscience.trajectory.DubinsPose2D import space.kscience.trajectory.Trajectory2D +import kotlin.math.PI import kotlin.test.Test import kotlin.test.assertEquals class ArcTests { @Test - fun arcTest() = with(Euclidean2DSpace){ + fun arc() = with(Euclidean2DSpace) { val circle = Circle2D(vector(0.0, 0.0), 2.0) val arc = CircleTrajectory2D.of( circle.center, @@ -26,7 +24,19 @@ class ArcTests { Trajectory2D.R ) assertEquals(circle.circumference / 4, arc.length, 1.0) - assertEquals(0.0, arc.start.bearing.degrees) + assertEquals(0.0, arc.begin.bearing.degrees) assertEquals(90.0, arc.end.bearing.degrees) } + + @Test + fun quarter() = with(Euclidean2DSpace) { + val circle = circle(1, 0, 1) + val arc = CircleTrajectory2D( + circle, + DubinsPose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians), + DubinsPose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians) + ) + assertEquals(Trajectory2D.R, arc.direction) + assertEquals(PI / 2, arc.length, 1e-4) + } } diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt new file mode 100644 index 0000000..5f4020e --- /dev/null +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt @@ -0,0 +1,135 @@ +/* + * 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.geometry + +import kotlin.math.pow +import kotlin.math.sqrt +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertFalse +import kotlin.test.assertTrue + +class CircleTests { + + @Test + fun circle() { + 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) + } + + @Test + fun circleIntersection() = with(Euclidean2DSpace) { + assertTrue { + intersectsOrInside( + circle(0.0, 0.0, 1.0), + circle(0.0, 0.0, 2.0) + ) + } + assertTrue { + intersectsOrInside( + circle(0.0, 1.0, 1.0), + circle(0.0, 0.0, 1.0) + ) + } + assertFalse { + intersectsOrInside( + circle(0.0, 1.0, 1.0), + circle(0.0, -1.1, 1.0) + ) + } + } + + @Test + fun circleLineIntersection() = with(Euclidean2DSpace) { + assertTrue { + intersects(circle(0, 0, 1), segment(1, 1, -1, 1)) + } + + assertTrue { + intersects(circle(1, 1, sqrt(2.0)/2), segment(1, 0, 0, 1)) + } + + assertTrue { + intersects(circle(1, 1, 1), segment(2, 2, 0, 2)) + } + + assertTrue { + intersects(circle(0, 0, 1), segment(1, -1, 1, 1)) + } + + assertTrue { + intersects(circle(0, 0, 1), segment(1, 0, -1, 0)) + } + + assertFalse { + intersects(circle(0, 0, 1), segment(1, 2, -1, 2)) + } + + assertFalse { + intersects(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0)) + } + } + + private fun Euclidean2DSpace.oldIntersect(circle: Circle2D, segment: LineSegment2D): Boolean{ + val begin = segment.begin + val end = segment.end + val lengthSquared = (begin.x - end.x).pow(2) + (begin.y - end.y).pow(2) + 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) + (end.y - circle.center.y).pow(2) - + circle.radius.pow(2) + + val aNormalized = lengthSquared / (lengthSquared * lengthSquared + b * b + c * c) + val bNormalized = b / (lengthSquared * lengthSquared + b * b + c * c) + val cNormalized = c / (lengthSquared * lengthSquared + b * b + c * c) + + val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized + if (d < 1e-6) { + return false + } else { + val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized + val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized + if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) { + return true + } + } + return false + } + + @Test + fun oldCircleLineIntersection() = with(Euclidean2DSpace){ + assertTrue { + oldIntersect(circle(0, 0, 1.1), segment(1, 1, -1, 1)) + } + + assertTrue { + oldIntersect(circle(1, 1, sqrt(2.0)/2+0.01), segment(1, 0, 0, 1)) + } + + assertTrue { + oldIntersect(circle(1, 1, 1.01), segment(2, 2, 0, 2)) + } + + assertTrue { + oldIntersect(circle(0, 0, 1.01), segment(1, -1, 1, 1)) + } + + assertTrue { + oldIntersect(circle(0, 0, 1.0), segment(2, 0, -2, 0)) + } + + assertFalse { + oldIntersect(circle(0, 0, 1), segment(1, 2, -1, 2)) + } + + assertFalse { + oldIntersect(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0)) + } + } +} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/LineTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/LineTests.kt similarity index 89% rename from trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/LineTests.kt rename to trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/LineTests.kt index 58de772..480e598 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/LineTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/LineTests.kt @@ -3,10 +3,8 @@ * 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.trajectory.segments +package space.kscience.kmath.geometry -import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.degrees import space.kscience.trajectory.StraightTrajectory2D import kotlin.math.pow import kotlin.math.sqrt diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt index 3402bf5..1d9f24b 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/DubinsTests.kt @@ -45,16 +45,16 @@ class DubinsTests { val b = path.segments[1] val c = path.segments[2] as CircleTrajectory2D - assertTrue(start.equalsFloat(a.start)) - assertTrue(end.equalsFloat(c.end)) + assertEquals(start, a.begin) + assertEquals(end, c.end) // Not working, theta double precision inaccuracy if (b is CircleTrajectory2D) { - assertTrue(a.end.equalsFloat(b.start)) - assertTrue(c.start.equalsFloat(b.end)) + assertEquals(a.end, b.begin) + assertEquals(c.begin, 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))) + assertEquals(a.end, DubinsPose2D(b.begin, b.bearing)) + assertEquals(c.begin, DubinsPose2D(b.end, b.bearing)) } } } diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt index b8b6f05..41a5311 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -8,38 +8,51 @@ package space.kscience.trajectory import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Euclidean2DSpace.vector import space.kscience.kmath.geometry.degrees +import kotlin.math.PI import kotlin.test.Test import kotlin.test.assertEquals import kotlin.test.assertTrue class ObstacleTest { + @Test - fun firstPath() { + 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(circle1) + val obstacle2 = Obstacle(circle2) + assertEquals(obstacle1, obstacle2) + } + + @Test + fun singeObstacle() { 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 outputTangents = Obstacle.avoidObstacles( + val outputTangents = Obstacles.avoidObstacles( DubinsPose2D(startPoint, startDirection), DubinsPose2D(finalPoint, finalDirection), startRadius, Obstacle(Circle2D(vector(7.0, 1.0), 5.0)) ) + assertTrue { outputTangents.isNotEmpty() } val length = outputTangents.minOf { it.length } assertEquals(27.2113183, length, 1e-6) } @Test - fun secondPath() { + fun twoObstacles() { val startPoint = vector(-5.0, -1.0) val startDirection = vector(1.0, 1.0) val radius = 0.5 val finalPoint = vector(20.0, 4.0) val finalDirection = vector(1.0, -1.0) - val paths = Obstacle.avoidObstacles( + val paths = Obstacles.avoidObstacles( DubinsPose2D(startPoint, startDirection), DubinsPose2D(finalPoint, finalDirection), radius, @@ -55,19 +68,36 @@ class ObstacleTest { Circle2D(vector(9.0, 4.0), 0.5) ) ) + assertTrue { paths.isNotEmpty() } val length = paths.minOf { it.length } assertEquals(28.9678224, length, 1e-6) } @Test - fun nearPoints() { + fun circumvention(){ + val obstacle = Obstacle( + Circle2D(vector(0.0, 0.0), 1.0), + Circle2D(vector(0.0, 1.0), 1.0), + Circle2D(vector(1.0, 1.0), 1.0), + Circle2D(vector(1.0, 0.0), 1.0) + ) + + val circumvention = obstacle.circumvention + + assertEquals(4, circumvention.segments.count { it is CircleTrajectory2D }) + + assertEquals(4 + 2* PI, circumvention.length, 1e-4) + } + + @Test + fun closePoints() { val startPoint = vector(-1.0, -1.0) val startDirection = vector(0.0, 1.0) val startRadius = 1.0 val finalPoint = vector(-1, -1) val finalDirection = vector(1.0, 0) - val paths = Obstacle.avoidObstacles( + val paths = Obstacles.avoidObstacles( DubinsPose2D(startPoint, startDirection), DubinsPose2D(finalPoint, finalDirection), startRadius, @@ -78,14 +108,15 @@ class ObstacleTest { Circle2D(vector(1.0, 0.0), 1.0) ) ) + assertTrue { paths.isNotEmpty() } val length = paths.minOf { it.length } println(length) //assertEquals(28.9678224, length, 1e-6) } @Test - fun fromMap() { - val paths = Obstacle.avoidObstacles( + fun largeCoordinates() { + val paths = Obstacles.avoidObstacles( DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees), DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees), 5000.0, @@ -98,34 +129,6 @@ class ObstacleTest { ) ) assertTrue { paths.isNotEmpty() } - val length = paths.minOf { it.length } - } - @Test - fun fromMapLess() { - - val paths = Obstacle.avoidObstacles( - DubinsPose2D(x = 48.4149535516561, y = 299.50862534208703, bearing = 3.401475378237137.degrees), - DubinsPose2D(x = 45.66638489126448, y = 283.00541087567504, bearing = 325.32183928982727.degrees), - 0.5, - Obstacle( - Circle2D(vector(x=44.60882236175772, y=289.52640759535935), radius=0.5), - Circle2D(vector(x=45.558751549431164, y=289.71165594902174), radius=0.5), - Circle2D(vector(x=46.590308440141426, y=289.3897500160981), radius=0.5), - Circle2D(vector(x=46.242119397653354, y=287.94964842121634), radius=0.5), - Circle2D(vector(x=44.92318047505464, y=288.0132403305273), radius=0.5) - ) - ) - val length = paths.minOf { it.length } - } - - @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 = ObstacleShell(listOf(circle1)) - val obstacle2 = ObstacleShell(listOf(circle2)) - assertEquals(obstacle1, obstacle2) } } \ No newline at end of file diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/CircleTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/segments/CircleTests.kt deleted file mode 100644 index 67fffb9..0000000 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/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.trajectory.segments - -import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.circumference -import kotlin.test.Test -import kotlin.test.assertEquals - -class CircleTests { - - @Test - fun arcTest() { - val center = Euclidean2DSpace.vector(0.0, 0.0) - val radius = 2.0 - val expectedCircumference = 12.56637 - val circle = Circle2D(center, radius) - assertEquals(expectedCircumference, circle.circumference, 1e-4) - } -} diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/math.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt similarity index 71% rename from trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/math.kt rename to trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt index f86a0e1..ff9c595 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/math.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/testutils.kt @@ -6,13 +6,15 @@ package space.kscience.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 assertEquals(expected: DubinsPose2D, actual: DubinsPose2D, precision: Double = 1e-6){ + kotlin.test.assertEquals(expected.x, actual.x, precision) + kotlin.test.assertEquals(expected.y, actual.y, precision) + kotlin.test.assertEquals(expected.bearing.radians, actual.bearing.radians, precision) +} fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)