forked from kscience/kmath
Merge pull request #495 from ESchouten/trajectory
Trajectory: Dubins path
This commit is contained in:
commit
ad30c68eba
7
docs/templates/README-TEMPLATE.md
vendored
7
docs/templates/README-TEMPLATE.md
vendored
@ -103,3 +103,10 @@ The project requires a lot of additional work. The most important thing we need
|
|||||||
required the most. Feel free to create feature requests. We are also welcome to code contributions, especially in issues
|
required the most. Feel free to create feature requests. We are also welcome to code contributions, especially in issues
|
||||||
marked with
|
marked with
|
||||||
[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
|
[waiting for a hero](https://github.com/mipt-npm/kmath/labels/waiting%20for%20a%20hero) label.
|
||||||
|
|
||||||
|
## Author
|
||||||
|
Erik Schouten
|
||||||
|
|
||||||
|
Github: ESchouten
|
||||||
|
|
||||||
|
Email: erik-schouten@hotmail.nl
|
||||||
|
32
kmath-trajectory/README.md
Normal file
32
kmath-trajectory/README.md
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
# Module kmath-trajectory
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
## Artifact:
|
||||||
|
|
||||||
|
The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.0`.
|
||||||
|
|
||||||
|
**Gradle Groovy:**
|
||||||
|
```groovy
|
||||||
|
repositories {
|
||||||
|
maven { url 'https://repo.kotlin.link' }
|
||||||
|
mavenCentral()
|
||||||
|
}
|
||||||
|
|
||||||
|
dependencies {
|
||||||
|
implementation 'space.kscience:kmath-trajectory:0.3.0'
|
||||||
|
}
|
||||||
|
```
|
||||||
|
**Gradle Kotlin DSL:**
|
||||||
|
```kotlin
|
||||||
|
repositories {
|
||||||
|
maven("https://repo.kotlin.link")
|
||||||
|
mavenCentral()
|
||||||
|
}
|
||||||
|
|
||||||
|
dependencies {
|
||||||
|
implementation("space.kscience:kmath-trajectory:0.3.0")
|
||||||
|
}
|
||||||
|
```
|
15
kmath-trajectory/build.gradle.kts
Normal file
15
kmath-trajectory/build.gradle.kts
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
plugins {
|
||||||
|
kotlin("multiplatform")
|
||||||
|
id("ru.mipt.npm.gradle.common")
|
||||||
|
id("ru.mipt.npm.gradle.native")
|
||||||
|
}
|
||||||
|
|
||||||
|
kotlin.sourceSets.commonMain {
|
||||||
|
dependencies {
|
||||||
|
api(projects.kmath.kmathGeometry)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
readme {
|
||||||
|
maturity = ru.mipt.npm.gradle.Maturity.PROTOTYPE
|
||||||
|
}
|
@ -0,0 +1,136 @@
|
|||||||
|
/*
|
||||||
|
* Copyright 2018-2021 KMath contributors.
|
||||||
|
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
|
||||||
|
*/
|
||||||
|
|
||||||
|
package space.kscience.kmath.trajectory.dubins
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.segments.Arc
|
||||||
|
import space.kscience.kmath.trajectory.segments.Segment
|
||||||
|
import space.kscience.kmath.trajectory.segments.Straight
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Circle
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||||
|
import kotlin.math.acos
|
||||||
|
import kotlin.math.cos
|
||||||
|
import kotlin.math.sin
|
||||||
|
|
||||||
|
public class DubinsPath(
|
||||||
|
public val a: Arc,
|
||||||
|
public val b: Segment,
|
||||||
|
public val c: Arc,
|
||||||
|
) {
|
||||||
|
|
||||||
|
public val type: TYPE = TYPE.valueOf(
|
||||||
|
arrayOf(
|
||||||
|
a.direction.name[0],
|
||||||
|
if (b is Arc) b.direction.name[0] else 'S',
|
||||||
|
c.direction.name[0]
|
||||||
|
).toCharArray().concatToString()
|
||||||
|
)
|
||||||
|
|
||||||
|
public val length: Double = a.length + b.length + c.length
|
||||||
|
|
||||||
|
public enum class TYPE {
|
||||||
|
RLR, LRL, RSR, LSL, RSL, LSR
|
||||||
|
}
|
||||||
|
|
||||||
|
public companion object {
|
||||||
|
public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List<DubinsPath> =
|
||||||
|
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: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
|
||||||
|
all(start, end, turningRadius).minByOrNull { it.length }!!
|
||||||
|
|
||||||
|
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
||||||
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
|
val centers = Straight(c1.center, c2.center)
|
||||||
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
|
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
||||||
|
var dX = turningRadius * sin(theta)
|
||||||
|
var dY = turningRadius * cos(theta)
|
||||||
|
val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
|
val e = Circle(p, turningRadius)
|
||||||
|
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
|
||||||
|
theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
||||||
|
dX = turningRadius * sin(theta)
|
||||||
|
dY = turningRadius * cos(theta)
|
||||||
|
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
|
||||||
|
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
|
||||||
|
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
|
||||||
|
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
|
||||||
|
return DubinsPath(a1, a2, a3)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
||||||
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
|
val centers = Straight(c1.center, c2.center)
|
||||||
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
|
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
||||||
|
var dX = turningRadius * sin(theta)
|
||||||
|
var dY = turningRadius * cos(theta)
|
||||||
|
val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
|
val e = Circle(p, turningRadius)
|
||||||
|
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
|
||||||
|
theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
||||||
|
dX = turningRadius * sin(theta)
|
||||||
|
dY = turningRadius * cos(theta)
|
||||||
|
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
|
||||||
|
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
|
||||||
|
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
|
||||||
|
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
|
||||||
|
return DubinsPath(a1, a2, a3)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
|
||||||
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
|
val s = leftOuterTangent(c1, c2)
|
||||||
|
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
|
||||||
|
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
|
||||||
|
return DubinsPath(a1, s, a3)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
|
||||||
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
|
val s = rightOuterTangent(c1, c2)
|
||||||
|
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
|
||||||
|
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
|
||||||
|
return DubinsPath(a1, s, a3)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
||||||
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
|
val s = rightInnerTangent(c1, c2)
|
||||||
|
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
|
||||||
|
|
||||||
|
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
|
||||||
|
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
|
||||||
|
return DubinsPath(a1, s, a3)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
||||||
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
|
val s = leftInnerTangent(c1, c2)
|
||||||
|
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
|
||||||
|
|
||||||
|
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
|
||||||
|
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
|
||||||
|
return DubinsPath(a1, s, a3)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,67 @@
|
|||||||
|
/*
|
||||||
|
* Copyright 2018-2021 KMath contributors.
|
||||||
|
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
|
||||||
|
*/
|
||||||
|
|
||||||
|
package space.kscience.kmath.trajectory.dubins
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.segments.Straight
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Circle
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||||
|
import kotlin.math.PI
|
||||||
|
import kotlin.math.acos
|
||||||
|
import kotlin.math.cos
|
||||||
|
import kotlin.math.sin
|
||||||
|
|
||||||
|
private enum class SIDE {
|
||||||
|
LEFT, RIGHT
|
||||||
|
}
|
||||||
|
|
||||||
|
internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first
|
||||||
|
internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second
|
||||||
|
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle, Circle> {
|
||||||
|
val dX = radius * cos(theta)
|
||||||
|
val dY = radius * sin(theta)
|
||||||
|
return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius)
|
||||||
|
}
|
||||||
|
|
||||||
|
internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT)
|
||||||
|
internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT)
|
||||||
|
private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
|
||||||
|
val centers = Straight(a.center, b.center)
|
||||||
|
val p1 = when (side) {
|
||||||
|
SIDE.LEFT -> Vector2D(
|
||||||
|
a.center.x - a.radius * cos(centers.theta),
|
||||||
|
a.center.y + a.radius * sin(centers.theta)
|
||||||
|
)
|
||||||
|
SIDE.RIGHT -> Vector2D(
|
||||||
|
a.center.x + a.radius * cos(centers.theta),
|
||||||
|
a.center.y - a.radius * sin(centers.theta)
|
||||||
|
)
|
||||||
|
}
|
||||||
|
return Straight(
|
||||||
|
p1,
|
||||||
|
Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT)
|
||||||
|
internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT)
|
||||||
|
private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? {
|
||||||
|
val centers = Straight(base.center, direction.center)
|
||||||
|
if (centers.length < base.radius * 2) return null
|
||||||
|
val angle = theta(
|
||||||
|
when (side) {
|
||||||
|
SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
|
||||||
|
SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
|
||||||
|
}
|
||||||
|
)
|
||||||
|
val dX = base.radius * sin(angle)
|
||||||
|
val dY = base.radius * cos(angle)
|
||||||
|
val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
|
||||||
|
val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
|
||||||
|
return Straight(p1, p2)
|
||||||
|
}
|
||||||
|
|
||||||
|
internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
|
@ -0,0 +1,59 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.dubins.theta
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Circle
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||||
|
import kotlin.math.PI
|
||||||
|
|
||||||
|
public data class Arc(
|
||||||
|
public val circle: Circle,
|
||||||
|
public val start: Pose2D,
|
||||||
|
public val end: Pose2D
|
||||||
|
) : Segment {
|
||||||
|
|
||||||
|
internal companion object {
|
||||||
|
fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
|
||||||
|
val s1 = Straight(center, start)
|
||||||
|
val s2 = Straight(center, end)
|
||||||
|
val pose1 = calculatePose(start, s1.theta, direction)
|
||||||
|
val pose2 = calculatePose(end, s2.theta, direction)
|
||||||
|
return Arc(Circle(center, s1.length), pose1, pose2)
|
||||||
|
}
|
||||||
|
|
||||||
|
private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
|
||||||
|
Pose2D.of(
|
||||||
|
vector,
|
||||||
|
when (direction) {
|
||||||
|
Direction.LEFT -> theta(theta - PI / 2)
|
||||||
|
Direction.RIGHT -> theta(theta + PI / 2)
|
||||||
|
}
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
internal enum class Direction {
|
||||||
|
LEFT, RIGHT
|
||||||
|
}
|
||||||
|
|
||||||
|
override val length: Double
|
||||||
|
get() {
|
||||||
|
val angle: Double =
|
||||||
|
theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
|
||||||
|
val proportion = angle / (2 * PI)
|
||||||
|
return circle.circumference * proportion
|
||||||
|
}
|
||||||
|
|
||||||
|
internal val direction: Direction
|
||||||
|
get() = if (start.y < circle.center.y) {
|
||||||
|
if (start.theta > PI) Direction.RIGHT else Direction.LEFT
|
||||||
|
} else if (start.y > circle.center.y) {
|
||||||
|
if (start.theta < PI) Direction.RIGHT else Direction.LEFT
|
||||||
|
} else {
|
||||||
|
if (start.theta == 0.0) {
|
||||||
|
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||||
|
} else {
|
||||||
|
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
@ -0,0 +1,5 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
|
public interface Segment {
|
||||||
|
public val length: Double
|
||||||
|
}
|
@ -0,0 +1,18 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.dubins.theta
|
||||||
|
import kotlin.math.PI
|
||||||
|
import kotlin.math.atan2
|
||||||
|
|
||||||
|
public data class Straight(
|
||||||
|
internal val start: Vector2D,
|
||||||
|
internal val end: Vector2D
|
||||||
|
) : Segment {
|
||||||
|
override val length: Double
|
||||||
|
get() = start.distanceTo(end)
|
||||||
|
|
||||||
|
internal val theta: Double
|
||||||
|
get() = theta(atan2(end.x - start.x, end.y - start.y))
|
||||||
|
}
|
@ -0,0 +1,11 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments.components
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import kotlin.math.PI
|
||||||
|
|
||||||
|
public open class Circle(
|
||||||
|
internal val center: Vector2D,
|
||||||
|
internal val radius: Double
|
||||||
|
) {
|
||||||
|
internal val circumference = radius * 2 * PI
|
||||||
|
}
|
@ -0,0 +1,13 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments.components
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
|
||||||
|
public data class Pose2D(
|
||||||
|
override val x: Double,
|
||||||
|
override val y: Double,
|
||||||
|
public val theta: Double
|
||||||
|
) : Vector2D {
|
||||||
|
internal companion object {
|
||||||
|
internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,26 @@
|
|||||||
|
package space.kscience.kmath.trajectory
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.segments.Straight
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||||
|
import kotlin.math.PI
|
||||||
|
import kotlin.math.abs
|
||||||
|
import kotlin.math.sin
|
||||||
|
|
||||||
|
const val maxFloatDelta = 0.000001
|
||||||
|
|
||||||
|
fun Double.radiansToDegrees() = this * 180 / PI
|
||||||
|
|
||||||
|
fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
|
||||||
|
fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
|
||||||
|
|
||||||
|
fun Straight.inverse() = Straight(end, start)
|
||||||
|
fun Straight.shift(shift: Int, width: Double): Straight {
|
||||||
|
val dX = width * sin(inverse().theta)
|
||||||
|
val dY = width * sin(theta)
|
||||||
|
|
||||||
|
return Straight(
|
||||||
|
Vector2D(start.x - dX * shift, start.y - dY * shift),
|
||||||
|
Vector2D(end.x - dX * shift, end.y - dY * shift)
|
||||||
|
)
|
||||||
|
}
|
@ -0,0 +1,66 @@
|
|||||||
|
/*
|
||||||
|
* Copyright 2018-2021 KMath contributors.
|
||||||
|
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
|
||||||
|
*/
|
||||||
|
|
||||||
|
package space.kscience.kmath.trajectory.dubins
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.equalFloat
|
||||||
|
import space.kscience.kmath.trajectory.equalsFloat
|
||||||
|
import space.kscience.kmath.trajectory.inverse
|
||||||
|
import space.kscience.kmath.trajectory.segments.Arc
|
||||||
|
import space.kscience.kmath.trajectory.segments.Straight
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||||
|
import space.kscience.kmath.trajectory.shift
|
||||||
|
import kotlin.test.Test
|
||||||
|
import kotlin.test.assertNotNull
|
||||||
|
import kotlin.test.assertTrue
|
||||||
|
|
||||||
|
|
||||||
|
class DubinsTests {
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun dubinsTest() {
|
||||||
|
val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
|
||||||
|
val lineP1 = straight.shift(1, 10.0).inverse()
|
||||||
|
|
||||||
|
val start = Pose2D.of(straight.end, straight.theta)
|
||||||
|
val end = Pose2D.of(lineP1.start, lineP1.theta)
|
||||||
|
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 -> p.type === it.key }
|
||||||
|
assertNotNull(path, "Path ${it.key} not found")
|
||||||
|
println("${it.key}: ${path.length}")
|
||||||
|
assertTrue(it.value.equalFloat(path.length))
|
||||||
|
|
||||||
|
assertTrue(start.equalsFloat(path.a.start))
|
||||||
|
assertTrue(end.equalsFloat(path.c.end))
|
||||||
|
|
||||||
|
// Not working, theta double precision inaccuracy
|
||||||
|
if (path.b is Arc) {
|
||||||
|
val b = path.b as Arc
|
||||||
|
assertTrue(path.a.end.equalsFloat(b.start))
|
||||||
|
assertTrue(path.c.start.equalsFloat(b.end))
|
||||||
|
} else if (path.b is Straight) {
|
||||||
|
val b = path.b as Straight
|
||||||
|
assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
|
||||||
|
assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,19 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.radiansToDegrees
|
||||||
|
import space.kscience.kmath.trajectory.segments.components.Circle
|
||||||
|
import kotlin.test.Test
|
||||||
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
|
class ArcTests {
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun arcTest() {
|
||||||
|
val circle = Circle(Vector2D(0.0, 0.0), 2.0)
|
||||||
|
val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
|
||||||
|
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||||
|
assertEquals(0.0, arc.start.theta.radiansToDegrees())
|
||||||
|
assertEquals(90.0, arc.end.theta.radiansToDegrees())
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,32 @@
|
|||||||
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.radiansToDegrees
|
||||||
|
import kotlin.math.pow
|
||||||
|
import kotlin.math.sqrt
|
||||||
|
import kotlin.test.Test
|
||||||
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
|
class LineTests {
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun lineTest() {
|
||||||
|
val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
|
||||||
|
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
|
||||||
|
assertEquals(45.0, straight.theta.radiansToDegrees())
|
||||||
|
}
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun lineAngleTest() {
|
||||||
|
val zero = Vector2D(0.0, 0.0)
|
||||||
|
val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
|
||||||
|
assertEquals(0.0, north.theta.radiansToDegrees())
|
||||||
|
val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
|
||||||
|
assertEquals(90.0, east.theta.radiansToDegrees())
|
||||||
|
val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
|
||||||
|
assertEquals(180.0, south.theta.radiansToDegrees())
|
||||||
|
val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
|
||||||
|
assertEquals(270.0, west.theta.radiansToDegrees())
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,23 @@
|
|||||||
|
/*
|
||||||
|
* Copyright 2018-2021 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.components
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.Vector2D
|
||||||
|
import space.kscience.kmath.trajectory.maxFloatDelta
|
||||||
|
import kotlin.test.Test
|
||||||
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
|
class CircleTests {
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun arcTest() {
|
||||||
|
val center = Vector2D(0.0, 0.0)
|
||||||
|
val radius = 2.0
|
||||||
|
val expectedCircumference = 12.56637
|
||||||
|
val circle = Circle(center, radius)
|
||||||
|
assertEquals(expectedCircumference, circle.circumference, maxFloatDelta)
|
||||||
|
}
|
||||||
|
}
|
@ -44,6 +44,7 @@ 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