[WIP] disentangling obstacles phase 2

This commit is contained in:
Alexander Nozik 2023-04-11 17:08:28 +03:00
parent dbfe61b949
commit e553e33d4c
18 changed files with 215 additions and 164 deletions

View File

@ -74,7 +74,7 @@ fun App() {
.modifyAttribute(ColorAttribute, Color.Blue) .modifyAttribute(ColorAttribute, Color.Blue)
.modifyAttribute(AlphaAttribute, 0.4f) .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 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) val marker2 = rectangle(55.8 to 38.5, size = DpSize(10.dp, 10.dp)).color(Color.Magenta)

View File

@ -1,13 +1,14 @@
plugins { plugins {
kotlin("multiplatform") id("space.kscience.gradle.mpp")
id("org.jetbrains.compose") id("org.jetbrains.compose")
`maven-publish` `maven-publish`
} }
kotlin { kscience{
explicitApi = org.jetbrains.kotlin.gradle.dsl.ExplicitApiMode.Warning
jvmToolchain(11)
jvm() jvm()
}
kotlin {
sourceSets { sourceSets {
commonMain { commonMain {
dependencies { dependencies {
@ -19,8 +20,6 @@ kotlin {
api("io.github.microutils:kotlin-logging:2.1.23") api("io.github.microutils:kotlin-logging:2.1.23")
} }
} }
val jvmMain by getting {
}
val jvmTest by getting { val jvmTest by getting {
dependencies { dependencies {
implementation("io.ktor:ktor-client-cio") implementation("io.ktor:ktor-client-cio")
@ -28,18 +27,11 @@ kotlin {
implementation(spclibs.kotlinx.coroutines.test) implementation(spclibs.kotlinx.coroutines.test)
implementation(spclibs.logback.classic) implementation(spclibs.logback.classic)
implementation(kotlin("test-junit5"))
implementation("org.junit.jupiter:junit-jupiter:${spclibs.versions.junit.get()}")
} }
} }
} }
} }
tasks.withType<Test> {
useJUnitPlatform()
}
readme { readme {
description = "Compose-multiplaform implementation for web-mercator tiled maps" description = "Compose-multiplaform implementation for web-mercator tiled maps"
maturity = space.kscience.gradle.Maturity.EXPERIMENTAL maturity = space.kscience.gradle.Maturity.EXPERIMENTAL

View File

@ -92,14 +92,14 @@ public fun FeatureGroup<Gmc>.multiLine(
id: String? = null, id: String? = null,
): FeatureRef<Gmc, MultiLineFeature<Gmc>> = feature(id, MultiLineFeature(space, points.map(::coordinatesOf))) ): 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>, position: Pair<Double, Double>,
image: ImageVector, image: ImageVector,
size: DpSize = DpSize(20.dp, 20.dp), size: DpSize = DpSize(20.dp, 20.dp),
id: String? = null, id: String? = null,
): FeatureRef<Gmc, VectorImageFeature<Gmc>> = feature( ): FeatureRef<Gmc, VectorIconFeature<Gmc>> = feature(
id, id,
VectorImageFeature( VectorIconFeature(
space, space,
coordinatesOf(position), coordinatesOf(position),
size, size,

View File

@ -1,10 +1,7 @@
package center.sciprog.maps.coordinates package center.sciprog.maps.coordinates
import kotlinx.serialization.Serializable import kotlinx.serialization.Serializable
import space.kscience.kmath.geometry.Angle import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.degrees
import space.kscience.kmath.geometry.normalized
import space.kscience.kmath.geometry.radians
/** /**
* Geodetic coordinated * Geodetic coordinated
@ -16,7 +13,7 @@ public class GeodeticMapCoordinates(
public val latitude: Angle, public val latitude: Angle,
public val longitude: Angle, public val longitude: Angle,
public val elevation: Distance? = null, public val elevation: Distance? = null,
) { ) : Vector2D<Angle> {
init { init {
require(latitude in (-Angle.piDiv2)..(Angle.piDiv2)) { require(latitude in (-Angle.piDiv2)..(Angle.piDiv2)) {
"Latitude $latitude is not in (-PI/2)..(PI/2)" "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 { 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 GeodeticMapCoordinates other as GeodeticMapCoordinates
if (latitude != other.latitude) return false return latitude == other.latitude && longitude == other.longitude
if (longitude != other.longitude) return false
return true
} }
override fun hashCode(): Int { override fun hashCode(): Int {

View File

@ -267,8 +267,11 @@ public data class DrawFeature<T : Any>(
override fun withAttributes(modify: (Attributes) -> Attributes): Feature<T> = copy(attributes = modify(attributes)) override fun withAttributes(modify: (Attributes) -> Attributes): Feature<T> = copy(attributes = modify(attributes))
} }
/**
* Fixed size bitmap icon
*/
@Stable @Stable
public data class BitmapImageFeature<T : Any>( public data class BitmapIconFeature<T : Any>(
override val space: CoordinateSpace<T>, override val space: CoordinateSpace<T>,
override val center: T, override val center: T,
public val size: DpSize, 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)) override fun withAttributes(modify: (Attributes) -> Attributes): Feature<T> = copy(attributes = modify(attributes))
} }
/**
* Fixed size vector icon
*/
@Stable @Stable
public data class VectorImageFeature<T : Any>( public data class VectorIconFeature<T : Any>(
override val space: CoordinateSpace<T>, override val space: CoordinateSpace<T>,
override val center: T, override val center: T,
public val size: DpSize, 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. * @param rectangle the size of background in scheme size units. The screen units to scheme units ratio equals scale.
*/ */

View File

@ -235,16 +235,16 @@ public fun <T : Any> FeatureGroup<T>.polygon(
PolygonFeature(space, points, attributes) PolygonFeature(space, points, attributes)
) )
public fun <T : Any> FeatureGroup<T>.image( public fun <T : Any> FeatureGroup<T>.icon(
position: T, position: T,
image: ImageVector, image: ImageVector,
size: DpSize = DpSize(image.defaultWidth, image.defaultHeight), size: DpSize = DpSize(image.defaultWidth, image.defaultHeight),
attributes: Attributes = Attributes.EMPTY, attributes: Attributes = Attributes.EMPTY,
id: String? = null, id: String? = null,
): FeatureRef<T, VectorImageFeature<T>> = ): FeatureRef<T, VectorIconFeature<T>> =
feature( feature(
id, id,
VectorImageFeature( VectorIconFeature(
space, space,
position, position,
size, size,

View File

@ -3,10 +3,7 @@ package center.sciprog.maps.features
import androidx.compose.ui.geometry.Offset import androidx.compose.ui.geometry.Offset
import androidx.compose.ui.geometry.Size import androidx.compose.ui.geometry.Size
import androidx.compose.ui.graphics.* import androidx.compose.ui.graphics.*
import androidx.compose.ui.graphics.drawscope.DrawScope import androidx.compose.ui.graphics.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.painter.Painter import androidx.compose.ui.graphics.painter.Painter
import center.sciprog.attributes.plus import center.sciprog.attributes.plus
import org.jetbrains.skia.Font 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 offset = feature.center.toOffset()
val size = feature.size.toSize() val size = feature.size.toSize()
translate(offset.x - size.width / 2, offset.y - size.height / 2) { translate(offset.x - size.width / 2, offset.y - size.height / 2) {

View 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)
}

View File

@ -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)
}

View File

@ -1,11 +1,14 @@
plugins { plugins {
kotlin("multiplatform") id("space.kscience.gradle.mpp")
id("org.jetbrains.compose") id("org.jetbrains.compose")
`maven-publish` `maven-publish`
} }
kotlin { kscience{
jvm() jvm()
}
kotlin {
sourceSets { sourceSets {
commonMain { commonMain {
dependencies { dependencies {

View File

@ -5,11 +5,12 @@ import androidx.compose.ui.unit.dp
import center.sciprog.maps.features.CoordinateSpace import center.sciprog.maps.features.CoordinateSpace
import center.sciprog.maps.features.Rectangle import center.sciprog.maps.features.Rectangle
import center.sciprog.maps.features.ViewPoint import center.sciprog.maps.features.ViewPoint
import space.kscience.kmath.geometry.Vector2D
import kotlin.math.abs import kotlin.math.abs
import kotlin.math.max import kotlin.math.max
import kotlin.math.min 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( internal data class XYRectangle(
override val a: XY, override val a: XY,
@ -28,21 +29,21 @@ internal data class XYRectangle(
// } // }
} }
val Rectangle<XY>.top get() = max(a.y, b.y) public val Rectangle<XY>.top: Float get() = max(a.y, b.y)
val Rectangle<XY>.bottom get() = min(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) public val Rectangle<XY>.right: Float get() = max(a.x, b.x)
val Rectangle<XY>.left get() = min(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) public 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>.height: Float get() = abs(a.y - b.y)
public val Rectangle<XY>.leftTop: XY get() = XY(left, top) public val Rectangle<XY>.leftTop: XY get() = XY(left, top)
public val Rectangle<XY>.rightBottom: XY get() = XY(right, bottom) public val Rectangle<XY>.rightBottom: XY get() = XY(right, bottom)
internal val defaultCanvasSize = DpSize(512.dp, 512.dp) internal val defaultCanvasSize = DpSize(512.dp, 512.dp)
data class XYViewPoint( public data class XYViewPoint(
override val focus: XY, override val focus: XY,
override val zoom: Float = 1f, override val zoom: Float = 1f,
) : ViewPoint<XY> ) : ViewPoint<XY>

View File

@ -9,7 +9,7 @@ import center.sciprog.maps.features.ViewPoint
import kotlin.math.abs import kotlin.math.abs
import kotlin.math.pow import kotlin.math.pow
object XYCoordinateSpace : CoordinateSpace<XY> { public object XYCoordinateSpace : CoordinateSpace<XY> {
override fun Rectangle(first: XY, second: XY): Rectangle<XY> = override fun Rectangle(first: XY, second: XY): Rectangle<XY> =
XYRectangle(first, second) XYRectangle(first, second)

View File

@ -9,7 +9,7 @@ import androidx.compose.ui.unit.dp
import center.sciprog.maps.features.* import center.sciprog.maps.features.*
import kotlin.math.min import kotlin.math.min
class XYViewScope( public class XYViewScope(
config: ViewConfig<XY>, config: ViewConfig<XY>,
) : CoordinateViewScope<XY>(config) { ) : CoordinateViewScope<XY>(config) {
override val space: CoordinateSpace<XY> override val space: CoordinateSpace<XY>
@ -45,7 +45,7 @@ class XYViewScope(
return DpRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y) return DpRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
} }
companion object{ public companion object{
@Composable @Composable
public fun remember( public fun remember(
config: ViewConfig<XY> = ViewConfig(), config: ViewConfig<XY> = ViewConfig(),

View File

@ -15,7 +15,7 @@ import kotlin.math.ceil
internal fun Pair<Number, Number>.toCoordinates(): XY = XY(first.toFloat(), second.toFloat()) internal fun Pair<Number, Number>.toCoordinates(): XY = XY(first.toFloat(), second.toFloat())
fun FeatureGroup<XY>.background( public fun FeatureGroup<XY>.background(
width: Float, width: Float,
height: Float, height: Float,
offset: XY = XY(0f, 0f), 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>, centerCoordinates: Pair<Number, Number>,
size: Dp = 5.dp, size: Dp = 5.dp,
id: String? = null, id: String? = null,
): FeatureRef<XY, CircleFeature<XY>> = circle(centerCoordinates.toCoordinates(), size, id = id) ): FeatureRef<XY, CircleFeature<XY>> = circle(centerCoordinates.toCoordinates(), size, id = id)
fun FeatureGroup<XY>.draw( public fun FeatureGroup<XY>.draw(
position: Pair<Number, Number>, position: Pair<Number, Number>,
id: String? = null, id: String? = null,
draw: DrawScope.() -> Unit, draw: DrawScope.() -> Unit,
): FeatureRef<XY, DrawFeature<XY>> = draw(position.toCoordinates(), id = id, draw = draw) ): FeatureRef<XY, DrawFeature<XY>> = draw(position.toCoordinates(), id = id, draw = draw)
fun FeatureGroup<XY>.line( public fun FeatureGroup<XY>.line(
aCoordinates: Pair<Number, Number>, aCoordinates: Pair<Number, Number>,
bCoordinates: Pair<Number, Number>, bCoordinates: Pair<Number, Number>,
id: String? = null, id: String? = null,
@ -69,15 +69,15 @@ public fun FeatureGroup<XY>.arc(
id = id id = id
) )
fun FeatureGroup<XY>.image( public fun FeatureGroup<XY>.image(
position: Pair<Number, Number>, position: Pair<Number, Number>,
image: ImageVector, image: ImageVector,
size: DpSize = DpSize(image.defaultWidth, image.defaultHeight), size: DpSize = DpSize(image.defaultWidth, image.defaultHeight),
id: String? = null, id: String? = null,
): FeatureRef<XY, VectorImageFeature<XY>> = ): FeatureRef<XY, VectorIconFeature<XY>> =
image(position.toCoordinates(), image, size = size, id = id) icon(position.toCoordinates(), image, size = size, id = id)
fun FeatureGroup<XY>.text( public fun FeatureGroup<XY>.text(
position: Pair<Number, Number>, position: Pair<Number, Number>,
text: String, text: String,
id: String? = null, id: String? = null,

View File

@ -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 offset = feature.center.toOffset()
val imageSize = feature.size.toSize() val imageSize = feature.size.toSize()
translate(offset.x - imageSize.width / 2, offset.y - imageSize.height / 2) { translate(offset.x - imageSize.width / 2, offset.y - imageSize.height / 2) {

View File

@ -48,6 +48,7 @@ include(
":trajectory-kt", ":trajectory-kt",
":maps-kt-core", ":maps-kt-core",
":maps-kt-geojson", ":maps-kt-geojson",
// ":maps-kt-geotiff",
":maps-kt-features", ":maps-kt-features",
":maps-kt-compose", ":maps-kt-compose",
":maps-kt-scheme", ":maps-kt-scheme",

View File

@ -35,85 +35,40 @@ private fun TangentPath(vararg tangents: Tangent) = TangentPath(listOf(*tangents
* Create inner and outer tangents between two circles. * Create inner and outer tangents between two circles.
* This method returns a map of segments using [DubinsPath] connection type notation. * This method returns a map of segments using [DubinsPath] connection type notation.
*/ */
internal fun Circle2D.tangentsToCircle( internal fun tangentsToCircle(
other: Circle2D, first: Circle2D,
second: Circle2D,
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) { ): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
//return empty map for concentric circles //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 // A line connecting centers
val line = LineSegment(center, other.center) val line = LineSegment(first.center, second.center)
// Distance between centers // Distance between centers
val distance = line.begin.distanceTo(line.end) 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 var angle2: Double
val routes = mapOf( return listOf(
DubinsPath.Type.RSR to Pair(radius, other.radius), DubinsPath.Type.RSR,
DubinsPath.Type.RSL to Pair(radius, -other.radius), DubinsPath.Type.RSL,
DubinsPath.Type.LSR to Pair(-radius, other.radius), DubinsPath.Type.LSR,
DubinsPath.Type.LSL to Pair(-radius, -other.radius) DubinsPath.Type.LSL
) ).associateWith { route ->
return buildMap { val r1 = when (route.first) {
for ((route, r1r2) in routes) { Trajectory2D.L -> -first.radius
val r1 = r1r2.first Trajectory2D.R -> first.radius
val r2 = r1r2.second }
val r2 = when (route.third) {
Trajectory2D.L -> -second.radius
Trajectory2D.R -> second.radius
}
val r = if (r1.sign == r2.sign) { val r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue r1.absoluteValue - r2.absoluteValue
} else { } else {
r1.absoluteValue + r2.absoluteValue r1.absoluteValue + r2.absoluteValue
} }
if (distance * distance >= r * r) { if (distance * distance < r * r) error("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))
put(
route,
LineSegment(
center + w * r1,
other.center + w * r2
)
)
} else {
throw Exception("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) val l = sqrt(distance * distance - r * r)
angle2 = if (r1.absoluteValue > r2.absoluteValue) { angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l) angle1 + r1.sign * atan2(r.absoluteValue, l)
@ -121,27 +76,72 @@ private fun dubinsTangentsToCircles(
angle1 - r2.sign * atan2(r.absoluteValue, l) angle1 - r2.sign * atan2(r.absoluteValue, l)
} }
val w = vector(-cos(angle2), sin(angle2)) val w = vector(-cos(angle2), sin(angle2))
put(
route, LineSegment(
Tangent( first.center + w * r1,
startCircle = Circle2D(firstCircle.center, firstCircle.radius), second.center + w * r2
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")
}
}
} }
} }
//
//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( internal class Obstacle(
public val circles: List<Circle2D>, 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 { private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, Tangent> = buildMap {
for (circle1 in first.circles) { for (circle1 in first.circles) {
for (circle2 in second.circles) { for (circle2 in second.circles) {
for (tangent in dubinsTangentsToCircles(circle1, circle2, first, second)) { for ((pathType, segment) in tangentsToCircle(circle1, circle2)) {
if (!(tangent.value.intersectObstacle(first)) val tangent = Tangent(
and !(tangent.value.intersectObstacle(second)) circle1,
circle2,
first,
second,
segment,
pathType.first,
pathType.third
)
if (!(tangent.intersectObstacle(first))
and !(tangent.intersectObstacle(second))
) { ) {
put( put(
tangent.key, pathType,
tangent.value tangent
) )
} }
} }
@ -506,7 +516,7 @@ internal fun findAllPaths(
val currentObstacle = tangentPath.last().endObstacle val currentObstacle = tangentPath.last().endObstacle
var nextObstacle: Obstacle? = null var nextObstacle: Obstacle? = null
if (currentObstacle != finalObstacle) { if (currentObstacle != finalObstacle) {
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type( val tangentToFinal: Tangent? = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
currentDirection, currentDirection,
Trajectory2D.S, Trajectory2D.S,
j j

View File

@ -44,7 +44,7 @@ class TangentTest {
) )
) )
val tangentMap = c1.tangentsToCircle(c2) val tangentMap = tangentsToCircle(c1, c2)
val tangentMapKeys = tangentMap.keys.toList() val tangentMapKeys = tangentMap.keys.toList()
val tangentMapValues = tangentMap.values.toList() val tangentMapValues = tangentMap.values.toList()
@ -58,6 +58,6 @@ class TangentTest {
fun concentric(){ fun concentric(){
val c1 = Circle2D(vector(0.0, 0.0), 10.0) val c1 = Circle2D(vector(0.0, 0.0), 10.0)
val c2 = Circle2D(vector(0.0, 0.0), 1.0) val c2 = Circle2D(vector(0.0, 0.0), 1.0)
assertEquals(emptyMap(), c1.tangentsToCircle(c2)) assertEquals(emptyMap(), tangentsToCircle(c1, c2))
} }
} }