diff --git a/kmath-commons/src/main/kotlin/space/kscience/kmath/commons/transform/Transformations.kt b/kmath-commons/src/main/kotlin/space/kscience/kmath/commons/transform/Transformations.kt index 2243dc5d9..a77da2d2f 100644 --- a/kmath-commons/src/main/kotlin/space/kscience/kmath/commons/transform/Transformations.kt +++ b/kmath-commons/src/main/kotlin/space/kscience/kmath/commons/transform/Transformations.kt @@ -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.toArray(): Array = + private fun Buffer.toCmComplexArray(): Array = Array(size) { org.apache.commons.math3.complex.Complex(get(it).re, get(it).im) } - private fun Buffer.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 = { - FastFourierTransformer(normalization).transform(it.toArray(), direction).asBuffer() + ): BufferTransform = BufferTransform { + FastFourierTransformer(normalization).transform(it.toCmComplexArray(), direction).asBuffer() } public fun realFourier( normalization: DftNormalization = DftNormalization.STANDARD, direction: TransformType = TransformType.FORWARD, - ): SuspendBufferTransform = { - FastFourierTransformer(normalization).transform(it.asArray(), direction).asBuffer() + ): BufferTransform = BufferTransform { + FastFourierTransformer(normalization).transform(it.toDoubleArray(), direction).asBuffer() } public fun sine( normalization: DstNormalization = DstNormalization.STANDARD_DST_I, direction: TransformType = TransformType.FORWARD, - ): SuspendBufferTransform = { - FastSineTransformer(normalization).transform(it.asArray(), direction).asBuffer() + ): BufferTransform = DoubleBufferTransform { + FastSineTransformer(normalization).transform(it.array, direction).asBuffer() } public fun cosine( normalization: DctNormalization = DctNormalization.STANDARD_DCT_I, direction: TransformType = TransformType.FORWARD, - ): SuspendBufferTransform = { - FastCosineTransformer(normalization).transform(it.asArray(), direction).asBuffer() + ): BufferTransform = BufferTransform { + FastCosineTransformer(normalization).transform(it.toDoubleArray(), direction).asBuffer() } public fun hadamard( direction: TransformType = TransformType.FORWARD, - ): SuspendBufferTransform = { - FastHadamardTransformer().transform(it.asArray(), direction).asBuffer() + ): BufferTransform = DoubleBufferTransform { + FastHadamardTransformer().transform(it.array, direction).asBuffer() } } /** * Process given [Flow] with commons-math fft transformation */ -@FlowPreview -public fun Flow>.FFT( +public fun Flow>.fft( normalization: DftNormalization = DftNormalization.STANDARD, direction: TransformType = TransformType.FORWARD, ): Flow> { val transform = Transformations.fourier(normalization, direction) - return map { transform(it) } + return map(transform::transform) } -@FlowPreview @JvmName("realFFT") -public fun Flow>.FFT( +public fun Flow>.fft( normalization: DftNormalization = DftNormalization.STANDARD, direction: TransformType = TransformType.FORWARD, ): Flow> { 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.FFT( +public fun Flow.fft( bufferSize: Int = Int.MAX_VALUE, normalization: DftNormalization = DftNormalization.STANDARD, direction: TransformType = TransformType.FORWARD, -): Flow = chunked(bufferSize).FFT(normalization, direction).spread() +): Flow = chunked(bufferSize).fft(normalization, direction).spread() /** * Map a complex flow into real flow by taking real part of each number diff --git a/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Complex.kt b/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Complex.kt index 5804e47c1..7bf8af4e8 100644 --- a/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Complex.kt +++ b/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Complex.kt @@ -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) diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/operations/bufferOperation.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/operations/bufferOperation.kt index 3d25d8750..0a5d6b964 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/operations/bufferOperation.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/operations/bufferOperation.kt @@ -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 = (Buffer) -> Buffer +public fun interface BufferTransform { + public fun transform(arg: Buffer): Buffer +} -/** - * Typealias for buffer transformations with suspend function. - */ -public typealias SuspendBufferTransform = suspend (Buffer) -> Buffer +///** +// * Type alias for buffer transformations with suspend function. +// */ +//public fun interface SuspendBufferTransform{ +// public suspend fun transform(arg: Buffer): Buffer +//} /** diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/structures/DoubleBuffer.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/structures/DoubleBuffer.kt index 4dd9003f3..5d80f2b1f 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/structures/DoubleBuffer.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/structures/DoubleBuffer.kt @@ -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 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.toDoubleArray(): DoubleArray = when (this) { - is DoubleBuffer -> array.copyOf() + is DoubleBuffer -> array else -> DoubleArray(size, ::get) } +public fun Buffer.toDoubleBuffer(): DoubleBuffer = when (this) { + is DoubleBuffer -> this + else -> DoubleArray(size, ::get).asBuffer() +} + /** * Returns [DoubleBuffer] over this array. * @@ -62,3 +69,10 @@ public fun Buffer.toDoubleArray(): DoubleArray = when (this) { * @return the new buffer. */ public fun DoubleArray.asBuffer(): DoubleBuffer = DoubleBuffer(this) + + +public fun interface DoubleBufferTransform : BufferTransform { + public fun transform(arg: DoubleBuffer): DoubleBuffer + + override fun transform(arg: Buffer): DoubleBuffer = arg.toDoubleBuffer() +} diff --git a/kmath-core/src/jvmTest/kotlin/space/kscience/kmath/misc/JBigTest.kt b/kmath-core/src/jvmTest/kotlin/space/kscience/kmath/misc/JBigTest.kt new file mode 100644 index 000000000..f7f8027e6 --- /dev/null +++ b/kmath-core/src/jvmTest/kotlin/space/kscience/kmath/misc/JBigTest.kt @@ -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) + } +} \ No newline at end of file diff --git a/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/streaming/BufferFlow.kt b/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/streaming/BufferFlow.kt index 00c874751..8aa5a937c 100644 --- a/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/streaming/BufferFlow.kt +++ b/kmath-coroutines/src/commonMain/kotlin/space/kscience/kmath/streaming/BufferFlow.kt @@ -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 Buffer.asFlow(): Flow = iterator().asFlow() /** * Flat map a [Flow] of [Buffer] into continuous [Flow] of elements */ -@FlowPreview public fun Flow>.spread(): Flow = flatMapConcat { it.asFlow() } /** diff --git a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensor.kt b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensor.kt index e9589eb0a..d3308a69f 100644 --- a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensor.kt +++ b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensor.kt @@ -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() diff --git a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensorAlgebra.kt b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensorAlgebra.kt index f563d00ae..67248890c 100644 --- a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensorAlgebra.kt +++ b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/DoubleTensorAlgebra.kt @@ -59,20 +59,16 @@ public open class DoubleTensorAlgebra : } } - public inline fun Tensor.mapIndexedInPlace(operation: (IntArray, Double) -> Double) { - indices.forEach { set(it, operation(it, get(it))) } + public inline fun Tensor.mapIndexedInPlace(operation: DoubleField.(IntArray, Double) -> Double) { + indices.forEach { set(it, DoubleField.operation(it, get(it))) } } @Suppress("OVERRIDE_BY_INLINE") final override inline fun StructureND.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, @@ -92,7 +88,7 @@ public open class DoubleTensorAlgebra : public inline fun StructureND.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.valueOrNull(): Double? { val dt = asDoubleTensor() diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt index e90e3de41..6a340f6b9 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -23,64 +23,65 @@ internal fun Pose2D.getTangentCircles(radius: Double): Pair 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) } } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt index 03b0fcec9..1085f7847 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt @@ -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) } } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt index 3b2535cee..64513f6e2 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt @@ -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) ) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt index 166567475..b545b7c94 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -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))) } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt index 86c79a5d6..17277c35e 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -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()) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt index c5e88c1f1..4b54d775c 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -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()) } }