[WIP] A lot of bugfixes

This commit is contained in:
Alexander Nozik 2023-05-01 16:56:42 +03:00
parent 614ca8d6f3
commit f0da3efd27
12 changed files with 472 additions and 648 deletions

View File

@ -0,0 +1,30 @@
plugins {
kotlin("multiplatform")
id("org.jetbrains.compose")
}
val ktorVersion: String by rootProject.extra
kotlin {
jvm()
jvmToolchain(11)
sourceSets {
val jvmMain by getting {
dependencies {
implementation(projects.mapsKtScheme)
implementation(projects.trajectoryKt)
implementation(compose.desktop.currentOs)
implementation(spclibs.logback.classic)
}
}
val jvmTest by getting
}
}
compose {
desktop {
application {
mainClass = "MainKt"
}
}
}

View File

@ -0,0 +1,166 @@
import androidx.compose.desktop.ui.tooling.preview.Preview
import androidx.compose.foundation.layout.Column
import androidx.compose.material.Button
import androidx.compose.material.MaterialTheme
import androidx.compose.material.Scaffold
import androidx.compose.material.Text
import androidx.compose.runtime.*
import androidx.compose.ui.graphics.Color
import androidx.compose.ui.window.Window
import androidx.compose.ui.window.application
import center.sciprog.maps.features.*
import center.sciprog.maps.scheme.SchemeView
import center.sciprog.maps.scheme.XY
import space.kscience.kmath.geometry.Angle
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.trajectory.*
import kotlin.random.Random
private fun DoubleVector2D.toXY() = XY(x.toFloat(), y.toFloat())
fun FeatureGroup<XY>.trajectory(
trajectory: Trajectory2D,
colorPicker: (Trajectory2D) -> Color = { Color.Blue },
): FeatureRef<XY, FeatureGroup<XY>> = group {
when (trajectory) {
is StraightTrajectory2D -> line(
aCoordinates = trajectory.begin.toXY(),
bCoordinates = trajectory.end.toXY(),
).color(colorPicker(trajectory))
is CircleTrajectory2D -> with(Euclidean2DSpace) {
val topLeft = trajectory.circle.center + vector(-trajectory.circle.radius, trajectory.circle.radius)
val bottomRight = trajectory.circle.center + vector(trajectory.circle.radius, -trajectory.circle.radius)
val rectangle = Rectangle(
topLeft.toXY(),
bottomRight.toXY()
)
arc(
oval = rectangle,
startAngle = trajectory.arcStart - Angle.piDiv2,
arcLength = trajectory.arcAngle,
).color(colorPicker(trajectory))
}
is CompositeTrajectory2D -> trajectory.segments.forEach {
trajectory(it, colorPicker)
}
}
}
fun FeatureGroup<XY>.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) -> Color = { Color.Red }) {
trajectory(obstacle.circumvention, colorPicker)
polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray)
}
@Composable
@Preview
fun closePoints() {
SchemeView {
val obstacle = Obstacle(
Circle2D(Euclidean2DSpace.vector(0.0, 0.0), 1.0),
Circle2D(Euclidean2DSpace.vector(0.0, 1.0), 1.0),
Circle2D(Euclidean2DSpace.vector(1.0, 1.0), 1.0),
Circle2D(Euclidean2DSpace.vector(1.0, 0.0), 1.0)
)
val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-1, -1, Angle.pi),
Pose2D(-1, -1, Angle.piDiv2),
1.0,
obstacle
)
obstacle(obstacle)
trajectory(paths.first()) { Color.Green }
trajectory(paths.last()) { Color.Magenta }
}
}
@Composable
@Preview
fun singleObstacle() {
SchemeView {
val obstacle = Obstacle(Circle2D(Euclidean2DSpace.vector(7.0, 1.0), 5.0))
obstacle(obstacle)
Obstacles.avoidObstacles(
Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20, 4, Angle.pi * 3 / 4),
0.5,
obstacle
).forEach {
trajectory(it).color(Color(Random.nextInt()))
}
}
}
@Composable
@Preview
fun doubleObstacle() {
SchemeView {
val obstacles = arrayOf(
Obstacle(
Circle2D(Euclidean2DSpace.vector(1.0, 6.5), 0.5),
Circle2D(Euclidean2DSpace.vector(2.0, 1.0), 0.5),
Circle2D(Euclidean2DSpace.vector(6.0, 0.0), 0.5),
Circle2D(Euclidean2DSpace.vector(5.0, 5.0), 0.5)
), Obstacle(
Circle2D(Euclidean2DSpace.vector(10.0, 1.0), 0.5),
Circle2D(Euclidean2DSpace.vector(16.0, 0.0), 0.5),
Circle2D(Euclidean2DSpace.vector(14.0, 6.0), 0.5),
Circle2D(Euclidean2DSpace.vector(9.0, 4.0), 0.5)
)
)
obstacles.forEach { obstacle(it) }
Obstacles.avoidObstacles(
Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20, 4, Angle.pi * 3 / 4),
0.5,
*obstacles
).forEach {
trajectory(it).color(Color(Random.nextInt()))
}
}
}
@Composable
@Preview
fun playground() {
val examples = listOf(
"Close starting points"
)
var currentExample by remember { mutableStateOf(examples.first()) }
Scaffold(floatingActionButton = {
Column {
examples.forEach {
Button(onClick = { currentExample = it }) {
Text(it)
}
}
}
}) {
when (currentExample) {
examples[0] -> closePoints()
}
}
}
fun main() = application {
Window(title = "Trajectory-playground", onCloseRequest = ::exitApplication) {
MaterialTheme {
playground()
}
}
}

View File

@ -63,7 +63,7 @@ public fun FeatureGroup<XY>.arc(
arcLength: Angle,
id: String? = null,
): FeatureRef<XY, ArcFeature<XY>> = arc(
oval = XYCoordinateSpace.Rectangle(center.toCoordinates(), radius, radius),
oval = XYCoordinateSpace.Rectangle(center.toCoordinates(), 2*radius, 2*radius),
startAngle = startAngle,
arcLength = arcLength,
id = id

View File

@ -54,6 +54,7 @@ include(
":maps-kt-scheme",
":demo:maps",
":demo:scheme",
":demo:polygon-editor"
":demo:polygon-editor",
":demo:trajectory-playground"
)

View File

@ -1,8 +1,7 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.operations.DoubleField.pow
import space.kscience.trajectory.Pose2D
import space.kscience.trajectory.Trajectory2D
import space.kscience.trajectory.*
import kotlin.math.sign
public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
@ -49,6 +48,15 @@ public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSe
}
}
public fun Euclidean2DSpace.intersectsTrajectory(segment: LineSegment2D, trajectory: Trajectory2D): Boolean =
when (trajectory) {
is CircleTrajectory2D -> intersects(segment, trajectory.circle)
is StraightTrajectory2D -> intersects(segment, trajectory)
is CompositeTrajectory2D -> trajectory.segments.any { trajectorySegment ->
intersectsTrajectory(segment, trajectorySegment)
}
}
/**
* Compute tangent pose to a circle
*

View File

@ -1,6 +1,7 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.misc.zipWithNextCircular
import space.kscience.trajectory.Trajectory2D
public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Double> = object : Polygon<Double> {
override val points: List<Vector2D<Double>> get() = points
@ -8,3 +9,9 @@ public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Doubl
public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, segment: LineSegment2D): Boolean =
polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, segment) }
public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, circle: Circle2D): Boolean =
polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, circle) }
public fun Euclidean2DSpace.intersectsTrajectory(polygon: Polygon<Double>, trajectory: Trajectory2D): Boolean =
polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { edge -> intersectsTrajectory(edge, trajectory) }

View File

@ -5,24 +5,27 @@
package space.kscience.trajectory
import space.kscience.kmath.geometry.Angle
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.*
import space.kscience.kmath.misc.zipWithNextCircular
public interface Obstacle {
public val circles: List<Circle2D>
public val arcs: List<CircleTrajectory2D>
public val center: Vector2D<Double>
/**
* A closed right-handed circuit minimal path circumvention of an obstacle.
*/
public val circumvention: CompositeTrajectory2D
public val polygon: Polygon<Double>
/**
* Check if obstacle has intersection with given [Trajectory2D]
*/
public fun intersects(trajectory: Trajectory2D): Boolean =
Euclidean2DSpace.trajectoryIntersects(circumvention, trajectory)
Euclidean2DSpace.intersectsTrajectory(polygon, trajectory)
public companion object {
@ -30,58 +33,20 @@ public interface Obstacle {
}
}
private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) : Obstacle {
override val arcs: List<CircleTrajectory2D> by lazy {
circumvention.segments.filterIsInstance<CircleTrajectory2D>()
}
override val center: Vector2D<Double> by lazy {
Euclidean2DSpace.vector(
circles.sumOf { it.center.x } / circles.size,
circles.sumOf { it.center.y } / circles.size
arcs.sumOf { it.center.x } / arcs.size,
arcs.sumOf { it.center.y } / arcs.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 start = DubinsPose2D(
// top,
// Angle.piDiv2
// )
return@lazy CompositeTrajectory2D(
CircleTrajectory2D(circles.first(), Angle.zero, Angle.zero)
)
}
//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<Trajectory2D> = 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 val polygon: Polygon<Double> by lazy {
Euclidean2DSpace.polygon(arcs.map { it.circle.center })
}
override fun equals(other: Any?): Boolean {
@ -90,24 +55,66 @@ private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
other as ObstacleImpl
return circles == other.circles
return arcs == other.arcs
}
override fun hashCode(): Int {
return circles.hashCode()
return arcs.hashCode()
}
override fun toString(): String {
return "Obstacle(circles=$circles)"
return "Obstacle(circles=$arcs)"
}
}
public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace) {
val center = vector(
circles.sumOf { it.center.x },
circles.sumOf { it.center.y }
)/ circles.size
require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
if (circles.size == 1) {
return ObstacleImpl(
CompositeTrajectory2D(
CircleTrajectory2D(circles.first(), Angle.zero, Angle.piTimes2)
)
)
}
//TODO use convex hull
//distinct and sorted in right-handed direction
val convex = circles.distinct().sortedBy {
(it.center - center).bearing
}
val tangents = convex.zipWithNextCircular { a: Circle2D, b: Circle2D ->
tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR]
?: error("Can't find right handed circumvention")
}
val trajectory: List<Trajectory2D> = buildList {
for (i in 0 until tangents.lastIndex) {
add(tangents[i])
add(CircleTrajectory2D(convex[i + 1], tangents[i].endPose, tangents[i + 1].beginPose, Trajectory2D.R))
}
add(tangents.last())
add(CircleTrajectory2D(convex[0], tangents.last().endPose, tangents.first().beginPose, Trajectory2D.R))
}
val circumvention = CompositeTrajectory2D(trajectory)
return ObstacleImpl(circumvention)
}
}
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles))
public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
ObstacleImpl(points.map { Circle2D(it, radius) })
Obstacle(points.map { Circle2D(it, radius) })

View File

@ -1,389 +0,0 @@
package space.kscience.trajectory
//
//
//private class LR<T>(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<ObstacleTangent>) {
// 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<Circle2D>,
//) : Obstacle {
// override val circles: List<Circle2D>
// override val center: Vector2D<Double>
// private val shell: List<LineSegment2D>
// 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<ObstacleTangent> {
// 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<DoubleVector2D, DoubleVector2D> {
// 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<Circle2D> {
// 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<Obstacle>,
//): List<Obstacle> {
// return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) }
//}
//
///**
// * Check if all proposed paths have ended at [finalObstacle]
// */
//private fun allFinished(
// paths: List<TangentPath>,
// 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<ObstacleShell>,
//): List<CompositeTrajectory2D> {
// 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<CompositeTrajectory2D>()
//
// 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<TangentPath> = 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<TangentPath>()
// // 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<ObstacleTangent> = 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
//}

View File

@ -1,11 +1,8 @@
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<Obstacle>) {
@ -16,7 +13,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
val direction: Trajectory2D.Direction,
) {
val obstacle: Obstacle get() = obstacles[obstacleIndex]
val circle: Circle2D get() = obstacle.circles[nodeIndex]
val circle: Circle2D get() = obstacle.arcs[nodeIndex].circle
}
private inner class ObstacleTangent(
@ -47,18 +44,16 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
): Map<DubinsPath.Type, ObstacleTangent> = 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(
for (firstCircleIndex in first.arcs.indices) {
val firstCircle = first.arcs[firstCircleIndex]
for (secondCircleIndex in second.arcs.indices) {
val secondCircle = second.arcs[secondCircleIndex]
for ((pathType, segment) in tangentsBetweenArcs(
firstCircle,
secondCircle
)) {
if (!intersects(firstPolygon, segment) && !intersects(secondPolygon, segment)) {
if (!first.intersects(segment) && !second.intersects(segment)) {
put(
pathType,
ObstacleTangent(
@ -75,21 +70,19 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
private fun tangentsFromCircle(
circle: Circle2D,
direction: Trajectory2D.Direction,
private fun tangentsFromArc(
arc: CircleTrajectory2D,
obstacleIndex: Int,
): Map<DubinsPath.Type, ObstacleTangent> = 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
for (circleIndex in obstacle.arcs.indices) {
val obstacleArc = obstacle.arcs[circleIndex]
for ((pathType, segment) in tangentsBetweenArcs(
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
obstacleArc
)) {
if (pathType.first == direction && !intersects(polygon, segment)) {
if (pathType.first == arc.direction && !intersects(obstacle.polygon, segment)) {
put(
pathType,
ObstacleTangent(
@ -104,21 +97,19 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
}
private fun tangentToCircle(
private fun tangentToArc(
obstacleIndex: Int,
obstacleDirection: Trajectory2D.Direction,
circle: Circle2D,
direction: Trajectory2D.Direction,
arc: CircleTrajectory2D
): 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)
for (circleIndex in obstacle.arcs.indices) {
val obstacleArc = obstacle.arcs[circleIndex]
tangentsBetweenArcs(
obstacleArc,
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
)[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
!obstacle.intersects(it)
}?.let {
return ObstacleTangent(
it,
@ -163,11 +154,13 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
first.circle,
tangent1.tangentTrajectory.endPose,
first.endPose,
tangent1.to.direction
)
circumvention[circumvention.lastIndex] = CircleTrajectory2D(
last.circle,
last.beginPose,
tangent2.tangentTrajectory.beginPose
tangent2.tangentTrajectory.beginPose,
tangent2.from.direction
)
return CompositeTrajectory2D(circumvention)
}
@ -186,19 +179,18 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
)
}
public fun allTrajectoriesAvoiding(
startCircle: Circle2D,
startDirection: Trajectory2D.Direction,
endCircle: Circle2D,
endDirection: Trajectory2D.Direction,
): Collection<Trajectory2D> {
val directTangent: StraightTrajectory2D = tangentsBetweenCircles(startCircle, endCircle).get(
DubinsPath.Type(startDirection, Trajectory2D.S, endDirection)
) ?: return emptySet()
private fun avoiding(
dubinsPath: CompositeTrajectory2D,
): Collection<Trajectory2D> = with(Euclidean2DSpace) {
//fast return if no obstacles intersect direct path
if (obstacles.none { it.intersects(directTangent) }) return listOf(directTangent)
if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
val endArc = dubinsPath.segments.last() as CircleTrajectory2D
/**
* Continue current tangent to final point or to the next obstacle
*/
/**
* Continue current tangent to final point or to the next obstacle
*/
@ -207,14 +199,13 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
require(connection != null)
//indices of obstacles that are not on previous path
val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }
val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }.toSet()
//a tangent to end point, null if tangent could not be constructed
val tangentToEnd: ObstacleTangent = tangentToCircle(
val tangentToEnd: ObstacleTangent = tangentToArc(
connection.obstacleIndex,
connection.direction,
endCircle,
endDirection
endArc
) ?: return emptySet()
if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
@ -239,125 +230,113 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
//find nearest obstacle that has valid tangents to
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
compareByDescending<Int> { obstacles[it].intersects(directTangent) } //take intersecting obstacles
.thenBy { startCircle.center.distanceTo(obstacles[it].center) } //then nearest
).firstNotNullOf { obstacleIndex ->
tangentsFromCircle(startCircle, startDirection, obstacleIndex).values
compareByDescending<Int> { obstacles[it].intersects(dubinsPath) } //take intersecting obstacles
.thenBy { beginArc.circle.center.distanceTo(obstacles[it].center) } //then nearest
).firstNotNullOfOrNull { obstacleIndex ->
tangentsFromArc(beginArc, obstacleIndex).values
.filter { it.isValid }.takeIf { it.isNotEmpty() }
}
}?: return emptySet()
var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
while (!paths.all { it.isFinished }) {
paths = paths.flatMap { it.nextSteps() }
}
return paths.map { it.toTrajectory() }
return paths.map {
CompositeTrajectory2D(
//arc from starting point
CircleTrajectory2D(
beginArc.circle,
beginArc.beginPose,
it.tangents.first().tangentTrajectory.beginPose,
beginArc.direction
),
it.toTrajectory(),
//arc to the end point
CircleTrajectory2D(
endArc.circle,
it.tangents.last().tangentTrajectory.endPose,
endArc.endPose,
endArc.direction
),
)
}
}
public fun allTrajectories(
start: Pose2D,
finish: Pose2D,
radius: Double,
): List<Trajectory2D> {
val dubinsPaths: List<CompositeTrajectory2D> = DubinsPath.all(start, finish, radius)
return dubinsPaths.flatMap {
avoiding(it)
}
}
public companion object {
private data class LR<T>(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<DoubleVector2D, DoubleVector2D> =
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: Pose2D,
r: Double,
): LR<Circle2D> = with(Euclidean2DSpace) {
val direction = Pose2D.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)
)
}
}
// private data class LR<T>(val l: T, val r: T) {
// operator fun get(direction: Trajectory2D.Direction) = when (direction) {
// Trajectory2D.L -> l
// Trajectory2D.R -> r
// }
// }
//
// private fun constructTangentCircles(
// pose: Pose2D,
// r: Double,
// ): LR<Circle2D> = with(Euclidean2DSpace) {
// val center1 = pose + vector(r*sin(pose.bearing + Angle.piDiv2), r*cos(pose.bearing + Angle.piDiv2))
// val center2 = pose + vector(r*sin(pose.bearing - Angle.piDiv2), r*cos(pose.bearing - Angle.piDiv2))
// LR(
// Circle2D(center2, r),
// Circle2D(center1, r)
// )
// }
public fun avoidObstacles(
start: Pose2D,
finish: Pose2D,
startingRadius: Double,
radius: Double,
obstacleList: List<Obstacle>,
finalRadius: Double = startingRadius,
): List<Trajectory2D> {
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
)
)
}
}
}
return obstacles.allTrajectories(start, finish, radius)
}
public fun avoidObstacles(
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
radius: Double,
vararg obstacles: Obstacle,
): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
): List<Trajectory2D> = avoidObstacles(start, finish, radius, obstacles.toList())
public fun avoidPolygons(
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
radius: Double,
vararg polygons: Polygon<Double>,
): List<Trajectory2D> {
val obstacles: List<Obstacle> = polygons.map { polygon ->
Obstacle(polygon.points, trajectoryRadius)
Obstacle(polygon.points, radius)
}
return avoidObstacles(start, finish, trajectoryRadius, obstacles)
return avoidObstacles(start, finish, radius, obstacles)
}
public fun avoidPolygons(
start: Pose2D,
finish: Pose2D,
trajectoryRadius: Double,
radius: Double,
polygons: Collection<Polygon<Double>>,
): List<Trajectory2D> {
val obstacles: List<Obstacle> = polygons.map { polygon ->
Obstacle(polygon.points, trajectoryRadius)
Obstacle(polygon.points, radius)
}
return avoidObstacles(start, finish, trajectoryRadius, obstacles)
return avoidObstacles(start, finish, radius, obstacles)
}
}

View File

@ -89,6 +89,8 @@ public data class CircleTrajectory2D(
circle.radius * kotlin.math.abs(arcAngle.radians)
}
val center: Vector2D<Double> get() = circle.center
override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, arcEnd, -arcAngle)
@ -101,25 +103,6 @@ public fun CircleTrajectory2D(
end: DoubleVector2D,
direction: Trajectory2D.Direction,
): CircleTrajectory2D = with(Euclidean2DSpace) {
// fun calculatePose(
// vector: DoubleVector2D,
// theta: Angle,
// direction: Trajectory2D.Direction,
// ): DubinsPose2D = DubinsPose2D(
// vector,
// when (direction) {
// Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
// Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
// }
// )
//
// val s1 = StraightTrajectory2D(center, start)
// val s2 = StraightTrajectory2D(center, end)
// val pose1 = calculatePose(start, s1.bearing, direction)
// val pose2 = calculatePose(end, s2.bearing, direction)
// val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
// if (trajectory.direction != direction) error("Trajectory direction mismatch")
// return trajectory
val startVector = start - center
val endVector = end - center
val startRadius = norm(startVector)
@ -147,6 +130,36 @@ public fun CircleTrajectory2D(
)
}
public fun CircleTrajectory2D(
circle: Circle2D,
start: DoubleVector2D,
end: DoubleVector2D,
direction: Trajectory2D.Direction,
): CircleTrajectory2D = with(Euclidean2DSpace) {
val startVector = start - circle.center
val endVector = end - circle.center
val startBearing = startVector.bearing
val endBearing = endVector.bearing
CircleTrajectory2D(
circle,
startBearing,
when (direction) {
Trajectory2D.L -> if (endBearing >= startBearing) {
endBearing - startBearing - Angle.piTimes2
} else {
endBearing - startBearing
}
Trajectory2D.R -> if (endBearing >= startBearing) {
endBearing - startBearing
} else {
endBearing + Angle.piTimes2 - startBearing
}
}
)
}
@Deprecated("Use angle notation instead")
public fun CircleTrajectory2D(
circle: Circle2D,
beginPose: Pose2D,
@ -172,18 +185,18 @@ public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Tr
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
CompositeTrajectory2D(segments.toList())
public fun Euclidean2DSpace.trajectoryIntersects(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
public fun Euclidean2DSpace.intersectsTrajectory(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 CompositeTrajectory2D -> b.segments.any { intersectsTrajectory(it, a) }
}
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 -> b.segments.any { intersectsTrajectory(it, a) }
}
is CompositeTrajectory2D -> a.segments.any { trajectoryIntersects(it, b) }
is CompositeTrajectory2D -> a.segments.any { intersectsTrajectory(it, b) }
}

View File

@ -1,6 +1,7 @@
package space.kscience.trajectory
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.trajectory.DubinsPath.Type
import kotlin.math.*
@ -62,12 +63,30 @@ internal fun tangentsBetweenCircles(
}
}
internal fun tangentsBetweenArcs(
first: CircleTrajectory2D,
second: CircleTrajectory2D,
): Map<Type, StraightTrajectory2D> {
fun CircleTrajectory2D.containsPoint(point: DoubleVector2D): Boolean = with(Euclidean2DSpace){
val radiusVectorBearing = (point - center).bearing
return when(direction){
Trajectory2D.L -> radiusVectorBearing in arcEnd..arcStart
Trajectory2D.R -> radiusVectorBearing in arcStart..arcEnd
}
}
return tangentsBetweenCircles(first.circle, second.circle).filterValues {
first.containsPoint(it.begin) && second.containsPoint(it.end)
}
}
/**
* 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]
require(fromIndex in arcs.indices) { "$fromIndex is not in ${arcs.indices}" }
val startCircle = arcs[fromIndex]
val segments = buildList {
val reserve = mutableListOf<Trajectory2D>()
@ -77,7 +96,7 @@ public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex:
}
var i = 0
while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) {
while (sourceSegments[i] !== startCircle) {
//put all segments before target circle on the reserve
reserve.add(sourceSegments[i])
i++
@ -104,8 +123,8 @@ public fun Obstacle.circumvention(
fromIndex: Int,
toIndex: Int,
): CompositeTrajectory2D {
require(toIndex in circles.indices) { "$toIndex is not in ${circles.indices}" }
val toCircle = circles[toIndex]
require(toIndex in arcs.indices) { "$toIndex is not in ${arcs.indices}" }
val toCircle = arcs[toIndex]
val fullCircumvention = circumvention(direction, fromIndex).segments
return CompositeTrajectory2D(
buildList {
@ -114,7 +133,7 @@ public fun Obstacle.circumvention(
val segment = fullCircumvention[i]
add(segment)
i++
} while ((segment as? CircleTrajectory2D)?.circle != toCircle)
} while (segment !== toCircle)
}
)
}

View File

@ -5,6 +5,7 @@
package space.kscience.trajectory
import space.kscience.kmath.geometry.Angle
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.degrees
@ -27,16 +28,10 @@ class ObstacleTest {
@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 = Obstacles.avoidObstacles(
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
startRadius,
val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-5,-1, Angle.pi/4),
Pose2D(20,4, Angle.pi*3/4),
0.5,
Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
)
assertTrue { outputTangents.isNotEmpty() }
@ -46,16 +41,10 @@ class ObstacleTest {
@Test
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 = Obstacles.avoidObstacles(
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
radius,
Pose2D(-5,-1, Angle.pi/4),
Pose2D(20,4, Angle.pi*3/4),
0.5,
Obstacle(
Circle2D(vector(1.0, 6.5), 0.5),
Circle2D(vector(2.0, 1.0), 0.5),
@ -91,27 +80,21 @@ class ObstacleTest {
@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 = Obstacles.avoidObstacles(
Pose2D(startPoint, startDirection),
Pose2D(finalPoint, finalDirection),
startRadius,
Obstacle(
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 paths: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-0.9, -0.9, Angle.pi),
Pose2D(-0.9, -0.9, Angle.piDiv2),
1.0,
obstacle
)
assertTrue { paths.isNotEmpty() }
val length = paths.minOf { it.length }
println(length)
//assertEquals(28.9678224, length, 1e-6)
assertEquals(18.37, paths.minOf { it.length }, 1e-2)
}
@Test