Differentiate obstacles
This commit is contained in:
parent
5d0e0f054e
commit
d8548ab162
@ -1,14 +1,14 @@
|
|||||||
import kotlin.io.path.readText
|
import kotlin.io.path.readText
|
||||||
|
|
||||||
job("Build") {
|
job("Build") {
|
||||||
gradlew("spc.registry.jetbrains.space/p/sci/containers/kotlin-ci:1.0.3", "build")
|
gradlew("spc.registry.jetbrains.space/p/sci/containers/kotlin-ci:lastest", "build")
|
||||||
}
|
}
|
||||||
|
|
||||||
job("Publish") {
|
job("Publish") {
|
||||||
startOn {
|
startOn {
|
||||||
gitPush { enabled = false }
|
gitPush { enabled = false }
|
||||||
}
|
}
|
||||||
container("openjdk:11") {
|
container("spc.registry.jetbrains.space/p/sci/containers/kotlin-ci:latest") {
|
||||||
env["SPACE_USER"] = Secrets("space_user")
|
env["SPACE_USER"] = Secrets("space_user")
|
||||||
env["SPACE_TOKEN"] = Secrets("space_token")
|
env["SPACE_TOKEN"] = Secrets("space_token")
|
||||||
kotlinScript { api ->
|
kotlinScript { api ->
|
||||||
|
@ -29,7 +29,7 @@ ksciencePublish{
|
|||||||
if (isInDevelopment) {
|
if (isInDevelopment) {
|
||||||
"https://maven.pkg.jetbrains.space/spc/p/sci/dev"
|
"https://maven.pkg.jetbrains.space/spc/p/sci/dev"
|
||||||
} else {
|
} else {
|
||||||
"https://maven.pkg.jetbrains.space/spc/p/sci/release"
|
"https://maven.pkg.jetbrains.space/spc/p/sci/maven"
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
sonatype()
|
sonatype()
|
||||||
|
@ -17,18 +17,51 @@ public interface Obstacle {
|
|||||||
* A closed right-handed circuit minimal path circumvention of the obstacle.
|
* A closed right-handed circuit minimal path circumvention of the obstacle.
|
||||||
*/
|
*/
|
||||||
public val circumvention: CompositeTrajectory2D
|
public val circumvention: CompositeTrajectory2D
|
||||||
|
//
|
||||||
|
// /**
|
||||||
|
// * A polygon created from the arc centers of the obstacle
|
||||||
|
// */
|
||||||
|
// public val core: Polygon<Double>
|
||||||
|
|
||||||
/**
|
public fun intersectsTrajectory(trajectory: Trajectory2D): Boolean
|
||||||
* A polygon created from the arc centers of the obstacle
|
|
||||||
*/
|
|
||||||
public val core: Polygon<Double>
|
|
||||||
|
|
||||||
public companion object {
|
public companion object {
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) : Obstacle {
|
private class CircleObstacle(val circle: Circle2D) : Obstacle {
|
||||||
|
override val center: Vector2D<Double> get() = circle.center
|
||||||
|
|
||||||
|
override val arcs: List<CircleTrajectory2D>
|
||||||
|
get() = listOf(CircleTrajectory2D(circle, Angle.zero, Angle.piTimes2))
|
||||||
|
|
||||||
|
override val circumvention: CompositeTrajectory2D
|
||||||
|
get() = CompositeTrajectory2D(arcs)
|
||||||
|
|
||||||
|
|
||||||
|
override fun intersectsTrajectory(trajectory: Trajectory2D): Boolean =
|
||||||
|
Euclidean2DSpace.intersectsTrajectory(circumvention, trajectory)
|
||||||
|
|
||||||
|
override fun equals(other: Any?): Boolean {
|
||||||
|
if (this === other) return true
|
||||||
|
if (other == null || this::class != other::class) return false
|
||||||
|
|
||||||
|
other as CircleObstacle
|
||||||
|
|
||||||
|
return circle == other.circle
|
||||||
|
}
|
||||||
|
|
||||||
|
override fun hashCode(): Int {
|
||||||
|
return circle.hashCode()
|
||||||
|
}
|
||||||
|
|
||||||
|
override fun toString(): String = "Obstacle(circle=$circle)"
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
private class CoreObstacle(override val circumvention: CompositeTrajectory2D) : Obstacle {
|
||||||
override val arcs: List<CircleTrajectory2D> by lazy {
|
override val arcs: List<CircleTrajectory2D> by lazy {
|
||||||
circumvention.segments.filterIsInstance<CircleTrajectory2D>()
|
circumvention.segments.filterIsInstance<CircleTrajectory2D>()
|
||||||
}
|
}
|
||||||
@ -40,15 +73,19 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) :
|
|||||||
)
|
)
|
||||||
}
|
}
|
||||||
|
|
||||||
override val core: Polygon<Double> by lazy {
|
val core: Polygon<Double> by lazy {
|
||||||
Euclidean2DSpace.polygon(arcs.map { it.circle.center })
|
Euclidean2DSpace.polygon(arcs.map { it.circle.center })
|
||||||
}
|
}
|
||||||
|
|
||||||
|
override fun intersectsTrajectory(trajectory: Trajectory2D): Boolean =
|
||||||
|
Euclidean2DSpace.intersectsTrajectory(core, trajectory)
|
||||||
|
|
||||||
|
|
||||||
override fun equals(other: Any?): Boolean {
|
override fun equals(other: Any?): Boolean {
|
||||||
if (this === other) return true
|
if (this === other) return true
|
||||||
if (other == null || this::class != other::class) return false
|
if (other == null || this::class != other::class) return false
|
||||||
|
|
||||||
other as ObstacleImpl
|
other as CoreObstacle
|
||||||
|
|
||||||
return arcs == other.arcs
|
return arcs == other.arcs
|
||||||
}
|
}
|
||||||
@ -63,16 +100,17 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) :
|
|||||||
}
|
}
|
||||||
|
|
||||||
public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace) {
|
public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace) {
|
||||||
|
require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
|
||||||
|
//Create a single circle obstacle
|
||||||
|
if(circles.size == 1) return CircleObstacle(circles.first())
|
||||||
|
|
||||||
val center = vector(
|
val center = vector(
|
||||||
circles.sumOf { it.center.x },
|
circles.sumOf { it.center.x },
|
||||||
circles.sumOf { it.center.y }
|
circles.sumOf { it.center.y }
|
||||||
)/ circles.size
|
) / circles.size
|
||||||
|
|
||||||
|
|
||||||
require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
|
|
||||||
|
|
||||||
if (circles.size == 1) {
|
if (circles.size == 1) {
|
||||||
return ObstacleImpl(
|
return CoreObstacle(
|
||||||
CompositeTrajectory2D(
|
CompositeTrajectory2D(
|
||||||
CircleTrajectory2D(circles.first(), Angle.zero, Angle.piTimes2)
|
CircleTrajectory2D(circles.first(), Angle.zero, Angle.piTimes2)
|
||||||
)
|
)
|
||||||
@ -102,7 +140,7 @@ public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace)
|
|||||||
val circumvention = CompositeTrajectory2D(trajectory)
|
val circumvention = CompositeTrajectory2D(trajectory)
|
||||||
|
|
||||||
|
|
||||||
return ObstacleImpl(circumvention)
|
return CoreObstacle(circumvention)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -46,8 +46,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
val isValid by lazy {
|
val isValid by lazy {
|
||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
obstacles.indices.none {
|
obstacles.indices.none {
|
||||||
it != from?.obstacleIndex && it != to?.obstacleIndex && intersectsTrajectory(
|
it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersectsTrajectory(
|
||||||
obstacles[it].core,
|
|
||||||
tangentTrajectory
|
tangentTrajectory
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
@ -78,8 +77,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
secondCircle
|
secondCircle
|
||||||
)) {
|
)) {
|
||||||
if (
|
if (
|
||||||
!intersectsTrajectory(first.core, segment)
|
!first.intersectsTrajectory(segment)
|
||||||
&& !intersectsTrajectory(second.core, segment)
|
&& !second.intersectsTrajectory(segment)
|
||||||
) {
|
) {
|
||||||
put(
|
put(
|
||||||
pathType,
|
pathType,
|
||||||
@ -109,11 +108,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
|
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
|
||||||
obstacleArc
|
obstacleArc
|
||||||
)) {
|
)) {
|
||||||
if (pathType.first == arc.direction && !intersectsTrajectory(
|
if (pathType.first == arc.direction && !obstacle.intersectsTrajectory(segment)) {
|
||||||
obstacle.core,
|
|
||||||
segment
|
|
||||||
)
|
|
||||||
) {
|
|
||||||
put(
|
put(
|
||||||
pathType,
|
pathType,
|
||||||
ObstacleTangent(
|
ObstacleTangent(
|
||||||
@ -140,7 +135,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
obstacleArc.circle,
|
obstacleArc.circle,
|
||||||
arc.circle
|
arc.circle
|
||||||
)[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
|
)[DubinsPath.Type(obstacleDirection, Trajectory2D.S, arc.direction)]?.takeIf {
|
||||||
obstacleArc.containsPoint(it.begin) && !intersectsTrajectory(obstacle.core, it)
|
obstacleArc.containsPoint(it.begin) && !obstacle.intersectsTrajectory(it)
|
||||||
}?.let {
|
}?.let {
|
||||||
return ObstacleTangent(
|
return ObstacleTangent(
|
||||||
it,
|
it,
|
||||||
@ -214,12 +209,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
dubinsPath: CompositeTrajectory2D,
|
dubinsPath: CompositeTrajectory2D,
|
||||||
): Collection<Trajectory2D> = with(Euclidean2DSpace) {
|
): Collection<Trajectory2D> = with(Euclidean2DSpace) {
|
||||||
//fast return if no obstacles intersect the direct path
|
//fast return if no obstacles intersect the direct path
|
||||||
if (
|
if (obstacles.none { it.intersectsTrajectory(dubinsPath) }) return listOf(dubinsPath)
|
||||||
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 beginArc = dubinsPath.segments.first() as CircleTrajectory2D
|
||||||
val endArc = dubinsPath.segments.last() as CircleTrajectory2D
|
val endArc = dubinsPath.segments.last() as CircleTrajectory2D
|
||||||
@ -245,22 +235,14 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
) ?: return emptySet()
|
) ?: return emptySet()
|
||||||
|
|
||||||
// if no intersections, finish
|
// if no intersections, finish
|
||||||
if (obstacles.indices.none {
|
if (
|
||||||
intersectsTrajectory(
|
obstacles.indices.none { obstacles[it].intersectsTrajectory(tangentToEnd.tangentTrajectory) }
|
||||||
obstacles[it].core,
|
) return setOf(TangentPath(tangents + tangentToEnd))
|
||||||
tangentToEnd.tangentTrajectory
|
|
||||||
)
|
|
||||||
}) return setOf(
|
|
||||||
TangentPath(tangents + tangentToEnd)
|
|
||||||
)
|
|
||||||
|
|
||||||
// tangents to other obstacles
|
// tangents to other obstacles
|
||||||
return remainingObstacleIndices.sortedWith(
|
return remainingObstacleIndices.sortedWith(
|
||||||
compareByDescending<Int> {
|
compareByDescending<Int> {
|
||||||
intersectsTrajectory(
|
obstacles[it].intersectsTrajectory(tangentToEnd.tangentTrajectory)
|
||||||
obstacles[it].core,
|
|
||||||
tangentToEnd.tangentTrajectory
|
|
||||||
)
|
|
||||||
//take intersecting obstacles
|
//take intersecting obstacles
|
||||||
}.thenBy {
|
}.thenBy {
|
||||||
connection.circle.center.distanceTo(obstacles[it].center)
|
connection.circle.center.distanceTo(obstacles[it].center)
|
||||||
@ -281,7 +263,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
|||||||
//find the nearest obstacle that has valid tangents to
|
//find the nearest obstacle that has valid tangents to
|
||||||
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
|
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
|
||||||
compareByDescending<Int> {
|
compareByDescending<Int> {
|
||||||
intersectsTrajectory(obstacles[it].core, dubinsPath)
|
obstacles[it].intersectsTrajectory(dubinsPath)
|
||||||
//take intersecting obstacles
|
//take intersecting obstacles
|
||||||
}.thenBy {
|
}.thenBy {
|
||||||
beginArc.circle.center.distanceTo(obstacles[it].center)
|
beginArc.circle.center.distanceTo(obstacles[it].center)
|
||||||
|
@ -37,7 +37,7 @@ class ObstacleTest {
|
|||||||
)
|
)
|
||||||
assertTrue { outputTangents.isNotEmpty() }
|
assertTrue { outputTangents.isNotEmpty() }
|
||||||
val length = outputTangents.minOf { it.length }
|
val length = outputTangents.minOf { it.length }
|
||||||
assertEquals(25.0, length, 2.0)
|
assertEquals(26.0, length, 2.0)
|
||||||
}
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
|
Loading…
Reference in New Issue
Block a user