forked from kscience/kmath
search for shortest path algorithm
This commit is contained in:
Artyom Degtyarev
parent
cc0fb2a718
commit
2c13386646
@ -6,6 +6,7 @@
|
||||
package space.kscience.kmath.geometry
|
||||
|
||||
import kotlinx.serialization.Serializable
|
||||
import space.kscience.kmath.operations.DoubleField.pow
|
||||
|
||||
/**
|
||||
* A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized,
|
||||
|
||||
@ -0,0 +1,413 @@
|
||||
/*
|
||||
* Copyright 2018-2023 KMath contributors.
|
||||
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
|
||||
*/
|
||||
|
||||
package space.kscience.kmath.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.minus
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.times
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.norm
|
||||
import space.kscience.kmath.operations.DoubleField.pow
|
||||
import kotlin.math.*
|
||||
|
||||
public fun LineSegment2D.length(): Double {
|
||||
return ((end.y - begin.y).pow(2.0) + (end.x - begin.x).pow(2.0)).pow(0.5)
|
||||
}
|
||||
public class DubinsObstacle(
|
||||
public val circles: List<Circle2D>
|
||||
) {
|
||||
public val tangents: List<DubinsTangent> = boundaryTangents().first
|
||||
public val boundaryRoute: DubinsPath.Type = boundaryTangents().second
|
||||
public val center: Vector2D<Double> =
|
||||
vector(this.circles.sumOf{it.center.x} / this.circles.size,
|
||||
this.circles.sumOf{it.center.y} / this.circles.size)
|
||||
private fun boundaryTangents(): Pair<List<DubinsTangent>, DubinsPath.Type> {
|
||||
// outer tangents for a polygon circles can be either lsl or rsr
|
||||
|
||||
fun Circle2D.dubinsTangentsToCircles(
|
||||
other: Circle2D,
|
||||
): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
|
||||
val line = LineSegment(center, other.center)
|
||||
val d = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
|
||||
var r: Double
|
||||
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
|
||||
r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
if (d * d > r * r) {
|
||||
val l = (d * d - r * r).pow(0.5)
|
||||
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, DubinsTangent(Circle2D(center, radius),
|
||||
other,
|
||||
this@DubinsObstacle,
|
||||
this@DubinsObstacle,
|
||||
LineSegment2D(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
val firstCircles = this.circles.slice(-this.circles.size..-1)
|
||||
val secondCircles = this.circles.slice(-this.circles.size+1..0)
|
||||
val lslTangents = firstCircles.zip(secondCircles)
|
||||
{a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.LSL]!!}
|
||||
val rsrTangents = firstCircles.zip(secondCircles)
|
||||
{a, b -> a.dubinsTangentsToCircles(b)[DubinsPath.Type.RSR]!!}
|
||||
val center = vector(
|
||||
this.circles.sumOf { it.center.x } / this.circles.size,
|
||||
this.circles.sumOf { it.center.y } / this.circles.size
|
||||
)
|
||||
val lslToCenter = lslTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
|
||||
lslTangents.sumOf { it.lineSegment.end.distanceTo(center) }
|
||||
val rsrToCenter = rsrTangents.sumOf { it.lineSegment.begin.distanceTo(center) } +
|
||||
rsrTangents.sumOf { it.lineSegment.end.distanceTo(center) }
|
||||
return if (rsrToCenter >= lslToCenter) {
|
||||
Pair(rsrTangents, DubinsPath.Type.RSR)
|
||||
} else {
|
||||
Pair(lslTangents, DubinsPath.Type.LSL)
|
||||
}
|
||||
}
|
||||
|
||||
public fun nextTangent(circle: Circle2D, route: DubinsPath.Type): DubinsTangent {
|
||||
if (route == this.boundaryRoute) {
|
||||
for (i in this.circles.indices) {
|
||||
if (this.circles[i] == circle) {
|
||||
return this.tangents[i]
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (i in this.circles.indices) {
|
||||
if (this.circles[i] == circle) {
|
||||
return DubinsTangent(this.circles[i],
|
||||
this.circles[i-1],
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(this.tangents[i-1].lineSegment.end,
|
||||
this.tangents[i-1].lineSegment.begin),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
error("next tangent not found")
|
||||
}
|
||||
|
||||
public fun equals(other: DubinsObstacle): Boolean {
|
||||
return this.circles == other.circles
|
||||
}
|
||||
}
|
||||
|
||||
public data class DubinsTangent(val startCircle: Circle2D,
|
||||
val endCircle: Circle2D,
|
||||
val startObstacle: DubinsObstacle,
|
||||
val endObstacle: DubinsObstacle,
|
||||
val lineSegment: LineSegment2D,
|
||||
val route: PathTypes)
|
||||
|
||||
public fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
|
||||
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
|
||||
return v1.x * v2.y - v1.y * v2.x
|
||||
}
|
||||
if (crossProduct(other.begin - this.begin, other.end - this.begin).sign ==
|
||||
crossProduct(other.begin - this.end, other.end - this.end).sign) {
|
||||
return false
|
||||
}
|
||||
if (crossProduct(this.begin - other.begin, this.end - other.begin).sign ==
|
||||
crossProduct(this.begin - other.end, this.end - other.end).sign) {
|
||||
return false
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
public fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
|
||||
val a = (this.begin.x - this.end.x).pow(2.0) + (this.begin.y - this.end.y).pow(2.0)
|
||||
val b = 2 * ((this.begin.x - this.end.x) * (this.end.x - circle.center.x) +
|
||||
(this.begin.y - this.end.y) * (this.end.y - circle.center.y))
|
||||
val c = (this.end.x - circle.center.x).pow(2.0) + (this.end.y - circle.center.y) -
|
||||
circle.radius.pow(2.0)
|
||||
val d = b.pow(2.0) - 4 * a * c
|
||||
if (d < 1e-6) {
|
||||
return false
|
||||
}
|
||||
else {
|
||||
val t1 = (-b - d.pow(0.5)) * 0.5 / a
|
||||
val t2 = (-b + d.pow(0.5)) * 0.5 / a
|
||||
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
public fun DubinsTangent.intersectObstacle(obstacle: DubinsObstacle): Boolean {
|
||||
for (tangent in obstacle.tangents) {
|
||||
if (this.lineSegment.intersectSegment(tangent.lineSegment)) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
for (circle in obstacle.circles) {
|
||||
if (this.lineSegment.intersectCircle(circle)) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
public fun outerTangents(first: DubinsObstacle, second: DubinsObstacle): MutableMap<DubinsPath.Type, DubinsTangent> {
|
||||
return 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))) {
|
||||
put(
|
||||
tangent.key,
|
||||
tangent.value
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}.toMutableMap()
|
||||
}
|
||||
|
||||
public fun arcLength(circle: Circle2D,
|
||||
point1: DoubleVector2D,
|
||||
point2: DoubleVector2D,
|
||||
route: DubinsPath.SimpleType): Double {
|
||||
val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
|
||||
val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
|
||||
var angle = 0.0
|
||||
when (route) {
|
||||
DubinsPath.SimpleType.L -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
phi2 - phi1
|
||||
} else {
|
||||
2 * PI + phi2 - phi1
|
||||
}
|
||||
}
|
||||
DubinsPath.SimpleType.R -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
2 * PI - (phi2 - phi1)
|
||||
} else {
|
||||
-(phi2 - phi1)
|
||||
}
|
||||
}
|
||||
DubinsPath.SimpleType.S -> {
|
||||
error("L or R route is expected")
|
||||
}
|
||||
}
|
||||
return circle.radius * angle
|
||||
}
|
||||
|
||||
public fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
|
||||
return Pair(
|
||||
r * vector(v.y / norm(v), -v.x / norm(v)),
|
||||
r * vector(-v.y / norm(v), v.x / norm(v))
|
||||
)
|
||||
}
|
||||
|
||||
public fun constructTangentCircles(point: DoubleVector2D,
|
||||
direction: DoubleVector2D,
|
||||
r: Double): Map<DubinsPath.SimpleType, Circle2D> {
|
||||
val center1 = point + normalVectors(direction, r).first
|
||||
val center2 = point + normalVectors(direction, r).second
|
||||
val p1 = center1 - point
|
||||
val p2 = center2 - point
|
||||
return if (atan2(p1.y, p1.x) - atan2(p2.y, p2.x) in listOf(PI/2, -3*PI/2)) {
|
||||
mapOf(DubinsPath.SimpleType.L to Circle2D(center1, r),
|
||||
DubinsPath.SimpleType.R to Circle2D(center2, r))
|
||||
}
|
||||
else {
|
||||
mapOf(DubinsPath.SimpleType.L to Circle2D(center2, r),
|
||||
DubinsPath.SimpleType.R to Circle2D(center1, r))
|
||||
}
|
||||
}
|
||||
|
||||
public fun sortedObstacles(currentObstacle: DubinsObstacle,
|
||||
obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
|
||||
return obstacles.sortedBy {norm(it.center - currentObstacle.center)}.reversed()
|
||||
}
|
||||
|
||||
public fun tangentsAlongTheObstacle(initialCircle: Circle2D,
|
||||
initialRoute: DubinsPath.Type,
|
||||
finalCircle: Circle2D,
|
||||
obstacle: DubinsObstacle): MutableList<DubinsTangent> {
|
||||
val dubinsTangents = mutableListOf<DubinsTangent>()
|
||||
var tangent = obstacle.nextTangent(initialCircle, initialRoute)
|
||||
dubinsTangents.add(tangent)
|
||||
while (tangent.endCircle != finalCircle) {
|
||||
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute)
|
||||
dubinsTangents.add(tangent)
|
||||
}
|
||||
return dubinsTangents
|
||||
}
|
||||
|
||||
public fun allFinished(paths: List<List<DubinsTangent>>,
|
||||
finalObstacle: DubinsObstacle): Boolean {
|
||||
for (path in paths) {
|
||||
if (path[-1].endObstacle != finalObstacle) {
|
||||
return false
|
||||
}
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
public fun pathLength(path: List<DubinsTangent>): Double {
|
||||
val tangentsLength = path.sumOf{norm(it.lineSegment.end - it.lineSegment.begin)}
|
||||
val arcsLength = buildList<Double>{
|
||||
for (i in 1..path.size) {
|
||||
add(arcLength(path[i].startCircle,
|
||||
path[i-1].lineSegment.end,
|
||||
path[i].lineSegment.begin,
|
||||
path[i].route[0]))
|
||||
}
|
||||
}.sum()
|
||||
return tangentsLength + arcsLength
|
||||
}
|
||||
|
||||
public fun shortestPath(path: List<List<DubinsTangent>>): List<List<DubinsTangent>> {
|
||||
return path.sortedBy { pathLength(it) }
|
||||
}
|
||||
|
||||
public typealias Path = List<DubinsTangent>
|
||||
public fun findAllPaths(
|
||||
startingPoint: DoubleVector2D,
|
||||
startingDirection: DoubleVector2D,
|
||||
startingRadius: Double,
|
||||
finalPoint: DoubleVector2D,
|
||||
finalDirection: DoubleVector2D,
|
||||
finalRadius: Double,
|
||||
obstacles: List<DubinsObstacle>
|
||||
) {
|
||||
val initialCircles = constructTangentCircles(
|
||||
startingPoint,
|
||||
startingDirection,
|
||||
startingRadius)
|
||||
val finalCircles = constructTangentCircles(
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius)
|
||||
var outputTangents = mutableMapOf<PathTypes, List<Path>>()
|
||||
for (i in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
|
||||
for (j in listOf(DubinsPath.SimpleType.L, DubinsPath.SimpleType.R)) {
|
||||
val finalCircle = finalCircles[j]!!
|
||||
val finalObstacle = DubinsObstacle(listOf(finalCircle))
|
||||
outputTangents[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)] = listOf(
|
||||
listOf(DubinsTangent(
|
||||
initialCircles[i]!!,
|
||||
initialCircles[i]!!,
|
||||
DubinsObstacle(listOf(initialCircles[i]!!)),
|
||||
DubinsObstacle(listOf(initialCircles[i]!!)),
|
||||
LineSegment2D(startingPoint, startingPoint),
|
||||
listOf(i, DubinsPath.SimpleType.S, i)
|
||||
)))
|
||||
var currentObstacle = DubinsObstacle(listOf(initialCircles[i]!!))
|
||||
while (!allFinished(outputTangents[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)]!!, finalObstacle)) {
|
||||
var newOutputTangents = listOf<Path>()
|
||||
for (line in outputTangents[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)]!!) {
|
||||
var currentCircle = line[-1].endCircle
|
||||
var currentDirection = line[-1].route[-1]
|
||||
var currentObstacle = line[-1].endObstacle
|
||||
var nextObstacle = DubinsObstacle(listOf())
|
||||
if (currentObstacle != finalObstacle) {
|
||||
var tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.toType(listOf(
|
||||
currentDirection,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)
|
||||
)]
|
||||
for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
|
||||
if (tangentToFinal!!.intersectObstacle(obstacle)) {
|
||||
nextObstacle = obstacle
|
||||
break
|
||||
}
|
||||
}
|
||||
if (nextObstacle == DubinsObstacle(listOf())) {
|
||||
nextObstacle = finalObstacle
|
||||
}
|
||||
var nextTangents = outerTangents(currentObstacle, nextObstacle)
|
||||
// for (pathType in listOf(
|
||||
// listOf(DubinsPath.SimpleType.L,
|
||||
// DubinsPath.SimpleType.S,
|
||||
// DubinsPath.SimpleType.L),
|
||||
// listOf(DubinsPath.SimpleType.L,
|
||||
// DubinsPath.SimpleType.S,
|
||||
// DubinsPath.SimpleType.R),
|
||||
// listOf(DubinsPath.SimpleType.R,
|
||||
// DubinsPath.SimpleType.S,
|
||||
// DubinsPath.SimpleType.L),
|
||||
// listOf(DubinsPath.SimpleType.R,
|
||||
// DubinsPath.SimpleType.S,
|
||||
// DubinsPath.SimpleType.R)
|
||||
// )) {
|
||||
for (pathType in nextTangents.keys) {
|
||||
for (obstacle in obstacles) {
|
||||
// in Python code here try/except was used, but seems unneeded
|
||||
if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
|
||||
nextTangents.remove(pathType)
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
if (nextObstacle == finalObstacle) {
|
||||
nextTangents =
|
||||
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
|
||||
(DubinsPath.toSimpleTypes(it.key)[0] == j)}
|
||||
as MutableMap<DubinsPath.Type, DubinsTangent>
|
||||
}
|
||||
else {
|
||||
nextTangents =
|
||||
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
|
||||
as MutableMap<DubinsPath.Type, DubinsTangent>
|
||||
}
|
||||
TODO("rewrite fragment from Python")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -77,10 +77,118 @@ private fun innerTangent(
|
||||
@Suppress("DuplicatedCode")
|
||||
public object DubinsPath {
|
||||
|
||||
// public class ArcType(private val type: Type){
|
||||
// public val first: SimpleType
|
||||
// get() {
|
||||
// if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) {
|
||||
// return SimpleType.R
|
||||
// }
|
||||
// else if (type in listOf(Type.LSL, Type.LSR, Type.LRL)) {
|
||||
// return SimpleType.L
|
||||
// }
|
||||
// error("Wrong DubinsPath.Type")
|
||||
// }
|
||||
//
|
||||
// public val last: SimpleType
|
||||
// get() {
|
||||
// if (type in listOf(Type.RSR, Type.LSR, Type.RLR)) {
|
||||
// return SimpleType.R
|
||||
// }
|
||||
// else if (type in listOf(Type.LSL, Type.RSL, Type.LRL)) {
|
||||
// return SimpleType.L
|
||||
// }
|
||||
// error("Wrong DubinsPath.Type")
|
||||
// }
|
||||
// public val intermediate: SimpleType
|
||||
// get() {
|
||||
// if (type == Type.RLR) {
|
||||
// return SimpleType.L
|
||||
// }
|
||||
// else if (type == Type.LRL) {
|
||||
// return SimpleType.R
|
||||
// }
|
||||
// error("This DubinsPath.Type doesn't contain intermediate arc")
|
||||
// }
|
||||
// }
|
||||
|
||||
public enum class SimpleType {
|
||||
R, S, L
|
||||
}
|
||||
|
||||
public enum class Type {
|
||||
RLR, LRL, RSR, LSL, RSL, LSR
|
||||
}
|
||||
|
||||
public fun toSimpleTypes(type: Type): List<SimpleType> {
|
||||
when (type) {
|
||||
Type.RLR -> {
|
||||
return listOf(SimpleType.R, SimpleType.L, SimpleType.R)
|
||||
}
|
||||
Type.LRL -> {
|
||||
return listOf(SimpleType.L, SimpleType.R, SimpleType.L)
|
||||
}
|
||||
Type.RSR -> {
|
||||
return listOf(SimpleType.R, SimpleType.S, SimpleType.R)
|
||||
}
|
||||
Type.LSL -> {
|
||||
return listOf(SimpleType.L, SimpleType.S, SimpleType.L)
|
||||
}
|
||||
Type.RSL -> {
|
||||
return listOf(SimpleType.R, SimpleType.S, SimpleType.L)
|
||||
}
|
||||
Type.LSR -> {
|
||||
return listOf(SimpleType.L, SimpleType.S, SimpleType.R)
|
||||
}
|
||||
else -> error("This type doesn't exist")
|
||||
}
|
||||
}
|
||||
|
||||
public fun toType(types: List<SimpleType>): Type {
|
||||
when (types) {
|
||||
listOf(SimpleType.R, SimpleType.L, SimpleType.R) -> {
|
||||
return Type.RLR
|
||||
}
|
||||
listOf(SimpleType.L, SimpleType.R, SimpleType.L) -> {
|
||||
return Type.LRL
|
||||
}
|
||||
listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
|
||||
return Type.RSR
|
||||
}
|
||||
listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
|
||||
return Type.LSL
|
||||
}
|
||||
listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
|
||||
return Type.RSL
|
||||
}
|
||||
listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
|
||||
return Type.LSR
|
||||
}
|
||||
else -> error("This type doesn't exist")
|
||||
}
|
||||
}
|
||||
|
||||
// public class PathTypes(private val inputTypes: List<SimpleType>) {
|
||||
// public val type: Type
|
||||
// get() {
|
||||
// when (this.inputTypes) {
|
||||
// listOf(SimpleType.R, SimpleType.S, SimpleType.R) -> {
|
||||
// return Type.RSR
|
||||
// }
|
||||
// listOf(SimpleType.R, SimpleType.S, SimpleType.L) -> {
|
||||
// return Type.RSL
|
||||
// }
|
||||
// listOf(SimpleType.L, SimpleType.S, SimpleType.R) -> {
|
||||
// return Type.LSR
|
||||
// }
|
||||
// listOf(SimpleType.L, SimpleType.S, SimpleType.L) -> {
|
||||
// return Type.LSL
|
||||
// }
|
||||
// else -> error("Wrong list of SimpleTypes")
|
||||
// }
|
||||
// }
|
||||
// public val chain: List<SimpleType> = this.inputTypes
|
||||
// }
|
||||
|
||||
/**
|
||||
* Return Dubins trajectory type or null if trajectory is not a Dubins path
|
||||
*/
|
||||
@ -243,6 +351,8 @@ public object DubinsPath {
|
||||
}
|
||||
}
|
||||
|
||||
public typealias PathTypes = List<DubinsPath.SimpleType>
|
||||
|
||||
public fun interface MaxCurvature {
|
||||
public fun compute(startPoint: PhaseVector2D): Double
|
||||
}
|
||||
|
||||
@ -5,10 +5,10 @@
|
||||
|
||||
package space.kscience.kmath.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.DoubleVector2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.LineSegment
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.times
|
||||
import space.kscience.kmath.operations.DoubleField.pow
|
||||
import kotlin.math.*
|
||||
|
||||
/**
|
||||
@ -17,7 +17,7 @@ import kotlin.math.*
|
||||
*/
|
||||
public fun Circle2D.tangentsToCircle(
|
||||
other: Circle2D,
|
||||
): Map<DubinsPath.Type, LineSegment<DoubleVector2D>> = with(Euclidean2DSpace) {
|
||||
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
|
||||
val line = LineSegment(center, other.center)
|
||||
val d = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
|
||||
@ -48,14 +48,66 @@ public fun Circle2D.tangentsToCircle(
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
put(
|
||||
route,
|
||||
LineSegment(
|
||||
LineSegment2D(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
)
|
||||
)
|
||||
}
|
||||
else {
|
||||
throw Exception("Circles should not be")
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public fun dubinsTangentsToCircles(
|
||||
firstCircle: Circle2D,
|
||||
secondCircle: Circle2D,
|
||||
firstObstacle: DubinsObstacle,
|
||||
secondObstacle: DubinsObstacle
|
||||
): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
|
||||
val line = LineSegment(firstCircle.center, secondCircle.center)
|
||||
val d = 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, 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 (d * d > r * r) {
|
||||
val l = (d * d - r * r).pow(0.5)
|
||||
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
|
||||
put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
|
||||
secondCircle,
|
||||
firstObstacle,
|
||||
secondObstacle,
|
||||
LineSegment2D(
|
||||
firstCircle.center + w * r1,
|
||||
secondCircle.center + w * r2
|
||||
),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user