From ec77cd1fc324df8cfba80272186e3e3c40184522 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 21 Aug 2022 19:17:38 +0300 Subject: [PATCH] Geometry overhaul --- kmath-dimensions/build.gradle.kts | 3 +- kmath-geometry/build.gradle.kts | 7 +- .../space/kscience/kmath/geometry/Circle2D.kt | 2 +- .../kmath/geometry/Euclidean2DSpace.kt | 61 ++++++++++----- .../kmath/geometry/Euclidean3DSpace.kt | 76 ++++++++++--------- .../kscience/kmath/geometry/GeometrySpace.kt | 3 +- .../space/kscience/kmath/geometry/Line.kt | 9 ++- .../kscience/kmath/geometry/rotations3D.kt | 14 ++-- .../kmath/geometry/Euclidean2DSpaceTest.kt | 40 +++++----- .../kmath/geometry/Euclidean3DSpaceTest.kt | 50 ++++++------ .../kmath/geometry/ProjectionAlongTest.kt | 20 ++--- .../kmath/geometry/ProjectionOntoLineTest.kt | 46 +++++------ .../kscience/kmath/geometry/RotationTest.kt | 4 +- .../kscience/kmath/geometry/Vector2DTest.kt | 2 +- .../kscience/kmath/geometry/Vector3DTest.kt | 2 +- .../kscience/kmath/geometry/testUtils.kt | 4 +- .../kscience/kmath/geometry/lineExtensions.kt | 20 +++++ .../kscience/kmath/trajectory/DubinsPath.kt | 36 ++++----- .../space/kscience/kmath/trajectory/Pose2D.kt | 18 ++--- .../kscience/kmath/trajectory/Trajectory.kt | 10 +-- .../space/kscience/kmath/trajectory/Math.kt | 8 +- .../kmath/trajectory/dubins/DubinsTests.kt | 7 +- .../kmath/trajectory/segments/ArcTests.kt | 8 +- .../kmath/trajectory/segments/CircleTests.kt | 4 +- .../kmath/trajectory/segments/LineTests.kt | 15 ++-- 25 files changed, 260 insertions(+), 209 deletions(-) create mode 100644 kmath-geometry/src/jvmMain/kotlin/space/kscience/kmath/geometry/lineExtensions.kt diff --git a/kmath-dimensions/build.gradle.kts b/kmath-dimensions/build.gradle.kts index ee1950fa8..1c042ab70 100644 --- a/kmath-dimensions/build.gradle.kts +++ b/kmath-dimensions/build.gradle.kts @@ -1,6 +1,5 @@ plugins { - kotlin("multiplatform") - id("space.kscience.gradle.common") + id("space.kscience.gradle.mpp") id("space.kscience.gradle.native") } diff --git a/kmath-geometry/build.gradle.kts b/kmath-geometry/build.gradle.kts index c9d8823bc..c2057bcbe 100644 --- a/kmath-geometry/build.gradle.kts +++ b/kmath-geometry/build.gradle.kts @@ -1,6 +1,5 @@ plugins { - kotlin("multiplatform") - id("space.kscience.gradle.common") + id("space.kscience.gradle.mpp") id("space.kscience.gradle.native") } @@ -10,6 +9,10 @@ kotlin.sourceSets.commonMain { } } +kscience { + withContextReceivers() +} + readme { maturity = space.kscience.gradle.Maturity.PROTOTYPE } diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt index 8623335b9..c665ae6bb 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt @@ -11,7 +11,7 @@ import kotlin.math.PI * A circle in 2D space */ public class Circle2D( - public val center: Vector2D, + public val center: DoubleVector2D, public val radius: Double ) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt index a83cb3ac7..f622bbb76 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean2DSpace.kt @@ -6,45 +6,64 @@ package space.kscience.kmath.geometry import space.kscience.kmath.linear.Point +import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.ScaleOperations -import space.kscience.kmath.operations.invoke +import kotlin.math.pow import kotlin.math.sqrt -public interface Vector2D : Point, Vector { - public val x: Double - public val y: Double +public interface Vector2D : Point, Vector { + public val x: T + public val y: T override val size: Int get() = 2 - override operator fun get(index: Int): Double = when (index) { + override operator fun get(index: Int): T = when (index) { 0 -> x 1 -> y else -> error("Accessing outside of point bounds") } - override operator fun iterator(): Iterator = listOf(x, y).iterator() + override operator fun iterator(): Iterator = iterator { + yield(x) + yield(y) + } } -public val Vector2D.r: Double - get() = Euclidean2DSpace { norm() } -public fun Vector2D(x: Double, y: Double): Vector2D = Vector2DImpl(x, y) +public operator fun Vector2D.component1(): T = x +public operator fun Vector2D.component2(): T = y + +public typealias DoubleVector2D = Vector2D + +public val Vector2D.r: Double get() = Euclidean2DSpace.norm(this) -private data class Vector2DImpl( - override val x: Double, - override val y: Double, -) : Vector2D /** * 2D Euclidean space */ -public object Euclidean2DSpace : GeometrySpace, ScaleOperations { - override val zero: Vector2D by lazy { Vector2D(0.0, 0.0) } +public object Euclidean2DSpace : GeometrySpace, + ScaleOperations, + Norm { - public fun Vector2D.norm(): Double = sqrt(x * x + y * y) - override fun Vector2D.unaryMinus(): Vector2D = Vector2D(-x, -y) + private data class Vector2DImpl( + override val x: Double, + override val y: Double, + ) : DoubleVector2D - override fun Vector2D.distanceTo(other: Vector2D): Double = (this - other).norm() - override fun add(left: Vector2D, right: Vector2D): Vector2D = Vector2D(left.x + right.x, left.y + right.y) - override fun scale(a: Vector2D, value: Double): Vector2D = Vector2D(a.x * value, a.y * value) - override fun Vector2D.dot(other: Vector2D): Double = x * other.x + y * other.y + public fun vector(x: Number, y: Number): DoubleVector2D = Vector2DImpl(x.toDouble(), y.toDouble()) + + override val zero: DoubleVector2D by lazy { vector(0.0, 0.0) } + + override fun norm(arg: DoubleVector2D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2)) + + override fun DoubleVector2D.unaryMinus(): DoubleVector2D = vector(-x, -y) + + override fun DoubleVector2D.distanceTo(other: DoubleVector2D): Double = norm(this - other) + override fun add(left: DoubleVector2D, right: DoubleVector2D): DoubleVector2D = + vector(left.x + right.x, left.y + right.y) + + override fun scale(a: DoubleVector2D, value: Double): DoubleVector2D = vector(a.x * value, a.y * value) + override fun DoubleVector2D.dot(other: DoubleVector2D): Double = x * other.x + y * other.y + + public val xAxis: DoubleVector2D = vector(1.0, 0.0) + public val yAxis: DoubleVector2D = vector(0.0, 1.0) } diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index 3bfd265b6..93be2df52 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -6,73 +6,79 @@ package space.kscience.kmath.geometry import space.kscience.kmath.linear.Point +import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.ScaleOperations -import space.kscience.kmath.operations.invoke import space.kscience.kmath.structures.Buffer +import kotlin.math.pow import kotlin.math.sqrt -public interface Vector3D : Point, Vector { - public val x: Double - public val y: Double - public val z: Double +public interface Vector3D : Point, Vector { + public val x: T + public val y: T + public val z: T override val size: Int get() = 3 - override operator fun get(index: Int): Double = when (index) { + override operator fun get(index: Int): T = when (index) { 0 -> x 1 -> y 2 -> z else -> error("Accessing outside of point bounds") } - override operator fun iterator(): Iterator = listOf(x, y, z).iterator() + override operator fun iterator(): Iterator = listOf(x, y, z).iterator() } -public operator fun Vector3D.component1(): Double = x -public operator fun Vector3D.component2(): Double = y -public operator fun Vector3D.component3(): Double = z +public operator fun Vector3D.component1(): T = x +public operator fun Vector3D.component2(): T = y +public operator fun Vector3D.component3(): T = z -public fun Buffer.asVector3D(): Vector3D = object : Vector3D { +public fun Buffer.asVector3D(): Vector3D = object : Vector3D { init { require(this@asVector3D.size == 3) { "Buffer of size 3 is required for Vector3D" } } - override val x: Double get() = this@asVector3D[0] - override val y: Double get() = this@asVector3D[1] - override val z: Double get() = this@asVector3D[2] + override val x: T get() = this@asVector3D[0] + override val y: T get() = this@asVector3D[1] + override val z: T get() = this@asVector3D[2] override fun toString(): String = this@asVector3D.toString() - } -public val Vector3D.r: Double get() = Euclidean3DSpace { norm() } +public typealias DoubleVector3D = Vector3D -private data class Vector3DImpl( - override val x: Double, - override val y: Double, - override val z: Double, -) : Vector3D +public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this) +public object Euclidean3DSpace : GeometrySpace, ScaleOperations, + Norm { + private data class Vector3DImpl( + override val x: Double, + override val y: Double, + override val z: Double, + ) : DoubleVector3D -public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z) + public fun vector(x: Number, y: Number, z: Number): DoubleVector3D = + Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble()) -public object Euclidean3DSpace : GeometrySpace, ScaleOperations { - override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) } + override val zero: DoubleVector3D by lazy { vector(0.0, 0.0, 0.0) } - public fun Vector3D.norm(): Double = sqrt(x * x + y * y + z * z) - override fun Vector3D.unaryMinus(): Vector3D = Vector3D(-x, -y, -z) + override fun norm(arg: DoubleVector3D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2) + arg.z.pow(2)) - override fun Vector3D.distanceTo(other: Vector3D): Double = (this - other).norm() + public fun DoubleVector3D.norm(): Double = norm(this) - override fun add(left: Vector3D, right: Vector3D): Vector3D = - Vector3D(left.x + right.x, left.y + right.y, left.z + right.z) + override fun DoubleVector3D.unaryMinus(): DoubleVector3D = vector(-x, -y, -z) - override fun scale(a: Vector3D, value: Double): Vector3D = - Vector3D(a.x * value, a.y * value, a.z * value) + override fun DoubleVector3D.distanceTo(other: DoubleVector3D): Double = (this - other).norm() - override fun Vector3D.dot(other: Vector3D): Double = + override fun add(left: DoubleVector3D, right: DoubleVector3D): DoubleVector3D = + vector(left.x + right.x, left.y + right.y, left.z + right.z) + + override fun scale(a: DoubleVector3D, value: Double): DoubleVector3D = + vector(a.x * value, a.y * value, a.z * value) + + override fun DoubleVector3D.dot(other: DoubleVector3D): Double = x * other.x + y * other.y + z * other.z - public val xAxis: Vector3D = Vector3D(1.0, 0.0, 0.0) - public val yAxis: Vector3D = Vector3D(0.0, 1.0, 0.0) - public val zAxis: Vector3D = Vector3D(0.0, 0.0, 1.0) + public val xAxis: DoubleVector3D = vector(1.0, 0.0, 0.0) + public val yAxis: DoubleVector3D = vector(0.0, 1.0, 0.0) + public val zAxis: DoubleVector3D = vector(0.0, 0.0, 1.0) } diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/GeometrySpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/GeometrySpace.kt index d4245c744..b06ff9989 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/GeometrySpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/GeometrySpace.kt @@ -6,11 +6,12 @@ package space.kscience.kmath.geometry import space.kscience.kmath.operations.Group +import space.kscience.kmath.operations.Norm import space.kscience.kmath.operations.ScaleOperations public interface Vector -public interface GeometrySpace : Group, ScaleOperations { +public interface GeometrySpace : Group, ScaleOperations, Norm { /** * L2 distance */ diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt index 85bfcdd8f..2251af600 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt @@ -11,5 +11,10 @@ package space.kscience.kmath.geometry */ public data class Line(val base: V, val direction: V) -public typealias Line2D = Line -public typealias Line3D = Line +public typealias Line2D = Line +public typealias Line3D = Line + +/** + * A directed line segment between [begin] and [end] + */ +public data class LineSegment(val begin: V, val end: V) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt index c0d90463a..2500c78d6 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt @@ -15,7 +15,7 @@ import space.kscience.kmath.operations.DoubleField import kotlin.math.pow import kotlin.math.sqrt -internal fun Vector3D.toQuaternion(): Quaternion = Quaternion(0.0, x, y, z) +internal fun DoubleVector3D.toQuaternion(): Quaternion = Quaternion(0.0, x, y, z) /** * Angle in radians denoted by this quaternion rotation @@ -25,7 +25,7 @@ public val Quaternion.theta: Radians get() = (kotlin.math.acos(normalized().w) * /** * Create a normalized Quaternion from rotation angle and rotation vector */ -public fun Quaternion.Companion.fromRotation(theta: Angle, vector: Vector3D): Quaternion { +public fun Quaternion.Companion.fromRotation(theta: Angle, vector: DoubleVector3D): Quaternion { val s = sin(theta / 2) val c = cos(theta / 2) val norm = with(Euclidean3DSpace) { vector.norm() } @@ -35,9 +35,9 @@ public fun Quaternion.Companion.fromRotation(theta: Angle, vector: Vector3D): Qu /** * An axis of quaternion rotation */ -public val Quaternion.vector: Vector3D +public val Quaternion.vector: DoubleVector3D get() { - return object : Vector3D { + return object : DoubleVector3D { private val sint2 = sqrt(1 - w * w) override val x: Double get() = this@vector.x / sint2 override val y: Double get() = this@vector.y / sint2 @@ -49,7 +49,7 @@ public val Quaternion.vector: Vector3D /** * Rotate a vector in a [Euclidean3DSpace] */ -public fun Euclidean3DSpace.rotate(vector: Vector3D, q: Quaternion): Vector3D = with(QuaternionField) { +public fun Euclidean3DSpace.rotate(vector: DoubleVector3D, q: Quaternion): DoubleVector3D = with(QuaternionField) { val p = vector.toQuaternion() (q * p * q.reciprocal).vector } @@ -58,10 +58,10 @@ public fun Euclidean3DSpace.rotate(vector: Vector3D, q: Quaternion): Vector3D = * Use a composition of quaternions to create a rotation */ @UnstableKMathAPI -public fun Euclidean3DSpace.rotate(vector: Vector3D, composition: QuaternionField.() -> Quaternion): Vector3D = +public fun Euclidean3DSpace.rotate(vector: DoubleVector3D, composition: QuaternionField.() -> Quaternion): DoubleVector3D = rotate(vector, QuaternionField.composition()) -public fun Euclidean3DSpace.rotate(vector: Vector3D, matrix: Matrix): Vector3D { +public fun Euclidean3DSpace.rotate(vector: DoubleVector3D, matrix: Matrix): DoubleVector3D { require(matrix.colNum == 3 && matrix.rowNum == 3) { "Square 3x3 rotation matrix is required" } return with(DoubleField.linearSpace) { matrix.dot(vector).asVector3D() } } diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean2DSpaceTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean2DSpaceTest.kt index 6b5f474bc..8ca6ba248 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean2DSpaceTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean2DSpaceTest.kt @@ -12,16 +12,16 @@ import kotlin.test.assertEquals internal class Euclidean2DSpaceTest { @Test fun zero() { - assertVectorEquals(Vector2D(0.0, 0.0), Euclidean2DSpace.zero) + assertVectorEquals(Euclidean2DSpace.vector(0.0, 0.0), Euclidean2DSpace.zero) } @Test fun norm() { with(Euclidean2DSpace) { - assertEquals(0.0, zero.norm()) - assertEquals(1.0, Vector2D(1.0, 0.0).norm()) - assertEquals(sqrt(2.0), Vector2D(1.0, 1.0).norm()) - assertEquals(sqrt(5.002001), Vector2D(-2.0, 1.001).norm()) + assertEquals(0.0, norm(zero)) + assertEquals(1.0, norm(vector(1.0, 0.0))) + assertEquals(sqrt(2.0), norm(vector(1.0, 1.0))) + assertEquals(sqrt(5.002001), norm(vector(-2.0, 1.001))) } } @@ -29,16 +29,16 @@ internal class Euclidean2DSpaceTest { fun dotProduct() { with(Euclidean2DSpace) { assertEquals(0.0, zero dot zero) - assertEquals(0.0, zero dot Vector2D(1.0, 0.0)) - assertEquals(0.0, Vector2D(-2.0, 0.001) dot zero) - assertEquals(0.0, Vector2D(1.0, 0.0) dot Vector2D(0.0, 1.0)) + assertEquals(0.0, zero dot vector(1.0, 0.0)) + assertEquals(0.0, vector(-2.0, 0.001) dot zero) + assertEquals(0.0, vector(1.0, 0.0) dot vector(0.0, 1.0)) - assertEquals(1.0, Vector2D(1.0, 0.0) dot Vector2D(1.0, 0.0)) - assertEquals(-2.0, Vector2D(0.0, 1.0) dot Vector2D(1.0, -2.0)) - assertEquals(2.0, Vector2D(1.0, 1.0) dot Vector2D(1.0, 1.0)) - assertEquals(4.001001, Vector2D(-2.0, 1.001) dot Vector2D(-2.0, 0.001)) + assertEquals(1.0, vector(1.0, 0.0) dot vector(1.0, 0.0)) + assertEquals(-2.0, vector(0.0, 1.0) dot vector(1.0, -2.0)) + assertEquals(2.0, vector(1.0, 1.0) dot vector(1.0, 1.0)) + assertEquals(4.001001, vector(-2.0, 1.001) dot vector(-2.0, 0.001)) - assertEquals(-4.998, Vector2D(1.0, 2.0) dot Vector2D(-5.0, 0.001)) + assertEquals(-4.998, vector(1.0, 2.0) dot vector(-5.0, 0.001)) } } @@ -46,12 +46,12 @@ internal class Euclidean2DSpaceTest { fun add() { with(Euclidean2DSpace) { assertVectorEquals( - Vector2D(-2.0, 0.001), - Vector2D(-2.0, 0.001) + zero + vector(-2.0, 0.001), + vector(-2.0, 0.001) + zero ) assertVectorEquals( - Vector2D(-3.0, 3.001), - Vector2D(2.0, 3.0) + Vector2D(-5.0, 0.001) + vector(-3.0, 3.001), + vector(2.0, 3.0) + vector(-5.0, 0.001) ) } } @@ -59,9 +59,9 @@ internal class Euclidean2DSpaceTest { @Test fun multiply() { with(Euclidean2DSpace) { - assertVectorEquals(Vector2D(-4.0, 0.0), Vector2D(-2.0, 0.0) * 2) - assertVectorEquals(Vector2D(4.0, 0.0), Vector2D(-2.0, 0.0) * -2) - assertVectorEquals(Vector2D(300.0, 0.0003), Vector2D(100.0, 0.0001) * 3) + assertVectorEquals(vector(-4.0, 0.0), vector(-2.0, 0.0) * 2) + assertVectorEquals(vector(4.0, 0.0), vector(-2.0, 0.0) * -2) + assertVectorEquals(vector(300.0, 0.0003), vector(100.0, 0.0001) * 3) } } } diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt index 0bc91e77e..191ba54ab 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Euclidean3DSpaceTest.kt @@ -11,21 +11,21 @@ import kotlin.test.assertEquals internal class Euclidean3DSpaceTest { @Test fun zero() { - assertVectorEquals(Vector3D(0.0, 0.0, 0.0), Euclidean3DSpace.zero) + assertVectorEquals(Euclidean3DSpace.vector(0.0, 0.0, 0.0), Euclidean3DSpace.zero) } @Test fun distance() { with(Euclidean3DSpace) { assertEquals(0.0, zero.distanceTo(zero)) - assertEquals(1.0, zero.distanceTo(Vector3D(1.0, 0.0, 0.0))) - assertEquals(kotlin.math.sqrt(5.000001), Vector3D(1.0, -2.0, 0.001).distanceTo(zero)) - assertEquals(0.0, Vector3D(1.0, -2.0, 0.001).distanceTo(Vector3D(1.0, -2.0, 0.001))) - assertEquals(0.0, Vector3D(1.0, 0.0, 0.0).distanceTo(Vector3D(1.0, 0.0, 0.0))) - assertEquals(kotlin.math.sqrt(2.0), Vector3D(1.0, 0.0, 0.0).distanceTo(Vector3D(1.0, 1.0, 1.0))) - assertEquals(3.1622778182822584, Vector3D(0.0, 1.0, 0.0).distanceTo(Vector3D(1.0, -2.0, 0.001))) - assertEquals(0.0, Vector3D(1.0, -2.0, 0.001).distanceTo(Vector3D(1.0, -2.0, 0.001))) - assertEquals(9.695050335093676, Vector3D(1.0, 2.0, 3.0).distanceTo(Vector3D(7.0, -5.0, 0.001))) + assertEquals(1.0, zero.distanceTo(vector(1.0, 0.0, 0.0))) + assertEquals(kotlin.math.sqrt(5.000001), vector(1.0, -2.0, 0.001).distanceTo(zero)) + assertEquals(0.0, vector(1.0, -2.0, 0.001).distanceTo(vector(1.0, -2.0, 0.001))) + assertEquals(0.0, vector(1.0, 0.0, 0.0).distanceTo(vector(1.0, 0.0, 0.0))) + assertEquals(kotlin.math.sqrt(2.0), vector(1.0, 0.0, 0.0).distanceTo(vector(1.0, 1.0, 1.0))) + assertEquals(3.1622778182822584, vector(0.0, 1.0, 0.0).distanceTo(vector(1.0, -2.0, 0.001))) + assertEquals(0.0, vector(1.0, -2.0, 0.001).distanceTo(vector(1.0, -2.0, 0.001))) + assertEquals(9.695050335093676, vector(1.0, 2.0, 3.0).distanceTo(vector(7.0, -5.0, 0.001))) } } @@ -33,9 +33,9 @@ internal class Euclidean3DSpaceTest { fun norm() { with(Euclidean3DSpace) { assertEquals(0.0, zero.norm()) - assertEquals(1.0, Vector3D(1.0, 0.0, 0.0).norm()) - assertEquals(kotlin.math.sqrt(3.0), Vector3D(1.0, 1.0, 1.0).norm()) - assertEquals(kotlin.math.sqrt(5.000001), Vector3D(1.0, -2.0, 0.001).norm()) + assertEquals(1.0, vector(1.0, 0.0, 0.0).norm()) + assertEquals(kotlin.math.sqrt(3.0), vector(1.0, 1.0, 1.0).norm()) + assertEquals(kotlin.math.sqrt(5.000001), vector(1.0, -2.0, 0.001).norm()) } } @@ -43,16 +43,16 @@ internal class Euclidean3DSpaceTest { fun dotProduct() { with(Euclidean3DSpace) { assertEquals(0.0, zero dot zero) - assertEquals(0.0, zero dot Vector3D(1.0, 0.0, 0.0)) - assertEquals(0.0, Vector3D(1.0, -2.0, 0.001) dot zero) + assertEquals(0.0, zero dot vector(1.0, 0.0, 0.0)) + assertEquals(0.0, vector(1.0, -2.0, 0.001) dot zero) - assertEquals(1.0, Vector3D(1.0, 0.0, 0.0) dot Vector3D(1.0, 0.0, 0.0)) - assertEquals(1.0, Vector3D(1.0, 0.0, 0.0) dot Vector3D(1.0, 1.0, 1.0)) - assertEquals(-2.0, Vector3D(0.0, 1.0, 0.0) dot Vector3D(1.0, -2.0, 0.001)) - assertEquals(3.0, Vector3D(1.0, 1.0, 1.0) dot Vector3D(1.0, 1.0, 1.0)) - assertEquals(5.000001, Vector3D(1.0, -2.0, 0.001) dot Vector3D(1.0, -2.0, 0.001)) + assertEquals(1.0, vector(1.0, 0.0, 0.0) dot vector(1.0, 0.0, 0.0)) + assertEquals(1.0, vector(1.0, 0.0, 0.0) dot vector(1.0, 1.0, 1.0)) + assertEquals(-2.0, vector(0.0, 1.0, 0.0) dot vector(1.0, -2.0, 0.001)) + assertEquals(3.0, vector(1.0, 1.0, 1.0) dot vector(1.0, 1.0, 1.0)) + assertEquals(5.000001, vector(1.0, -2.0, 0.001) dot vector(1.0, -2.0, 0.001)) - assertEquals(-2.997, Vector3D(1.0, 2.0, 3.0) dot Vector3D(7.0, -5.0, 0.001)) + assertEquals(-2.997, vector(1.0, 2.0, 3.0) dot vector(7.0, -5.0, 0.001)) } } @@ -60,12 +60,12 @@ internal class Euclidean3DSpaceTest { fun add() { with(Euclidean3DSpace) { assertVectorEquals( - Vector3D(1.0, -2.0, 0.001), - Vector3D(1.0, -2.0, 0.001) + zero + vector(1.0, -2.0, 0.001), + vector(1.0, -2.0, 0.001) + zero ) assertVectorEquals( - Vector3D(8.0, -3.0, 3.001), - Vector3D(1.0, 2.0, 3.0) + Vector3D(7.0, -5.0, 0.001) + vector(8.0, -3.0, 3.001), + vector(1.0, 2.0, 3.0) + vector(7.0, -5.0, 0.001) ) } } @@ -73,7 +73,7 @@ internal class Euclidean3DSpaceTest { @Test fun multiply() { with(Euclidean3DSpace) { - assertVectorEquals(Vector3D(2.0, -4.0, 0.0), Vector3D(1.0, -2.0, 0.0) * 2) + assertVectorEquals(vector(2.0, -4.0, 0.0), vector(1.0, -2.0, 0.0) * 2) } } } diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionAlongTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionAlongTest.kt index dfb65a57c..cebbf5a4d 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionAlongTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionAlongTest.kt @@ -12,14 +12,14 @@ internal class ProjectionAlongTest { @Test fun projectionIntoYEqualsX() { with(Euclidean2DSpace) { - val normal = Vector2D(-2.0, 2.0) - val base = Vector2D(2.3, 2.3) + val normal = vector(-2.0, 2.0) + val base = vector(2.3, 2.3) assertVectorEquals(zero, projectAlong(zero, normal, base)) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> val d = (y - x) / 2.0 - assertVectorEquals(Vector2D(x + d, y - d), projectAlong(Vector2D(x, y), normal, base)) + assertVectorEquals(vector(x + d, y - d), projectAlong(vector(x, y), normal, base)) } } } @@ -30,28 +30,28 @@ internal class ProjectionAlongTest { val a = 5.0 val b = -3.0 val c = -15.0 - val normal = Vector2D(-5.0, 3.0) - val base = Vector2D(3.0, 0.0) + val normal = vector(-5.0, 3.0) + val base = vector(3.0, 0.0) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> val xProj = (b * (b * x - a * y) - a * c) / (a * a + b * b) val yProj = (a * (-b * x + a * y) - b * c) / (a * a + b * b) - assertVectorEquals(Vector2D(xProj, yProj), projectAlong(Vector2D(x, y), normal, base)) + assertVectorEquals(vector(xProj, yProj), projectAlong(vector(x, y), normal, base)) } } } @Test - fun projectOntoPlane() { - val normal = Vector3D(1.0, 3.5, 0.07) - val base = Vector3D(2.0, -0.0037, 11.1111) + fun projectOntoPlane() = with(Euclidean3DSpace){ + val normal = vector(1.0, 3.5, 0.07) + val base = vector(2.0, -0.0037, 11.1111) with(Euclidean3DSpace) { val testDomain = (-10.0..10.0).generateList(0.43) for (x in testDomain) { for (y in testDomain) { for (z in testDomain) { - val v = Vector3D(x, y, z) + val v = vector(x, y, z) val result = projectAlong(v, normal, base) // assert that result is on plane diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt index 076025110..2ca0b5f76 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt @@ -12,10 +12,10 @@ internal class ProjectionOntoLineTest { @Test fun projectionIntoOx() { with(Euclidean2DSpace) { - val ox = Line(zero, Vector2D(1.0, 0.0)) + val ox = Line(zero, vector(1.0, 0.0)) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> - assertVectorEquals(Vector2D(x, 0.0), projectToLine(Vector2D(x, y), ox)) + assertVectorEquals(vector(x, 0.0), projectToLine(vector(x, y), ox)) } } } @@ -23,10 +23,10 @@ internal class ProjectionOntoLineTest { @Test fun projectionIntoOy() { with(Euclidean2DSpace) { - val line = Line(zero, Vector2D(0.0, 1.0)) + val line = Line(zero, vector(0.0, 1.0)) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> - assertVectorEquals(Vector2D(0.0, y), projectToLine(Vector2D(x, y), line)) + assertVectorEquals(vector(0.0, y), projectToLine(vector(x, y), line)) } } } @@ -34,13 +34,13 @@ internal class ProjectionOntoLineTest { @Test fun projectionIntoYEqualsX() { with(Euclidean2DSpace) { - val line = Line(zero, Vector2D(1.0, 1.0)) + val line = Line(zero, vector(1.0, 1.0)) assertVectorEquals(zero, projectToLine(zero, line)) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> val d = (y - x) / 2.0 - assertVectorEquals(Vector2D(x + d, y - d), projectToLine(Vector2D(x, y), line)) + assertVectorEquals(vector(x + d, y - d), projectToLine(vector(x, y), line)) } } } @@ -51,38 +51,38 @@ internal class ProjectionOntoLineTest { val a = 5.0 val b = -3.0 val c = -15.0 - val line = Line(Vector2D(3.0, 0.0), Vector2D(3.0, 5.0)) + val line = Line(vector(3.0, 0.0), vector(3.0, 5.0)) grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> val xProj = (b * (b * x - a * y) - a * c) / (a * a + b * b) val yProj = (a * (-b * x + a * y) - b * c) / (a * a + b * b) - assertVectorEquals(Vector2D(xProj, yProj), projectToLine(Vector2D(x, y), line)) + assertVectorEquals(vector(xProj, yProj), projectToLine(vector(x, y), line)) } } } @Test - fun projectionOntoLine3d() { + fun projectionOntoLine3d() = with(Euclidean3DSpace) { val line = Line3D( - base = Vector3D(1.0, 3.5, 0.07), - direction = Vector3D(2.0, -0.0037, 11.1111) + base = vector(1.0, 3.5, 0.07), + direction = vector(2.0, -0.0037, 11.1111) ) - with(Euclidean3DSpace) { - val testDomain = (-10.0..10.0).generateList(0.43) - for (x in testDomain) { - for (y in testDomain) { - for (z in testDomain) { - val v = Vector3D(x, y, z) - val result = projectToLine(v, line) - // assert that result is on line - assertTrue(isCollinear(result - line.base, line.direction)) - // assert that PV vector is orthogonal to direction vector - assertTrue(isOrthogonal(v - result, line.direction)) - } + val testDomain = (-10.0..10.0).generateList(0.43) + for (x in testDomain) { + for (y in testDomain) { + for (z in testDomain) { + val v = vector(x, y, z) + val result = projectToLine(v, line) + + // assert that result is on the line + assertTrue(isCollinear(result - line.base, line.direction)) + // assert that PV vector is orthogonal to direction vector + assertTrue(isOrthogonal(v - result, line.direction)) } } } + } } diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt index 033055c19..8febe6722 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt @@ -15,7 +15,7 @@ class RotationTest { @Test fun differentRotations() = with(Euclidean3DSpace) { - val vector = Vector3D(1.0, 1.0, 1.0) + val vector = vector(1.0, 1.0, 1.0) val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized() val rotatedByQ = rotate(vector, q) val matrix = q.toRotationMatrix() @@ -36,7 +36,7 @@ class RotationTest { @Test fun fromRotation() { - val q = Quaternion.fromRotation(0.3.radians, Vector3D(1.0, 1.0, 1.0)) + val q = Quaternion.fromRotation(0.3.radians, Euclidean3DSpace.vector(1.0, 1.0, 1.0)) assertBufferEquals(DoubleBuffer(0.9887711, 0.0862781, 0.0862781, 0.0862781), q) } diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector2DTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector2DTest.kt index 5e45b4870..c98bfa45a 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector2DTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector2DTest.kt @@ -10,7 +10,7 @@ import kotlin.test.Test import kotlin.test.assertEquals internal class Vector2DTest { - private val vector = Vector2D(1.0, -7.999) + private val vector = Euclidean2DSpace.vector(1.0, -7.999) @Test fun size() { diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector3DTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector3DTest.kt index 55bab4775..fabcaa9aa 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector3DTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/Vector3DTest.kt @@ -10,7 +10,7 @@ import kotlin.test.Test import kotlin.test.assertEquals internal class Vector3DTest { - private val vector = Vector3D(1.0, -7.999, 0.001) + private val vector = Euclidean3DSpace.vector(1.0, -7.999, 0.001) @Test fun size() { diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/testUtils.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/testUtils.kt index 0f957529d..f8b64bc64 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/testUtils.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/testUtils.kt @@ -25,12 +25,12 @@ fun grid( return xs.flatMap { x -> ys.map { y -> x to y } } } -fun assertVectorEquals(expected: Vector2D, actual: Vector2D, absoluteTolerance: Double = 1e-6) { +fun assertVectorEquals(expected: DoubleVector2D, actual: DoubleVector2D, absoluteTolerance: Double = 1e-6) { assertEquals(expected.x, actual.x, absoluteTolerance) assertEquals(expected.y, actual.y, absoluteTolerance) } -fun assertVectorEquals(expected: Vector3D, actual: Vector3D, absoluteTolerance: Double = 1e-6) { +fun assertVectorEquals(expected: DoubleVector3D, actual: DoubleVector3D, absoluteTolerance: Double = 1e-6) { assertEquals(expected.x, actual.x, absoluteTolerance) assertEquals(expected.y, actual.y, absoluteTolerance) assertEquals(expected.z, actual.z, absoluteTolerance) diff --git a/kmath-geometry/src/jvmMain/kotlin/space/kscience/kmath/geometry/lineExtensions.kt b/kmath-geometry/src/jvmMain/kotlin/space/kscience/kmath/geometry/lineExtensions.kt new file mode 100644 index 000000000..5fcd2b23e --- /dev/null +++ b/kmath-geometry/src/jvmMain/kotlin/space/kscience/kmath/geometry/lineExtensions.kt @@ -0,0 +1,20 @@ +/* + * Copyright 2018-2021 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +import space.kscience.kmath.geometry.GeometrySpace +import space.kscience.kmath.geometry.Line +import space.kscience.kmath.geometry.LineSegment +import space.kscience.kmath.geometry.Vector +import space.kscience.kmath.operations.Group + +/** + * Get a line, containing this [LineSegment] + */ +context(Group) public val LineSegment.line: Line get() = Line(begin, end - begin) + +/** + * Get a length of a line segment + */ +context(GeometrySpace) public val LineSegment.length: Double get() = norm(end - begin) \ No newline at end of file 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 134342b33..a7a98288c 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 @@ -6,8 +6,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D import kotlin.math.PI import kotlin.math.acos import kotlin.math.cos @@ -17,10 +17,10 @@ internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles( internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second -internal fun Pose2D.getTangentCircles(radius: Double): Pair { +internal fun Pose2D.getTangentCircles(radius: Double): Pair = with(Euclidean2DSpace) { val dX = radius * cos(theta) val dY = radius * sin(theta) - return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(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) @@ -29,21 +29,21 @@ internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = oute ArcSegment.Direction.RIGHT ) -private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment { +private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment = with(Euclidean2DSpace){ val centers = StraightSegment(a.center, b.center) val p1 = when (side) { - ArcSegment.Direction.LEFT -> Vector2D( + ArcSegment.Direction.LEFT -> vector( a.center.x - a.radius * cos(centers.theta), a.center.y + a.radius * sin(centers.theta) ) - ArcSegment.Direction.RIGHT -> Vector2D( + ArcSegment.Direction.RIGHT -> vector( a.center.x + a.radius * cos(centers.theta), a.center.y - a.radius * sin(centers.theta) ) } return StraightSegment( p1, - Vector2D(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)) ) } @@ -53,7 +53,7 @@ internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegm internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? = innerTangent(base, direction, ArcSegment.Direction.RIGHT) -private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? { +private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? = with(Euclidean2DSpace){ val centers = StraightSegment(base.center, direction.center) if (centers.length < base.radius * 2) return null val angle = theta( @@ -64,8 +64,8 @@ private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.D ) val dX = base.radius * sin(angle) val dY = base.radius * cos(angle) - val p1 = Vector2D(base.center.x + dX, base.center.y + dY) - val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) + 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) } @@ -106,7 +106,7 @@ 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? { + 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) @@ -115,20 +115,20 @@ public class DubinsPath( var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) - val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) - val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + 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 = Vector2D(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 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) } - public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? { + 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) @@ -137,13 +137,13 @@ public class DubinsPath( var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) var dX = turningRadius * sin(theta) var dY = turningRadius * cos(theta) - val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2) val e = Circle2D(p, turningRadius) - val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + 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 = Vector2D(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 a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT) val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt index 1f7c4a52e..a7570156c 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt @@ -5,29 +5,29 @@ package space.kscience.kmath.trajectory +import space.kscience.kmath.geometry.DoubleVector2D import space.kscience.kmath.geometry.Vector -import space.kscience.kmath.geometry.Vector2D import kotlin.math.atan2 /** * Combination of [Vector] and its view angle */ -public interface Pose2D: Vector2D{ - public val coordinate: Vector2D +public interface Pose2D: DoubleVector2D{ + public val coordinate: DoubleVector2D public val theta: Double } public class PhaseVector2D( - override val coordinate: Vector2D, - public val velocity: Vector2D -): Pose2D, Vector2D by coordinate{ + override val coordinate: DoubleVector2D, + public val velocity: DoubleVector2D +): Pose2D, DoubleVector2D by coordinate{ override val theta: Double get() = atan2(velocity.y, velocity.x) } internal class Pose2DImpl( - override val coordinate: Vector2D, + override val coordinate: DoubleVector2D, override val theta: Double -) : Pose2D, Vector2D by coordinate +) : Pose2D, DoubleVector2D by coordinate -public fun Pose2D(coordinate: Vector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta) \ No newline at end of file +public fun Pose2D(coordinate: DoubleVector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta) \ No newline at end of file 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 2e97d43b0..554607cbd 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 @@ -6,8 +6,8 @@ package space.kscience.kmath.trajectory import space.kscience.kmath.geometry.Circle2D +import space.kscience.kmath.geometry.DoubleVector2D import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.circumference import kotlin.math.PI import kotlin.math.atan2 @@ -20,8 +20,8 @@ public sealed interface Trajectory { * Straight path segment. The order of start and end defines the direction */ public data class StraightSegment( - internal val start: Vector2D, - internal val end: Vector2D, + internal val start: DoubleVector2D, + internal val end: DoubleVector2D, ) : Trajectory { override val length: Double get() = start.distanceTo(end) @@ -68,9 +68,9 @@ public data class ArcSegment( } public companion object { - public fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): ArcSegment { + public fun of(center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, direction: Direction): ArcSegment { fun calculatePose( - vector: Vector2D, + vector: DoubleVector2D, theta: Double, direction: Direction, ): Pose2D = Pose2D( 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 7ee68bd92..5a9cc5be8 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 @@ -1,6 +1,6 @@ package space.kscience.kmath.trajectory -import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin @@ -13,12 +13,12 @@ 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 { +fun StraightSegment.shift(shift: Int, width: Double): StraightSegment = with(Euclidean2DSpace){ val dX = width * sin(inverse().theta) val dY = width * sin(theta) return StraightSegment( - Vector2D(start.x - dX * shift, start.y - dY * shift), - Vector2D(end.x - dX * shift, end.y - dY * shift) + 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 09375a400..68a5f8454 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 @@ -5,8 +5,7 @@ package space.kscience.kmath.trajectory.dubins -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.trajectory.* import kotlin.test.Test import kotlin.test.assertNotNull @@ -16,8 +15,8 @@ import kotlin.test.assertTrue class DubinsTests { @Test - fun dubinsTest() { - val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + fun dubinsTest() = with(Euclidean2DSpace){ + val straight = StraightSegment(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) 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 c66fa201b..88423ee99 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 @@ -1,7 +1,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.circumference import space.kscience.kmath.trajectory.ArcSegment import space.kscience.kmath.trajectory.radiansToDegrees @@ -11,9 +11,9 @@ import kotlin.test.assertEquals class ArcTests { @Test - fun arcTest() { - val circle = Circle2D(Vector2D(0.0, 0.0), 2.0) - val arc = ArcSegment.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), ArcSegment.Direction.RIGHT) + 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) 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/CircleTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt index 5170c1db5..f1f9c6a3b 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt @@ -6,7 +6,7 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.geometry.Euclidean2DSpace import space.kscience.kmath.geometry.circumference import space.kscience.kmath.trajectory.maxFloatDelta import kotlin.test.Test @@ -16,7 +16,7 @@ class CircleTests { @Test fun arcTest() { - val center = Vector2D(0.0, 0.0) + val center = Euclidean2DSpace.vector(0.0, 0.0) val radius = 2.0 val expectedCircumference = 12.56637 val circle = Circle2D(center, radius) 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 11eaa0fb3..b8fe256c7 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 @@ -1,7 +1,6 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.StraightSegment import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.math.pow @@ -12,22 +11,22 @@ import kotlin.test.assertEquals class LineTests { @Test - fun lineTest() { - val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + fun lineTest() = with(Euclidean2DSpace){ + val straight = StraightSegment(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()) } @Test - fun lineAngleTest() { + fun lineAngleTest() = with(Euclidean2DSpace){ //val zero = Vector2D(0.0, 0.0) - val north = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + val north = StraightSegment(Euclidean2DSpace.zero, vector(0.0, 2.0)) assertEquals(0.0, north.theta.radiansToDegrees()) - val east = StraightSegment(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + val east = StraightSegment(Euclidean2DSpace.zero, vector(2.0, 0.0)) assertEquals(90.0, east.theta.radiansToDegrees()) - val south = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + val south = StraightSegment(Euclidean2DSpace.zero, vector(0.0, -2.0)) assertEquals(180.0, south.theta.radiansToDegrees()) - val west = StraightSegment(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + val west = StraightSegment(Euclidean2DSpace.zero, vector(-2.0, 0.0)) assertEquals(270.0, west.theta.radiansToDegrees()) } }