Differentiate obstacles
This commit is contained in:
parent
5d0e0f054e
commit
d8548ab162
@ -1,14 +1,14 @@
|
||||
import kotlin.io.path.readText
|
||||
|
||||
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") {
|
||||
startOn {
|
||||
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_TOKEN"] = Secrets("space_token")
|
||||
kotlinScript { api ->
|
||||
|
@ -29,7 +29,7 @@ ksciencePublish{
|
||||
if (isInDevelopment) {
|
||||
"https://maven.pkg.jetbrains.space/spc/p/sci/dev"
|
||||
} else {
|
||||
"https://maven.pkg.jetbrains.space/spc/p/sci/release"
|
||||
"https://maven.pkg.jetbrains.space/spc/p/sci/maven"
|
||||
}
|
||||
)
|
||||
sonatype()
|
||||
|
@ -17,18 +17,51 @@ public interface Obstacle {
|
||||
* A closed right-handed circuit minimal path circumvention of the obstacle.
|
||||
*/
|
||||
public val circumvention: CompositeTrajectory2D
|
||||
//
|
||||
// /**
|
||||
// * A polygon created from the arc centers of the obstacle
|
||||
// */
|
||||
// public val core: Polygon<Double>
|
||||
|
||||
/**
|
||||
* A polygon created from the arc centers of the obstacle
|
||||
*/
|
||||
public val core: Polygon<Double>
|
||||
public fun intersectsTrajectory(trajectory: Trajectory2D): Boolean
|
||||
|
||||
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 {
|
||||
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 })
|
||||
}
|
||||
|
||||
override fun intersectsTrajectory(trajectory: Trajectory2D): Boolean =
|
||||
Euclidean2DSpace.intersectsTrajectory(core, trajectory)
|
||||
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (this === other) return true
|
||||
if (other == null || this::class != other::class) return false
|
||||
|
||||
other as ObstacleImpl
|
||||
other as CoreObstacle
|
||||
|
||||
return arcs == other.arcs
|
||||
}
|
||||
@ -63,16 +100,17 @@ private class ObstacleImpl(override val circumvention: CompositeTrajectory2D) :
|
||||
}
|
||||
|
||||
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(
|
||||
circles.sumOf { it.center.x },
|
||||
circles.sumOf { it.center.y }
|
||||
)/ circles.size
|
||||
|
||||
|
||||
require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
|
||||
) / circles.size
|
||||
|
||||
if (circles.size == 1) {
|
||||
return ObstacleImpl(
|
||||
return CoreObstacle(
|
||||
CompositeTrajectory2D(
|
||||
CircleTrajectory2D(circles.first(), Angle.zero, Angle.piTimes2)
|
||||
)
|
||||
@ -102,7 +140,7 @@ public fun Obstacle(circles: List<Circle2D>): Obstacle = with(Euclidean2DSpace)
|
||||
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 {
|
||||
with(Euclidean2DSpace) {
|
||||
obstacles.indices.none {
|
||||
it != from?.obstacleIndex && it != to?.obstacleIndex && intersectsTrajectory(
|
||||
obstacles[it].core,
|
||||
it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersectsTrajectory(
|
||||
tangentTrajectory
|
||||
)
|
||||
}
|
||||
@ -78,8 +77,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
||||
secondCircle
|
||||
)) {
|
||||
if (
|
||||
!intersectsTrajectory(first.core, segment)
|
||||
&& !intersectsTrajectory(second.core, segment)
|
||||
!first.intersectsTrajectory(segment)
|
||||
&& !second.intersectsTrajectory(segment)
|
||||
) {
|
||||
put(
|
||||
pathType,
|
||||
@ -109,11 +108,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
||||
arc.copy(arcAngle = Angle.piTimes2), //extend arc to full circle
|
||||
obstacleArc
|
||||
)) {
|
||||
if (pathType.first == arc.direction && !intersectsTrajectory(
|
||||
obstacle.core,
|
||||
segment
|
||||
)
|
||||
) {
|
||||
if (pathType.first == arc.direction && !obstacle.intersectsTrajectory(segment)) {
|
||||
put(
|
||||
pathType,
|
||||
ObstacleTangent(
|
||||
@ -140,7 +135,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) && !intersectsTrajectory(obstacle.core, it)
|
||||
obstacleArc.containsPoint(it.begin) && !obstacle.intersectsTrajectory(it)
|
||||
}?.let {
|
||||
return ObstacleTangent(
|
||||
it,
|
||||
@ -214,12 +209,7 @@ 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.arcs.size == 1 && intersectsTrajectory(it.circumvention, dubinsPath)) // special case for one-point obstacles
|
||||
|| intersectsTrajectory(it.core, dubinsPath)
|
||||
}
|
||||
) return listOf(dubinsPath)
|
||||
if (obstacles.none { it.intersectsTrajectory(dubinsPath) }) return listOf(dubinsPath)
|
||||
|
||||
val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
|
||||
val endArc = dubinsPath.segments.last() as CircleTrajectory2D
|
||||
@ -245,22 +235,14 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
|
||||
) ?: return emptySet()
|
||||
|
||||
// if no intersections, finish
|
||||
if (obstacles.indices.none {
|
||||
intersectsTrajectory(
|
||||
obstacles[it].core,
|
||||
tangentToEnd.tangentTrajectory
|
||||
)
|
||||
}) return setOf(
|
||||
TangentPath(tangents + tangentToEnd)
|
||||
)
|
||||
if (
|
||||
obstacles.indices.none { obstacles[it].intersectsTrajectory(tangentToEnd.tangentTrajectory) }
|
||||
) return setOf(TangentPath(tangents + tangentToEnd))
|
||||
|
||||
// tangents to other obstacles
|
||||
return remainingObstacleIndices.sortedWith(
|
||||
compareByDescending<Int> {
|
||||
intersectsTrajectory(
|
||||
obstacles[it].core,
|
||||
tangentToEnd.tangentTrajectory
|
||||
)
|
||||
obstacles[it].intersectsTrajectory(tangentToEnd.tangentTrajectory)
|
||||
//take intersecting obstacles
|
||||
}.thenBy {
|
||||
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
|
||||
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
|
||||
compareByDescending<Int> {
|
||||
intersectsTrajectory(obstacles[it].core, dubinsPath)
|
||||
obstacles[it].intersectsTrajectory(dubinsPath)
|
||||
//take intersecting obstacles
|
||||
}.thenBy {
|
||||
beginArc.circle.center.distanceTo(obstacles[it].center)
|
||||
|
@ -37,7 +37,7 @@ class ObstacleTest {
|
||||
)
|
||||
assertTrue { outputTangents.isNotEmpty() }
|
||||
val length = outputTangents.minOf { it.length }
|
||||
assertEquals(25.0, length, 2.0)
|
||||
assertEquals(26.0, length, 2.0)
|
||||
}
|
||||
|
||||
@Test
|
||||
|
Loading…
Reference in New Issue
Block a user