kscience/maps-kt
8
0
Fork 0
Code Issues Pull Requests 3 Packages Projects Releases 1 Wiki Activity

Fix circle line intersection and add a special case for a single-point obstacle

Browse Source
This commit is contained in:
Alexander Nozik 2023-05-06 12:50:44 +03:00
parent 05ef3aa4dd
commit 11b11118fa
8 changed files with 128 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
demo/trajectory-playground/src/jvmMain/kotlin/Main.kt
View File

@ -20,6 +20,8 @@ import kotlin.random.Random
private fun DoubleVector2D.toXY() = XY(x.toFloat(), y.toFloat())
private val random = Random(123)
fun FeatureGroup<XY>.trajectory(
trajectory: Trajectory2D,
colorPicker: (Trajectory2D) -> Color = { Color.Blue },
@ -57,8 +59,8 @@ fun FeatureGroup<XY>.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) ->
polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray)
}
fun FeatureGroup<XY>.pose(pose2D: Pose2D) = with(Euclidean2DSpace){
line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY() )
fun FeatureGroup<XY>.pose(pose2D: Pose2D) = with(Euclidean2DSpace) {
line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY())
}
@Composable
@ -72,18 +74,25 @@ fun closePoints() {
Circle2D(Euclidean2DSpace.vector(1.0, 1.0), 1.0),
Circle2D(Euclidean2DSpace.vector(1.0, 0.0), 1.0)
)
val enter = Pose2D(-0.8, -0.8, Angle.pi)
val exit = Pose2D(-0.8, -0.8, Angle.piDiv2)
pose(enter)
pose(exit)
val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-1, -1, Angle.pi),
Pose2D(-1, -1, Angle.piDiv2),
enter,
exit,
1.0,
obstacle
)
obstacle(obstacle)
trajectory(paths.first()) { Color.Green }
trajectory(paths.last()) { Color.Magenta }
paths.forEach {
val color = Color(random.nextInt())
trajectory(it) { color }
}
}
}
@ -93,14 +102,21 @@ fun closePoints() {
fun singleObstacle() {
SchemeView {
val obstacle = Obstacle(Circle2D(Euclidean2DSpace.vector(7.0, 1.0), 5.0))
val enter = Pose2D(-5, -1, Angle.pi / 4)
val exit = Pose2D(20, 4, Angle.pi * 3 / 4)
pose(enter)
pose(exit)
obstacle(obstacle)
Obstacles.avoidObstacles(
Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20, 4, Angle.pi * 3 / 4),
enter,
exit,
0.5,
obstacle
).forEach {
trajectory(it).color(Color(Random.nextInt()))
val color = Color(random.nextInt())
trajectory(it) { color }
}
}
}
@ -135,7 +151,8 @@ fun doubleObstacle() {
0.5,
*obstacles
).forEach {
trajectory(it).color(Color(Random.nextInt()))
val color = Color(random.nextInt())
trajectory(it) { color }
}
}
}

2
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt
View File

@ -39,7 +39,7 @@ public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D)
val t1 = (-b - discriminant) / (2 * a) // first intersection point in relative coordinates
val t2 = (-b + discriminant) / (2 * a) //second intersection point in relative coordinates
return t1.sign != t2.sign || (t1-1.0).sign != (t2-1).sign
return t1 in 0.0..1.0 || t2 in 0.0..1.0
}

6
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/polygonExtensions.kt
View File

@ -14,4 +14,8 @@ public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, circle: Circle2
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) }
polygon.points.zipWithNextCircular { l, r ->
segment(l, r)
}.any { edge ->
intersectsTrajectory(edge, trajectory)
}

13
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacle.kt
View File

@ -14,19 +14,14 @@ public interface Obstacle {
public val center: Vector2D<Double>
/**
* A closed right-handed circuit minimal path circumvention of an obstacle.
* A closed right-handed circuit minimal path circumvention of the obstacle.
*/
public val circumvention: CompositeTrajectory2D
public val polygon: Polygon<Double>
/**
* Check if obstacle has intersection with given [Trajectory2D]
* A polygon created from the arc centers of the obstacle
*/
public fun intersects(trajectory: Trajectory2D): Boolean =
Euclidean2DSpace.intersectsTrajectory(polygon, trajectory)
public val core: Polygon<Double>
public companion object {
@ -45,7 +40,7 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) :
)
}
override val polygon: Polygon<Double> by lazy {
override val core: Polygon<Double> by lazy {
Euclidean2DSpace.polygon(arcs.map { it.circle.center })
}

110
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt
View File

@ -4,6 +4,25 @@ import space.kscience.kmath.geometry.*
import kotlin.collections.component1
import kotlin.collections.component2
/**
* The same as [intersectsTrajectory], but bypasses same circles or same straights
*/
private fun Euclidean2DSpace.intersectsOtherTrajectory(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
is CircleTrajectory2D -> when (b) {
is CircleTrajectory2D -> a != b && intersectsOrInside(a.circle, b.circle)
is StraightTrajectory2D -> intersects(a.circle, b)
is CompositeTrajectory2D -> b.segments.any { intersectsOtherTrajectory(it, a) }
}
is StraightTrajectory2D -> when (b) {
is CircleTrajectory2D -> intersects(a, b.circle)
is StraightTrajectory2D -> a != b && intersects(a, b)
is CompositeTrajectory2D -> b.segments.any { intersectsOtherTrajectory(it, a) }
}
is CompositeTrajectory2D -> a.segments.any { intersectsOtherTrajectory(it, b) }
}
public class Obstacles(public val obstacles: List<Obstacle>) {
@ -22,11 +41,16 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
val to: ObstacleConnection?,
) {
/**
* If false this tangent intersects another obstacle
* If false, this tangent intersects another obstacle
*/
val isValid by lazy {
with(Euclidean2DSpace) {
obstacles.indices.none {
it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory)
it != from?.obstacleIndex && it != to?.obstacleIndex && intersectsTrajectory(
obstacles[it].core,
tangentTrajectory
)
}
}
}
}
@ -53,7 +77,10 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
firstCircle,
secondCircle
)) {
if (!first.intersects(segment) && !second.intersects(segment)) {
if (
!intersectsTrajectory(first.core, segment)
&& !intersectsTrajectory(second.core, segment)
) {
put(
pathType,
ObstacleTangent(
@ -82,7 +109,11 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
obstacleArc
)) {
if (pathType.first == arc.direction && !intersects(obstacle.polygon, segment)) {
if (pathType.first == arc.direction && !intersectsTrajectory(
obstacle.core,
segment
)
) {
put(
pathType,
ObstacleTangent(
@ -100,7 +131,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
private fun tangentToArc(
obstacleIndex: Int,
obstacleDirection: Trajectory2D.Direction,
arc: CircleTrajectory2D
arc: CircleTrajectory2D,
): ObstacleTangent? = with(Euclidean2DSpace) {
val obstacle = obstacles[obstacleIndex]
for (circleIndex in obstacle.arcs.indices) {
@ -109,7 +140,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
obstacleArc.circle,
arc.circle
)[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
obstacleArc.containsPoint(it.begin) && !obstacle.intersects(it)
obstacleArc.containsPoint(it.begin) && !intersectsTrajectory(obstacle.core, it)
}?.let {
return ObstacleTangent(
it,
@ -131,7 +162,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
/**
* Circumvention trajectory alongside obstacle. Replacing first and last arcs with appropriate cuts
* Circumvention trajectory alongside the obstacle. Replacing first and last arcs with appropriate cuts
*/
private fun trajectoryBetween(tangent1: ObstacleTangent, tangent2: ObstacleTangent): CompositeTrajectory2D {
require(tangent1.to != null)
@ -149,7 +180,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
//cutting first and last arcs to accommodate connection points
val first = circumvention.first() as CircleTrajectory2D
val last = circumvention.last() as CircleTrajectory2D
//arc between end of the tangent and end of previous arc (begin of the next one)
//arc between the end of the tangent and end of the previous arc (begin of the next one)
circumvention[0] = CircleTrajectory2D(
first.circle,
tangent1.tangentTrajectory.endPose,
@ -169,7 +200,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
val isFinished get() = tangents.last().to == null
fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
buildList<Trajectory2D> {
buildList {
add(tangents.first().tangentTrajectory)
tangents.zipWithNext().forEach { (l, r) ->
addAll(trajectoryBetween(l, r).segments)
@ -183,7 +214,12 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
dubinsPath: CompositeTrajectory2D,
): Collection<Trajectory2D> = with(Euclidean2DSpace) {
//fast return if no obstacles intersect the direct path
if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
if (
obstacles.none {
(it.arcs.size == 1 && intersectsTrajectory(it.circumvention, dubinsPath)) // special case for one-point obstacles
|| intersectsTrajectory(it.core, dubinsPath)
}
) return listOf(dubinsPath)
val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
val endArc = dubinsPath.segments.last() as CircleTrajectory2D
@ -198,7 +234,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
val connection = tangents.last().to
require(connection != null)
//indices of obstacles that are not on previous path
//indices of obstacles that are not on the previous path
val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }.toSet()
//a tangent to end point, null if tangent could not be constructed
@ -209,14 +245,27 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
) ?: return emptySet()
// if no intersections, finish
if (obstacles.indices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
if (obstacles.indices.none {
intersectsTrajectory(
obstacles[it].core,
tangentToEnd.tangentTrajectory
)
}) return setOf(
TangentPath(tangents + tangentToEnd)
)
// tangents to other obstacles
return remainingObstacleIndices.sortedWith(
compareByDescending<Int> { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles
.thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest
compareByDescending<Int> {
intersectsTrajectory(
obstacles[it].core,
tangentToEnd.tangentTrajectory
)
//take intersecting obstacles
}.thenBy {
connection.circle.center.distanceTo(obstacles[it].center)
//then nearest
}
).firstNotNullOf { nextObstacleIndex ->
//all tangents from cache
getAllTangents(connection.obstacleIndex, nextObstacleIndex).filter {
@ -229,19 +278,24 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
}
//find nearest obstacle that has valid tangents to
//find the nearest obstacle that has valid tangents to
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
compareByDescending<Int> { obstacles[it].intersects(dubinsPath) } //take intersecting obstacles
.thenBy { beginArc.circle.center.distanceTo(obstacles[it].center) } //then nearest
compareByDescending<Int> {
intersectsTrajectory(obstacles[it].core, 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()
} ?: return emptySet()
var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
while (!paths.all { it.isFinished }) {
paths = paths.flatMap { if(it.isFinished) listOf(it) else it.nextSteps() }
paths = paths.flatMap { if (it.isFinished) listOf(it) else it.nextSteps() }
}
return paths.map {
CompositeTrajectory2D(
@ -279,24 +333,6 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
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 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,

7
trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt
View File

@ -6,7 +6,6 @@
package space.kscience.kmath.geometry
import space.kscience.trajectory.CircleTrajectory2D
import space.kscience.trajectory.Pose2D
import space.kscience.trajectory.Trajectory2D
import kotlin.math.PI
import kotlin.test.Test
@ -35,11 +34,11 @@ class ArcTests {
val circle = circle(1, 0, 1)
val arc = CircleTrajectory2D(
circle,
Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
Pose2D(x = 1.0, y = -1.0, bearing = (PI * 3 / 2).radians)
(PI/2).radians,
(PI/2).radians
)
assertEquals(Trajectory2D.R, arc.direction)
assertEquals(PI / 2, arc.length, 1e-4)
assertEquals(PI, arc.arcEnd.radians, 1e-4)
}
@Test

10
trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/CircleTests.kt
View File

@ -48,7 +48,15 @@ class CircleTests {
@Test
fun circleLineIntersection() = with(Euclidean2DSpace) {
assertTrue {
intersects(circle(0, 0, 1), segment(1, 1, -1, 1))
intersects(circle(0, 0, 1.0), segment(1, 1, -1, 1))
}
assertFalse {
intersects(circle(0, 0, 1.0), segment(1, 1, 0.5, 1))
}
assertFalse {
intersects(circle(0, 0, 1.0), segment(0, 0.5, 0, -0.5))
}
assertTrue {

6
trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
View File

@ -28,7 +28,7 @@ class ObstacleTest {
}
@Test
fun singeObstacle() {
fun singePoint() {
val outputTangents: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-5, -1, Angle.pi / 4),
Pose2D(20, 4, Angle.pi * 3 / 4),
@ -118,8 +118,8 @@ class ObstacleTest {
Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0)
)
startPoints.forEach { start->
endPoints.forEach { end->
startPoints.forEach { start ->
endPoints.forEach { end ->
val paths = Obstacles.avoidObstacles(
start,
end,