forked from kscience/kmath
Disentangle obstacle code phase 1
This commit is contained in:
parent
d08424428e
commit
f809e40791
@ -1,519 +0,0 @@
|
||||
/*
|
||||
* 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.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
|
||||
val secondCircles = this.circles.slice(1..this.circles.lastIndex) +
|
||||
this.circles[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) {
|
||||
if (i > 0) {
|
||||
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))
|
||||
}
|
||||
else {
|
||||
return DubinsTangent(this.circles[0],
|
||||
this.circles.last(),
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(this.tangents.last().lineSegment.end,
|
||||
this.tangents.last().lineSegment.begin),
|
||||
DubinsPath.toSimpleTypes(route))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
error("next tangent not found")
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (other == null || other !is DubinsObstacle) return false
|
||||
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)
|
||||
|
||||
private 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
|
||||
}
|
||||
|
||||
private 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).pow(2.0) -
|
||||
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
|
||||
}
|
||||
|
||||
private 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
|
||||
}
|
||||
|
||||
private 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()
|
||||
}
|
||||
|
||||
private 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
|
||||
}
|
||||
|
||||
private 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))
|
||||
)
|
||||
}
|
||||
|
||||
private 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
|
||||
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.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))
|
||||
}
|
||||
}
|
||||
|
||||
private fun sortedObstacles(currentObstacle: DubinsObstacle,
|
||||
obstacles: List<DubinsObstacle>): List<DubinsObstacle> {
|
||||
return obstacles.sortedBy {norm(it.center - currentObstacle.center)}//.reversed()
|
||||
}
|
||||
|
||||
private 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
|
||||
}
|
||||
|
||||
private fun allFinished(paths: List<List<DubinsTangent>>,
|
||||
finalObstacle: DubinsObstacle): Boolean {
|
||||
for (path in paths) {
|
||||
if (path.last().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.lastIndex) {
|
||||
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<DubinsTangent> {
|
||||
return path.sortedBy { pathLength(it) }[0]
|
||||
}
|
||||
|
||||
public typealias Path = List<DubinsTangent>
|
||||
public fun findAllPaths(
|
||||
startingPoint: DoubleVector2D,
|
||||
startingDirection: DoubleVector2D,
|
||||
startingRadius: Double,
|
||||
finalPoint: DoubleVector2D,
|
||||
finalDirection: DoubleVector2D,
|
||||
finalRadius: Double,
|
||||
obstacles: List<DubinsObstacle>
|
||||
): List<MutableList<DubinsTangent>> {
|
||||
val initialCircles = constructTangentCircles(
|
||||
startingPoint,
|
||||
startingDirection,
|
||||
startingRadius)
|
||||
val finalCircles = constructTangentCircles(
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius)
|
||||
var path = mutableMapOf<PathTypes, MutableList<MutableList<DubinsTangent>>>()
|
||||
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))
|
||||
path[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)] = mutableListOf(
|
||||
mutableListOf(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(path[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)]!!, finalObstacle)) {
|
||||
var newPaths = mutableListOf<MutableList<DubinsTangent>>()
|
||||
for (line in path[listOf(i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j)]!!) {
|
||||
var currentCircle = line.last().endCircle
|
||||
var currentDirection = line.last().route.last()
|
||||
var currentObstacle = line.last().endObstacle
|
||||
var nextObstacle: DubinsObstacle? = null
|
||||
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 == null) {
|
||||
nextObstacle = finalObstacle
|
||||
}
|
||||
var nextTangents = outerTangents(currentObstacle, nextObstacle)
|
||||
|
||||
for (pathType in DubinsPath.Type.values()) {
|
||||
for (obstacle in obstacles) {
|
||||
// in Python code here try/except was used, but seems unneeded
|
||||
if (nextTangents.containsKey(pathType)) {
|
||||
if (nextTangents[pathType]!!.intersectObstacle(obstacle)) {
|
||||
nextTangents.remove(pathType)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
nextTangents = if (nextObstacle == finalObstacle) {
|
||||
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection) and
|
||||
(DubinsPath.toSimpleTypes(it.key)[2] == j)}
|
||||
as MutableMap<DubinsPath.Type, DubinsTangent>
|
||||
} else {
|
||||
nextTangents.filter {(DubinsPath.toSimpleTypes(it.key)[0] == currentDirection)}
|
||||
as MutableMap<DubinsPath.Type, DubinsTangent>
|
||||
}
|
||||
var tangentsAlong = mutableListOf<DubinsTangent>()
|
||||
for (tangent in nextTangents.values) {
|
||||
if (tangent.startCircle == line.last().endCircle) {
|
||||
val lengthMaxPossible = arcLength(
|
||||
tangent.startCircle,
|
||||
line.last().lineSegment.end,
|
||||
tangent.startObstacle.nextTangent(
|
||||
tangent.startCircle,
|
||||
DubinsPath.toType(listOf(currentDirection, DubinsPath.SimpleType.S, currentDirection)),
|
||||
).lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
val lengthCalculated = arcLength(
|
||||
tangent.startCircle,
|
||||
line.last().lineSegment.end,
|
||||
tangent.lineSegment.begin,
|
||||
currentDirection)
|
||||
if (lengthCalculated > lengthMaxPossible) {
|
||||
tangentsAlong = tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
DubinsPath.toType(listOf(
|
||||
currentDirection,
|
||||
DubinsPath.SimpleType.S,
|
||||
currentDirection)),
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
}
|
||||
else {
|
||||
tangentsAlong = mutableListOf<DubinsTangent>()
|
||||
}
|
||||
}
|
||||
else {
|
||||
tangentsAlong = tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
DubinsPath.toType(listOf(
|
||||
currentDirection,
|
||||
DubinsPath.SimpleType.S,
|
||||
currentDirection)),
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
}
|
||||
newPaths.add((line + tangentsAlong + listOf(tangent)).toMutableList())
|
||||
}
|
||||
path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)] = newPaths
|
||||
}
|
||||
else {
|
||||
// minor changes from Python code
|
||||
newPaths.add(line)
|
||||
path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)] = newPaths
|
||||
}
|
||||
}
|
||||
path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)] = newPaths
|
||||
}
|
||||
for (lineId in path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)]!!.indices) {
|
||||
val lastDirection = path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)]!![lineId].last().route[2]
|
||||
path[listOf(
|
||||
i,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)]!![lineId].add(DubinsTangent(
|
||||
finalCircles[j]!!,
|
||||
finalCircles[j]!!,
|
||||
DubinsObstacle(
|
||||
listOf(finalCircles[j]!!)
|
||||
),
|
||||
DubinsObstacle(
|
||||
listOf(finalCircles[j]!!)
|
||||
),
|
||||
LineSegment2D(finalPoint, finalPoint),
|
||||
listOf(
|
||||
lastDirection,
|
||||
DubinsPath.SimpleType.S,
|
||||
j
|
||||
)
|
||||
))
|
||||
}
|
||||
}
|
||||
}
|
||||
return path[listOf(
|
||||
DubinsPath.SimpleType.L,
|
||||
DubinsPath.SimpleType.S,
|
||||
DubinsPath.SimpleType.L
|
||||
)]!! + path[listOf(
|
||||
DubinsPath.SimpleType.L,
|
||||
DubinsPath.SimpleType.S,
|
||||
DubinsPath.SimpleType.R
|
||||
)]!! + path[listOf(
|
||||
DubinsPath.SimpleType.R,
|
||||
DubinsPath.SimpleType.S,
|
||||
DubinsPath.SimpleType.L
|
||||
)]!! + path[listOf(
|
||||
DubinsPath.SimpleType.R,
|
||||
DubinsPath.SimpleType.S,
|
||||
DubinsPath.SimpleType.R)]!!
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -7,6 +7,8 @@ package space.kscience.kmath.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import space.kscience.kmath.trajectory.Trajectory2D.Type
|
||||
import space.kscience.kmath.trajectory.Trajectory2D.Type.*
|
||||
import kotlin.math.acos
|
||||
|
||||
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
||||
@ -19,27 +21,21 @@ internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circ
|
||||
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
||||
}
|
||||
|
||||
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D =
|
||||
outerTangent(a, b, CircleTrajectory2D.Direction.LEFT)
|
||||
|
||||
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D = outerTangent(
|
||||
a, b,
|
||||
CircleTrajectory2D.Direction.RIGHT
|
||||
)
|
||||
|
||||
private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D =
|
||||
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): StraightTrajectory2D =
|
||||
with(Euclidean2DSpace) {
|
||||
val centers = StraightTrajectory2D(a.center, b.center)
|
||||
val p1 = when (side) {
|
||||
CircleTrajectory2D.Direction.LEFT -> vector(
|
||||
a.center.x - a.radius * cos(centers.bearing),
|
||||
a.center.y + a.radius * sin(centers.bearing)
|
||||
val centers = StraightTrajectory2D(from.center, to.center)
|
||||
val p1 = when (direction) {
|
||||
L -> vector(
|
||||
from.center.x - from.radius * cos(centers.bearing),
|
||||
from.center.y + from.radius * sin(centers.bearing)
|
||||
)
|
||||
|
||||
CircleTrajectory2D.Direction.RIGHT -> vector(
|
||||
a.center.x + a.radius * cos(centers.bearing),
|
||||
a.center.y - a.radius * sin(centers.bearing)
|
||||
R -> vector(
|
||||
from.center.x + from.radius * cos(centers.bearing),
|
||||
from.center.y - from.radius * sin(centers.bearing)
|
||||
)
|
||||
|
||||
else -> error("S trajectory type not allowed")
|
||||
}
|
||||
return StraightTrajectory2D(
|
||||
p1,
|
||||
@ -47,29 +43,25 @@ private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory2D.Dire
|
||||
)
|
||||
}
|
||||
|
||||
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
|
||||
innerTangent(base, direction, CircleTrajectory2D.Direction.LEFT)
|
||||
|
||||
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
|
||||
innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT)
|
||||
|
||||
private fun innerTangent(
|
||||
base: Circle2D,
|
||||
direction: Circle2D,
|
||||
side: CircleTrajectory2D.Direction,
|
||||
from: Circle2D,
|
||||
to: Circle2D,
|
||||
direction: Type,
|
||||
): StraightTrajectory2D? =
|
||||
with(Euclidean2DSpace) {
|
||||
val centers = StraightTrajectory2D(base.center, direction.center)
|
||||
if (centers.length < base.radius * 2) return null
|
||||
val angle = when (side) {
|
||||
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians
|
||||
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians
|
||||
val centers = StraightTrajectory2D(from.center, to.center)
|
||||
if (centers.length < from.radius * 2) return null
|
||||
val angle = when (direction) {
|
||||
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
|
||||
R -> centers.bearing - acos(from.radius * 2 / centers.length).radians
|
||||
else -> error("S trajectory type not allowed")
|
||||
}.normalized()
|
||||
|
||||
val dX = base.radius * sin(angle)
|
||||
val dY = base.radius * cos(angle)
|
||||
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
||||
val p2 = vector(direction.center.x - dX, direction.center.y - dY)
|
||||
val dX = from.radius * sin(angle)
|
||||
val dY = from.radius * cos(angle)
|
||||
val p1 = vector(from.center.x + dX, from.center.y + dY)
|
||||
val p2 = vector(to.center.x - dX, to.center.y - dY)
|
||||
return StraightTrajectory2D(p1, p2)
|
||||
}
|
||||
|
||||
@ -77,118 +69,25 @@ 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 data class Type(
|
||||
public val first: Trajectory2D.Type,
|
||||
public val second: Trajectory2D.Type,
|
||||
public val third: Trajectory2D.Type,
|
||||
) {
|
||||
public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
|
||||
|
||||
public enum class SimpleType {
|
||||
R, S, L
|
||||
}
|
||||
override fun toString(): String = "${first.name}${second.name}${third.name}"
|
||||
|
||||
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 companion object {
|
||||
public val RLR: Type = Type(R, L, R)
|
||||
public val LRL: Type = Type(L, R, L)
|
||||
public val RSR: Type = Type(R, S, R)
|
||||
public val LSL: Type = Type(L, S, L)
|
||||
public val RSL: Type = Type(R, S, L)
|
||||
public val LSR: Type = Type(L, S, R)
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
*/
|
||||
@ -197,12 +96,10 @@ public object DubinsPath {
|
||||
val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
|
||||
val b = trajectory2D.segments[1]
|
||||
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
||||
return Type.valueOf(
|
||||
arrayOf(
|
||||
a.direction.name[0],
|
||||
if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
|
||||
c.direction.name[0]
|
||||
).toCharArray().concatToString()
|
||||
return Type(
|
||||
a.direction,
|
||||
if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S,
|
||||
c.direction
|
||||
)
|
||||
}
|
||||
|
||||
@ -240,9 +137,9 @@ public object DubinsPath {
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -257,9 +154,9 @@ public object DubinsPath {
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -284,9 +181,9 @@ public object DubinsPath {
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -301,9 +198,9 @@ public object DubinsPath {
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -313,45 +210,45 @@ public object DubinsPath {
|
||||
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
||||
val c1 = start.getRightCircle(turningRadius)
|
||||
val c2 = end.getRightCircle(turningRadius)
|
||||
val s = leftOuterTangent(c1, c2)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
||||
val s = outerTangent(c1, c2, L)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
||||
return CompositeTrajectory2D(a1, s, a3)
|
||||
}
|
||||
|
||||
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
||||
val c1 = start.getLeftCircle(turningRadius)
|
||||
val c2 = end.getLeftCircle(turningRadius)
|
||||
val s = rightOuterTangent(c1, c2)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
||||
val s = outerTangent(c1, c2, R)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
||||
return CompositeTrajectory2D(a1, s, a3)
|
||||
}
|
||||
|
||||
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
||||
val c1 = start.getRightCircle(turningRadius)
|
||||
val c2 = end.getLeftCircle(turningRadius)
|
||||
val s = rightInnerTangent(c1, c2)
|
||||
val s = innerTangent(c1, c2, R)
|
||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
||||
return CompositeTrajectory2D(a1, s, a3)
|
||||
}
|
||||
|
||||
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
||||
val c1 = start.getLeftCircle(turningRadius)
|
||||
val c2 = end.getRightCircle(turningRadius)
|
||||
val s = leftInnerTangent(c1, c2)
|
||||
val s = innerTangent(c1, c2, L)
|
||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
||||
return CompositeTrajectory2D(a1, s, a3)
|
||||
}
|
||||
}
|
||||
|
||||
public typealias PathTypes = List<DubinsPath.SimpleType>
|
||||
public typealias PathTypes = List<Type>
|
||||
|
||||
public fun interface MaxCurvature {
|
||||
public fun compute(startPoint: PhaseVector2D): Double
|
||||
|
||||
@ -0,0 +1,619 @@
|
||||
/*
|
||||
* 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.norm
|
||||
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.operations.DoubleField.pow
|
||||
import kotlin.math.*
|
||||
|
||||
internal data class Tangent(
|
||||
val startCircle: Circle2D,
|
||||
val endCircle: Circle2D,
|
||||
val startObstacle: Obstacle,
|
||||
val endObstacle: Obstacle,
|
||||
val lineSegment: LineSegment2D,
|
||||
val trajectoryType: List<Trajectory2D.Type>,
|
||||
)
|
||||
|
||||
private class TangentPath(val tangents: List<Tangent>) {
|
||||
fun last() = tangents.last()
|
||||
}
|
||||
|
||||
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,
|
||||
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
|
||||
//return empty map for concentric circles
|
||||
if (center.equalsVector(other.center)) return emptyMap()
|
||||
|
||||
// A line connecting centers
|
||||
val line = LineSegment(center, other.center)
|
||||
// Distance between centers
|
||||
val distance = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(other.center.x - center.x, other.center.y - 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 = Euclidean2DSpace.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)
|
||||
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 = LineSegment2D(
|
||||
firstCircle.center + w * r1,
|
||||
secondCircle.center + w * r2
|
||||
),
|
||||
trajectoryType = route.toList()
|
||||
)
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public class Obstacle(
|
||||
public val circles: List<Circle2D>,
|
||||
) {
|
||||
internal val tangents: List<Tangent> = boundaryTangents().first
|
||||
public val boundaryRoute: DubinsPath.Type = boundaryTangents().second
|
||||
|
||||
public val center: Vector2D<Double> = vector(
|
||||
circles.sumOf { it.center.x } / circles.size,
|
||||
circles.sumOf { it.center.y } / circles.size
|
||||
)
|
||||
|
||||
private fun boundaryTangents(): Pair<List<Tangent>, DubinsPath.Type> {
|
||||
// outer tangents for a polygon circles can be either lsl or rsr
|
||||
|
||||
fun Circle2D.dubinsTangentsToCircles(
|
||||
other: Circle2D,
|
||||
): Map<DubinsPath.Type, Tangent> = 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.LSL to Pair(-radius, -other.radius)
|
||||
)
|
||||
return buildMap {
|
||||
for ((routeType, 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(
|
||||
routeType, Tangent(
|
||||
Circle2D(center, radius),
|
||||
other,
|
||||
this@Obstacle,
|
||||
this@Obstacle,
|
||||
LineSegment2D(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
),
|
||||
routeType.toList()
|
||||
)
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
val firstCircles = circles
|
||||
val secondCircles = circles.slice(1..circles.lastIndex) +
|
||||
circles[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(
|
||||
circles.sumOf { it.center.x } / circles.size,
|
||||
circles.sumOf { it.center.y } / 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)
|
||||
}
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (other == null || other !is Obstacle) return false
|
||||
return circles == other.circles
|
||||
}
|
||||
|
||||
override fun hashCode(): Int {
|
||||
return circles.hashCode()
|
||||
}
|
||||
}
|
||||
|
||||
private fun Obstacle.nextTangent(circle: Circle2D, routeType: DubinsPath.Type): Tangent {
|
||||
if (routeType == boundaryRoute) {
|
||||
for (i in circles.indices) {
|
||||
if (circles[i] == circle) {
|
||||
return tangents[i]
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (i in circles.indices) {
|
||||
if (circles[i] == circle) {
|
||||
if (i > 0) {
|
||||
return Tangent(
|
||||
circles[i],
|
||||
circles[i - 1],
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(
|
||||
tangents[i - 1].lineSegment.end,
|
||||
tangents[i - 1].lineSegment.begin
|
||||
),
|
||||
routeType.toList()
|
||||
)
|
||||
} else {
|
||||
return Tangent(
|
||||
circles[0],
|
||||
circles.last(),
|
||||
this,
|
||||
this,
|
||||
LineSegment2D(
|
||||
tangents.last().lineSegment.end,
|
||||
tangents.last().lineSegment.begin
|
||||
),
|
||||
routeType.toList()
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
error("next tangent not found")
|
||||
}
|
||||
|
||||
public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles))
|
||||
|
||||
private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean {
|
||||
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
|
||||
return v1.x * v2.y - v1.y * v2.x
|
||||
}
|
||||
return if (crossProduct(other.begin - begin, other.end - begin).sign ==
|
||||
crossProduct(other.begin - end, other.end - end).sign
|
||||
) {
|
||||
false
|
||||
} else {
|
||||
crossProduct(begin - other.begin, end - other.begin).sign != crossProduct(
|
||||
begin - other.end,
|
||||
end - other.end
|
||||
).sign
|
||||
}
|
||||
}
|
||||
|
||||
private fun LineSegment2D.intersectCircle(circle: Circle2D): Boolean {
|
||||
val a = (begin.x - end.x).pow(2.0) + (begin.y - end.y).pow(2.0)
|
||||
val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
|
||||
(begin.y - end.y) * (end.y - circle.center.y))
|
||||
val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
|
||||
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
|
||||
}
|
||||
|
||||
private fun Tangent.intersectObstacle(obstacle: Obstacle): Boolean {
|
||||
for (tangent in obstacle.tangents) {
|
||||
if (lineSegment.intersectSegment(tangent.lineSegment)) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
for (circle in obstacle.circles) {
|
||||
if (lineSegment.intersectCircle(circle)) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
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))
|
||||
) {
|
||||
put(
|
||||
tangent.key,
|
||||
tangent.value
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private fun arcLength(
|
||||
circle: Circle2D,
|
||||
point1: DoubleVector2D,
|
||||
point2: DoubleVector2D,
|
||||
route: Trajectory2D.Type,
|
||||
): 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) {
|
||||
Trajectory2D.Type.L -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
phi2 - phi1
|
||||
} else {
|
||||
2 * PI + phi2 - phi1
|
||||
}
|
||||
}
|
||||
|
||||
Trajectory2D.Type.R -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
2 * PI - (phi2 - phi1)
|
||||
} else {
|
||||
-(phi2 - phi1)
|
||||
}
|
||||
}
|
||||
|
||||
Trajectory2D.Type.S -> {
|
||||
error("L or R route is expected")
|
||||
}
|
||||
}
|
||||
return circle.radius * angle
|
||||
}
|
||||
|
||||
private 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))
|
||||
)
|
||||
}
|
||||
|
||||
private fun constructTangentCircles(
|
||||
point: DoubleVector2D,
|
||||
direction: DoubleVector2D,
|
||||
r: Double,
|
||||
): Map<Trajectory2D.Type, Circle2D> {
|
||||
val center1 = point + normalVectors(direction, r).first
|
||||
val center2 = point + normalVectors(direction, r).second
|
||||
val p1 = center1 - point
|
||||
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
|
||||
mapOf(
|
||||
Trajectory2D.Type.L to Circle2D(center1, r),
|
||||
Trajectory2D.Type.R to Circle2D(center2, r)
|
||||
)
|
||||
} else {
|
||||
mapOf(
|
||||
Trajectory2D.Type.L to Circle2D(center2, r),
|
||||
Trajectory2D.Type.R to Circle2D(center1, r)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
private fun sortedObstacles(
|
||||
currentObstacle: Obstacle,
|
||||
obstacles: List<Obstacle>,
|
||||
): List<Obstacle> {
|
||||
return obstacles.sortedBy { norm(it.center - currentObstacle.center) }//.reversed()
|
||||
}
|
||||
|
||||
private fun tangentsAlongTheObstacle(
|
||||
initialCircle: Circle2D,
|
||||
initialRoute: DubinsPath.Type,
|
||||
finalCircle: Circle2D,
|
||||
obstacle: Obstacle,
|
||||
): List<Tangent> {
|
||||
val dubinsTangents = mutableListOf<Tangent>()
|
||||
var tangent = obstacle.nextTangent(initialCircle, initialRoute)
|
||||
dubinsTangents.add(tangent)
|
||||
while (tangent.endCircle != finalCircle) {
|
||||
tangent = obstacle.nextTangent(tangent.endCircle, initialRoute)
|
||||
dubinsTangents.add(tangent)
|
||||
}
|
||||
return dubinsTangents
|
||||
}
|
||||
|
||||
private fun allFinished(
|
||||
paths: List<TangentPath>,
|
||||
finalObstacle: Obstacle,
|
||||
): Boolean {
|
||||
for (path in paths) {
|
||||
if (path.last().endObstacle != finalObstacle) {
|
||||
return false
|
||||
}
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
|
||||
|
||||
|
||||
private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
|
||||
buildList {
|
||||
tangents.zipWithNext().forEach { (left, right) ->
|
||||
add(left.lineSegment.toTrajectory())
|
||||
add(
|
||||
CircleTrajectory2D.of(
|
||||
right.startCircle.center,
|
||||
left.lineSegment.end,
|
||||
right.lineSegment.begin,
|
||||
right.trajectoryType.first()
|
||||
)
|
||||
)
|
||||
}
|
||||
|
||||
add(tangents.last().lineSegment.toTrajectory())
|
||||
}
|
||||
)
|
||||
|
||||
internal fun findAllPaths(
|
||||
startingPoint: DoubleVector2D,
|
||||
startingDirection: DoubleVector2D,
|
||||
startingRadius: Double,
|
||||
finalPoint: DoubleVector2D,
|
||||
finalDirection: DoubleVector2D,
|
||||
finalRadius: Double,
|
||||
obstacles: List<Obstacle>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val initialCircles = constructTangentCircles(
|
||||
startingPoint,
|
||||
startingDirection,
|
||||
startingRadius
|
||||
)
|
||||
val finalCircles = constructTangentCircles(
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius
|
||||
)
|
||||
val trajectories = mutableListOf<CompositeTrajectory2D>()
|
||||
for (i in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) {
|
||||
for (j in listOf(Trajectory2D.Type.L, Trajectory2D.Type.R)) {
|
||||
val finalCircle = finalCircles[j]!!
|
||||
val finalObstacle = Obstacle(listOf(finalCircle))
|
||||
var currentPaths: List<TangentPath> = listOf(
|
||||
TangentPath(
|
||||
Tangent(
|
||||
initialCircles[i]!!,
|
||||
initialCircles[i]!!,
|
||||
Obstacle(listOf(initialCircles[i]!!)),
|
||||
Obstacle(listOf(initialCircles[i]!!)),
|
||||
LineSegment2D(startingPoint, startingPoint),
|
||||
listOf(i, Trajectory2D.Type.S, i)
|
||||
)
|
||||
)
|
||||
)
|
||||
while (!allFinished(currentPaths, finalObstacle)) {
|
||||
val newPaths = mutableListOf<TangentPath>()
|
||||
for (tangentPath: TangentPath in currentPaths) {
|
||||
val currentCircle = tangentPath.last().endCircle
|
||||
val currentDirection = tangentPath.last().trajectoryType.last()
|
||||
val currentObstacle = tangentPath.last().endObstacle
|
||||
var nextObstacle: Obstacle? = null
|
||||
if (currentObstacle != finalObstacle) {
|
||||
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.Type.S,
|
||||
j
|
||||
)]
|
||||
for (obstacle in sortedObstacles(currentObstacle, obstacles)) {
|
||||
if (tangentToFinal!!.intersectObstacle(obstacle)) {
|
||||
nextObstacle = obstacle
|
||||
break
|
||||
}
|
||||
}
|
||||
if (nextObstacle == null) {
|
||||
nextObstacle = finalObstacle
|
||||
}
|
||||
val nextTangents: Map<DubinsPath.Type, Tangent> = outerTangents(currentObstacle, nextObstacle)
|
||||
.filter { (key, tangent) ->
|
||||
obstacles.none { obstacle -> tangent.intersectObstacle(obstacle) } &&
|
||||
key.first == currentDirection &&
|
||||
(nextObstacle != finalObstacle || key.third == j)
|
||||
}
|
||||
|
||||
var tangentsAlong: List<Tangent>
|
||||
for (tangent in nextTangents.values) {
|
||||
if (tangent.startCircle == tangentPath.last().endCircle) {
|
||||
val lengthMaxPossible = arcLength(
|
||||
tangent.startCircle,
|
||||
tangentPath.last().lineSegment.end,
|
||||
tangent.startObstacle.nextTangent(
|
||||
tangent.startCircle,
|
||||
DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.Type.S,
|
||||
currentDirection
|
||||
),
|
||||
).lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
val lengthCalculated = arcLength(
|
||||
tangent.startCircle,
|
||||
tangentPath.last().lineSegment.end,
|
||||
tangent.lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
|
||||
tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.Type.S,
|
||||
currentDirection
|
||||
),
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
} else {
|
||||
emptyList()
|
||||
}
|
||||
} else {
|
||||
tangentsAlong = tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.Type.S,
|
||||
currentDirection
|
||||
),
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
}
|
||||
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
||||
}
|
||||
} else {
|
||||
// minor changes from Python code
|
||||
newPaths.add(tangentPath)
|
||||
}
|
||||
}
|
||||
currentPaths = newPaths
|
||||
}
|
||||
|
||||
trajectories += currentPaths.map { tangentPath ->
|
||||
val lastDirection: Trajectory2D.Type = tangentPath.last().trajectoryType[2]
|
||||
val end = finalCircles[j]!!
|
||||
TangentPath(
|
||||
tangentPath.tangents +
|
||||
Tangent(
|
||||
end,
|
||||
end,
|
||||
Obstacle(end),
|
||||
Obstacle(end),
|
||||
LineSegment2D(finalPoint, finalPoint),
|
||||
listOf(
|
||||
lastDirection,
|
||||
Trajectory2D.Type.S,
|
||||
j
|
||||
)
|
||||
)
|
||||
)
|
||||
}.map { it.toTrajectory() }
|
||||
}
|
||||
}
|
||||
return trajectories
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -16,6 +16,12 @@ import kotlin.math.atan2
|
||||
@Serializable
|
||||
public sealed interface Trajectory2D {
|
||||
public val length: Double
|
||||
|
||||
public enum class Type {
|
||||
R,
|
||||
S,
|
||||
L
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -27,11 +33,14 @@ public data class StraightTrajectory2D(
|
||||
public val start: DoubleVector2D,
|
||||
public val end: DoubleVector2D,
|
||||
) : Trajectory2D {
|
||||
|
||||
override val length: Double get() = start.distanceTo(end)
|
||||
|
||||
public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized()
|
||||
}
|
||||
|
||||
public fun StraightTrajectory2D.toSegment(): LineSegment<Vector2D<Double>> = LineSegment2D(start, end)
|
||||
|
||||
/**
|
||||
* An arc segment
|
||||
*/
|
||||
@ -43,15 +52,11 @@ public data class CircleTrajectory2D(
|
||||
public val end: DubinsPose2D,
|
||||
) : Trajectory2D {
|
||||
|
||||
public enum class Direction {
|
||||
LEFT, RIGHT
|
||||
}
|
||||
|
||||
/**
|
||||
* Arc length in radians
|
||||
*/
|
||||
val arcLength: Angle
|
||||
get() = if (direction == Direction.LEFT) {
|
||||
get() = if (direction == Trajectory2D.Type.L) {
|
||||
start.bearing - end.bearing
|
||||
} else {
|
||||
end.bearing - start.bearing
|
||||
@ -62,16 +67,16 @@ public data class CircleTrajectory2D(
|
||||
circle.radius * arcLength.radians
|
||||
}
|
||||
|
||||
public val direction: Direction by lazy {
|
||||
public val direction: Trajectory2D.Type by lazy {
|
||||
if (start.y < circle.center.y) {
|
||||
if (start.bearing > Angle.pi) Direction.RIGHT else Direction.LEFT
|
||||
if (start.bearing > Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
|
||||
} else if (start.y > circle.center.y) {
|
||||
if (start.bearing < Angle.pi) Direction.RIGHT else Direction.LEFT
|
||||
if (start.bearing < Angle.pi) Trajectory2D.Type.R else Trajectory2D.Type.L
|
||||
} else {
|
||||
if (start.bearing == Angle.zero) {
|
||||
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||
if (start.x < circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
|
||||
} else {
|
||||
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||
if (start.x > circle.center.x) Trajectory2D.Type.R else Trajectory2D.Type.L
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -81,17 +86,18 @@ public data class CircleTrajectory2D(
|
||||
center: DoubleVector2D,
|
||||
start: DoubleVector2D,
|
||||
end: DoubleVector2D,
|
||||
direction: Direction,
|
||||
direction: Trajectory2D.Type,
|
||||
): CircleTrajectory2D {
|
||||
fun calculatePose(
|
||||
vector: DoubleVector2D,
|
||||
theta: Angle,
|
||||
direction: Direction,
|
||||
direction: Trajectory2D.Type,
|
||||
): DubinsPose2D = DubinsPose2D(
|
||||
vector,
|
||||
when (direction) {
|
||||
Direction.LEFT -> (theta - Angle.piDiv2).normalized()
|
||||
Direction.RIGHT -> (theta + Angle.piDiv2).normalized()
|
||||
Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized()
|
||||
Trajectory2D.Type.R -> (theta + Angle.piDiv2).normalized()
|
||||
else -> error("S trajectory type is not allowed in circle constructor")
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
@ -1,114 +0,0 @@
|
||||
/*
|
||||
* 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 kotlin.math.*
|
||||
|
||||
/**
|
||||
* Create inner and outer tangents between two circles.
|
||||
* This method returns a map of segments using [DubinsPath] connection type notation.
|
||||
*/
|
||||
public fun Circle2D.tangentsToCircle(
|
||||
other: Circle2D,
|
||||
): Map<DubinsPath.Type, LineSegment<DoubleVector2D>> = with(Euclidean2DSpace) {
|
||||
//return empty map for concentric circles
|
||||
if(center.equalsVector(other.center)) return@tangentsToCircle emptyMap()
|
||||
|
||||
// A line connecting centers
|
||||
val line = LineSegment(center, other.center)
|
||||
// Distance between centers
|
||||
val distance = line.begin.distanceTo(line.end)
|
||||
val angle1 = atan2(other.center.x - center.x, other.center.y - 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 = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
|
||||
put(
|
||||
route,
|
||||
LineSegment(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
)
|
||||
)
|
||||
} else {
|
||||
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 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, 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, 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")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,13 +1,12 @@
|
||||
/*
|
||||
* Copyright 2018-2022 KMath contributors.
|
||||
* 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.dubins
|
||||
package space.kscience.kmath.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.equalsFloat
|
||||
import space.kscience.kmath.trajectory.*
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertNotNull
|
||||
import kotlin.test.assertTrue
|
||||
@ -37,7 +36,7 @@ class DubinsTests {
|
||||
)
|
||||
|
||||
expectedLengths.forEach {
|
||||
val path = dubins.find { p -> DubinsPath.trajectoryTypeOf(p) === it.key }
|
||||
val path = dubins.find { p -> DubinsPath.trajectoryTypeOf(p) == it.key }
|
||||
assertNotNull(path, "Path ${it.key} not found")
|
||||
println("${it.key}: ${path.length}")
|
||||
assertTrue(it.value.equalsFloat(path.length))
|
||||
@ -7,12 +7,10 @@ package space.kscience.kmath.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||
import space.kscience.kmath.geometry.equalsFloat
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
import kotlin.test.assertTrue
|
||||
|
||||
class DubinsTest {
|
||||
class ObstacleTest {
|
||||
@Test
|
||||
fun firstPath() {
|
||||
val startPoint = vector(-5.0, -1.0)
|
||||
@ -22,8 +20,13 @@ class DubinsTest {
|
||||
val finalDirection = vector(1.0, -1.0)
|
||||
val finalRadius = 0.5
|
||||
|
||||
val obstacles = listOf(DubinsObstacle(listOf(
|
||||
Circle2D(vector(7.0, 1.0), 5.0))))
|
||||
val obstacles = listOf(
|
||||
Obstacle(
|
||||
listOf(
|
||||
Circle2D(vector(7.0, 1.0), 5.0)
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
val outputTangents = findAllPaths(
|
||||
startPoint,
|
||||
@ -32,8 +35,9 @@ class DubinsTest {
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius,
|
||||
obstacles)
|
||||
val length = pathLength(shortestPath(outputTangents))
|
||||
obstacles
|
||||
)
|
||||
val length = outputTangents.minOf { it.length }
|
||||
assertEquals(length, 27.2113183, 1e-6)
|
||||
}
|
||||
|
||||
@ -47,17 +51,21 @@ class DubinsTest {
|
||||
val finalRadius = 0.5
|
||||
|
||||
val obstacles = listOf(
|
||||
DubinsObstacle(listOf(
|
||||
Obstacle(
|
||||
listOf(
|
||||
Circle2D(vector(1.0, 6.5), 0.5),
|
||||
Circle2D(vector(2.0, 1.0), 0.5),
|
||||
Circle2D(vector(6.0, 0.0), 0.5),
|
||||
Circle2D(vector(5.0, 5.0), 0.5)
|
||||
)), DubinsObstacle(listOf(
|
||||
)
|
||||
), Obstacle(
|
||||
listOf(
|
||||
Circle2D(vector(10.0, 1.0), 0.5),
|
||||
Circle2D(vector(16.0, 0.0), 0.5),
|
||||
Circle2D(vector(14.0, 6.0), 0.5),
|
||||
Circle2D(vector(9.0, 4.0), 0.5)
|
||||
))
|
||||
)
|
||||
)
|
||||
)
|
||||
val paths = findAllPaths(
|
||||
startPoint,
|
||||
@ -66,22 +74,19 @@ class DubinsTest {
|
||||
finalPoint,
|
||||
finalDirection,
|
||||
finalRadius,
|
||||
obstacles)
|
||||
val length = pathLength(shortestPath(paths))
|
||||
obstacles
|
||||
)
|
||||
val length = paths.minOf { it.length }
|
||||
assertEquals(length, 28.9678224, 1e-6)
|
||||
}
|
||||
@Test
|
||||
fun equalCircles() {
|
||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
println(circle1 == circle2)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun equalObstacles() {
|
||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val obstacle1 = DubinsObstacle(listOf(circle1))
|
||||
val obstacle2 = DubinsObstacle(listOf(circle2))
|
||||
println(obstacle1 == obstacle2)
|
||||
assertEquals(circle1, circle2)
|
||||
val obstacle1 = Obstacle(listOf(circle1))
|
||||
val obstacle2 = Obstacle(listOf(circle2))
|
||||
assertEquals(obstacle1, obstacle2)
|
||||
}
|
||||
}
|
||||
@ -10,6 +10,7 @@ import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.circumference
|
||||
import space.kscience.kmath.geometry.degrees
|
||||
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
||||
import space.kscience.kmath.trajectory.Trajectory2D
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
@ -22,7 +23,7 @@ class ArcTests {
|
||||
circle.center,
|
||||
vector(-2.0, 0.0),
|
||||
vector(0.0, 2.0),
|
||||
CircleTrajectory2D.Direction.RIGHT
|
||||
Trajectory2D.Type.R
|
||||
)
|
||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||
assertEquals(0.0, arc.start.bearing.degrees)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user