[WIP] disentangling obstacles phase 2
This commit is contained in:
parent
dbfe61b949
commit
e553e33d4c
@ -74,7 +74,7 @@ fun App() {
|
||||
.modifyAttribute(ColorAttribute, Color.Blue)
|
||||
.modifyAttribute(AlphaAttribute, 0.4f)
|
||||
|
||||
image(pointOne, Icons.Filled.Home)
|
||||
icon(pointOne, Icons.Filled.Home)
|
||||
|
||||
val marker1 = rectangle(55.744 to 38.614, size = DpSize(10.dp, 10.dp)).color(Color.Magenta)
|
||||
val marker2 = rectangle(55.8 to 38.5, size = DpSize(10.dp, 10.dp)).color(Color.Magenta)
|
||||
|
@ -1,13 +1,14 @@
|
||||
plugins {
|
||||
kotlin("multiplatform")
|
||||
id("space.kscience.gradle.mpp")
|
||||
id("org.jetbrains.compose")
|
||||
`maven-publish`
|
||||
}
|
||||
|
||||
kotlin {
|
||||
explicitApi = org.jetbrains.kotlin.gradle.dsl.ExplicitApiMode.Warning
|
||||
jvmToolchain(11)
|
||||
kscience{
|
||||
jvm()
|
||||
}
|
||||
|
||||
kotlin {
|
||||
sourceSets {
|
||||
commonMain {
|
||||
dependencies {
|
||||
@ -19,8 +20,6 @@ kotlin {
|
||||
api("io.github.microutils:kotlin-logging:2.1.23")
|
||||
}
|
||||
}
|
||||
val jvmMain by getting {
|
||||
}
|
||||
val jvmTest by getting {
|
||||
dependencies {
|
||||
implementation("io.ktor:ktor-client-cio")
|
||||
@ -28,18 +27,11 @@ kotlin {
|
||||
implementation(spclibs.kotlinx.coroutines.test)
|
||||
|
||||
implementation(spclibs.logback.classic)
|
||||
|
||||
implementation(kotlin("test-junit5"))
|
||||
implementation("org.junit.jupiter:junit-jupiter:${spclibs.versions.junit.get()}")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
tasks.withType<Test> {
|
||||
useJUnitPlatform()
|
||||
}
|
||||
|
||||
readme {
|
||||
description = "Compose-multiplaform implementation for web-mercator tiled maps"
|
||||
maturity = space.kscience.gradle.Maturity.EXPERIMENTAL
|
||||
|
@ -92,14 +92,14 @@ public fun FeatureGroup<Gmc>.multiLine(
|
||||
id: String? = null,
|
||||
): FeatureRef<Gmc, MultiLineFeature<Gmc>> = feature(id, MultiLineFeature(space, points.map(::coordinatesOf)))
|
||||
|
||||
public fun FeatureGroup<Gmc>.image(
|
||||
public fun FeatureGroup<Gmc>.icon(
|
||||
position: Pair<Double, Double>,
|
||||
image: ImageVector,
|
||||
size: DpSize = DpSize(20.dp, 20.dp),
|
||||
id: String? = null,
|
||||
): FeatureRef<Gmc, VectorImageFeature<Gmc>> = feature(
|
||||
): FeatureRef<Gmc, VectorIconFeature<Gmc>> = feature(
|
||||
id,
|
||||
VectorImageFeature(
|
||||
VectorIconFeature(
|
||||
space,
|
||||
coordinatesOf(position),
|
||||
size,
|
||||
|
@ -1,10 +1,7 @@
|
||||
package center.sciprog.maps.coordinates
|
||||
|
||||
import kotlinx.serialization.Serializable
|
||||
import space.kscience.kmath.geometry.Angle
|
||||
import space.kscience.kmath.geometry.degrees
|
||||
import space.kscience.kmath.geometry.normalized
|
||||
import space.kscience.kmath.geometry.radians
|
||||
import space.kscience.kmath.geometry.*
|
||||
|
||||
/**
|
||||
* Geodetic coordinated
|
||||
@ -16,7 +13,7 @@ public class GeodeticMapCoordinates(
|
||||
public val latitude: Angle,
|
||||
public val longitude: Angle,
|
||||
public val elevation: Distance? = null,
|
||||
) {
|
||||
) : Vector2D<Angle> {
|
||||
init {
|
||||
require(latitude in (-Angle.piDiv2)..(Angle.piDiv2)) {
|
||||
"Latitude $latitude is not in (-PI/2)..(PI/2)"
|
||||
@ -26,16 +23,17 @@ public class GeodeticMapCoordinates(
|
||||
}
|
||||
}
|
||||
|
||||
override val x: Angle get() = longitude
|
||||
|
||||
override val y: Angle get() = latitude
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (this === other) return true
|
||||
if (other == null || this::class != other::class) return false
|
||||
|
||||
other as GeodeticMapCoordinates
|
||||
|
||||
if (latitude != other.latitude) return false
|
||||
if (longitude != other.longitude) return false
|
||||
|
||||
return true
|
||||
return latitude == other.latitude && longitude == other.longitude
|
||||
}
|
||||
|
||||
override fun hashCode(): Int {
|
||||
|
@ -267,8 +267,11 @@ public data class DrawFeature<T : Any>(
|
||||
override fun withAttributes(modify: (Attributes) -> Attributes): Feature<T> = copy(attributes = modify(attributes))
|
||||
}
|
||||
|
||||
/**
|
||||
* Fixed size bitmap icon
|
||||
*/
|
||||
@Stable
|
||||
public data class BitmapImageFeature<T : Any>(
|
||||
public data class BitmapIconFeature<T : Any>(
|
||||
override val space: CoordinateSpace<T>,
|
||||
override val center: T,
|
||||
public val size: DpSize,
|
||||
@ -282,8 +285,11 @@ public data class BitmapImageFeature<T : Any>(
|
||||
override fun withAttributes(modify: (Attributes) -> Attributes): Feature<T> = copy(attributes = modify(attributes))
|
||||
}
|
||||
|
||||
/**
|
||||
* Fixed size vector icon
|
||||
*/
|
||||
@Stable
|
||||
public data class VectorImageFeature<T : Any>(
|
||||
public data class VectorIconFeature<T : Any>(
|
||||
override val space: CoordinateSpace<T>,
|
||||
override val center: T,
|
||||
public val size: DpSize,
|
||||
@ -301,7 +307,7 @@ public data class VectorImageFeature<T : Any>(
|
||||
}
|
||||
|
||||
/**
|
||||
* An image that is bound to coordinates and is scaled together with them
|
||||
* An image that is bound to coordinates and is scaled (and possibly warped) together with them
|
||||
*
|
||||
* @param rectangle the size of background in scheme size units. The screen units to scheme units ratio equals scale.
|
||||
*/
|
||||
|
@ -235,16 +235,16 @@ public fun <T : Any> FeatureGroup<T>.polygon(
|
||||
PolygonFeature(space, points, attributes)
|
||||
)
|
||||
|
||||
public fun <T : Any> FeatureGroup<T>.image(
|
||||
public fun <T : Any> FeatureGroup<T>.icon(
|
||||
position: T,
|
||||
image: ImageVector,
|
||||
size: DpSize = DpSize(image.defaultWidth, image.defaultHeight),
|
||||
attributes: Attributes = Attributes.EMPTY,
|
||||
id: String? = null,
|
||||
): FeatureRef<T, VectorImageFeature<T>> =
|
||||
): FeatureRef<T, VectorIconFeature<T>> =
|
||||
feature(
|
||||
id,
|
||||
VectorImageFeature(
|
||||
VectorIconFeature(
|
||||
space,
|
||||
position,
|
||||
size,
|
||||
|
@ -3,10 +3,7 @@ package center.sciprog.maps.features
|
||||
import androidx.compose.ui.geometry.Offset
|
||||
import androidx.compose.ui.geometry.Size
|
||||
import androidx.compose.ui.graphics.*
|
||||
import androidx.compose.ui.graphics.drawscope.DrawScope
|
||||
import androidx.compose.ui.graphics.drawscope.Stroke
|
||||
import androidx.compose.ui.graphics.drawscope.drawIntoCanvas
|
||||
import androidx.compose.ui.graphics.drawscope.translate
|
||||
import androidx.compose.ui.graphics.drawscope.*
|
||||
import androidx.compose.ui.graphics.painter.Painter
|
||||
import center.sciprog.attributes.plus
|
||||
import org.jetbrains.skia.Font
|
||||
@ -72,9 +69,9 @@ public fun <T : Any> DrawScope.drawFeature(
|
||||
|
||||
}
|
||||
|
||||
is BitmapImageFeature -> drawImage(feature.image, feature.center.toOffset())
|
||||
is BitmapIconFeature -> drawImage(feature.image, feature.center.toOffset())
|
||||
|
||||
is VectorImageFeature -> {
|
||||
is VectorIconFeature -> {
|
||||
val offset = feature.center.toOffset()
|
||||
val size = feature.size.toSize()
|
||||
translate(offset.x - size.width / 2, offset.y - size.height / 2) {
|
||||
|
17
maps-kt-geotiff/build.gradle.kts
Normal file
17
maps-kt-geotiff/build.gradle.kts
Normal file
@ -0,0 +1,17 @@
|
||||
plugins {
|
||||
id("space.kscience.gradle.jvm")
|
||||
`maven-publish`
|
||||
}
|
||||
|
||||
repositories {
|
||||
maven("https://repo.osgeo.org/repository/release/")
|
||||
}
|
||||
|
||||
dependencies {
|
||||
api("org.geotools:gt-geotiff:27.2") {
|
||||
exclude(group = "javax.media", module = "jai_core")
|
||||
}
|
||||
|
||||
api(projects.mapsKtCore)
|
||||
api(projects.mapsKtFeatures)
|
||||
}
|
@ -0,0 +1,26 @@
|
||||
package center.sciprog.maps.geotiff
|
||||
|
||||
import center.sciprog.maps.coordinates.Gmc
|
||||
import center.sciprog.maps.features.Feature
|
||||
import center.sciprog.maps.features.FeatureGroup
|
||||
import center.sciprog.maps.features.FeatureRef
|
||||
import kotlinx.serialization.json.Json
|
||||
import kotlinx.serialization.json.jsonObject
|
||||
import org.geotools.gce.geotiff.GeoTiffReader
|
||||
import org.geotools.util.factory.Hints
|
||||
import java.io.File
|
||||
import java.net.URL
|
||||
|
||||
|
||||
public fun FeatureGroup<Gmc>.geoJson(
|
||||
geoTiffUrl: URL,
|
||||
id: String? = null,
|
||||
): FeatureRef<Gmc, Feature<Gmc>> {
|
||||
val reader = GeoTiffReader
|
||||
val jsonString = geoJsonUrl.readText()
|
||||
val json = Json.parseToJsonElement(jsonString).jsonObject
|
||||
val geoJson = GeoJson(json)
|
||||
|
||||
return geoJson(geoJson, id)
|
||||
}
|
||||
|
@ -1,11 +1,14 @@
|
||||
plugins {
|
||||
kotlin("multiplatform")
|
||||
id("space.kscience.gradle.mpp")
|
||||
id("org.jetbrains.compose")
|
||||
`maven-publish`
|
||||
}
|
||||
|
||||
kotlin {
|
||||
kscience{
|
||||
jvm()
|
||||
}
|
||||
|
||||
kotlin {
|
||||
sourceSets {
|
||||
commonMain {
|
||||
dependencies {
|
||||
|
@ -5,11 +5,12 @@ import androidx.compose.ui.unit.dp
|
||||
import center.sciprog.maps.features.CoordinateSpace
|
||||
import center.sciprog.maps.features.Rectangle
|
||||
import center.sciprog.maps.features.ViewPoint
|
||||
import space.kscience.kmath.geometry.Vector2D
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.max
|
||||
import kotlin.math.min
|
||||
|
||||
data class XY(val x: Float, val y: Float)
|
||||
public data class XY(override val x: Float, override val y: Float): Vector2D<Float>
|
||||
|
||||
internal data class XYRectangle(
|
||||
override val a: XY,
|
||||
@ -28,21 +29,21 @@ internal data class XYRectangle(
|
||||
// }
|
||||
}
|
||||
|
||||
val Rectangle<XY>.top get() = max(a.y, b.y)
|
||||
val Rectangle<XY>.bottom get() = min(a.y, b.y)
|
||||
public val Rectangle<XY>.top: Float get() = max(a.y, b.y)
|
||||
public val Rectangle<XY>.bottom: Float get() = min(a.y, b.y)
|
||||
|
||||
val Rectangle<XY>.right get() = max(a.x, b.x)
|
||||
val Rectangle<XY>.left get() = min(a.x, b.x)
|
||||
public val Rectangle<XY>.right: Float get() = max(a.x, b.x)
|
||||
public val Rectangle<XY>.left: Float get() = min(a.x, b.x)
|
||||
|
||||
val Rectangle<XY>.width: Float get() = abs(a.x - b.x)
|
||||
val Rectangle<XY>.height: Float get() = abs(a.y - b.y)
|
||||
public val Rectangle<XY>.width: Float get() = abs(a.x - b.x)
|
||||
public val Rectangle<XY>.height: Float get() = abs(a.y - b.y)
|
||||
|
||||
public val Rectangle<XY>.leftTop: XY get() = XY(left, top)
|
||||
public val Rectangle<XY>.rightBottom: XY get() = XY(right, bottom)
|
||||
|
||||
internal val defaultCanvasSize = DpSize(512.dp, 512.dp)
|
||||
|
||||
data class XYViewPoint(
|
||||
public data class XYViewPoint(
|
||||
override val focus: XY,
|
||||
override val zoom: Float = 1f,
|
||||
) : ViewPoint<XY>
|
||||
|
@ -9,7 +9,7 @@ import center.sciprog.maps.features.ViewPoint
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.pow
|
||||
|
||||
object XYCoordinateSpace : CoordinateSpace<XY> {
|
||||
public object XYCoordinateSpace : CoordinateSpace<XY> {
|
||||
override fun Rectangle(first: XY, second: XY): Rectangle<XY> =
|
||||
XYRectangle(first, second)
|
||||
|
||||
|
@ -9,7 +9,7 @@ import androidx.compose.ui.unit.dp
|
||||
import center.sciprog.maps.features.*
|
||||
import kotlin.math.min
|
||||
|
||||
class XYViewScope(
|
||||
public class XYViewScope(
|
||||
config: ViewConfig<XY>,
|
||||
) : CoordinateViewScope<XY>(config) {
|
||||
override val space: CoordinateSpace<XY>
|
||||
@ -45,7 +45,7 @@ class XYViewScope(
|
||||
return DpRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
|
||||
}
|
||||
|
||||
companion object{
|
||||
public companion object{
|
||||
@Composable
|
||||
public fun remember(
|
||||
config: ViewConfig<XY> = ViewConfig(),
|
||||
|
@ -15,7 +15,7 @@ import kotlin.math.ceil
|
||||
|
||||
internal fun Pair<Number, Number>.toCoordinates(): XY = XY(first.toFloat(), second.toFloat())
|
||||
|
||||
fun FeatureGroup<XY>.background(
|
||||
public fun FeatureGroup<XY>.background(
|
||||
width: Float,
|
||||
height: Float,
|
||||
offset: XY = XY(0f, 0f),
|
||||
@ -37,19 +37,19 @@ fun FeatureGroup<XY>.background(
|
||||
)
|
||||
}
|
||||
|
||||
fun FeatureGroup<XY>.circle(
|
||||
public fun FeatureGroup<XY>.circle(
|
||||
centerCoordinates: Pair<Number, Number>,
|
||||
size: Dp = 5.dp,
|
||||
id: String? = null,
|
||||
): FeatureRef<XY, CircleFeature<XY>> = circle(centerCoordinates.toCoordinates(), size, id = id)
|
||||
|
||||
fun FeatureGroup<XY>.draw(
|
||||
public fun FeatureGroup<XY>.draw(
|
||||
position: Pair<Number, Number>,
|
||||
id: String? = null,
|
||||
draw: DrawScope.() -> Unit,
|
||||
): FeatureRef<XY, DrawFeature<XY>> = draw(position.toCoordinates(), id = id, draw = draw)
|
||||
|
||||
fun FeatureGroup<XY>.line(
|
||||
public fun FeatureGroup<XY>.line(
|
||||
aCoordinates: Pair<Number, Number>,
|
||||
bCoordinates: Pair<Number, Number>,
|
||||
id: String? = null,
|
||||
@ -69,15 +69,15 @@ public fun FeatureGroup<XY>.arc(
|
||||
id = id
|
||||
)
|
||||
|
||||
fun FeatureGroup<XY>.image(
|
||||
public fun FeatureGroup<XY>.image(
|
||||
position: Pair<Number, Number>,
|
||||
image: ImageVector,
|
||||
size: DpSize = DpSize(image.defaultWidth, image.defaultHeight),
|
||||
id: String? = null,
|
||||
): FeatureRef<XY, VectorImageFeature<XY>> =
|
||||
image(position.toCoordinates(), image, size = size, id = id)
|
||||
): FeatureRef<XY, VectorIconFeature<XY>> =
|
||||
icon(position.toCoordinates(), image, size = size, id = id)
|
||||
|
||||
fun FeatureGroup<XY>.text(
|
||||
public fun FeatureGroup<XY>.text(
|
||||
position: Pair<Number, Number>,
|
||||
text: String,
|
||||
id: String? = null,
|
||||
|
@ -120,9 +120,9 @@ fun FeatureStateSnapshot<XY>.generateSvg(
|
||||
)
|
||||
}
|
||||
|
||||
is BitmapImageFeature -> drawImage(feature.image, feature.center.toOffset())
|
||||
is BitmapIconFeature -> drawImage(feature.image, feature.center.toOffset())
|
||||
|
||||
is VectorImageFeature -> {
|
||||
is VectorIconFeature -> {
|
||||
val offset = feature.center.toOffset()
|
||||
val imageSize = feature.size.toSize()
|
||||
translate(offset.x - imageSize.width / 2, offset.y - imageSize.height / 2) {
|
||||
|
@ -48,6 +48,7 @@ include(
|
||||
":trajectory-kt",
|
||||
":maps-kt-core",
|
||||
":maps-kt-geojson",
|
||||
// ":maps-kt-geotiff",
|
||||
":maps-kt-features",
|
||||
":maps-kt-compose",
|
||||
":maps-kt-scheme",
|
||||
|
@ -35,113 +35,113 @@ private fun TangentPath(vararg tangents: Tangent) = TangentPath(listOf(*tangents
|
||||
* Create inner and outer tangents between two circles.
|
||||
* This method returns a map of segments using [DubinsPath] connection type notation.
|
||||
*/
|
||||
internal fun Circle2D.tangentsToCircle(
|
||||
other: Circle2D,
|
||||
internal fun tangentsToCircle(
|
||||
first: Circle2D,
|
||||
second: Circle2D,
|
||||
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
|
||||
//return empty map for concentric circles
|
||||
if (center.equalsVector(other.center)) return emptyMap()
|
||||
if (first.center.equalsVector(second.center)) return emptyMap()
|
||||
|
||||
// A line connecting centers
|
||||
val line = LineSegment(center, other.center)
|
||||
val line = LineSegment(first.center, second.center)
|
||||
// Distance between centers
|
||||
val distance = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
|
||||
val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y)
|
||||
var angle2: Double
|
||||
val routes = mapOf(
|
||||
DubinsPath.Type.RSR to Pair(radius, other.radius),
|
||||
DubinsPath.Type.RSL to Pair(radius, -other.radius),
|
||||
DubinsPath.Type.LSR to Pair(-radius, other.radius),
|
||||
DubinsPath.Type.LSL to Pair(-radius, -other.radius)
|
||||
)
|
||||
return buildMap {
|
||||
for ((route, r1r2) in routes) {
|
||||
val r1 = r1r2.first
|
||||
val r2 = r1r2.second
|
||||
val r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
if (distance * distance >= r * r) {
|
||||
val l = sqrt(distance * distance - r * r)
|
||||
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
put(
|
||||
route,
|
||||
LineSegment(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
)
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
return listOf(
|
||||
DubinsPath.Type.RSR,
|
||||
DubinsPath.Type.RSL,
|
||||
DubinsPath.Type.LSR,
|
||||
DubinsPath.Type.LSL
|
||||
).associateWith { route ->
|
||||
val r1 = when (route.first) {
|
||||
Trajectory2D.L -> -first.radius
|
||||
Trajectory2D.R -> first.radius
|
||||
}
|
||||
}
|
||||
}
|
||||
val r2 = when (route.third) {
|
||||
Trajectory2D.L -> -second.radius
|
||||
Trajectory2D.R -> second.radius
|
||||
}
|
||||
val r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
if (distance * distance < r * r) error("Circles should not intersect")
|
||||
|
||||
private fun dubinsTangentsToCircles(
|
||||
firstCircle: Circle2D,
|
||||
secondCircle: Circle2D,
|
||||
firstObstacle: Obstacle,
|
||||
secondObstacle: Obstacle,
|
||||
): Map<DubinsPath.Type, Tangent> = with(Euclidean2DSpace) {
|
||||
val line = LineSegment(firstCircle.center, secondCircle.center)
|
||||
val distance = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(
|
||||
secondCircle.center.x - firstCircle.center.x,
|
||||
secondCircle.center.y - firstCircle.center.y
|
||||
)
|
||||
var r: Double
|
||||
var angle2: Double
|
||||
val routes = mapOf(
|
||||
DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
|
||||
DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
|
||||
DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
|
||||
DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
|
||||
)
|
||||
return buildMap {
|
||||
for ((route: DubinsPath.Type, r1r2) in routes) {
|
||||
val r1 = r1r2.first
|
||||
val r2 = r1r2.second
|
||||
r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
if (distance * distance >= r * r) {
|
||||
val l = sqrt(distance * distance - r * r)
|
||||
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
put(
|
||||
route,
|
||||
Tangent(
|
||||
startCircle = Circle2D(firstCircle.center, firstCircle.radius),
|
||||
endCircle = secondCircle,
|
||||
startObstacle = firstObstacle,
|
||||
endObstacle = secondObstacle,
|
||||
lineSegment = LineSegment(
|
||||
firstCircle.center + w * r1,
|
||||
secondCircle.center + w * r2
|
||||
),
|
||||
startDirection = route.first,
|
||||
endDirection = route.third
|
||||
)
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
val l = sqrt(distance * distance - r * r)
|
||||
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
|
||||
LineSegment(
|
||||
first.center + w * r1,
|
||||
second.center + w * r2
|
||||
)
|
||||
}
|
||||
}
|
||||
//
|
||||
//private fun dubinsTangentsToCircles(
|
||||
// firstCircle: Circle2D,
|
||||
// secondCircle: Circle2D,
|
||||
// firstObstacle: Obstacle,
|
||||
// secondObstacle: Obstacle,
|
||||
//): Map<DubinsPath.Type, Tangent> = with(Euclidean2DSpace) {
|
||||
// val line = LineSegment(firstCircle.center, secondCircle.center)
|
||||
// val distance = line.begin.distanceTo(line.end)
|
||||
// val angle1 = atan2(
|
||||
// secondCircle.center.x - firstCircle.center.x,
|
||||
// secondCircle.center.y - firstCircle.center.y
|
||||
// )
|
||||
// var r: Double
|
||||
// var angle2: Double
|
||||
// val routes = mapOf(
|
||||
// DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
|
||||
// DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
|
||||
// DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
|
||||
// DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
|
||||
// )
|
||||
// return buildMap {
|
||||
// for ((route: DubinsPath.Type, r1r2) in routes) {
|
||||
// val r1 = r1r2.first
|
||||
// val r2 = r1r2.second
|
||||
// r = if (r1.sign == r2.sign) {
|
||||
// r1.absoluteValue - r2.absoluteValue
|
||||
// } else {
|
||||
// r1.absoluteValue + r2.absoluteValue
|
||||
// }
|
||||
// if (distance * distance >= r * r) {
|
||||
// val l = sqrt(distance * distance - r * r)
|
||||
// angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
// angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
// } else {
|
||||
// angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
// }
|
||||
// val w = vector(-cos(angle2), sin(angle2))
|
||||
// put(
|
||||
// route,
|
||||
// Tangent(
|
||||
// startCircle = Circle2D(firstCircle.center, firstCircle.radius),
|
||||
// endCircle = secondCircle,
|
||||
// startObstacle = firstObstacle,
|
||||
// endObstacle = secondObstacle,
|
||||
// lineSegment = LineSegment(
|
||||
// firstCircle.center + w * r1,
|
||||
// secondCircle.center + w * r2
|
||||
// ),
|
||||
// startDirection = route.first,
|
||||
// endDirection = route.third
|
||||
// )
|
||||
// )
|
||||
// } else {
|
||||
// throw Exception("Circles should not intersect")
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
internal class Obstacle(
|
||||
public val circles: List<Circle2D>,
|
||||
@ -335,13 +335,23 @@ private fun Tangent.intersectObstacle(obstacle: Obstacle): Boolean {
|
||||
private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, Tangent> = buildMap {
|
||||
for (circle1 in first.circles) {
|
||||
for (circle2 in second.circles) {
|
||||
for (tangent in dubinsTangentsToCircles(circle1, circle2, first, second)) {
|
||||
if (!(tangent.value.intersectObstacle(first))
|
||||
and !(tangent.value.intersectObstacle(second))
|
||||
for ((pathType, segment) in tangentsToCircle(circle1, circle2)) {
|
||||
val tangent = Tangent(
|
||||
circle1,
|
||||
circle2,
|
||||
first,
|
||||
second,
|
||||
segment,
|
||||
pathType.first,
|
||||
pathType.third
|
||||
)
|
||||
|
||||
if (!(tangent.intersectObstacle(first))
|
||||
and !(tangent.intersectObstacle(second))
|
||||
) {
|
||||
put(
|
||||
tangent.key,
|
||||
tangent.value
|
||||
pathType,
|
||||
tangent
|
||||
)
|
||||
}
|
||||
}
|
||||
@ -506,7 +516,7 @@ internal fun findAllPaths(
|
||||
val currentObstacle = tangentPath.last().endObstacle
|
||||
var nextObstacle: Obstacle? = null
|
||||
if (currentObstacle != finalObstacle) {
|
||||
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
val tangentToFinal: Tangent? = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.S,
|
||||
j
|
||||
|
@ -44,7 +44,7 @@ class TangentTest {
|
||||
)
|
||||
)
|
||||
|
||||
val tangentMap = c1.tangentsToCircle(c2)
|
||||
val tangentMap = tangentsToCircle(c1, c2)
|
||||
val tangentMapKeys = tangentMap.keys.toList()
|
||||
val tangentMapValues = tangentMap.values.toList()
|
||||
|
||||
@ -58,6 +58,6 @@ class TangentTest {
|
||||
fun concentric(){
|
||||
val c1 = Circle2D(vector(0.0, 0.0), 10.0)
|
||||
val c2 = Circle2D(vector(0.0, 0.0), 1.0)
|
||||
assertEquals(emptyMap(), c1.tangentsToCircle(c2))
|
||||
assertEquals(emptyMap(), tangentsToCircle(c1, c2))
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user