Geometry overhaul

This commit is contained in:
Alexander Nozik 2022-08-21 19:17:38 +03:00
parent 98e21f3c3a
commit ec77cd1fc3
No known key found for this signature in database
GPG Key ID: F7FCF2DD25C71357
25 changed files with 260 additions and 209 deletions

View File

@ -1,6 +1,5 @@
plugins { plugins {
kotlin("multiplatform") id("space.kscience.gradle.mpp")
id("space.kscience.gradle.common")
id("space.kscience.gradle.native") id("space.kscience.gradle.native")
} }

View File

@ -1,6 +1,5 @@
plugins { plugins {
kotlin("multiplatform") id("space.kscience.gradle.mpp")
id("space.kscience.gradle.common")
id("space.kscience.gradle.native") id("space.kscience.gradle.native")
} }
@ -10,6 +9,10 @@ kotlin.sourceSets.commonMain {
} }
} }
kscience {
withContextReceivers()
}
readme { readme {
maturity = space.kscience.gradle.Maturity.PROTOTYPE maturity = space.kscience.gradle.Maturity.PROTOTYPE
} }

View File

@ -11,7 +11,7 @@ import kotlin.math.PI
* A circle in 2D space * A circle in 2D space
*/ */
public class Circle2D( public class Circle2D(
public val center: Vector2D, public val center: DoubleVector2D,
public val radius: Double public val radius: Double
) )

View File

@ -6,45 +6,64 @@
package space.kscience.kmath.geometry package space.kscience.kmath.geometry
import space.kscience.kmath.linear.Point import space.kscience.kmath.linear.Point
import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations import space.kscience.kmath.operations.ScaleOperations
import space.kscience.kmath.operations.invoke import kotlin.math.pow
import kotlin.math.sqrt import kotlin.math.sqrt
public interface Vector2D : Point<Double>, Vector { public interface Vector2D<T> : Point<T>, Vector {
public val x: Double public val x: T
public val y: Double public val y: T
override val size: Int get() = 2 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 0 -> x
1 -> y 1 -> y
else -> error("Accessing outside of point bounds") else -> error("Accessing outside of point bounds")
} }
override operator fun iterator(): Iterator<Double> = listOf(x, y).iterator() override operator fun iterator(): Iterator<T> = 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 <T> Vector2D<T>.component1(): T = x
public operator fun <T> Vector2D<T>.component2(): T = y
public typealias DoubleVector2D = Vector2D<Double>
public val Vector2D<Double>.r: Double get() = Euclidean2DSpace.norm(this)
private data class Vector2DImpl(
override val x: Double,
override val y: Double,
) : Vector2D
/** /**
* 2D Euclidean space * 2D Euclidean space
*/ */
public object Euclidean2DSpace : GeometrySpace<Vector2D>, ScaleOperations<Vector2D> { public object Euclidean2DSpace : GeometrySpace<DoubleVector2D>,
override val zero: Vector2D by lazy { Vector2D(0.0, 0.0) } ScaleOperations<DoubleVector2D>,
Norm<DoubleVector2D, Double> {
public fun Vector2D.norm(): Double = sqrt(x * x + y * y) private data class Vector2DImpl(
override fun Vector2D.unaryMinus(): Vector2D = Vector2D(-x, -y) override val x: Double,
override val y: Double,
) : DoubleVector2D
override fun Vector2D.distanceTo(other: Vector2D): Double = (this - other).norm() public fun vector(x: Number, y: Number): DoubleVector2D = Vector2DImpl(x.toDouble(), y.toDouble())
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 val zero: DoubleVector2D by lazy { vector(0.0, 0.0) }
override fun Vector2D.dot(other: Vector2D): Double = x * other.x + y * other.y
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)
} }

View File

@ -6,73 +6,79 @@
package space.kscience.kmath.geometry package space.kscience.kmath.geometry
import space.kscience.kmath.linear.Point import space.kscience.kmath.linear.Point
import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations import space.kscience.kmath.operations.ScaleOperations
import space.kscience.kmath.operations.invoke
import space.kscience.kmath.structures.Buffer import space.kscience.kmath.structures.Buffer
import kotlin.math.pow
import kotlin.math.sqrt import kotlin.math.sqrt
public interface Vector3D : Point<Double>, Vector { public interface Vector3D<T> : Point<T>, Vector {
public val x: Double public val x: T
public val y: Double public val y: T
public val z: Double public val z: T
override val size: Int get() = 3 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 0 -> x
1 -> y 1 -> y
2 -> z 2 -> z
else -> error("Accessing outside of point bounds") else -> error("Accessing outside of point bounds")
} }
override operator fun iterator(): Iterator<Double> = listOf(x, y, z).iterator() override operator fun iterator(): Iterator<T> = listOf(x, y, z).iterator()
} }
public operator fun Vector3D.component1(): Double = x public operator fun <T> Vector3D<T>.component1(): T = x
public operator fun Vector3D.component2(): Double = y public operator fun <T> Vector3D<T>.component2(): T = y
public operator fun Vector3D.component3(): Double = z public operator fun <T> Vector3D<T>.component3(): T = z
public fun Buffer<Double>.asVector3D(): Vector3D = object : Vector3D { public fun <T> Buffer<T>.asVector3D(): Vector3D<T> = object : Vector3D<T> {
init { init {
require(this@asVector3D.size == 3) { "Buffer of size 3 is required for Vector3D" } require(this@asVector3D.size == 3) { "Buffer of size 3 is required for Vector3D" }
} }
override val x: Double get() = this@asVector3D[0] override val x: T get() = this@asVector3D[0]
override val y: Double get() = this@asVector3D[1] override val y: T get() = this@asVector3D[1]
override val z: Double get() = this@asVector3D[2] override val z: T get() = this@asVector3D[2]
override fun toString(): String = this@asVector3D.toString() override fun toString(): String = this@asVector3D.toString()
} }
public val Vector3D.r: Double get() = Euclidean3DSpace { norm() } public typealias DoubleVector3D = Vector3D<Double>
private data class Vector3DImpl( public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this)
override val x: Double,
override val y: Double,
override val z: Double,
) : Vector3D
public object Euclidean3DSpace : GeometrySpace<DoubleVector3D>, ScaleOperations<DoubleVector3D>,
Norm<DoubleVector3D, Double> {
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<Vector3D>, ScaleOperations<Vector3D> { override val zero: DoubleVector3D by lazy { vector(0.0, 0.0, 0.0) }
override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) }
public fun Vector3D.norm(): Double = sqrt(x * x + y * y + z * z) override fun norm(arg: DoubleVector3D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2) + arg.z.pow(2))
override fun Vector3D.unaryMinus(): Vector3D = Vector3D(-x, -y, -z)
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 = override fun DoubleVector3D.unaryMinus(): DoubleVector3D = vector(-x, -y, -z)
Vector3D(left.x + right.x, left.y + right.y, left.z + right.z)
override fun scale(a: Vector3D, value: Double): Vector3D = override fun DoubleVector3D.distanceTo(other: DoubleVector3D): Double = (this - other).norm()
Vector3D(a.x * value, a.y * value, a.z * value)
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 x * other.x + y * other.y + z * other.z
public val xAxis: Vector3D = Vector3D(1.0, 0.0, 0.0) public val xAxis: DoubleVector3D = vector(1.0, 0.0, 0.0)
public val yAxis: Vector3D = Vector3D(0.0, 1.0, 0.0) public val yAxis: DoubleVector3D = vector(0.0, 1.0, 0.0)
public val zAxis: Vector3D = Vector3D(0.0, 0.0, 1.0) public val zAxis: DoubleVector3D = vector(0.0, 0.0, 1.0)
} }

View File

@ -6,11 +6,12 @@
package space.kscience.kmath.geometry package space.kscience.kmath.geometry
import space.kscience.kmath.operations.Group import space.kscience.kmath.operations.Group
import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations import space.kscience.kmath.operations.ScaleOperations
public interface Vector public interface Vector
public interface GeometrySpace<V : Vector> : Group<V>, ScaleOperations<V> { public interface GeometrySpace<V : Vector> : Group<V>, ScaleOperations<V>, Norm<V, Double> {
/** /**
* L2 distance * L2 distance
*/ */

View File

@ -11,5 +11,10 @@ package space.kscience.kmath.geometry
*/ */
public data class Line<out V : Vector>(val base: V, val direction: V) public data class Line<out V : Vector>(val base: V, val direction: V)
public typealias Line2D = Line<Vector2D> public typealias Line2D = Line<DoubleVector2D>
public typealias Line3D = Line<Vector3D> public typealias Line3D = Line<DoubleVector3D>
/**
* A directed line segment between [begin] and [end]
*/
public data class LineSegment<out V : Vector>(val begin: V, val end: V)

View File

@ -15,7 +15,7 @@ import space.kscience.kmath.operations.DoubleField
import kotlin.math.pow import kotlin.math.pow
import kotlin.math.sqrt 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 * 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 * 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 s = sin(theta / 2)
val c = cos(theta / 2) val c = cos(theta / 2)
val norm = with(Euclidean3DSpace) { vector.norm() } 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 * An axis of quaternion rotation
*/ */
public val Quaternion.vector: Vector3D public val Quaternion.vector: DoubleVector3D
get() { get() {
return object : Vector3D { return object : DoubleVector3D {
private val sint2 = sqrt(1 - w * w) private val sint2 = sqrt(1 - w * w)
override val x: Double get() = this@vector.x / sint2 override val x: Double get() = this@vector.x / sint2
override val y: Double get() = this@vector.y / 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] * 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() val p = vector.toQuaternion()
(q * p * q.reciprocal).vector (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 * Use a composition of quaternions to create a rotation
*/ */
@UnstableKMathAPI @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()) rotate(vector, QuaternionField.composition())
public fun Euclidean3DSpace.rotate(vector: Vector3D, matrix: Matrix<Double>): Vector3D { public fun Euclidean3DSpace.rotate(vector: DoubleVector3D, matrix: Matrix<Double>): DoubleVector3D {
require(matrix.colNum == 3 && matrix.rowNum == 3) { "Square 3x3 rotation matrix is required" } require(matrix.colNum == 3 && matrix.rowNum == 3) { "Square 3x3 rotation matrix is required" }
return with(DoubleField.linearSpace) { matrix.dot(vector).asVector3D() } return with(DoubleField.linearSpace) { matrix.dot(vector).asVector3D() }
} }

View File

@ -12,16 +12,16 @@ import kotlin.test.assertEquals
internal class Euclidean2DSpaceTest { internal class Euclidean2DSpaceTest {
@Test @Test
fun zero() { fun zero() {
assertVectorEquals(Vector2D(0.0, 0.0), Euclidean2DSpace.zero) assertVectorEquals(Euclidean2DSpace.vector(0.0, 0.0), Euclidean2DSpace.zero)
} }
@Test @Test
fun norm() { fun norm() {
with(Euclidean2DSpace) { with(Euclidean2DSpace) {
assertEquals(0.0, zero.norm()) assertEquals(0.0, norm(zero))
assertEquals(1.0, Vector2D(1.0, 0.0).norm()) assertEquals(1.0, norm(vector(1.0, 0.0)))
assertEquals(sqrt(2.0), Vector2D(1.0, 1.0).norm()) assertEquals(sqrt(2.0), norm(vector(1.0, 1.0)))
assertEquals(sqrt(5.002001), Vector2D(-2.0, 1.001).norm()) assertEquals(sqrt(5.002001), norm(vector(-2.0, 1.001)))
} }
} }
@ -29,16 +29,16 @@ internal class Euclidean2DSpaceTest {
fun dotProduct() { fun dotProduct() {
with(Euclidean2DSpace) { with(Euclidean2DSpace) {
assertEquals(0.0, zero dot zero) assertEquals(0.0, zero dot zero)
assertEquals(0.0, zero dot Vector2D(1.0, 0.0)) assertEquals(0.0, zero dot vector(1.0, 0.0))
assertEquals(0.0, Vector2D(-2.0, 0.001) dot zero) assertEquals(0.0, vector(-2.0, 0.001) dot zero)
assertEquals(0.0, Vector2D(1.0, 0.0) dot Vector2D(0.0, 1.0)) 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(1.0, vector(1.0, 0.0) dot vector(1.0, 0.0))
assertEquals(-2.0, Vector2D(0.0, 1.0) dot Vector2D(1.0, -2.0)) assertEquals(-2.0, vector(0.0, 1.0) dot vector(1.0, -2.0))
assertEquals(2.0, Vector2D(1.0, 1.0) dot Vector2D(1.0, 1.0)) assertEquals(2.0, vector(1.0, 1.0) dot vector(1.0, 1.0))
assertEquals(4.001001, Vector2D(-2.0, 1.001) dot Vector2D(-2.0, 0.001)) 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() { fun add() {
with(Euclidean2DSpace) { with(Euclidean2DSpace) {
assertVectorEquals( assertVectorEquals(
Vector2D(-2.0, 0.001), vector(-2.0, 0.001),
Vector2D(-2.0, 0.001) + zero vector(-2.0, 0.001) + zero
) )
assertVectorEquals( assertVectorEquals(
Vector2D(-3.0, 3.001), vector(-3.0, 3.001),
Vector2D(2.0, 3.0) + Vector2D(-5.0, 0.001) vector(2.0, 3.0) + vector(-5.0, 0.001)
) )
} }
} }
@ -59,9 +59,9 @@ internal class Euclidean2DSpaceTest {
@Test @Test
fun multiply() { fun multiply() {
with(Euclidean2DSpace) { with(Euclidean2DSpace) {
assertVectorEquals(Vector2D(-4.0, 0.0), Vector2D(-2.0, 0.0) * 2) assertVectorEquals(vector(-4.0, 0.0), vector(-2.0, 0.0) * 2)
assertVectorEquals(Vector2D(4.0, 0.0), Vector2D(-2.0, 0.0) * -2) assertVectorEquals(vector(4.0, 0.0), vector(-2.0, 0.0) * -2)
assertVectorEquals(Vector2D(300.0, 0.0003), Vector2D(100.0, 0.0001) * 3) assertVectorEquals(vector(300.0, 0.0003), vector(100.0, 0.0001) * 3)
} }
} }
} }

View File

@ -11,21 +11,21 @@ import kotlin.test.assertEquals
internal class Euclidean3DSpaceTest { internal class Euclidean3DSpaceTest {
@Test @Test
fun zero() { fun zero() {
assertVectorEquals(Vector3D(0.0, 0.0, 0.0), Euclidean3DSpace.zero) assertVectorEquals(Euclidean3DSpace.vector(0.0, 0.0, 0.0), Euclidean3DSpace.zero)
} }
@Test @Test
fun distance() { fun distance() {
with(Euclidean3DSpace) { with(Euclidean3DSpace) {
assertEquals(0.0, zero.distanceTo(zero)) assertEquals(0.0, zero.distanceTo(zero))
assertEquals(1.0, zero.distanceTo(Vector3D(1.0, 0.0, 0.0))) assertEquals(1.0, zero.distanceTo(vector(1.0, 0.0, 0.0)))
assertEquals(kotlin.math.sqrt(5.000001), Vector3D(1.0, -2.0, 0.001).distanceTo(zero)) assertEquals(kotlin.math.sqrt(5.000001), vector(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, vector(1.0, -2.0, 0.001).distanceTo(vector(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(0.0, vector(1.0, 0.0, 0.0).distanceTo(vector(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(kotlin.math.sqrt(2.0), vector(1.0, 0.0, 0.0).distanceTo(vector(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(3.1622778182822584, vector(0.0, 1.0, 0.0).distanceTo(vector(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(0.0, vector(1.0, -2.0, 0.001).distanceTo(vector(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(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() { fun norm() {
with(Euclidean3DSpace) { with(Euclidean3DSpace) {
assertEquals(0.0, zero.norm()) assertEquals(0.0, zero.norm())
assertEquals(1.0, Vector3D(1.0, 0.0, 0.0).norm()) assertEquals(1.0, vector(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(3.0), vector(1.0, 1.0, 1.0).norm())
assertEquals(kotlin.math.sqrt(5.000001), Vector3D(1.0, -2.0, 0.001).norm()) assertEquals(kotlin.math.sqrt(5.000001), vector(1.0, -2.0, 0.001).norm())
} }
} }
@ -43,16 +43,16 @@ internal class Euclidean3DSpaceTest {
fun dotProduct() { fun dotProduct() {
with(Euclidean3DSpace) { with(Euclidean3DSpace) {
assertEquals(0.0, zero dot zero) assertEquals(0.0, zero dot zero)
assertEquals(0.0, zero dot Vector3D(1.0, 0.0, 0.0)) assertEquals(0.0, zero dot vector(1.0, 0.0, 0.0))
assertEquals(0.0, Vector3D(1.0, -2.0, 0.001) dot zero) 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, vector(1.0, 0.0, 0.0) dot vector(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(1.0, vector(1.0, 0.0, 0.0) dot vector(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(-2.0, vector(0.0, 1.0, 0.0) dot vector(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(3.0, vector(1.0, 1.0, 1.0) dot vector(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(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() { fun add() {
with(Euclidean3DSpace) { with(Euclidean3DSpace) {
assertVectorEquals( assertVectorEquals(
Vector3D(1.0, -2.0, 0.001), vector(1.0, -2.0, 0.001),
Vector3D(1.0, -2.0, 0.001) + zero vector(1.0, -2.0, 0.001) + zero
) )
assertVectorEquals( assertVectorEquals(
Vector3D(8.0, -3.0, 3.001), vector(8.0, -3.0, 3.001),
Vector3D(1.0, 2.0, 3.0) + Vector3D(7.0, -5.0, 0.001) vector(1.0, 2.0, 3.0) + vector(7.0, -5.0, 0.001)
) )
} }
} }
@ -73,7 +73,7 @@ internal class Euclidean3DSpaceTest {
@Test @Test
fun multiply() { fun multiply() {
with(Euclidean3DSpace) { 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)
} }
} }
} }

View File

@ -12,14 +12,14 @@ internal class ProjectionAlongTest {
@Test @Test
fun projectionIntoYEqualsX() { fun projectionIntoYEqualsX() {
with(Euclidean2DSpace) { with(Euclidean2DSpace) {
val normal = Vector2D(-2.0, 2.0) val normal = vector(-2.0, 2.0)
val base = Vector2D(2.3, 2.3) val base = vector(2.3, 2.3)
assertVectorEquals(zero, projectAlong(zero, normal, base)) assertVectorEquals(zero, projectAlong(zero, normal, base))
grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) ->
val d = (y - x) / 2.0 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 a = 5.0
val b = -3.0 val b = -3.0
val c = -15.0 val c = -15.0
val normal = Vector2D(-5.0, 3.0) val normal = vector(-5.0, 3.0)
val base = Vector2D(3.0, 0.0) val base = vector(3.0, 0.0)
grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> 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 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) 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 @Test
fun projectOntoPlane() { fun projectOntoPlane() = with(Euclidean3DSpace){
val normal = Vector3D(1.0, 3.5, 0.07) val normal = vector(1.0, 3.5, 0.07)
val base = Vector3D(2.0, -0.0037, 11.1111) val base = vector(2.0, -0.0037, 11.1111)
with(Euclidean3DSpace) { with(Euclidean3DSpace) {
val testDomain = (-10.0..10.0).generateList(0.43) val testDomain = (-10.0..10.0).generateList(0.43)
for (x in testDomain) { for (x in testDomain) {
for (y in testDomain) { for (y in testDomain) {
for (z in testDomain) { for (z in testDomain) {
val v = Vector3D(x, y, z) val v = vector(x, y, z)
val result = projectAlong(v, normal, base) val result = projectAlong(v, normal, base)
// assert that result is on plane // assert that result is on plane

View File

@ -12,10 +12,10 @@ internal class ProjectionOntoLineTest {
@Test @Test
fun projectionIntoOx() { fun projectionIntoOx() {
with(Euclidean2DSpace) { 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) -> 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 @Test
fun projectionIntoOy() { fun projectionIntoOy() {
with(Euclidean2DSpace) { 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) -> 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 @Test
fun projectionIntoYEqualsX() { fun projectionIntoYEqualsX() {
with(Euclidean2DSpace) { 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)) assertVectorEquals(zero, projectToLine(zero, line))
grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) -> grid(-10.0..10.0, -10.0..10.0, 0.15).forEach { (x, y) ->
val d = (y - x) / 2.0 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 a = 5.0
val b = -3.0 val b = -3.0
val c = -15.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) -> 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 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) 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 @Test
fun projectionOntoLine3d() { fun projectionOntoLine3d() = with(Euclidean3DSpace) {
val line = Line3D( val line = Line3D(
base = Vector3D(1.0, 3.5, 0.07), base = vector(1.0, 3.5, 0.07),
direction = Vector3D(2.0, -0.0037, 11.1111) 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 val testDomain = (-10.0..10.0).generateList(0.43)
assertTrue(isCollinear(result - line.base, line.direction)) for (x in testDomain) {
// assert that PV vector is orthogonal to direction vector for (y in testDomain) {
assertTrue(isOrthogonal(v - result, line.direction)) 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))
} }
} }
} }
} }
} }

View File

@ -15,7 +15,7 @@ class RotationTest {
@Test @Test
fun differentRotations() = with(Euclidean3DSpace) { 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 q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
val rotatedByQ = rotate(vector, q) val rotatedByQ = rotate(vector, q)
val matrix = q.toRotationMatrix() val matrix = q.toRotationMatrix()
@ -36,7 +36,7 @@ class RotationTest {
@Test @Test
fun fromRotation() { 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) assertBufferEquals(DoubleBuffer(0.9887711, 0.0862781, 0.0862781, 0.0862781), q)
} }

View File

@ -10,7 +10,7 @@ import kotlin.test.Test
import kotlin.test.assertEquals import kotlin.test.assertEquals
internal class Vector2DTest { internal class Vector2DTest {
private val vector = Vector2D(1.0, -7.999) private val vector = Euclidean2DSpace.vector(1.0, -7.999)
@Test @Test
fun size() { fun size() {

View File

@ -10,7 +10,7 @@ import kotlin.test.Test
import kotlin.test.assertEquals import kotlin.test.assertEquals
internal class Vector3DTest { 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 @Test
fun size() { fun size() {

View File

@ -25,12 +25,12 @@ fun grid(
return xs.flatMap { x -> ys.map { y -> x to y } } 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.x, actual.x, absoluteTolerance)
assertEquals(expected.y, actual.y, 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.x, actual.x, absoluteTolerance)
assertEquals(expected.y, actual.y, absoluteTolerance) assertEquals(expected.y, actual.y, absoluteTolerance)
assertEquals(expected.z, actual.z, absoluteTolerance) assertEquals(expected.z, actual.z, absoluteTolerance)

View File

@ -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<V>) public val <V : Vector> LineSegment<V>.line: Line<V> get() = Line(begin, end - begin)
/**
* Get a length of a line segment
*/
context(GeometrySpace<V>) public val <V : Vector> LineSegment<V>.length: Double get() = norm(end - begin)

View File

@ -6,8 +6,8 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import kotlin.math.PI import kotlin.math.PI
import kotlin.math.acos import kotlin.math.acos
import kotlin.math.cos 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.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> { internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
val dX = radius * cos(theta) val dX = radius * cos(theta)
val dY = radius * sin(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) 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 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 centers = StraightSegment(a.center, b.center)
val p1 = when (side) { val p1 = when (side) {
ArcSegment.Direction.LEFT -> Vector2D( ArcSegment.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 -> Vector2D( ArcSegment.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 StraightSegment(
p1, 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? = internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, ArcSegment.Direction.RIGHT) 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) val centers = StraightSegment(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(
@ -64,8 +64,8 @@ private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.D
) )
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 = Vector2D(base.center.x + dX, base.center.y + dY) val p1 = vector(base.center.x + dX, base.center.y + dY)
val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) val p2 = vector(direction.center.x - dX, direction.center.y - dY)
return StraightSegment(p1, p2) return StraightSegment(p1, p2)
} }
@ -106,7 +106,7 @@ 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? { 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 = StraightSegment(c1.center, c2.center)
@ -115,20 +115,20 @@ public class DubinsPath(
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)
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 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))) theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(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 a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.RIGHT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT) val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT) val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, a2, a3) 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 c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val centers = StraightSegment(c1.center, c2.center) 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 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)
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 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))) theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(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 a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.LEFT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT) val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT) val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT)

View File

@ -5,29 +5,29 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Vector import space.kscience.kmath.geometry.Vector
import space.kscience.kmath.geometry.Vector2D
import kotlin.math.atan2 import kotlin.math.atan2
/** /**
* Combination of [Vector] and its view angle * Combination of [Vector] and its view angle
*/ */
public interface Pose2D: Vector2D{ public interface Pose2D: DoubleVector2D{
public val coordinate: Vector2D public val coordinate: DoubleVector2D
public val theta: Double public val theta: Double
} }
public class PhaseVector2D( public class PhaseVector2D(
override val coordinate: Vector2D, override val coordinate: DoubleVector2D,
public val velocity: Vector2D public val velocity: DoubleVector2D
): Pose2D, Vector2D by coordinate{ ): Pose2D, DoubleVector2D by coordinate{
override val theta: Double get() = atan2(velocity.y, velocity.x) override val theta: Double get() = atan2(velocity.y, velocity.x)
} }
internal class Pose2DImpl( internal class Pose2DImpl(
override val coordinate: Vector2D, override val coordinate: DoubleVector2D,
override val theta: Double override val theta: Double
) : Pose2D, Vector2D by coordinate ) : Pose2D, DoubleVector2D by coordinate
public fun Pose2D(coordinate: Vector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta) public fun Pose2D(coordinate: DoubleVector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta)

View File

@ -6,8 +6,8 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference import space.kscience.kmath.geometry.circumference
import kotlin.math.PI import kotlin.math.PI
import kotlin.math.atan2 import kotlin.math.atan2
@ -20,8 +20,8 @@ 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 StraightSegment(
internal val start: Vector2D, internal val start: DoubleVector2D,
internal val end: Vector2D, internal val end: DoubleVector2D,
) : Trajectory { ) : Trajectory {
override val length: Double get() = start.distanceTo(end) override val length: Double get() = start.distanceTo(end)
@ -68,9 +68,9 @@ public data class ArcSegment(
} }
public companion object { 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( fun calculatePose(
vector: Vector2D, vector: DoubleVector2D,
theta: Double, theta: Double,
direction: Direction, direction: Direction,
): Pose2D = Pose2D( ): Pose2D = Pose2D(

View File

@ -1,6 +1,6 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Euclidean2DSpace
import kotlin.math.PI import kotlin.math.PI
import kotlin.math.abs import kotlin.math.abs
import kotlin.math.sin 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 Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
fun StraightSegment.inverse() = StraightSegment(end, start) 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 dX = width * sin(inverse().theta)
val dY = width * sin(theta) val dY = width * sin(theta)
return StraightSegment( return StraightSegment(
Vector2D(start.x - dX * shift, start.y - dY * shift), vector(start.x - dX * shift, start.y - dY * shift),
Vector2D(end.x - dX * shift, end.y - dY * shift) vector(end.x - dX * shift, end.y - dY * shift)
) )
} }

View File

@ -5,8 +5,7 @@
package space.kscience.kmath.trajectory.dubins package space.kscience.kmath.trajectory.dubins
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.* import space.kscience.kmath.trajectory.*
import kotlin.test.Test import kotlin.test.Test
import kotlin.test.assertNotNull import kotlin.test.assertNotNull
@ -16,8 +15,8 @@ import kotlin.test.assertTrue
class DubinsTests { class DubinsTests {
@Test @Test
fun dubinsTest() { fun dubinsTest() = with(Euclidean2DSpace){
val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) val straight = StraightSegment(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)

View File

@ -1,7 +1,7 @@
package space.kscience.kmath.trajectory.segments package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Circle2D 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.geometry.circumference
import space.kscience.kmath.trajectory.ArcSegment import space.kscience.kmath.trajectory.ArcSegment
import space.kscience.kmath.trajectory.radiansToDegrees import space.kscience.kmath.trajectory.radiansToDegrees
@ -11,9 +11,9 @@ import kotlin.test.assertEquals
class ArcTests { class ArcTests {
@Test @Test
fun arcTest() { fun arcTest() = with(Euclidean2DSpace){
val circle = Circle2D(Vector2D(0.0, 0.0), 2.0) val circle = Circle2D(vector(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) 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(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.Circle2D 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.geometry.circumference
import space.kscience.kmath.trajectory.maxFloatDelta import space.kscience.kmath.trajectory.maxFloatDelta
import kotlin.test.Test import kotlin.test.Test
@ -16,7 +16,7 @@ class CircleTests {
@Test @Test
fun arcTest() { fun arcTest() {
val center = Vector2D(0.0, 0.0) val center = Euclidean2DSpace.vector(0.0, 0.0)
val radius = 2.0 val radius = 2.0
val expectedCircumference = 12.56637 val expectedCircumference = 12.56637
val circle = Circle2D(center, radius) val circle = Circle2D(center, radius)

View File

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