From cac5841401bfb04e4e011d1955c5f8036f8e76d5 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 23 Apr 2023 11:01:08 +0300 Subject: [PATCH] Obstacle avoidance refactoring --- .../space/kscience/trajectory/Obstacle.kt | 557 ++---------------- .../kscience/trajectory/obstacleInternal.kt | 526 +++++++++++++++++ .../space/kscience/trajectory/ObstacleTest.kt | 78 +-- 3 files changed, 614 insertions(+), 547 deletions(-) create mode 100644 trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt 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 e769a2f..f3b40fd 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt @@ -5,531 +5,62 @@ 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.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.misc.zipWithNextCircular -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 +import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.LineSegment2D +import space.kscience.kmath.geometry.Polygon +import space.kscience.kmath.geometry.Vector2D -private class LR(val l: T, val r: T) { - operator fun get(direction: Trajectory2D.Direction) = when (direction) { - Trajectory2D.L -> l - Trajectory2D.R -> r - } -} +public interface Obstacle { + public val circles: List + public val center: Vector2D -private class TangentPath(val tangents: List) { - fun last() = tangents.last() -} + public fun intersects(segment: LineSegment2D): Boolean -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 tangentsBetweenCircles( - first: Circle2D, - second: Circle2D, -): Map = with(Euclidean2DSpace) { - //return empty map for concentric circles - if (first.center.equalsVector(second.center)) return emptyMap() - - // A line connecting centers - val line = LineSegment(first.center, second.center) - // Distance between centers - val distance = line.begin.distanceTo(line.end) - val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y) - var angle2: Double - - return listOf( - DubinsPath.Type.RSR, - DubinsPath.Type.RSL, - DubinsPath.Type.LSR, - DubinsPath.Type.LSL - ).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 - } - if (distance * distance < r * r) error("Circles should not intersect") - - 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)) - - LineSegment( - first.center + w * r1, - second.center + w * r2 - ) - } -} - -internal class Obstacle( - public val circles: List, -) { - - public val center: Vector2D = vector( - circles.sumOf { it.center.x } / circles.size, - circles.sumOf { it.center.y } / circles.size - ) - - internal val tangents: List - public val direction: Trajectory2D.Direction - - init { - if (circles.size < 2) { - tangents = emptyList() - direction = Trajectory2D.R - } else { - val lslTangents = circles.zipWithNextCircular { a, b -> - tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL]!! + public companion object { + public fun allPathsAvoiding( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + vararg obstacles: Obstacle, + ): List { + val obstacleShells: List = obstacles.map { polygon -> + ObstacleShell(polygon.circles) } - val rsrTangents = circles.zipWithNextCircular { a, b -> - tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR]!! + return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) + } + + public fun allPathsAvoiding( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + vararg obstacles: Polygon, + ): List { + val obstacleShells: List = obstacles.map { polygon -> + ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) }) } - val center = vector( - circles.sumOf { it.center.x } / circles.size, - circles.sumOf { it.center.y } / circles.size - ) - 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.tangents = rsrTangents - this.direction = Trajectory2D.R - } else { - this.tangents = lslTangents - this.direction = Trajectory2D.L - } - } - } - - internal fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent { - val circleIndex = circles.indexOf(circle) - if (circleIndex == -1) error("Circle does not belong to this tangent") - - val nextCircleIndex = if (direction == this.direction) { - if (circleIndex == circles.lastIndex) 0 else circleIndex + 1 - } else { - if (circleIndex == 0) circles.lastIndex else circleIndex - 1 + return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells) } - return Tangent( - circle, - circles[nextCircleIndex], - this, - this, - LineSegment( - tangents[nextCircleIndex].end, - tangents[nextCircleIndex].begin - ), - direction - ) - } - - 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)) +public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles)) -private fun LineSegment2D.intersectsSegment(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.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 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 -} - -/** - * Check if segment has any intersections with an obstacle - */ -private fun LineSegment2D.intersectsObstacle(obstacle: Obstacle): Boolean = - obstacle.tangents.any { tangent -> intersectsSegment(tangent) } - || obstacle.circles.any { circle -> intersectsCircle(circle) } - - -/** - * All tangents between two obstacles - * - * In general generates 4 paths. - * TODO check intersections. - */ -private fun outerTangents(first: Obstacle, second: Obstacle): Map = buildMap { - - for (firstCircle in first.circles) { - for (secondCircle in second.circles) { - for ((pathType, segment) in tangentsBetweenCircles(firstCircle, secondCircle)) { - val tangent = Tangent( - firstCircle, - secondCircle, - first, - second, - segment, - pathType.first, - pathType.third - ) - - if (!(tangent.intersectsObstacle(first)) && !(tangent.intersectsObstacle(second))) { - 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 * 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, -): 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 { 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 -} - -/** - * Check if all proposed paths have ended at [finalObstacle] - */ -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)) - - // 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 finalObstacle = Obstacle(finalCircles[j]) - - var currentPaths: List = listOf( - TangentPath( - //We need only the direction of the final segment from this - Tangent( - initialCircles[i], - initialCircles[i], - Obstacle(initialCircles[i]), - Obstacle(initialCircles[i]), - LineSegment(start, start), - 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 currentCircle = tangentPath.last().endCircle - val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection - val currentObstacle = tangentPath.last().endObstacle - - // 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: Tangent = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( - currentDirection, - Trajectory2D.S, - j - )] ?: TODO("Intersecting obstacles are not supported") - - // searching for the nearest obstacle that intersects with the direct path - val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle -> - tangentToFinal.intersectsObstacle(obstacle) - } ?: 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 -> tangent.intersectsObstacle(obstacle) } && // 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 (tangent.startObstacle.circles.size < 2){ - emptyList() - } else { - 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 - ) - // ensure that path does not go inside the obstacle - if (lengthCalculated > lengthMaxPossible) { - tangentsAlongTheObstacle( - currentCircle, - currentDirection, - tangent.startCircle, - currentObstacle - ) - } else { - emptyList() - } - } - } else { - tangentsAlongTheObstacle( - currentCircle, - currentDirection, - tangent.startCircle, - currentObstacle - ) - } - newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) - } - } - } - 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) - } -} +//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) } +// } +//} diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt new file mode 100644 index 0000000..7fef6b1 --- /dev/null +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt @@ -0,0 +1,526 @@ +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 Tangent( + val startCircle: Circle2D, + val endCircle: Circle2D, + val startObstacle: ObstacleShell, + val endObstacle: ObstacleShell, + val lineSegment: LineSegment2D, + val startDirection: Trajectory2D.Direction, + val endDirection: Trajectory2D.Direction = startDirection, +) : LineSegment2D by lineSegment + + +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: 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 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 + } + } + } + + /** + * 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) } + + fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent { + val circleIndex = circles.indexOf(circle) + 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 Tangent( + circle, + circles[nextCircleIndex], + this, + this, + LineSegment( + shell[nextCircleIndex].end, + shell[nextCircleIndex].begin + ), + direction + ) + } + + internal fun tangentsAlong( + initialCircle: Circle2D, + direction: Trajectory2D.Direction, + finalCircle: Circle2D, + ): List { + val dubinsTangents = mutableListOf() + var tangent = nextTangent(initialCircle, direction) + dubinsTangents.add(tangent) + while (tangent.endCircle != finalCircle) { + tangent = nextTangent(tangent.endCircle, direction) + dubinsTangents.add(tangent) + } + return dubinsTangents + } + + 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 { + 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.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 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 +} + + +/** + * All tangents between two obstacles + * + * In general generates 4 paths. + * TODO check intersections. + */ +private fun outerTangents(first: ObstacleShell, second: ObstacleShell): Map = buildMap { + + for (firstCircle in first.circles) { + for (secondCircle in second.circles) { + for ((pathType, segment) in tangentsBetweenCircles(firstCircle, secondCircle)) { + val tangent = Tangent( + firstCircle, + secondCircle, + first, + second, + segment, + pathType.first, + 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: ObstacleShell, + 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().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() = + Euclidean2DSpace.vector(space.kscience.kmath.geometry.cos(bearing), space.kscience.kmath.geometry.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 finalObstacle = ObstacleShell(finalCircles[j]) + + var currentPaths: List = listOf( + TangentPath( + //We need only the direction of the final segment from this + Tangent( + initialCircles[i], + initialCircles[i], + ObstacleShell(initialCircles[i]), + ObstacleShell(initialCircles[i]), + LineSegment(start, start), + 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 currentCircle = tangentPath.last().endCircle + val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection + val currentObstacle = tangentPath.last().endObstacle + + // 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: Tangent = 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 (tangent.startObstacle.circles.size < 2) { + emptyList() + } else { + 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 + ) + // ensure that path does not go inside the obstacle + if (lengthCalculated > lengthMaxPossible) { + currentObstacle.tangentsAlong( + currentCircle, + currentDirection, + tangent.startCircle, + ) + } else { + emptyList() + } + } + } else { + currentObstacle.tangentsAlong( + currentCircle, + currentDirection, + tangent.startCircle, + ) + } + newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent)) + } + } + } + currentPaths = newPaths + } + + trajectories += currentPaths.map { tangentPath -> + val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection + val end = finalCircles[j] + TangentPath( + tangentPath.tangents + + Tangent( + end, + end, + ObstacleShell(end), + ObstacleShell(end), + LineSegment(finish, finish), + startDirection = lastDirection, + endDirection = j + ) + ) + }.map { it.toTrajectory() } + } + } + return trajectories +} 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 6186c18..4285a0b 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -18,18 +18,12 @@ class ObstacleTest { 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(Circle2D(vector(7.0, 1.0), 5.0)) - ) - - val outputTangents = findAllPaths( + val outputTangents = Obstacle.allPathsAvoiding( DubinsPose2D.of(startPoint, startDirection), - startRadius, DubinsPose2D.of(finalPoint, finalDirection), - finalRadius, - obstacles + startRadius, + Obstacle(Circle2D(vector(7.0, 1.0), 5.0)) ) val length = outputTangents.minOf { it.length } assertEquals(27.2113183, length, 1e-6) @@ -39,46 +33,62 @@ class ObstacleTest { fun secondPath() { val startPoint = vector(-5.0, -1.0) val startDirection = vector(1.0, 1.0) - val startRadius = 0.5 + val radius = 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( + val paths = Obstacle.allPathsAvoiding( DubinsPose2D.of(startPoint, startDirection), - startRadius, DubinsPose2D.of(finalPoint, finalDirection), - finalRadius, - obstacles + radius, + Obstacle( + 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( + 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 length = paths.minOf { it.length } assertEquals(28.9678224, length, 1e-6) } + @Test + fun nearPoints() { + val startPoint = vector(-1.0, 0.0) + val startDirection = vector(0.0, 1.0) + val startRadius = 1.0 + val finalPoint = vector(0, -1) + val finalDirection = vector(1.0, 0) + + val paths = Obstacle.allPathsAvoiding( + DubinsPose2D.of(startPoint, startDirection), + DubinsPose2D.of(finalPoint, finalDirection), + startRadius, + 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 length = paths.minOf { it.length } + println(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)) + val obstacle1 = ObstacleShell(listOf(circle1)) + val obstacle2 = ObstacleShell(listOf(circle2)) assertEquals(obstacle1, obstacle2) } + } \ No newline at end of file