0.3.1-dev-11 #510

Merged
altavir merged 80 commits from dev into master 2023-04-05 18:46:36 +03:00
14 changed files with 202 additions and 138 deletions
Showing only changes of commit 6bf8d9d325 - Show all commits

View File

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

View File

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

View File

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

View File

@ -5,6 +5,7 @@
package space.kscience.kmath.structures package space.kscience.kmath.structures
import space.kscience.kmath.operations.BufferTransform
import kotlin.jvm.JvmInline 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) override fun toString(): String = Buffer.toString(this)
public companion object{ public companion object {
public fun zero(size: Int): DoubleBuffer = DoubleArray(size).asBuffer() 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. * 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. * 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. * 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]. * Returns a new [DoubleArray] containing all the elements of this [Buffer].
*/ */
public fun Buffer<Double>.toDoubleArray(): DoubleArray = when (this) { public fun Buffer<Double>.toDoubleArray(): DoubleArray = when (this) {
is DoubleBuffer -> array.copyOf() is DoubleBuffer -> array
else -> DoubleArray(size, ::get) 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. * Returns [DoubleBuffer] over this array.
* *
@ -62,3 +69,10 @@ public fun Buffer<Double>.toDoubleArray(): DoubleArray = when (this) {
* @return the new buffer. * @return the new buffer.
*/ */
public fun DoubleArray.asBuffer(): DoubleBuffer = DoubleBuffer(this) 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()
}

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)
}
}

View File

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

View File

@ -28,7 +28,7 @@ public class OffsetDoubleBuffer(
override fun get(index: Int): Double = source[index + offset] 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() override fun copy(): DoubleBuffer = source.array.copyOfRange(offset, offset + size).asBuffer()

View File

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

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

View File

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

View File

@ -17,12 +17,12 @@ fun Double.radiansToDegrees() = this * 180 / PI
fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta 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 Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
fun StraightSegment.inverse() = StraightSegment(end, start) fun StraightTrajectory.inverse() = StraightTrajectory(end, start)
fun StraightSegment.shift(shift: Int, width: Double): StraightSegment = with(Euclidean2DSpace){ fun StraightTrajectory.shift(shift: Int, width: Double): StraightTrajectory = with(Euclidean2DSpace){
val dX = width * sin(inverse().theta) val dX = width * sin(inverse().theta)
val dY = width * sin(theta) val dY = width * sin(theta)
return StraightSegment( return StraightTrajectory(
vector(start.x - dX * shift, start.y - dY * shift), vector(start.x - dX * shift, start.y - dY * shift),
vector(end.x - dX * shift, end.y - dY * shift) vector(end.x - dX * shift, end.y - dY * shift)
) )

View File

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

View File

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

View File

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