Refactor rotations. Add Rotation matrix to Euler conversion

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

@@ -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

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

@@ -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

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

@@ -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,
 )
 
 

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

@@ -14,7 +14,7 @@ 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
 
@@ -22,9 +22,7 @@ import kotlin.math.sqrt
 public interface Float32Vector2D : Vector2D<Float>
 
 
-public object Float32Space2D :
+public object Float32Space2D : GeometrySpace<Float32Vector2D, Float32> {
-    GeometrySpace<Float32Vector2D>,
-    ScaleOperations<Float32Vector2D> {
 
     @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)

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

@@ -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 :
+public object Float64Space2D : GeometrySpace<DoubleVector2D, Double> {
-    GeometrySpace<DoubleVector2D, Double>,
+
-    ScaleOperations<DoubleVector2D>,
+    public val linearSpace: Float64LinearSpace = Float64LinearSpace
-    Norm<DoubleVector2D, Double> {
 
     @Serializable
     @SerialName("Float64Vector2D")

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

@@ -14,7 +14,7 @@ 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
 
@@ -22,9 +22,7 @@ import kotlin.math.sqrt
 public interface Float32Vector3D : Vector3D<Float>
 
 
-public object Float32Space3D :
+public object Float32Space3D : GeometrySpace<Float32Vector3D, Float32> {
-    GeometrySpace<Float32Vector3D>,
-    ScaleOperations<Float32Vector3D>{
 
     @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)

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

@@ -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>,
+public object Float64Space3D : GeometrySpace<DoubleVector3D, Double>{
-    Norm<DoubleVector3D, Double> {
+
+    public val linearSpace: Float64LinearSpace = Float64LinearSpace
 
     @Serializable
     @SerialName("Float64Vector3D")

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

@@ -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.*
-import space.kscience.kmath.complex.QuaternionAlgebra
-import space.kscience.kmath.complex.normalized
-import space.kscience.kmath.complex.reciprocal
 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.*
-import kotlin.math.sqrt
 
-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) {
+public fun Float64Space3D.rotate(vector: DoubleVector3D, quaternion: Quaternion): DoubleVector3D =
-    val p = vector.toQuaternion()
+    with(QuaternionAlgebra) {
-    (q * p * q.reciprocal).vector
+        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(
@@ -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")
+}

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

@@ -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()
-}

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

@@ -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)
 
 /**

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

@@ -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)
     }
+
 }