remove trajectory
This commit is contained in:
parent
00ce7d5a48
commit
7cc6a4be40
@ -24,6 +24,7 @@
|
|||||||
### Deprecated
|
### Deprecated
|
||||||
|
|
||||||
### Removed
|
### Removed
|
||||||
|
- Trajectory moved to https://github.com/SciProgCentre/maps-kt
|
||||||
- Polynomials moved to https://github.com/SciProgCentre/kmath-polynomial
|
- Polynomials moved to https://github.com/SciProgCentre/kmath-polynomial
|
||||||
|
|
||||||
### Fixed
|
### Fixed
|
||||||
|
|||||||
@ -15,7 +15,7 @@ allprojects {
|
|||||||
}
|
}
|
||||||
|
|
||||||
group = "space.kscience"
|
group = "space.kscience"
|
||||||
version = "0.3.1-dev-10"
|
version = "0.3.1-dev-11"
|
||||||
}
|
}
|
||||||
|
|
||||||
subprojects {
|
subprojects {
|
||||||
|
|||||||
@ -9,7 +9,7 @@ kotlin.native.ignoreDisabledTargets=true
|
|||||||
org.gradle.configureondemand=true
|
org.gradle.configureondemand=true
|
||||||
org.gradle.jvmargs=-Xmx4096m
|
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
|
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-jupyter",
|
||||||
":kmath-symja",
|
":kmath-symja",
|
||||||
":kmath-jafama",
|
":kmath-jafama",
|
||||||
":kmath-trajectory",
|
|
||||||
":examples",
|
":examples",
|
||||||
":benchmarks",
|
":benchmarks",
|
||||||
)
|
)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user