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.*
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
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
|
import kotlin.math.acos
|
||||||
|
|
||||||
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
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)
|
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
||||||
}
|
}
|
||||||
|
|
||||||
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D =
|
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Type): 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 =
|
|
||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
val centers = StraightTrajectory2D(a.center, b.center)
|
val centers = StraightTrajectory2D(from.center, to.center)
|
||||||
val p1 = when (side) {
|
val p1 = when (direction) {
|
||||||
CircleTrajectory2D.Direction.LEFT -> vector(
|
L -> vector(
|
||||||
a.center.x - a.radius * cos(centers.bearing),
|
from.center.x - from.radius * cos(centers.bearing),
|
||||||
a.center.y + a.radius * sin(centers.bearing)
|
from.center.y + from.radius * sin(centers.bearing)
|
||||||
)
|
)
|
||||||
|
|
||||||
CircleTrajectory2D.Direction.RIGHT -> vector(
|
R -> vector(
|
||||||
a.center.x + a.radius * cos(centers.bearing),
|
from.center.x + from.radius * cos(centers.bearing),
|
||||||
a.center.y - a.radius * sin(centers.bearing)
|
from.center.y - from.radius * sin(centers.bearing)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
else -> error("S trajectory type not allowed")
|
||||||
}
|
}
|
||||||
return StraightTrajectory2D(
|
return StraightTrajectory2D(
|
||||||
p1,
|
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(
|
private fun innerTangent(
|
||||||
base: Circle2D,
|
from: Circle2D,
|
||||||
direction: Circle2D,
|
to: Circle2D,
|
||||||
side: CircleTrajectory2D.Direction,
|
direction: Type,
|
||||||
): StraightTrajectory2D? =
|
): StraightTrajectory2D? =
|
||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
val centers = StraightTrajectory2D(base.center, direction.center)
|
val centers = StraightTrajectory2D(from.center, to.center)
|
||||||
if (centers.length < base.radius * 2) return null
|
if (centers.length < from.radius * 2) return null
|
||||||
val angle = when (side) {
|
val angle = when (direction) {
|
||||||
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians
|
L -> centers.bearing + acos(from.radius * 2 / centers.length).radians
|
||||||
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians
|
R -> centers.bearing - acos(from.radius * 2 / centers.length).radians
|
||||||
|
else -> error("S trajectory type not allowed")
|
||||||
}.normalized()
|
}.normalized()
|
||||||
|
|
||||||
val dX = base.radius * sin(angle)
|
val dX = from.radius * sin(angle)
|
||||||
val dY = base.radius * cos(angle)
|
val dY = from.radius * cos(angle)
|
||||||
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
val p1 = vector(from.center.x + dX, from.center.y + dY)
|
||||||
val p2 = vector(direction.center.x - dX, direction.center.y - dY)
|
val p2 = vector(to.center.x - dX, to.center.y - dY)
|
||||||
return StraightTrajectory2D(p1, p2)
|
return StraightTrajectory2D(p1, p2)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -77,118 +69,25 @@ private fun innerTangent(
|
|||||||
@Suppress("DuplicatedCode")
|
@Suppress("DuplicatedCode")
|
||||||
public object DubinsPath {
|
public object DubinsPath {
|
||||||
|
|
||||||
// public class ArcType(private val type: Type){
|
public data class Type(
|
||||||
// public val first: SimpleType
|
public val first: Trajectory2D.Type,
|
||||||
// get() {
|
public val second: Trajectory2D.Type,
|
||||||
// if (this.type in listOf(Type.RSR, Type.RSL, Type.RLR)) {
|
public val third: Trajectory2D.Type,
|
||||||
// return SimpleType.R
|
) {
|
||||||
// }
|
public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
|
||||||
// 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 {
|
override fun toString(): String = "${first.name}${second.name}${third.name}"
|
||||||
R, S, L
|
|
||||||
}
|
|
||||||
|
|
||||||
public enum class Type {
|
public companion object {
|
||||||
RLR, LRL, RSR, LSL, RSL, LSR
|
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 fun toSimpleTypes(type: Type): List<SimpleType> {
|
public val LSL: Type = Type(L, S, L)
|
||||||
when (type) {
|
public val RSL: Type = Type(R, S, L)
|
||||||
Type.RLR -> {
|
public val LSR: Type = Type(L, S, R)
|
||||||
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
|
* 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 a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
|
||||||
val b = trajectory2D.segments[1]
|
val b = trajectory2D.segments[1]
|
||||||
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
||||||
return Type.valueOf(
|
return Type(
|
||||||
arrayOf(
|
a.direction,
|
||||||
a.direction.name[0],
|
if (b is CircleTrajectory2D) b.direction else Trajectory2D.Type.S,
|
||||||
if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
|
c.direction
|
||||||
c.direction.name[0]
|
|
||||||
).toCharArray().concatToString()
|
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -240,9 +137,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -257,9 +154,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.R)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.R)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -284,9 +181,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -301,9 +198,9 @@ public object DubinsPath {
|
|||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, Trajectory2D.Type.L)
|
||||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, Trajectory2D.Type.R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, Trajectory2D.Type.L)
|
||||||
CompositeTrajectory2D(a1, a2, a3)
|
CompositeTrajectory2D(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -313,45 +210,45 @@ public object DubinsPath {
|
|||||||
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
||||||
val c1 = start.getRightCircle(turningRadius)
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
val c2 = end.getRightCircle(turningRadius)
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
val s = leftOuterTangent(c1, c2)
|
val s = outerTangent(c1, c2, L)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
||||||
val c1 = start.getLeftCircle(turningRadius)
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
val c2 = end.getLeftCircle(turningRadius)
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
val s = rightOuterTangent(c1, c2)
|
val s = outerTangent(c1, c2, R)
|
||||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
||||||
val c1 = start.getRightCircle(turningRadius)
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
val c2 = end.getLeftCircle(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
|
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 a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.R)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.L)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? {
|
||||||
val c1 = start.getLeftCircle(turningRadius)
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
val c2 = end.getRightCircle(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
|
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 a1 = CircleTrajectory2D.of(c1.center, start, s.start, Trajectory2D.Type.L)
|
||||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, Trajectory2D.Type.R)
|
||||||
return CompositeTrajectory2D(a1, s, a3)
|
return CompositeTrajectory2D(a1, s, a3)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public typealias PathTypes = List<DubinsPath.SimpleType>
|
public typealias PathTypes = List<Type>
|
||||||
|
|
||||||
public fun interface MaxCurvature {
|
public fun interface MaxCurvature {
|
||||||
public fun compute(startPoint: PhaseVector2D): Double
|
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
|
@Serializable
|
||||||
public sealed interface Trajectory2D {
|
public sealed interface Trajectory2D {
|
||||||
public val length: Double
|
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 start: DoubleVector2D,
|
||||||
public val end: DoubleVector2D,
|
public val end: DoubleVector2D,
|
||||||
) : Trajectory2D {
|
) : Trajectory2D {
|
||||||
|
|
||||||
override val length: Double get() = start.distanceTo(end)
|
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 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
|
* An arc segment
|
||||||
*/
|
*/
|
||||||
@ -43,15 +52,11 @@ public data class CircleTrajectory2D(
|
|||||||
public val end: DubinsPose2D,
|
public val end: DubinsPose2D,
|
||||||
) : Trajectory2D {
|
) : Trajectory2D {
|
||||||
|
|
||||||
public enum class Direction {
|
|
||||||
LEFT, RIGHT
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Arc length in radians
|
* Arc length in radians
|
||||||
*/
|
*/
|
||||||
val arcLength: Angle
|
val arcLength: Angle
|
||||||
get() = if (direction == Direction.LEFT) {
|
get() = if (direction == Trajectory2D.Type.L) {
|
||||||
start.bearing - end.bearing
|
start.bearing - end.bearing
|
||||||
} else {
|
} else {
|
||||||
end.bearing - start.bearing
|
end.bearing - start.bearing
|
||||||
@ -62,16 +67,16 @@ public data class CircleTrajectory2D(
|
|||||||
circle.radius * arcLength.radians
|
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.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) {
|
} 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 {
|
} else {
|
||||||
if (start.bearing == Angle.zero) {
|
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 {
|
} 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,
|
center: DoubleVector2D,
|
||||||
start: DoubleVector2D,
|
start: DoubleVector2D,
|
||||||
end: DoubleVector2D,
|
end: DoubleVector2D,
|
||||||
direction: Direction,
|
direction: Trajectory2D.Type,
|
||||||
): CircleTrajectory2D {
|
): CircleTrajectory2D {
|
||||||
fun calculatePose(
|
fun calculatePose(
|
||||||
vector: DoubleVector2D,
|
vector: DoubleVector2D,
|
||||||
theta: Angle,
|
theta: Angle,
|
||||||
direction: Direction,
|
direction: Trajectory2D.Type,
|
||||||
): DubinsPose2D = DubinsPose2D(
|
): DubinsPose2D = DubinsPose2D(
|
||||||
vector,
|
vector,
|
||||||
when (direction) {
|
when (direction) {
|
||||||
Direction.LEFT -> (theta - Angle.piDiv2).normalized()
|
Trajectory2D.Type.L -> (theta - Angle.piDiv2).normalized()
|
||||||
Direction.RIGHT -> (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.
|
* 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.Euclidean2DSpace
|
||||||
import space.kscience.kmath.geometry.equalsFloat
|
import space.kscience.kmath.geometry.equalsFloat
|
||||||
import space.kscience.kmath.trajectory.*
|
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertNotNull
|
import kotlin.test.assertNotNull
|
||||||
import kotlin.test.assertTrue
|
import kotlin.test.assertTrue
|
||||||
@ -37,7 +36,7 @@ class DubinsTests {
|
|||||||
)
|
)
|
||||||
|
|
||||||
expectedLengths.forEach {
|
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")
|
assertNotNull(path, "Path ${it.key} not found")
|
||||||
println("${it.key}: ${path.length}")
|
println("${it.key}: ${path.length}")
|
||||||
assertTrue(it.value.equalsFloat(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.Circle2D
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||||
import space.kscience.kmath.geometry.equalsFloat
|
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertEquals
|
import kotlin.test.assertEquals
|
||||||
import kotlin.test.assertTrue
|
|
||||||
|
|
||||||
class DubinsTest {
|
class ObstacleTest {
|
||||||
@Test
|
@Test
|
||||||
fun firstPath() {
|
fun firstPath() {
|
||||||
val startPoint = vector(-5.0, -1.0)
|
val startPoint = vector(-5.0, -1.0)
|
||||||
@ -22,8 +20,13 @@ class DubinsTest {
|
|||||||
val finalDirection = vector(1.0, -1.0)
|
val finalDirection = vector(1.0, -1.0)
|
||||||
val finalRadius = 0.5
|
val finalRadius = 0.5
|
||||||
|
|
||||||
val obstacles = listOf(DubinsObstacle(listOf(
|
val obstacles = listOf(
|
||||||
Circle2D(vector(7.0, 1.0), 5.0))))
|
Obstacle(
|
||||||
|
listOf(
|
||||||
|
Circle2D(vector(7.0, 1.0), 5.0)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
val outputTangents = findAllPaths(
|
val outputTangents = findAllPaths(
|
||||||
startPoint,
|
startPoint,
|
||||||
@ -32,8 +35,9 @@ class DubinsTest {
|
|||||||
finalPoint,
|
finalPoint,
|
||||||
finalDirection,
|
finalDirection,
|
||||||
finalRadius,
|
finalRadius,
|
||||||
obstacles)
|
obstacles
|
||||||
val length = pathLength(shortestPath(outputTangents))
|
)
|
||||||
|
val length = outputTangents.minOf { it.length }
|
||||||
assertEquals(length, 27.2113183, 1e-6)
|
assertEquals(length, 27.2113183, 1e-6)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -47,17 +51,21 @@ class DubinsTest {
|
|||||||
val finalRadius = 0.5
|
val finalRadius = 0.5
|
||||||
|
|
||||||
val obstacles = listOf(
|
val obstacles = listOf(
|
||||||
DubinsObstacle(listOf(
|
Obstacle(
|
||||||
|
listOf(
|
||||||
Circle2D(vector(1.0, 6.5), 0.5),
|
Circle2D(vector(1.0, 6.5), 0.5),
|
||||||
Circle2D(vector(2.0, 1.0), 0.5),
|
Circle2D(vector(2.0, 1.0), 0.5),
|
||||||
Circle2D(vector(6.0, 0.0), 0.5),
|
Circle2D(vector(6.0, 0.0), 0.5),
|
||||||
Circle2D(vector(5.0, 5.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(10.0, 1.0), 0.5),
|
||||||
Circle2D(vector(16.0, 0.0), 0.5),
|
Circle2D(vector(16.0, 0.0), 0.5),
|
||||||
Circle2D(vector(14.0, 6.0), 0.5),
|
Circle2D(vector(14.0, 6.0), 0.5),
|
||||||
Circle2D(vector(9.0, 4.0), 0.5)
|
Circle2D(vector(9.0, 4.0), 0.5)
|
||||||
))
|
)
|
||||||
|
)
|
||||||
)
|
)
|
||||||
val paths = findAllPaths(
|
val paths = findAllPaths(
|
||||||
startPoint,
|
startPoint,
|
||||||
@ -66,22 +74,19 @@ class DubinsTest {
|
|||||||
finalPoint,
|
finalPoint,
|
||||||
finalDirection,
|
finalDirection,
|
||||||
finalRadius,
|
finalRadius,
|
||||||
obstacles)
|
obstacles
|
||||||
val length = pathLength(shortestPath(paths))
|
)
|
||||||
|
val length = paths.minOf { it.length }
|
||||||
assertEquals(length, 28.9678224, 1e-6)
|
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
|
@Test
|
||||||
fun equalObstacles() {
|
fun equalObstacles() {
|
||||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||||
val obstacle1 = DubinsObstacle(listOf(circle1))
|
assertEquals(circle1, circle2)
|
||||||
val obstacle2 = DubinsObstacle(listOf(circle2))
|
val obstacle1 = Obstacle(listOf(circle1))
|
||||||
println(obstacle1 == obstacle2)
|
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.circumference
|
||||||
import space.kscience.kmath.geometry.degrees
|
import space.kscience.kmath.geometry.degrees
|
||||||
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
||||||
|
import space.kscience.kmath.trajectory.Trajectory2D
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertEquals
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
@ -22,7 +23,7 @@ class ArcTests {
|
|||||||
circle.center,
|
circle.center,
|
||||||
vector(-2.0, 0.0),
|
vector(-2.0, 0.0),
|
||||||
vector(0.0, 2.0),
|
vector(0.0, 2.0),
|
||||||
CircleTrajectory2D.Direction.RIGHT
|
Trajectory2D.Type.R
|
||||||
)
|
)
|
||||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||||
assertEquals(0.0, arc.start.bearing.degrees)
|
assertEquals(0.0, arc.start.bearing.degrees)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user