forked from kscience/kmath
Geometry overhaul
This commit is contained in:
parent
98e21f3c3a
commit
ec77cd1fc3
@ -1,6 +1,5 @@
|
||||
plugins {
|
||||
kotlin("multiplatform")
|
||||
id("space.kscience.gradle.common")
|
||||
id("space.kscience.gradle.mpp")
|
||||
id("space.kscience.gradle.native")
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
|
||||
@ -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<Double>, Vector {
|
||||
public val x: Double
|
||||
public val y: Double
|
||||
public interface Vector2D<T> : Point<T>, 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<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
|
||||
*/
|
||||
public object Euclidean2DSpace : GeometrySpace<Vector2D>, ScaleOperations<Vector2D> {
|
||||
override val zero: Vector2D by lazy { Vector2D(0.0, 0.0) }
|
||||
public object Euclidean2DSpace : GeometrySpace<DoubleVector2D>,
|
||||
ScaleOperations<DoubleVector2D>,
|
||||
Norm<DoubleVector2D, Double> {
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
@ -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<Double>, Vector {
|
||||
public val x: Double
|
||||
public val y: Double
|
||||
public val z: Double
|
||||
public interface Vector3D<T> : Point<T>, 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<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 Vector3D.component2(): Double = y
|
||||
public operator fun Vector3D.component3(): Double = z
|
||||
public operator fun <T> Vector3D<T>.component1(): T = x
|
||||
public operator fun <T> Vector3D<T>.component2(): T = y
|
||||
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 {
|
||||
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<Double>
|
||||
|
||||
private data class Vector3DImpl(
|
||||
public val DoubleVector3D.r: Double get() = Euclidean3DSpace.norm(this)
|
||||
|
||||
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,
|
||||
) : Vector3D
|
||||
) : DoubleVector3D
|
||||
|
||||
public fun vector(x: Number, y: Number, z: Number): DoubleVector3D =
|
||||
Vector3DImpl(x.toDouble(), y.toDouble(), z.toDouble())
|
||||
|
||||
public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z)
|
||||
override val zero: DoubleVector3D by lazy { vector(0.0, 0.0, 0.0) }
|
||||
|
||||
public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector3D> {
|
||||
override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) }
|
||||
override fun norm(arg: DoubleVector3D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2) + arg.z.pow(2))
|
||||
|
||||
public fun Vector3D.norm(): Double = sqrt(x * x + y * y + z * z)
|
||||
override fun Vector3D.unaryMinus(): Vector3D = Vector3D(-x, -y, -z)
|
||||
public fun DoubleVector3D.norm(): Double = norm(this)
|
||||
|
||||
override fun Vector3D.distanceTo(other: Vector3D): Double = (this - other).norm()
|
||||
override fun DoubleVector3D.unaryMinus(): DoubleVector3D = vector(-x, -y, -z)
|
||||
|
||||
override fun add(left: Vector3D, right: Vector3D): Vector3D =
|
||||
Vector3D(left.x + right.x, left.y + right.y, left.z + right.z)
|
||||
override fun DoubleVector3D.distanceTo(other: DoubleVector3D): Double = (this - other).norm()
|
||||
|
||||
override fun scale(a: Vector3D, value: Double): Vector3D =
|
||||
Vector3D(a.x * value, a.y * value, a.z * value)
|
||||
override fun add(left: DoubleVector3D, right: DoubleVector3D): DoubleVector3D =
|
||||
vector(left.x + right.x, left.y + right.y, left.z + right.z)
|
||||
|
||||
override fun Vector3D.dot(other: Vector3D): Double =
|
||||
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)
|
||||
}
|
||||
|
||||
@ -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<V : Vector> : Group<V>, ScaleOperations<V> {
|
||||
public interface GeometrySpace<V : Vector> : Group<V>, ScaleOperations<V>, Norm<V, Double> {
|
||||
/**
|
||||
* L2 distance
|
||||
*/
|
||||
|
||||
@ -11,5 +11,10 @@ package space.kscience.kmath.geometry
|
||||
*/
|
||||
public data class Line<out V : Vector>(val base: V, val direction: V)
|
||||
|
||||
public typealias Line2D = Line<Vector2D>
|
||||
public typealias Line3D = Line<Vector3D>
|
||||
public typealias Line2D = Line<DoubleVector2D>
|
||||
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)
|
||||
|
||||
@ -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<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" }
|
||||
return with(DoubleField.linearSpace) { matrix.dot(vector).asVector3D() }
|
||||
}
|
||||
|
||||
@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 v = vector(x, y, z)
|
||||
val result = projectToLine(v, line)
|
||||
|
||||
// assert that result is on 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))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -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)
|
||||
}
|
||||
|
||||
@ -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() {
|
||||
|
||||
@ -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() {
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
@ -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<Circle2D, Circle2D> {
|
||||
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = 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)
|
||||
|
||||
@ -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)
|
||||
public fun Pose2D(coordinate: DoubleVector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta)
|
||||
@ -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(
|
||||
|
||||
@ -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)
|
||||
)
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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())
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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())
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user