0.3.1-dev-11 #510
@ -24,6 +24,7 @@
|
||||
### Deprecated
|
||||
|
||||
### Removed
|
||||
- Trajectory moved to https://github.com/SciProgCentre/maps-kt
|
||||
- Polynomials moved to https://github.com/SciProgCentre/kmath-polynomial
|
||||
|
||||
### Fixed
|
||||
|
||||
@ -15,7 +15,7 @@ allprojects {
|
||||
}
|
||||
|
||||
group = "space.kscience"
|
||||
version = "0.3.1-dev-10"
|
||||
version = "0.3.1-dev-11"
|
||||
}
|
||||
|
||||
subprojects {
|
||||
|
||||
@ -9,7 +9,7 @@ kotlin.native.ignoreDisabledTargets=true
|
||||
org.gradle.configureondemand=true
|
||||
org.gradle.jvmargs=-Xmx4096m
|
||||
|
||||
toolsVersion=0.14.5-kotlin-1.8.20-RC
|
||||
toolsVersion=0.14.6-kotlin-1.8.20
|
||||
|
||||
|
||||
org.gradle.parallel=true
|
||||
|
||||
@ -1,34 +0,0 @@
|
||||
# kmath-trajectory
|
||||
|
||||
|
||||
|
||||
|
||||
## Artifact:
|
||||
|
||||
The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.1-dev-1`.
|
||||
|
||||
**Gradle Groovy:**
|
||||
```groovy
|
||||
repositories {
|
||||
maven { url 'https://repo.kotlin.link' }
|
||||
mavenCentral()
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation 'space.kscience:kmath-trajectory:0.3.1-dev-1'
|
||||
}
|
||||
```
|
||||
**Gradle Kotlin DSL:**
|
||||
```kotlin
|
||||
repositories {
|
||||
maven("https://repo.kotlin.link")
|
||||
mavenCentral()
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation("space.kscience:kmath-trajectory:0.3.1-dev-1")
|
||||
}
|
||||
```
|
||||
|
||||
## Contributors
|
||||
Erik Schouten (github: @ESchouten, email: erik-schouten@hotmail.nl)
|
||||
@ -1,21 +0,0 @@
|
||||
plugins {
|
||||
id("space.kscience.gradle.mpp")
|
||||
}
|
||||
|
||||
kscience{
|
||||
jvm()
|
||||
js()
|
||||
native()
|
||||
|
||||
useContextReceivers()
|
||||
useSerialization()
|
||||
dependencies {
|
||||
api(projects.kmath.kmathGeometry)
|
||||
}
|
||||
}
|
||||
|
||||
readme {
|
||||
description = "Path and trajectory optimization (to be moved to a separate project)"
|
||||
maturity = space.kscience.gradle.Maturity.DEPRECATED
|
||||
propertyByTemplate("artifact", rootProject.file("docs/templates/ARTIFACT-TEMPLATE.md"))
|
||||
}
|
||||
@ -1,13 +0,0 @@
|
||||
# kmath-trajectory
|
||||
|
||||
|
||||
${features}
|
||||
|
||||
${artifact}
|
||||
|
||||
## Author
|
||||
Erik Schouten
|
||||
|
||||
Github: ESchouten
|
||||
|
||||
Email: erik-schouten@hotmail.nl
|
||||
@ -1,258 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.trajectory.Trajectory2D.*
|
||||
import kotlin.math.acos
|
||||
|
||||
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
||||
|
||||
internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
|
||||
|
||||
internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
|
||||
val dX = radius * cos(bearing)
|
||||
val dY = radius * sin(bearing)
|
||||
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
||||
}
|
||||
|
||||
private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): StraightTrajectory2D =
|
||||
with(Euclidean2DSpace) {
|
||||
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)
|
||||
)
|
||||
|
||||
R -> vector(
|
||||
from.center.x + from.radius * cos(centers.bearing),
|
||||
from.center.y - from.radius * sin(centers.bearing)
|
||||
)
|
||||
}
|
||||
return StraightTrajectory2D(
|
||||
p1,
|
||||
vector(p1.x + (centers.end.x - centers.begin.x), p1.y + (centers.end.y - centers.begin.y))
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
private fun innerTangent(
|
||||
from: Circle2D,
|
||||
to: Circle2D,
|
||||
direction: Direction,
|
||||
): StraightTrajectory2D? =
|
||||
with(Euclidean2DSpace) {
|
||||
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
|
||||
}.normalized()
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
|
||||
@Suppress("DuplicatedCode")
|
||||
public object DubinsPath {
|
||||
|
||||
public data class Type(
|
||||
public val first: Direction,
|
||||
public val second: Trajectory2D.Type,
|
||||
public val third: Direction,
|
||||
) {
|
||||
public fun toList(): List<Trajectory2D.Type> = listOf(first, second, third)
|
||||
|
||||
override fun toString(): String = "${first}${second}${third}"
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Return Dubins trajectory type or null if trajectory is not a Dubins path
|
||||
*/
|
||||
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type? {
|
||||
if (trajectory2D.segments.size != 3) return null
|
||||
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(
|
||||
a.direction,
|
||||
if (b is CircleTrajectory2D) b.direction else S,
|
||||
c.direction
|
||||
)
|
||||
}
|
||||
|
||||
public fun all(
|
||||
start: DubinsPose2D,
|
||||
end: DubinsPose2D,
|
||||
turningRadius: Double,
|
||||
): List<CompositeTrajectory2D> = listOfNotNull(
|
||||
rlr(start, end, turningRadius),
|
||||
lrl(start, end, turningRadius),
|
||||
rsr(start, end, turningRadius),
|
||||
lsl(start, end, turningRadius),
|
||||
rsl(start, end, turningRadius),
|
||||
lsr(start, end, turningRadius)
|
||||
)
|
||||
|
||||
public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D =
|
||||
all(start, end, turningRadius).minBy { it.length }
|
||||
|
||||
public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
|
||||
with(Euclidean2DSpace) {
|
||||
val c1 = start.getRightCircle(turningRadius)
|
||||
val c2 = end.getRightCircle(turningRadius)
|
||||
val centers = StraightTrajectory2D(c1.center, c2.center)
|
||||
if (centers.length > turningRadius * 4) return null
|
||||
|
||||
val firstVariant = run {
|
||||
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
var dX = turningRadius * sin(theta)
|
||||
var dY = turningRadius * cos(theta)
|
||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||
val e = Circle2D(p, turningRadius)
|
||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
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, R)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
val secondVariant = run {
|
||||
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
var dX = turningRadius * sin(theta)
|
||||
var dY = turningRadius * cos(theta)
|
||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||
val e = Circle2D(p, turningRadius)
|
||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
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, R)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, R)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
||||
}
|
||||
|
||||
public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D? =
|
||||
with(Euclidean2DSpace) {
|
||||
val c1 = start.getLeftCircle(turningRadius)
|
||||
val c2 = end.getLeftCircle(turningRadius)
|
||||
val centers = StraightTrajectory2D(c1.center, c2.center)
|
||||
if (centers.length > turningRadius * 4) return null
|
||||
|
||||
val firstVariant = run {
|
||||
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
var dX = turningRadius * sin(theta)
|
||||
var dY = turningRadius * cos(theta)
|
||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||
val e = Circle2D(p, turningRadius)
|
||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
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, L)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
val secondVariant = run {
|
||||
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
var dX = turningRadius * sin(theta)
|
||||
var dY = turningRadius * cos(theta)
|
||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||
val e = Circle2D(p, turningRadius)
|
||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||
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, L)
|
||||
val a2 = CircleTrajectory2D.of(e.center, p1, p2, R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, p2, end, L)
|
||||
CompositeTrajectory2D(a1, a2, a3)
|
||||
}
|
||||
|
||||
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
||||
}
|
||||
|
||||
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): CompositeTrajectory2D {
|
||||
val c1 = start.getRightCircle(turningRadius)
|
||||
val c2 = end.getRightCircle(turningRadius)
|
||||
val s = outerTangent(c1, c2, L)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, 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 = outerTangent(c1, c2, R)
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, 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 = innerTangent(c1, c2, R)
|
||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, 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 = innerTangent(c1, c2, L)
|
||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||
|
||||
val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L)
|
||||
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R)
|
||||
return CompositeTrajectory2D(a1, s, a3)
|
||||
}
|
||||
}
|
||||
|
||||
public typealias PathTypes = List<Type>
|
||||
|
||||
public fun interface MaxCurvature {
|
||||
public fun compute(startPoint: PhaseVector2D): Double
|
||||
}
|
||||
|
||||
public fun DubinsPath.shortest(
|
||||
start: PhaseVector2D,
|
||||
end: PhaseVector2D,
|
||||
maxCurvature: MaxCurvature,
|
||||
): CompositeTrajectory2D = shortest(start, end, maxCurvature.compute(start))
|
||||
|
||||
@ -1,75 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.
|
||||
*/
|
||||
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
|
||||
|
||||
package space.kscience.kmath.trajectory
|
||||
|
||||
import kotlinx.serialization.KSerializer
|
||||
import kotlinx.serialization.SerialName
|
||||
import kotlinx.serialization.Serializable
|
||||
import kotlinx.serialization.UseSerializers
|
||||
import kotlinx.serialization.descriptors.SerialDescriptor
|
||||
import kotlinx.serialization.encoding.Decoder
|
||||
import kotlinx.serialization.encoding.Encoder
|
||||
import space.kscience.kmath.geometry.*
|
||||
import kotlin.math.atan2
|
||||
|
||||
/**
|
||||
* Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
|
||||
*/
|
||||
@Serializable(DubinsPose2DSerializer::class)
|
||||
public interface DubinsPose2D : DoubleVector2D {
|
||||
public val coordinates: DoubleVector2D
|
||||
public val bearing: Angle
|
||||
|
||||
public companion object {
|
||||
public fun bearingToVector(bearing: Angle): Vector2D<Double> =
|
||||
Euclidean2DSpace.vector(cos(bearing), sin(bearing))
|
||||
|
||||
public fun vectorToBearing(vector2D: DoubleVector2D): Angle {
|
||||
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
|
||||
return atan2(vector2D.y, vector2D.x).radians
|
||||
}
|
||||
|
||||
public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
|
||||
DubinsPose2D(point, vectorToBearing(direction))
|
||||
}
|
||||
}
|
||||
|
||||
@Serializable
|
||||
public class PhaseVector2D(
|
||||
override val coordinates: DoubleVector2D,
|
||||
public val velocity: DoubleVector2D,
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||
override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
|
||||
}
|
||||
|
||||
@Serializable
|
||||
@SerialName("DubinsPose2D")
|
||||
private class DubinsPose2DImpl(
|
||||
override val coordinates: DoubleVector2D,
|
||||
override val bearing: Angle,
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||
|
||||
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
|
||||
}
|
||||
|
||||
public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
|
||||
private val proxySerializer = DubinsPose2DImpl.serializer()
|
||||
|
||||
override val descriptor: SerialDescriptor
|
||||
get() = proxySerializer.descriptor
|
||||
|
||||
override fun deserialize(decoder: Decoder): DubinsPose2D {
|
||||
return decoder.decodeSerializableValue(proxySerializer)
|
||||
}
|
||||
|
||||
override fun serialize(encoder: Encoder, value: DubinsPose2D) {
|
||||
val pose = value as? DubinsPose2DImpl ?: DubinsPose2DImpl(value.coordinates, value.bearing)
|
||||
encoder.encodeSerializableValue(proxySerializer, pose)
|
||||
}
|
||||
}
|
||||
|
||||
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)
|
||||
@ -1,614 +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.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 startDirection: Trajectory2D.Direction,
|
||||
val endDirection: Trajectory2D.Direction = startDirection,
|
||||
) : LineSegment2D by lineSegment
|
||||
|
||||
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 = 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 = LineSegment(
|
||||
firstCircle.center + w * r1,
|
||||
secondCircle.center + w * r2
|
||||
),
|
||||
startDirection = route.first,
|
||||
endDirection = route.third
|
||||
)
|
||||
)
|
||||
} else {
|
||||
throw Exception("Circles should not intersect")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal class Obstacle(
|
||||
public val circles: List<Circle2D>,
|
||||
) {
|
||||
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,
|
||||
LineSegment(
|
||||
center + w * r1,
|
||||
other.center + w * r2
|
||||
),
|
||||
startDirection = routeType.first,
|
||||
endDirection = routeType.third
|
||||
)
|
||||
)
|
||||
} 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)
|
||||
}
|
||||
}
|
||||
|
||||
internal fun nextTangent(circle: Circle2D, direction: Trajectory2D.Direction): Tangent {
|
||||
if (direction == boundaryRoute.first) {
|
||||
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,
|
||||
LineSegment(
|
||||
tangents[i - 1].lineSegment.end,
|
||||
tangents[i - 1].lineSegment.begin
|
||||
),
|
||||
direction
|
||||
)
|
||||
} else {
|
||||
return Tangent(
|
||||
circles[0],
|
||||
circles.last(),
|
||||
this,
|
||||
this,
|
||||
LineSegment(
|
||||
tangents.last().lineSegment.end,
|
||||
tangents.last().lineSegment.begin
|
||||
),
|
||||
direction
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
error("next tangent not found")
|
||||
}
|
||||
|
||||
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()
|
||||
}
|
||||
}
|
||||
|
||||
internal 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,
|
||||
direction: Trajectory2D.Direction,
|
||||
): 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 (direction) {
|
||||
Trajectory2D.L -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
phi2 - phi1
|
||||
} else {
|
||||
2 * PI + phi2 - phi1
|
||||
}
|
||||
}
|
||||
|
||||
Trajectory2D.R -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
2 * PI - (phi2 - phi1)
|
||||
} else {
|
||||
-(phi2 - phi1)
|
||||
}
|
||||
}
|
||||
}
|
||||
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.L to Circle2D(center1, r),
|
||||
Trajectory2D.R to Circle2D(center2, r)
|
||||
)
|
||||
} else {
|
||||
mapOf(
|
||||
Trajectory2D.L to Circle2D(center2, r),
|
||||
Trajectory2D.R to Circle2D(center1, r)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
private fun sortedObstacles(
|
||||
currentObstacle: Obstacle,
|
||||
obstacles: List<Obstacle>,
|
||||
): List<Obstacle> {
|
||||
return obstacles.sortedBy { norm(it.center - currentObstacle.center) }
|
||||
}
|
||||
|
||||
private fun tangentsAlongTheObstacle(
|
||||
initialCircle: Circle2D,
|
||||
direction: Trajectory2D.Direction,
|
||||
finalCircle: Circle2D,
|
||||
obstacle: Obstacle,
|
||||
): List<Tangent> {
|
||||
val dubinsTangents = mutableListOf<Tangent>()
|
||||
var tangent = obstacle.nextTangent(initialCircle, direction)
|
||||
dubinsTangents.add(tangent)
|
||||
while (tangent.endCircle != finalCircle) {
|
||||
tangent = obstacle.nextTangent(tangent.endCircle, direction)
|
||||
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.startDirection
|
||||
)
|
||||
)
|
||||
}
|
||||
|
||||
add(tangents.last().lineSegment.toTrajectory())
|
||||
}
|
||||
)
|
||||
|
||||
internal fun findAllPaths(
|
||||
start: DubinsPose2D,
|
||||
startingRadius: Double,
|
||||
finish: DubinsPose2D,
|
||||
finalRadius: Double,
|
||||
obstacles: List<Obstacle>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
fun DubinsPose2D.direction() = vector(cos(bearing), sin(bearing))
|
||||
|
||||
val initialCircles = constructTangentCircles(
|
||||
start,
|
||||
start.direction(),
|
||||
startingRadius
|
||||
)
|
||||
val finalCircles = constructTangentCircles(
|
||||
finish,
|
||||
finish.direction(),
|
||||
finalRadius
|
||||
)
|
||||
val trajectories = mutableListOf<CompositeTrajectory2D>()
|
||||
for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
|
||||
for (j in listOf(Trajectory2D.L, Trajectory2D.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]!!)),
|
||||
LineSegment(start, start),
|
||||
i
|
||||
)
|
||||
)
|
||||
)
|
||||
while (!allFinished(currentPaths, finalObstacle)) {
|
||||
val newPaths = mutableListOf<TangentPath>()
|
||||
for (tangentPath: TangentPath in currentPaths) {
|
||||
val currentCircle = tangentPath.last().endCircle
|
||||
val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
val currentObstacle = tangentPath.last().endObstacle
|
||||
var nextObstacle: Obstacle? = null
|
||||
if (currentObstacle != finalObstacle) {
|
||||
val tangentToFinal = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.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,
|
||||
currentDirection
|
||||
).lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
val lengthCalculated = arcLength(
|
||||
tangent.startCircle,
|
||||
tangentPath.last().lineSegment.end,
|
||||
tangent.lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
tangentsAlong = if (lengthCalculated > lengthMaxPossible) {
|
||||
tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
currentDirection,
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
} else {
|
||||
emptyList()
|
||||
}
|
||||
} else {
|
||||
tangentsAlong = tangentsAlongTheObstacle(
|
||||
currentCircle,
|
||||
currentDirection,
|
||||
tangent.startCircle,
|
||||
currentObstacle
|
||||
)
|
||||
}
|
||||
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
||||
}
|
||||
} else {
|
||||
newPaths.add(tangentPath)
|
||||
}
|
||||
}
|
||||
currentPaths = newPaths
|
||||
}
|
||||
|
||||
trajectories += currentPaths.map { tangentPath ->
|
||||
val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
val end = finalCircles[j]!!
|
||||
TangentPath(
|
||||
tangentPath.tangents +
|
||||
Tangent(
|
||||
end,
|
||||
end,
|
||||
Obstacle(end),
|
||||
Obstacle(end),
|
||||
LineSegment(finish, finish),
|
||||
startDirection = lastDirection,
|
||||
endDirection = j
|
||||
)
|
||||
)
|
||||
}.map { it.toTrajectory() }
|
||||
}
|
||||
}
|
||||
return trajectories
|
||||
}
|
||||
|
||||
|
||||
public object Obstacles {
|
||||
public fun allPathsAvoiding(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
obstaclePolygons: List<Polygon<Double>>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val obstacles: List<Obstacle> = obstaclePolygons.map { polygon ->
|
||||
Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) })
|
||||
}
|
||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,131 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.
|
||||
*/
|
||||
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
|
||||
|
||||
package space.kscience.kmath.trajectory
|
||||
|
||||
import kotlinx.serialization.SerialName
|
||||
import kotlinx.serialization.Serializable
|
||||
import kotlinx.serialization.UseSerializers
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import kotlin.math.atan2
|
||||
|
||||
@Serializable
|
||||
public sealed interface Trajectory2D {
|
||||
public val length: Double
|
||||
|
||||
|
||||
public sealed interface Type
|
||||
|
||||
public sealed interface Direction: Type
|
||||
|
||||
public object R : Direction {
|
||||
override fun toString(): String = "R"
|
||||
}
|
||||
|
||||
public object S : Type {
|
||||
override fun toString(): String = "L"
|
||||
}
|
||||
|
||||
public object L : Direction {
|
||||
override fun toString(): String = "L"
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Straight path segment. The order of start and end defines the direction
|
||||
*/
|
||||
@Serializable
|
||||
@SerialName("straight")
|
||||
public data class StraightTrajectory2D(
|
||||
override val begin: DoubleVector2D,
|
||||
override val end: DoubleVector2D,
|
||||
) : Trajectory2D, LineSegment2D {
|
||||
|
||||
override val length: Double get() = begin.distanceTo(end)
|
||||
|
||||
public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized()
|
||||
}
|
||||
|
||||
/**
|
||||
* An arc segment
|
||||
*/
|
||||
@Serializable
|
||||
@SerialName("arc")
|
||||
public data class CircleTrajectory2D(
|
||||
public val circle: Circle2D,
|
||||
public val start: DubinsPose2D,
|
||||
public val end: DubinsPose2D,
|
||||
) : Trajectory2D {
|
||||
|
||||
/**
|
||||
* Arc length in radians
|
||||
*/
|
||||
val arcLength: Angle
|
||||
get() = if (direction == Trajectory2D.L) {
|
||||
start.bearing - end.bearing
|
||||
} else {
|
||||
end.bearing - start.bearing
|
||||
}.normalized()
|
||||
|
||||
|
||||
override val length: Double by lazy {
|
||||
circle.radius * arcLength.radians
|
||||
}
|
||||
|
||||
public val direction: Trajectory2D.Direction by lazy {
|
||||
if (start.y < circle.center.y) {
|
||||
if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
} else if (start.y > circle.center.y) {
|
||||
if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
} else {
|
||||
if (start.bearing == Angle.zero) {
|
||||
if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
} else {
|
||||
if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public companion object {
|
||||
public fun of(
|
||||
center: DoubleVector2D,
|
||||
start: DoubleVector2D,
|
||||
end: DoubleVector2D,
|
||||
direction: Trajectory2D.Direction,
|
||||
): CircleTrajectory2D {
|
||||
fun calculatePose(
|
||||
vector: DoubleVector2D,
|
||||
theta: Angle,
|
||||
direction: Trajectory2D.Direction,
|
||||
): DubinsPose2D = DubinsPose2D(
|
||||
vector,
|
||||
when (direction) {
|
||||
Trajectory2D.L -> (theta - Angle.piDiv2).normalized()
|
||||
Trajectory2D.R -> (theta + Angle.piDiv2).normalized()
|
||||
}
|
||||
)
|
||||
|
||||
val s1 = StraightTrajectory2D(center, start)
|
||||
val s2 = StraightTrajectory2D(center, end)
|
||||
val pose1 = calculatePose(start, s1.bearing, direction)
|
||||
val pose2 = calculatePose(end, s2.bearing, direction)
|
||||
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
|
||||
if (trajectory.direction != direction) error("Trajectory direction mismatch")
|
||||
return trajectory
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Serializable
|
||||
@SerialName("composite")
|
||||
public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
|
||||
override val length: Double get() = segments.sumOf { it.length }
|
||||
}
|
||||
|
||||
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
|
||||
CompositeTrajectory2D(segments.toList())
|
||||
|
||||
@ -1,61 +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.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.equalsFloat
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertNotNull
|
||||
import kotlin.test.assertTrue
|
||||
|
||||
|
||||
class DubinsTests {
|
||||
|
||||
@Test
|
||||
fun dubinsTest() = with(Euclidean2DSpace){
|
||||
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
|
||||
val lineP1 = straight.shift(1, 10.0).inverse()
|
||||
|
||||
val start = DubinsPose2D(straight.end, straight.bearing)
|
||||
val end = DubinsPose2D(lineP1.begin, lineP1.bearing)
|
||||
val radius = 2.0
|
||||
val dubins = DubinsPath.all(start, end, radius)
|
||||
|
||||
val absoluteDistance = start.distanceTo(end)
|
||||
println("Absolute distance: $absoluteDistance")
|
||||
|
||||
val expectedLengths = mapOf(
|
||||
DubinsPath.Type.RLR to 13.067681939031397,
|
||||
DubinsPath.Type.RSR to 12.28318530717957,
|
||||
DubinsPath.Type.LSL to 32.84955592153878,
|
||||
DubinsPath.Type.RSL to 23.37758938854081,
|
||||
DubinsPath.Type.LSR to 23.37758938854081
|
||||
)
|
||||
|
||||
expectedLengths.forEach {
|
||||
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))
|
||||
|
||||
val a = path.segments[0] as CircleTrajectory2D
|
||||
val b = path.segments[1]
|
||||
val c = path.segments[2] as CircleTrajectory2D
|
||||
|
||||
assertTrue(start.equalsFloat(a.start))
|
||||
assertTrue(end.equalsFloat(c.end))
|
||||
|
||||
// Not working, theta double precision inaccuracy
|
||||
if (b is CircleTrajectory2D) {
|
||||
assertTrue(a.end.equalsFloat(b.start))
|
||||
assertTrue(c.start.equalsFloat(b.end))
|
||||
} else if (b is StraightTrajectory2D) {
|
||||
assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing)))
|
||||
assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,88 +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.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
class ObstacleTest {
|
||||
@Test
|
||||
fun firstPath() {
|
||||
val startPoint = vector(-5.0, -1.0)
|
||||
val startDirection = vector(1.0, 1.0)
|
||||
val startRadius = 0.5
|
||||
val finalPoint = vector(20.0, 4.0)
|
||||
val finalDirection = vector(1.0, -1.0)
|
||||
val finalRadius = 0.5
|
||||
|
||||
val obstacles = listOf(
|
||||
Obstacle(
|
||||
listOf(
|
||||
Circle2D(vector(7.0, 1.0), 5.0)
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
val outputTangents = findAllPaths(
|
||||
DubinsPose2D.of(startPoint, startDirection),
|
||||
startRadius,
|
||||
DubinsPose2D.of(finalPoint, finalDirection),
|
||||
finalRadius,
|
||||
obstacles
|
||||
)
|
||||
val length = outputTangents.minOf { it.length }
|
||||
assertEquals(27.2113183, length, 1e-6)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun secondPath() {
|
||||
val startPoint = vector(-5.0, -1.0)
|
||||
val startDirection = vector(1.0, 1.0)
|
||||
val startRadius = 0.5
|
||||
val finalPoint = vector(20.0, 4.0)
|
||||
val finalDirection = vector(1.0, -1.0)
|
||||
val finalRadius = 0.5
|
||||
|
||||
val obstacles = 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)
|
||||
)
|
||||
), 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(
|
||||
DubinsPose2D.of(startPoint, startDirection),
|
||||
startRadius,
|
||||
DubinsPose2D.of(finalPoint, finalDirection),
|
||||
finalRadius,
|
||||
obstacles
|
||||
)
|
||||
val length = paths.minOf { it.length }
|
||||
assertEquals(28.9678224, length, 1e-6)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun equalObstacles() {
|
||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
assertEquals(circle1, circle2)
|
||||
val obstacle1 = Obstacle(listOf(circle1))
|
||||
val obstacle2 = Obstacle(listOf(circle2))
|
||||
assertEquals(obstacle1, obstacle2)
|
||||
}
|
||||
}
|
||||
@ -1,64 +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.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||
import space.kscience.kmath.geometry.LineSegment
|
||||
import space.kscience.kmath.geometry.equalsLine
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
import kotlin.test.assertFailsWith
|
||||
import kotlin.test.assertTrue
|
||||
|
||||
class TangentTest {
|
||||
@Test
|
||||
fun tangents() {
|
||||
val c1 = Circle2D(vector(0.0, 0.0), 1.0)
|
||||
val c2 = Circle2D(vector(4.0, 0.0), 1.0)
|
||||
val routes = listOf(
|
||||
DubinsPath.Type.RSR,
|
||||
DubinsPath.Type.RSL,
|
||||
DubinsPath.Type.LSR,
|
||||
DubinsPath.Type.LSL
|
||||
)
|
||||
val segments = listOf(
|
||||
LineSegment(
|
||||
begin = vector(0.0, 1.0),
|
||||
end = vector(4.0, 1.0)
|
||||
),
|
||||
LineSegment(
|
||||
begin = vector(0.5, 0.8660254),
|
||||
end = vector(3.5, -0.8660254)
|
||||
),
|
||||
LineSegment(
|
||||
begin = vector(0.5, -0.8660254),
|
||||
end = vector(3.5, 0.8660254)
|
||||
),
|
||||
LineSegment(
|
||||
begin = vector(0.0, -1.0),
|
||||
end = vector(4.0, -1.0)
|
||||
)
|
||||
)
|
||||
|
||||
val tangentMap = c1.tangentsToCircle(c2)
|
||||
val tangentMapKeys = tangentMap.keys.toList()
|
||||
val tangentMapValues = tangentMap.values.toList()
|
||||
|
||||
assertEquals(routes, tangentMapKeys)
|
||||
for (i in segments.indices) {
|
||||
assertTrue(segments[i].equalsLine(Euclidean2DSpace, tangentMapValues[i]))
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
fun concentric(){
|
||||
val c1 = Circle2D(vector(0.0, 0.0), 10.0)
|
||||
val c2 = Circle2D(vector(0.0, 0.0), 1.0)
|
||||
assertEquals(emptyMap(), c1.tangentsToCircle(c2))
|
||||
}
|
||||
}
|
||||
@ -1,27 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.equalsFloat
|
||||
import space.kscience.kmath.geometry.radians
|
||||
import space.kscience.kmath.geometry.sin
|
||||
|
||||
|
||||
fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
|
||||
x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians)
|
||||
|
||||
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)
|
||||
|
||||
fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) {
|
||||
val dX = width * sin(inverse().bearing)
|
||||
val dY = width * sin(bearing)
|
||||
|
||||
return StraightTrajectory2D(
|
||||
vector(begin.x - dX * shift, begin.y - dY * shift),
|
||||
vector(end.x - dX * shift, end.y - dY * shift)
|
||||
)
|
||||
}
|
||||
@ -1,32 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.segments
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
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
|
||||
|
||||
class ArcTests {
|
||||
|
||||
@Test
|
||||
fun arcTest() = with(Euclidean2DSpace){
|
||||
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
||||
val arc = CircleTrajectory2D.of(
|
||||
circle.center,
|
||||
vector(-2.0, 0.0),
|
||||
vector(0.0, 2.0),
|
||||
Trajectory2D.R
|
||||
)
|
||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||
assertEquals(0.0, arc.start.bearing.degrees)
|
||||
assertEquals(90.0, arc.end.bearing.degrees)
|
||||
}
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.segments
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.circumference
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
class CircleTests {
|
||||
|
||||
@Test
|
||||
fun arcTest() {
|
||||
val center = Euclidean2DSpace.vector(0.0, 0.0)
|
||||
val radius = 2.0
|
||||
val expectedCircumference = 12.56637
|
||||
val circle = Circle2D(center, radius)
|
||||
assertEquals(expectedCircumference, circle.circumference, 1e-4)
|
||||
}
|
||||
}
|
||||
@ -1,37 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.segments
|
||||
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.degrees
|
||||
import space.kscience.kmath.trajectory.StraightTrajectory2D
|
||||
import kotlin.math.pow
|
||||
import kotlin.math.sqrt
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
class LineTests {
|
||||
|
||||
@Test
|
||||
fun lineTest() = with(Euclidean2DSpace){
|
||||
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
|
||||
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
|
||||
assertEquals(45.0, straight.bearing.degrees)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun lineAngleTest() = with(Euclidean2DSpace){
|
||||
//val zero = Vector2D(0.0, 0.0)
|
||||
val north = StraightTrajectory2D(zero, vector(0.0, 2.0))
|
||||
assertEquals(0.0, north.bearing.degrees)
|
||||
val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
|
||||
assertEquals(90.0, east.bearing.degrees)
|
||||
val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
|
||||
assertEquals(180.0, south.bearing.degrees)
|
||||
val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
|
||||
assertEquals(270.0, west.bearing.degrees)
|
||||
}
|
||||
}
|
||||
@ -44,7 +44,6 @@ include(
|
||||
":kmath-jupyter",
|
||||
":kmath-symja",
|
||||
":kmath-jafama",
|
||||
":kmath-trajectory",
|
||||
":examples",
|
||||
":benchmarks",
|
||||
)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user