Fix circle line intersection and add a special case for a single-point obstacle
This commit is contained in:
parent
05ef3aa4dd
commit
11b11118fa
@ -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 }
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -123,7 +139,7 @@ fun doubleObstacle() {
|
||||
)
|
||||
)
|
||||
|
||||
obstacles.forEach { obstacle(it) }
|
||||
obstacles.forEach { obstacle(it) }
|
||||
val enter = Pose2D(-5, -1, Angle.pi / 4)
|
||||
val exit = Pose2D(20, 4, Angle.pi * 3 / 4)
|
||||
pose(enter)
|
||||
@ -135,7 +151,8 @@ fun doubleObstacle() {
|
||||
0.5,
|
||||
*obstacles
|
||||
).forEach {
|
||||
trajectory(it).color(Color(Random.nextInt()))
|
||||
val color = Color(random.nextInt())
|
||||
trajectory(it) { color }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -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
|
||||
}
|
||||
|
||||
|
||||
|
@ -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)
|
||||
}
|
@ -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 })
|
||||
}
|
||||
|
||||
|
@ -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 {
|
||||
obstacles.indices.none {
|
||||
it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory)
|
||||
with(Euclidean2DSpace) {
|
||||
obstacles.indices.none {
|
||||
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,
|
||||
|
@ -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
|
||||
|
@ -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 {
|
||||
|
@ -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),
|
||||
@ -110,7 +110,7 @@ class ObstacleTest {
|
||||
Pose2D(x = 473093.1426061879, y = 2898525.45250675, bearing = Degrees(100.36609537114623))
|
||||
)
|
||||
|
||||
val obstacle = Obstacle(
|
||||
val obstacle = Obstacle(
|
||||
Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0),
|
||||
Circle2D(vector(x = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0),
|
||||
Circle2D(vector(x = 465903.08440141426, y = 2893897.500160981), radius = 5000.0),
|
||||
@ -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,
|
||||
|
Loading…
Reference in New Issue
Block a user