teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Refactor rotations. Add Rotation matrix to Euler conversion

Browse Source
This commit is contained in:
Alexander Nozik 2023-08-13 14:51:50 +03:00
parent 67994d35d9
commit eff70eb690
11 changed files with 228 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector2D.kt
View File

@ -7,7 +7,7 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.linear.Point
public interface Vector2D<T> : Point<T>, Vector {
public interface Vector2D<T> : Point<T> {
public val x: T
public val y: T
override val size: Int get() = 2

2
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector3D.kt
View File

@ -8,7 +8,7 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.linear.Point
import space.kscience.kmath.structures.Buffer
public interface Vector3D<T> : Point<T>, Vector {
public interface Vector3D<T> : Point<T> {
public val x: T
public val y: T
public val z: T

4
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Circle2D.kt
View File

@ -6,8 +6,6 @@
package space.kscience.kmath.geometry.euclidean2d
import kotlinx.serialization.Serializable
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import kotlin.math.*
import kotlin.math.PI
/**
@ -16,7 +14,7 @@ import kotlin.math.PI
@Serializable
public data class Circle2D(
@Serializable(Float64Space2D.VectorSerializer::class) public val center: DoubleVector2D,
public val radius: Double
public val radius: Double,
)

16
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float32Space2D.kt
View File

@ -14,17 +14,15 @@ import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.GeometrySpace
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.operations.Float32Field
import space.kscience.kmath.operations.ScaleOperations
import space.kscience.kmath.structures.Float32
import kotlin.math.pow
import kotlin.math.sqrt
@Serializable(Float32Space2D.VectorSerializer::class)
public interface Float32Vector2D: Vector2D<Float>
public interface Float32Vector2D : Vector2D<Float>
public object Float32Space2D :
GeometrySpace<Float32Vector2D>,
ScaleOperations<Float32Vector2D> {
public object Float32Space2D : GeometrySpace<Float32Vector2D, Float32> {
@Serializable
@SerialName("Float32Vector2D")
@ -53,13 +51,13 @@ public object Float32Space2D :
override val zero: Float32Vector2D by lazy { vector(0f, 0f) }
override fun norm(arg: Float32Vector2D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2)).toDouble()
override fun norm(arg: Float32Vector2D): Float32 = sqrt(arg.x.pow(2) + arg.y.pow(2))
public fun Float32Vector2D.norm(): Double = norm(this)
public fun Float32Vector2D.norm(): Float32 = norm(this)
override fun Float32Vector2D.unaryMinus(): Float32Vector2D = vector(-x, -y)
override fun Float32Vector2D.distanceTo(other: Float32Vector2D): Double = (this - other).norm()
override fun Float32Vector2D.distanceTo(other: Float32Vector2D): Float32 = (this - other).norm()
override fun add(left: Float32Vector2D, right: Float32Vector2D): Float32Vector2D =
vector(left.x + right.x, left.y + right.y)
@ -72,6 +70,8 @@ public object Float32Space2D :
public val xAxis: Float32Vector2D = vector(1.0, 0.0)
public val yAxis: Float32Vector2D = vector(0.0, 1.0)
override val defaultPrecision: Float32 = 1e-3f
}
public fun Float32Vector2D(x: Number, y: Number): Float32Vector2D = Float32Space2D.vector(x, y)

11
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float64Space2D.kt
View File

@ -13,14 +13,12 @@ import kotlinx.serialization.encoding.Decoder
import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.GeometrySpace
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.linear.Float64LinearSpace
import space.kscience.kmath.operations.Float64Field
import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations
import kotlin.math.pow
import kotlin.math.sqrt
public typealias DoubleVector2D = Vector2D<Double>
public typealias Float64Vector2D = Vector2D<Double>
@ -30,10 +28,9 @@ public val Vector2D<Double>.r: Double get() = Float64Space2D.norm(this)
/**
* 2D Euclidean space
*/
public object Float64Space2D :
GeometrySpace<DoubleVector2D, Double>,
ScaleOperations<DoubleVector2D>,
Norm<DoubleVector2D, Double> {
public object Float64Space2D : GeometrySpace<DoubleVector2D, Double> {
public val linearSpace: Float64LinearSpace = Float64LinearSpace
@Serializable
@SerialName("Float64Vector2D")

16
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float32Space3D.kt
View File

@ -14,17 +14,15 @@ import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.GeometrySpace
import space.kscience.kmath.geometry.Vector3D
import space.kscience.kmath.operations.Float32Field
import space.kscience.kmath.operations.ScaleOperations
import space.kscience.kmath.structures.Float32
import kotlin.math.pow
import kotlin.math.sqrt
@Serializable(Float32Space3D.VectorSerializer::class)
public interface Float32Vector3D: Vector3D<Float>
public interface Float32Vector3D : Vector3D<Float>
public object Float32Space3D :
GeometrySpace<Float32Vector3D>,
ScaleOperations<Float32Vector3D>{
public object Float32Space3D : GeometrySpace<Float32Vector3D, Float32> {
@Serializable
@SerialName("Float32Vector3D")
@ -54,13 +52,13 @@ public object Float32Space3D :
override val zero: Float32Vector3D by lazy { vector(0.0, 0.0, 0.0) }
override fun norm(arg: Float32Vector3D): Double = sqrt(arg.x.pow(2) + arg.y.pow(2) + arg.z.pow(2)).toDouble()
override fun norm(arg: Float32Vector3D): Float32 = sqrt(arg.x.pow(2) + arg.y.pow(2) + arg.z.pow(2))
public fun Float32Vector3D.norm(): Double = norm(this)
public fun Float32Vector3D.norm(): Float32 = norm(this)
override fun Float32Vector3D.unaryMinus(): Float32Vector3D = vector(-x, -y, -z)
override fun Float32Vector3D.distanceTo(other: Float32Vector3D): Double = (this - other).norm()
override fun Float32Vector3D.distanceTo(other: Float32Vector3D): Float32 = (this - other).norm()
override fun add(left: Float32Vector3D, right: Float32Vector3D): Float32Vector3D =
vector(left.x + right.x, left.y + right.y, left.z + right.z)
@ -101,6 +99,8 @@ public object Float32Space3D :
public val xAxis: Float32Vector3D = vector(1.0, 0.0, 0.0)
public val yAxis: Float32Vector3D = vector(0.0, 1.0, 0.0)
public val zAxis: Float32Vector3D = vector(0.0, 0.0, 1.0)
override val defaultPrecision: Float32 = 1e-3f
}
public fun Float32Vector3D(x: Number, y: Number, z: Number): Float32Vector3D = Float32Space3D.vector(x, y, z)

8
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float64Space3D.kt
View File

@ -13,9 +13,8 @@ import kotlinx.serialization.encoding.Decoder
import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.GeometrySpace
import space.kscience.kmath.geometry.Vector3D
import space.kscience.kmath.linear.Float64LinearSpace
import space.kscience.kmath.operations.Float64Field
import space.kscience.kmath.operations.Norm
import space.kscience.kmath.operations.ScaleOperations
import kotlin.math.pow
import kotlin.math.sqrt
@ -37,8 +36,9 @@ public typealias Float64Vector3D = Vector3D<Double>
public val DoubleVector3D.r: Double get() = Float64Space3D.norm(this)
public object Float64Space3D : GeometrySpace<DoubleVector3D, Double>, ScaleOperations<DoubleVector3D>,
Norm<DoubleVector3D, Double> {
public object Float64Space3D : GeometrySpace<DoubleVector3D, Double>{
public val linearSpace: Float64LinearSpace = Float64LinearSpace
@Serializable
@SerialName("Float64Vector3D")

191
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/rotations3D.kt
View File

@ -6,20 +6,36 @@
package space.kscience.kmath.geometry.euclidean3d
import space.kscience.kmath.UnstableKMathAPI
import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.QuaternionAlgebra
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.complex.reciprocal
import space.kscience.kmath.complex.*
import space.kscience.kmath.geometry.*
import space.kscience.kmath.linear.LinearSpace
import space.kscience.kmath.linear.Matrix
import space.kscience.kmath.linear.linearSpace
import space.kscience.kmath.linear.matrix
import space.kscience.kmath.operations.Float64Field
import kotlin.math.pow
import kotlin.math.sqrt
import kotlin.math.*
internal fun DoubleVector3D.toQuaternion(): Quaternion = Quaternion(0.0, x, y, z)
public operator fun Quaternion.times(other: Quaternion): Quaternion = QuaternionAlgebra.multiply(this, other)
public operator fun Quaternion.div(other: Quaternion): Quaternion = QuaternionAlgebra.divide(this, other)
public fun Quaternion.power(number: Number): Quaternion = QuaternionAlgebra.power(this, number)
/**
* Linear interpolation between [from] and [to] in spherical space
*/
public fun QuaternionAlgebra.slerp(from: Quaternion, to: Quaternion, fraction: Double): Quaternion =
(to / from).pow(fraction) * from
public fun QuaternionAlgebra.angleBetween(q1: Quaternion, q2: Quaternion): Angle = (q1.conjugate * q2).theta
public infix fun Quaternion.dot(other: Quaternion): Double = w * other.w + x * other.x + y * other.y + z * other.z
/**
* Represent a vector as quaternion with zero a rotation angle.
*/
internal fun DoubleVector3D.asQuaternion(): Quaternion = Quaternion(0.0, x, y, z)
/**
* Angle in radians denoted by this quaternion rotation
@ -51,23 +67,30 @@ public val Quaternion.vector: DoubleVector3D
}
/**
* Rotate a vector in a [Float64Space3D]
* Rotate a vector in a [Float64Space3D] with [quaternion]
*/
public fun Float64Space3D.rotate(vector: DoubleVector3D, q: Quaternion): DoubleVector3D = with(QuaternionAlgebra) {
val p = vector.toQuaternion()
(q * p * q.reciprocal).vector
}
public fun Float64Space3D.rotate(vector: DoubleVector3D, quaternion: Quaternion): DoubleVector3D =
with(QuaternionAlgebra) {
val p = vector.asQuaternion()
(quaternion * p * quaternion.reciprocal).vector
}
/**
* Use a composition of quaternions to create a rotation
*/
@UnstableKMathAPI
public fun Float64Space3D.rotate(vector: DoubleVector3D, composition: QuaternionAlgebra.() -> Quaternion): DoubleVector3D =
public fun Float64Space3D.rotate(
vector: DoubleVector3D,
composition: QuaternionAlgebra.() -> Quaternion,
): DoubleVector3D =
rotate(vector, QuaternionAlgebra.composition())
/**
* Rotate a [Float64] vector in 3D space with a rotation matrix
*/
public fun Float64Space3D.rotate(vector: DoubleVector3D, matrix: Matrix<Double>): DoubleVector3D {
require(matrix.colNum == 3 && matrix.rowNum == 3) { "Square 3x3 rotation matrix is required" }
return with(Float64Field.linearSpace) { matrix.dot(vector).asVector3D() }
return with(linearSpace) { (matrix dot vector).asVector3D() }
}
/**
@ -86,6 +109,8 @@ public fun Quaternion.toRotationMatrix(
}
/**
* Convert a quaternion to a rotation matrix
*
* taken from https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
*/
public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix<Double>): Quaternion {
@ -146,6 +171,8 @@ public enum class RotationOrder {
}
/**
* Create a quaternion from Euler angles
*
* Based on https://github.com/mrdoob/three.js/blob/master/src/math/Quaternion.js
*/
public fun Quaternion.Companion.fromEuler(
@ -154,13 +181,13 @@ public fun Quaternion.Companion.fromEuler(
c: Angle,
rotationOrder: RotationOrder,
): Quaternion {
val c1 = cos (a / 2)
val c2 = cos (b / 2)
val c3 = cos (c / 2)
val c1 = cos(a / 2)
val c2 = cos(b / 2)
val c3 = cos(c / 2)
val s1 = sin (a / 2)
val s2 = sin (b / 2)
val s3 = sin (c / 2)
val s1 = sin(a / 2)
val s2 = sin(b / 2)
val s3 = sin(c / 2)
return when (rotationOrder) {
@ -206,6 +233,130 @@ public fun Quaternion.Companion.fromEuler(
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3
)
else -> TODO("Proper Euler rotation orders are not supported yet")
}
}
/**
* A vector consisting of angles
*/
public data class AngleVector(override val x: Angle, override val y: Angle, override val z: Angle) : Vector3D<Angle> {
public companion object
}
public fun Quaternion.Companion.fromEuler(
angles: AngleVector,
rotationOrder: RotationOrder,
): Quaternion = fromEuler(angles.x, angles.y, angles.z, rotationOrder)
/**
* Based on https://github.com/mrdoob/three.js/blob/master/src/math/Euler.js
*/
public fun AngleVector.Companion.fromRotationMatrix(
matrix: Matrix<Double>,
rotationOrder: RotationOrder,
gimbaldLockThreshold: Double = 0.9999999,
): AngleVector = when (rotationOrder) {
RotationOrder.XYZ -> {
if (abs(matrix[0, 2]) < gimbaldLockThreshold) {
AngleVector(
atan2(-matrix[1, 2], matrix[2, 2]).radians,
asin(matrix[0, 2].coerceIn(-1.0, 1.0)).radians,
atan2(-matrix[0, 1], matrix[0, 0]).radians
)
} else {
AngleVector(
atan2(matrix[2, 1], matrix[1, 1]).radians,
asin(matrix[0, 2].coerceIn(-1.0, 1.0)).radians,
Angle.zero
)
}
}
RotationOrder.YXZ -> {
if (abs(matrix[1, 2]) < gimbaldLockThreshold) {
AngleVector(
x = asin(-matrix[1, 2].coerceIn(-1.0, 1.0)).radians,
y = atan2(matrix[0, 2], matrix[2, 2]).radians,
z = atan2(matrix[1, 0], matrix[1, 1]).radians,
)
} else {
AngleVector(
x = asin(-matrix[1, 2].coerceIn(-1.0, 1.0)).radians,
y = atan2(-matrix[2, 0], matrix[0, 0]).radians,
z = Angle.zero,
)
}
}
RotationOrder.ZXY -> {
if (abs(matrix[2, 1]) < gimbaldLockThreshold) {
AngleVector(
x = asin(matrix[2, 1].coerceIn(-1.0, 1.0)).radians,
y = atan2(-matrix[2, 0], matrix[2, 2]).radians,
z = atan2(-matrix[0, 1], matrix[1, 1]).radians,
)
} else {
AngleVector(
x = asin(matrix[2, 1].coerceIn(-1.0, 1.0)).radians,
y = Angle.zero,
z = atan2(matrix[1, 0], matrix[0, 0]).radians,
)
}
}
RotationOrder.ZYX -> {
if (abs(matrix[2, 0]) < gimbaldLockThreshold) {
AngleVector(
x = atan2(matrix[2, 1], matrix[2, 2]).radians,
y = asin(-matrix[2, 0].coerceIn(-1.0, 1.0)).radians,
z = atan2(matrix[1, 0], matrix[0, 0]).radians,
)
} else {
AngleVector(
x = Angle.zero,
y = asin(-matrix[2, 0].coerceIn(-1.0, 1.0)).radians,
z = atan2(-matrix[0, 1], matrix[1, 1]).radians,
)
}
}
RotationOrder.YZX -> {
if (abs(matrix[1, 0]) < gimbaldLockThreshold) {
AngleVector(
x = atan2(-matrix[1, 2], matrix[1, 1]).radians,
y = atan2(-matrix[2, 0], matrix[0, 0]).radians,
z = asin(matrix[1, 0].coerceIn(-1.0, 1.0)).radians,
)
} else {
AngleVector(
x = Angle.zero,
y = atan2(matrix[0, 2], matrix[2, 2]).radians,
z = asin(matrix[1, 0].coerceIn(-1.0, 1.0)).radians,
)
}
}
RotationOrder.XZY -> {
if (abs(matrix[0, 1]) < gimbaldLockThreshold) {
AngleVector(
x = atan2(matrix[2, 1], matrix[1, 1]).radians,
y = atan2(matrix[0, 2], matrix[0, 0]).radians,
z = asin(-matrix[0, 1].coerceIn(-1.0, 1.0)).radians,
)
} else {
AngleVector(
x = atan2(-matrix[1, 2], matrix[2, 2]).radians,
y = Angle.zero,
z = asin(-matrix[0, 1].coerceIn(-1.0, 1.0)).radians,
)
}
}
else -> TODO("Proper Euler rotation orders are not supported yet")
}

55
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/quaternionOperations.kt
View File

@ -1,55 +0,0 @@
/*
* Copyright 2018-2023 KMath contributors.
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
*/
package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.QuaternionAlgebra
import space.kscience.kmath.complex.conjugate
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.geometry.euclidean3d.Float32Space3D
import space.kscience.kmath.geometry.euclidean3d.Float32Vector3D
import space.kscience.kmath.geometry.euclidean3d.theta
import kotlin.math.asin
import kotlin.math.atan2
import kotlin.math.pow
public operator fun Quaternion.times(other: Quaternion): Quaternion = QuaternionAlgebra.multiply(this, other)
public operator fun Quaternion.div(other: Quaternion): Quaternion = QuaternionAlgebra.divide(this, other)
public fun Quaternion.power(number: Number): Quaternion = QuaternionAlgebra.power(this, number)
/**
* Linear interpolation between [from] and [to] in spherical space
*/
public fun QuaternionAlgebra.slerp(from: Quaternion, to: Quaternion, fraction: Double): Quaternion =
(to / from).pow(fraction) * from
public fun QuaternionAlgebra.angleBetween(q1: Quaternion, q2: Quaternion): Angle = (q1.conjugate * q2).theta
public val Quaternion.inclination: Radians get() = asin(2 * (w * y - x * z)).radians
public val Quaternion.azimuth: Angle get() = atan2(2 * (w * z + x * y), 1 - 2 * (y.pow(2) + z.pow(2))).radians.normalized()
public infix fun Quaternion.dot(other: Quaternion): Double = w * other.w + x * other.x + y * other.y + z * other.z
private fun Quaternion.normalizedToEuler(): Float32Vector3D {
val roll = atan2(2 * y * w + 2 * x * z, 1 - 2 * y * y - 2 * z * z);
val pitch = atan2(2 * x * w - 2 * y * z, 1 - 2 * x * x - 2 * z * z);
val yaw = asin(2 * x * y + 2 * z * w);
return Float32Vector3D(roll, pitch, yaw)
}
/**
* Quaternion to XYZ Cardan angles
*/
public fun Quaternion.toEuler(): Float32Vector3D = if (QuaternionAlgebra.norm(this) == 0.0) {
Float32Space3D.zero
} else {
normalized().normalizedToEuler()
}

9
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/vectorPrecision.kt
View File

@ -5,6 +5,11 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.geometry.euclidean2d.Float64Space2D
import space.kscience.kmath.geometry.euclidean2d.Float64Vector2D
import space.kscience.kmath.geometry.euclidean3d.Float64Space3D
import space.kscience.kmath.geometry.euclidean3d.Float64Vector3D
/**
* Vector equality within given [precision] (using [GeometrySpace.norm] provided by the space
@ -22,7 +27,7 @@ public fun <V : Any, D : Comparable<D>> V.equalsVector(
*/
public fun Float64Vector2D.equalsVector(
other: Float64Vector2D,
precision: Double = Euclidean3DSpace.defaultPrecision,
precision: Double = Float64Space2D.defaultPrecision,
): Boolean = equalsVector(Float64Space2D, other, precision)
/**
@ -30,7 +35,7 @@ public fun Float64Vector2D.equalsVector(
*/
public fun Float64Vector3D.equalsVector(
other: Float64Vector3D,
precision: Double = Euclidean3DSpace.defaultPrecision,
precision: Double = Float64Space3D.defaultPrecision,
): Boolean = equalsVector(Float64Space3D, other, precision)
/**

23
kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt
View File

@ -6,6 +6,7 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.QuaternionAlgebra
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.geometry.euclidean3d.*
import space.kscience.kmath.structures.Float64Buffer
@ -26,12 +27,31 @@ class RotationTest {
}
@Test
fun matrixConversion() {
fun matrixConversion() = with(QuaternionAlgebra){
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
val matrix = q.toRotationMatrix()
for (ro in listOf(
RotationOrder.XYZ,
RotationOrder.YXZ,
RotationOrder.ZXY,
RotationOrder.ZYX,
RotationOrder.YZX,
RotationOrder.XZY
)) {
val angles = AngleVector.fromRotationMatrix(matrix, ro)
val reconstructed = Quaternion.fromEuler(angles, ro)
if( reconstructed.w>0) {
assertBufferEquals(q, reconstructed)
} else{
assertBufferEquals(q, -reconstructed)
}
}
assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix))
}
@ -50,4 +70,5 @@ class RotationTest {
val q1 = Quaternion.fromEuler(0.1.radians, 0.2.radians, 0.3.radians, RotationOrder.XYZ)
assertBufferEquals(Float64Buffer(0.9818562, 0.0640713, 0.0911575, 0.1534393), q1)
}
}