kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

0.3.1-dev-11 #510

Merged
altavir merged 80 commits from dev into master 2023-04-05 18:46:36 +03:00
Conversation 0 Commits 80 Files Changed 264 +202 -138
14 changed files with 202 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6bf8d9d325 - Show all commits

53
kmath-commons/src/main/kotlin/space/kscience/kmath/commons/transform/Transformations.kt
View File

@ -10,28 +10,18 @@ import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.map
import org.apache.commons.math3.transform.*
import space.kscience.kmath.complex.Complex
import space.kscience.kmath.operations.SuspendBufferTransform
import space.kscience.kmath.operations.BufferTransform
import space.kscience.kmath.streaming.chunked
import space.kscience.kmath.streaming.spread
import space.kscience.kmath.structures.Buffer
import space.kscience.kmath.structures.DoubleBuffer
import space.kscience.kmath.structures.VirtualBuffer
import space.kscience.kmath.structures.asBuffer
import space.kscience.kmath.structures.*
/**
* Streaming and buffer transformations
* Streaming and buffer transformations with Commons-math algorithms
*/
public object Transformations {
private fun Buffer<Complex>.toArray(): Array<org.apache.commons.math3.complex.Complex> =
private fun Buffer<Complex>.toCmComplexArray(): Array<org.apache.commons.math3.complex.Complex> =
Array(size) { org.apache.commons.math3.complex.Complex(get(it).re, get(it).im) }
private fun Buffer<Double>.asArray() = if (this is DoubleBuffer) {
array
} else {
DoubleArray(size) { i -> get(i) }
}
/**
* Create a virtual buffer on top of array
*/
@ -43,70 +33,67 @@ public object Transformations {
public fun fourier(
normalization: DftNormalization = DftNormalization.STANDARD,
direction: TransformType = TransformType.FORWARD,
): SuspendBufferTransform<Complex, Complex> = {
FastFourierTransformer(normalization).transform(it.toArray(), direction).asBuffer()
): BufferTransform<Complex, Complex> = BufferTransform {
FastFourierTransformer(normalization).transform(it.toCmComplexArray(), direction).asBuffer()
}
public fun realFourier(
normalization: DftNormalization = DftNormalization.STANDARD,
direction: TransformType = TransformType.FORWARD,
): SuspendBufferTransform<Double, Complex> = {
FastFourierTransformer(normalization).transform(it.asArray(), direction).asBuffer()
): BufferTransform<Double, Complex> = BufferTransform {
FastFourierTransformer(normalization).transform(it.toDoubleArray(), direction).asBuffer()
}
public fun sine(
normalization: DstNormalization = DstNormalization.STANDARD_DST_I,
direction: TransformType = TransformType.FORWARD,
): SuspendBufferTransform<Double, Double> = {
FastSineTransformer(normalization).transform(it.asArray(), direction).asBuffer()
): BufferTransform<Double, Double> = DoubleBufferTransform {
FastSineTransformer(normalization).transform(it.array, direction).asBuffer()
}
public fun cosine(
normalization: DctNormalization = DctNormalization.STANDARD_DCT_I,
direction: TransformType = TransformType.FORWARD,
): SuspendBufferTransform<Double, Double> = {
FastCosineTransformer(normalization).transform(it.asArray(), direction).asBuffer()
): BufferTransform<Double, Double> = BufferTransform {
FastCosineTransformer(normalization).transform(it.toDoubleArray(), direction).asBuffer()
}
public fun hadamard(
direction: TransformType = TransformType.FORWARD,
): SuspendBufferTransform<Double, Double> = {
FastHadamardTransformer().transform(it.asArray(), direction).asBuffer()
): BufferTransform<Double, Double> = DoubleBufferTransform {
FastHadamardTransformer().transform(it.array, direction).asBuffer()
}
}
/**
* Process given [Flow] with commons-math fft transformation
*/
@FlowPreview
public fun Flow<Buffer<Complex>>.FFT(
public fun Flow<Buffer<Complex>>.fft(
normalization: DftNormalization = DftNormalization.STANDARD,
direction: TransformType = TransformType.FORWARD,
): Flow<Buffer<Complex>> {
val transform = Transformations.fourier(normalization, direction)
return map { transform(it) }
return map(transform::transform)
}
@FlowPreview
@JvmName("realFFT")
public fun Flow<Buffer<Double>>.FFT(
public fun Flow<Buffer<Double>>.fft(
normalization: DftNormalization = DftNormalization.STANDARD,
direction: TransformType = TransformType.FORWARD,
): Flow<Buffer<Complex>> {
val transform = Transformations.realFourier(normalization, direction)
return map(transform)
return map(transform::transform)
}
/**
* Process a continuous flow of real numbers in FFT splitting it in chunks of [bufferSize].
*/
@FlowPreview
@JvmName("realFFT")
public fun Flow<Double>.FFT(
public fun Flow<Double>.fft(
bufferSize: Int = Int.MAX_VALUE,
normalization: DftNormalization = DftNormalization.STANDARD,
direction: TransformType = TransformType.FORWARD,
): Flow<Complex> = chunked(bufferSize).FFT(normalization, direction).spread()
): Flow<Complex> = chunked(bufferSize).fft(normalization, direction).spread()
/**
* Map a complex flow into real flow by taking real part of each number

1
kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Complex.kt
View File

@ -193,7 +193,6 @@ public object ComplexField :
* @property re The real part.
* @property im The imaginary part.
*/
@OptIn(UnstableKMathAPI::class)
public data class Complex(val re: Double, val im: Double) {
public constructor(re: Number, im: Number) : this(re.toDouble(), im.toDouble())
public constructor(re: Number) : this(re.toDouble(), 0.0)

16
kmath-core/src/commonMain/kotlin/space/kscience/kmath/operations/bufferOperation.kt
View File

@ -9,14 +9,18 @@ import space.kscience.kmath.misc.UnstableKMathAPI
import space.kscience.kmath.structures.*
/**
* Typealias for buffer transformations.
* Type alias for buffer transformations.
*/
public typealias BufferTransform<T, R> = (Buffer<T>) -> Buffer<R>
public fun interface BufferTransform<T, R> {
public fun transform(arg: Buffer<T>): Buffer<R>
}
/**
* Typealias for buffer transformations with suspend function.
*/
public typealias SuspendBufferTransform<T, R> = suspend (Buffer<T>) -> Buffer<R>
///**
// * Type alias for buffer transformations with suspend function.
// */
//public fun interface SuspendBufferTransform<T, R>{
// public suspend fun transform(arg: Buffer<T>): Buffer<R>
//}
/**

20
kmath-core/src/commonMain/kotlin/space/kscience/kmath/structures/DoubleBuffer.kt
View File

@ -5,6 +5,7 @@
package space.kscience.kmath.structures
import space.kscience.kmath.operations.BufferTransform
import kotlin.jvm.JvmInline
/**
@ -28,7 +29,7 @@ public value class DoubleBuffer(public val array: DoubleArray) : MutableBuffer<D
override fun toString(): String = Buffer.toString(this)
public companion object{
public companion object {
public fun zero(size: Int): DoubleBuffer = DoubleArray(size).asBuffer()
}
}
@ -40,7 +41,8 @@ public value class DoubleBuffer(public val array: DoubleArray) : MutableBuffer<D
* The function [init] is called for each array element sequentially starting from the first one.
* It should return the value for a buffer element given its index.
*/
public inline fun DoubleBuffer(size: Int, init: (Int) -> Double): DoubleBuffer = DoubleBuffer(DoubleArray(size) { init(it) })
public inline fun DoubleBuffer(size: Int, init: (Int) -> Double): DoubleBuffer =
DoubleBuffer(DoubleArray(size) { init(it) })
/**
* Returns a new [DoubleBuffer] of given elements.
@ -51,10 +53,15 @@ public fun DoubleBuffer(vararg doubles: Double): DoubleBuffer = DoubleBuffer(dou
* Returns a new [DoubleArray] containing all the elements of this [Buffer].
*/
public fun Buffer<Double>.toDoubleArray(): DoubleArray = when (this) {
is DoubleBuffer -> array.copyOf()
is DoubleBuffer -> array
else -> DoubleArray(size, ::get)
}
public fun Buffer<Double>.toDoubleBuffer(): DoubleBuffer = when (this) {
is DoubleBuffer -> this
else -> DoubleArray(size, ::get).asBuffer()
}
/**
* Returns [DoubleBuffer] over this array.
*
@ -62,3 +69,10 @@ public fun Buffer<Double>.toDoubleArray(): DoubleArray = when (this) {
* @return the new buffer.
*/
public fun DoubleArray.asBuffer(): DoubleBuffer = DoubleBuffer(this)
public fun interface DoubleBufferTransform : BufferTransform<Double, Double> {
public fun transform(arg: DoubleBuffer): DoubleBuffer
override fun transform(arg: Buffer<Double>): DoubleBuffer = arg.toDoubleBuffer()
}

20
kmath-core/src/jvmTest/kotlin/space/kscience/kmath/misc/JBigTest.kt Normal file
View File

@ -0,0 +1,20 @@
/*
* 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.misc
import org.junit.jupiter.api.Test
import space.kscience.kmath.operations.JBigDecimalField
import kotlin.test.assertEquals
import kotlin.test.assertNotEquals
class JBigTest {
@Test
fun testExact() = with(JBigDecimalField) {
assertNotEquals(0.3, 0.1 + 0.2)
assertEquals(one * 0.3, one * 0.1 + one * 0.2)
}
}

7
kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/streaming/BufferFlow.kt
View File

@ -5,8 +5,10 @@
package space.kscience.kmath.streaming
import kotlinx.coroutines.FlowPreview
import kotlinx.coroutines.flow.*
import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.asFlow
import kotlinx.coroutines.flow.flatMapConcat
import kotlinx.coroutines.flow.flow
import space.kscience.kmath.chains.BlockingDoubleChain
import space.kscience.kmath.structures.Buffer
import space.kscience.kmath.structures.BufferFactory
@ -20,7 +22,6 @@ public fun <T> Buffer<T>.asFlow(): Flow<T> = iterator().asFlow()
/**
* Flat map a [Flow] of [Buffer] into continuous [Flow] of elements
*/
@FlowPreview
public fun <T> Flow<Buffer<T>>.spread(): Flow<T> = flatMapConcat { it.asFlow() }
/**

2
kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensor.kt
View File

@ -28,7 +28,7 @@ public class OffsetDoubleBuffer(
override fun get(index: Int): Double = source[index + offset]
/**
* Copy only a part of buffer that belongs to this tensor
* Copy only a part of buffer that belongs to this [OffsetDoubleBuffer]
*/
override fun copy(): DoubleBuffer = source.array.copyOfRange(offset, offset + size).asBuffer()

14
kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensorAlgebra.kt
View File

@ -59,20 +59,16 @@ public open class DoubleTensorAlgebra :
}
}
public inline fun Tensor<Double>.mapIndexedInPlace(operation: (IntArray, Double) -> Double) {
indices.forEach { set(it, operation(it, get(it))) }
public inline fun Tensor<Double>.mapIndexedInPlace(operation: DoubleField.(IntArray, Double) -> Double) {
indices.forEach { set(it, DoubleField.operation(it, get(it))) }
}
@Suppress("OVERRIDE_BY_INLINE")
final override inline fun StructureND<Double>.mapIndexed(transform: DoubleField.(index: IntArray, Double) -> Double): DoubleTensor {
val tensor = this.asDoubleTensor()
//TODO remove additional copy
val buffer = DoubleBuffer(tensor.source.size) {
DoubleField.transform(tensor.indices.index(it), tensor.source[it])
}
return DoubleTensor(tensor.shape, buffer)
return copyToTensor().apply { mapIndexedInPlace(transform) }
}
@Suppress("OVERRIDE_BY_INLINE")
final override inline fun zip(
left: StructureND<Double>,
@ -92,7 +88,7 @@ public open class DoubleTensorAlgebra :
public inline fun StructureND<Double>.reduceElements(transform: (DoubleBuffer) -> Double): Double =
transform(asDoubleTensor().source.copy())
//TODO do we need protective copy?
//TODO Add read-only DoubleBuffer wrapper. To avoid protective copy
override fun StructureND<Double>.valueOrNull(): Double? {
val dt = asDoubleTensor()

163
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
View File

@ -23,64 +23,65 @@ internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D>
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
}
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, ArcSegment.Direction.LEFT)
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b, CircleTrajectory.Direction.LEFT)
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b,
ArcSegment.Direction.RIGHT
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b,
CircleTrajectory.Direction.RIGHT
)
private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment = with(Euclidean2DSpace){
val centers = StraightSegment(a.center, b.center)
private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory = with(Euclidean2DSpace){
val centers = StraightTrajectory(a.center, b.center)
val p1 = when (side) {
ArcSegment.Direction.LEFT -> vector(
CircleTrajectory.Direction.LEFT -> vector(
a.center.x - a.radius * cos(centers.theta),
a.center.y + a.radius * sin(centers.theta)
)
ArcSegment.Direction.RIGHT -> vector(
CircleTrajectory.Direction.RIGHT -> vector(
a.center.x + a.radius * cos(centers.theta),
a.center.y - a.radius * sin(centers.theta)
)
}
return StraightSegment(
return StraightTrajectory(
p1,
vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
)
}
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, ArcSegment.Direction.LEFT)
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? =
innerTangent(base, direction, CircleTrajectory.Direction.LEFT)
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, ArcSegment.Direction.RIGHT)
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? =
innerTangent(base, direction, CircleTrajectory.Direction.RIGHT)
private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? = with(Euclidean2DSpace){
val centers = StraightSegment(base.center, direction.center)
private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory? = with(Euclidean2DSpace){
val centers = StraightTrajectory(base.center, direction.center)
if (centers.length < base.radius * 2) return null
val angle = theta(
when (side) {
ArcSegment.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
ArcSegment.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
CircleTrajectory.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
CircleTrajectory.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
}
)
val dX = base.radius * sin(angle)
val dY = base.radius * cos(angle)
val p1 = vector(base.center.x + dX, base.center.y + dY)
val p2 = vector(direction.center.x - dX, direction.center.y - dY)
return StraightSegment(p1, p2)
return StraightTrajectory(p1, p2)
}
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
@Suppress("DuplicatedCode")
public class DubinsPath(
public val a: ArcSegment,
public val a: CircleTrajectory,
public val b: Trajectory,
public val c: ArcSegment,
public val c: CircleTrajectory,
) : CompositeTrajectory(listOf(a,b,c)) {
public val type: TYPE = TYPE.valueOf(
arrayOf(
a.direction.name[0],
if (b is ArcSegment) b.direction.name[0] else 'S',
if (b is CircleTrajectory) b.direction.name[0] else 'S',
c.direction.name[0]
).toCharArray().concatToString()
)
@ -106,56 +107,98 @@ public class DubinsPath(
public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
all(start, end, turningRadius).minBy { it.length }
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace){
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val centers = StraightSegment(c1.center, c2.center)
val centers = StraightTrajectory(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 = 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 = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.RIGHT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, a2, a3)
val firstVariant = run {
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
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 = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT)
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT)
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT)
DubinsPath(a1, a2, a3)
}
val secondVariant = run {
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
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 = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT)
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT)
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT)
DubinsPath(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath?= with(Euclidean2DSpace) {
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val centers = StraightSegment(c1.center, c2.center)
val centers = StraightTrajectory(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 = 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 = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.LEFT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT)
return DubinsPath(a1, a2, a3)
val firstVariant = run {
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
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 = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT)
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT)
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT)
DubinsPath(a1, a2, a3)
}
val secondVariant = run{
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
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 = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT)
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT)
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT)
DubinsPath(a1, a2, a3)
}
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
}
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 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT)
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT)
return DubinsPath(a1, s, a3)
}
@ -163,8 +206,8 @@ public class DubinsPath(
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = rightOuterTangent(c1, c2)
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT)
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT)
return DubinsPath(a1, s, a3)
}
@ -174,8 +217,8 @@ public class DubinsPath(
val s = rightInnerTangent(c1, c2)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT)
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT)
return DubinsPath(a1, s, a3)
}
@ -185,8 +228,8 @@ public class DubinsPath(
val s = leftInnerTangent(c1, c2)
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT)
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT)
return DubinsPath(a1, s, a3)
}
}

12
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt
View File

@ -19,7 +19,7 @@ public sealed interface Trajectory {
/**
* Straight path segment. The order of start and end defines the direction
*/
public data class StraightSegment(
public data class StraightTrajectory(
internal val start: DoubleVector2D,
internal val end: DoubleVector2D,
) : Trajectory {
@ -31,7 +31,7 @@ public data class StraightSegment(
/**
* An arc segment
*/
public data class ArcSegment(
public data class CircleTrajectory(
public val circle: Circle2D,
public val start: Pose2D,
public val end: Pose2D,
@ -68,7 +68,7 @@ public data class ArcSegment(
}
public companion object {
public fun of(center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, direction: Direction): ArcSegment {
public fun of(center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, direction: Direction): CircleTrajectory {
fun calculatePose(
vector: DoubleVector2D,
theta: Double,
@ -81,11 +81,11 @@ public data class ArcSegment(
}
)
val s1 = StraightSegment(center, start)
val s2 = StraightSegment(center, end)
val s1 = StraightTrajectory(center, start)
val s2 = StraightTrajectory(center, end)
val pose1 = calculatePose(start, s1.theta, direction)
val pose2 = calculatePose(end, s2.theta, direction)
return ArcSegment(Circle2D(center, s1.length), pose1, pose2)
return CircleTrajectory(Circle2D(center, s1.length), pose1, pose2)
}
}
}

6
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
View File

@ -17,12 +17,12 @@ 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 StraightSegment.inverse() = StraightSegment(end, start)
fun StraightSegment.shift(shift: Int, width: Double): StraightSegment = with(Euclidean2DSpace){
fun StraightTrajectory.inverse() = StraightTrajectory(end, start)
fun StraightTrajectory.shift(shift: Int, width: Double): StraightTrajectory = with(Euclidean2DSpace){
val dX = width * sin(inverse().theta)
val dY = width * sin(theta)
return StraightSegment(
return StraightTrajectory(
vector(start.x - dX * shift, start.y - dY * shift),
vector(end.x - dX * shift, end.y - dY * shift)
)

10
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
View File

@ -16,7 +16,7 @@ class DubinsTests {
@Test
fun dubinsTest() = with(Euclidean2DSpace){
val straight = StraightSegment(vector(0.0, 0.0), vector(100.0, 100.0))
val straight = StraightTrajectory(vector(0.0, 0.0), vector(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = Pose2D(straight.end, straight.theta)
@ -45,12 +45,12 @@ class DubinsTests {
assertTrue(end.equalsFloat(path.c.end))
// Not working, theta double precision inaccuracy
if (path.b is ArcSegment) {
val b = path.b as ArcSegment
if (path.b is CircleTrajectory) {
val b = path.b as CircleTrajectory
assertTrue(path.a.end.equalsFloat(b.start))
assertTrue(path.c.start.equalsFloat(b.end))
} else if (path.b is StraightSegment) {
val b = path.b as StraightSegment
} else if (path.b is StraightTrajectory) {
val b = path.b as StraightTrajectory
assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta)))
assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta)))
}

4
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
View File

@ -8,7 +8,7 @@ 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.trajectory.ArcSegment
import space.kscience.kmath.trajectory.CircleTrajectory
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.test.Test
import kotlin.test.assertEquals
@ -18,7 +18,7 @@ class ArcTests {
@Test
fun arcTest() = with(Euclidean2DSpace){
val circle = Circle2D(vector(0.0, 0.0), 2.0)
val arc = ArcSegment.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), ArcSegment.Direction.RIGHT)
val arc = CircleTrajectory.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory.Direction.RIGHT)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.start.theta.radiansToDegrees())
assertEquals(90.0, arc.end.theta.radiansToDegrees())

12
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
View File

@ -6,7 +6,7 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.trajectory.StraightSegment
import space.kscience.kmath.trajectory.StraightTrajectory
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.math.pow
import kotlin.math.sqrt
@ -17,7 +17,7 @@ class LineTests {
@Test
fun lineTest() = with(Euclidean2DSpace){
val straight = StraightSegment(vector(0.0, 0.0), vector(100.0, 100.0))
val straight = StraightTrajectory(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.theta.radiansToDegrees())
}
@ -25,13 +25,13 @@ class LineTests {
@Test
fun lineAngleTest() = with(Euclidean2DSpace){
//val zero = Vector2D(0.0, 0.0)
val north = StraightSegment(Euclidean2DSpace.zero, vector(0.0, 2.0))
val north = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, 2.0))
assertEquals(0.0, north.theta.radiansToDegrees())
val east = StraightSegment(Euclidean2DSpace.zero, vector(2.0, 0.0))
val east = StraightTrajectory(Euclidean2DSpace.zero, vector(2.0, 0.0))
assertEquals(90.0, east.theta.radiansToDegrees())
val south = StraightSegment(Euclidean2DSpace.zero, vector(0.0, -2.0))
val south = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, -2.0))
assertEquals(180.0, south.theta.radiansToDegrees())
val west = StraightSegment(Euclidean2DSpace.zero, vector(-2.0, 0.0))
val west = StraightTrajectory(Euclidean2DSpace.zero, vector(-2.0, 0.0))
assertEquals(270.0, west.theta.radiansToDegrees())
}
}