[WIP] A lot of bugfixes
This commit is contained in:
parent
614ca8d6f3
commit
f0da3efd27
30
demo/trajectory-playground/build.gradle.kts
Normal file
30
demo/trajectory-playground/build.gradle.kts
Normal 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"
|
||||
}
|
||||
}
|
||||
}
|
||||
166
demo/trajectory-playground/src/jvmMain/kotlin/Main.kt
Normal file
166
demo/trajectory-playground/src/jvmMain/kotlin/Main.kt
Normal 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()
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -54,6 +54,7 @@ include(
|
||||
":maps-kt-scheme",
|
||||
":demo:maps",
|
||||
":demo:scheme",
|
||||
":demo:polygon-editor"
|
||||
":demo:polygon-editor",
|
||||
":demo:trajectory-playground"
|
||||
)
|
||||
|
||||
|
||||
@ -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
|
||||
*
|
||||
|
||||
@ -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) }
|
||||
@ -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(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles))
|
||||
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 = 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) })
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
//}
|
||||
@ -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)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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) }
|
||||
}
|
||||
|
||||
@ -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)
|
||||
}
|
||||
)
|
||||
}
|
||||
@ -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),
|
||||
@ -74,7 +63,7 @@ class ObstacleTest {
|
||||
}
|
||||
|
||||
@Test
|
||||
fun circumvention(){
|
||||
fun circumvention() {
|
||||
val obstacle = Obstacle(
|
||||
Circle2D(vector(0.0, 0.0), 1.0),
|
||||
Circle2D(vector(0.0, 1.0), 1.0),
|
||||
@ -86,32 +75,26 @@ class ObstacleTest {
|
||||
|
||||
assertEquals(4, circumvention.segments.count { it is CircleTrajectory2D })
|
||||
|
||||
assertEquals(4 + 2* PI, circumvention.length, 1e-4)
|
||||
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 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 = Obstacles.avoidObstacles(
|
||||
Pose2D(startPoint, startDirection),
|
||||
Pose2D(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 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user