Fix quaternion rotation tests

kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt

@@ -117,6 +117,8 @@ public val Quaternion.reciprocal: Quaternion
         return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2)
     }
 
+public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) }
+
 /**
  * A field of [Quaternion].
  */

kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt

@@ -138,6 +138,10 @@ private class MutableStructure2DWrapper<T>(val structure: MutableStructureND<T>)
     override fun equals(other: Any?): Boolean = false
 
     override fun hashCode(): Int = 0
+
+    override fun toString(): String {
+        return StructureND.toString(structure)
+    }
 }
 
 /**

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

@@ -14,7 +14,6 @@ import space.kscience.kmath.linear.linearSpace
 import space.kscience.kmath.linear.matrix
 import space.kscience.kmath.misc.UnstableKMathAPI
 import space.kscience.kmath.operations.DoubleField
-import space.kscience.kmath.operations.invoke
 import kotlin.math.pow
 import kotlin.math.sqrt
 
@@ -33,9 +32,9 @@ public val Quaternion.vector: Vector3D
         val sint2 = sqrt(1 - w * w)
 
         return object : Vector3D {
-            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
-            override val z: Double get() = this@vector.z/sint2
+            override val z: Double get() = this@vector.z / sint2
             override fun toString(): String = listOf(x, y, z).toString()
         }
     }
@@ -75,26 +74,43 @@ public fun Quaternion.toRotationMatrix(
 }
 
 /**
- * taken from https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
+ * taken from https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
  */
 public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix<Double>): Quaternion {
-    val t: Double
+    require(matrix.colNum == 3 && matrix.rowNum == 3) { "Rotation matrix should be 3x3 but is ${matrix.rowNum}x${matrix.colNum}" }
-    val q = if (matrix[2, 2] < 0) {
+    val trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2]
-        if (matrix[0, 0] > matrix[1, 1]) {
+
-            t = 1 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]
+    return if (trace > 0) {
-            Quaternion(t, matrix[0, 1] + matrix[1, 0], matrix[2, 0] + matrix[0, 2], matrix[1, 2] - matrix[2, 1])
+        val s = sqrt(trace + 1.0) * 2 // S=4*qw
-        } else {
+        Quaternion(
-            t = 1 - matrix[0, 0] + matrix[1, 1] - matrix[2, 2]
+            w = 0.25 * s,
-            Quaternion(matrix[0, 1] + matrix[1, 0], t, matrix[1, 2] + matrix[2, 1], matrix[2, 0] - matrix[0, 2])
+            x = (matrix[2, 1] - matrix[1, 2]) / s,
-        }
+            y = (matrix[0, 2] - matrix[2, 0]) / s,
+            z = (matrix[1, 0] - matrix[0, 1]) / s,
+        )
+    } else if ((matrix[0, 0] > matrix[1, 1]) && (matrix[0, 0] > matrix[2, 2])) {
+        val s = sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2 // S=4*qx
+        Quaternion(
+            w = (matrix[2, 1] - matrix[1, 2]) / s,
+            x = 0.25 * s,
+            y = (matrix[0, 1] + matrix[1, 0]) / s,
+            z = (matrix[0, 2] + matrix[2, 0]) / s,
+        )
+    } else if (matrix[1, 1] > matrix[2, 2]) {
+        val s = sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2 // S=4*qy
+        Quaternion(
+            w = (matrix[0, 2] - matrix[2, 0]) / s,
+            x = (matrix[0, 1] + matrix[1, 0]) / s,
+            y = 0.25 * s,
+            z = (matrix[1, 2] + matrix[2, 1]) / s,
+        )
     } else {
-        if (matrix[0, 0] < -matrix[1, 1]) {
+        val s = sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2 // S=4*qz
-            t = 1 - matrix[0, 0] - matrix[1, 1] + matrix[2, 2]
+        Quaternion(
-            Quaternion(matrix[2, 0] + matrix[0, 2], matrix[1, 2] + matrix[2, 1], t, matrix[0, 1] - matrix[1, 0])
+            w = (matrix[1, 0] - matrix[0, 1]) / s,
-        } else {
+            x = (matrix[0, 2] + matrix[2, 0]) / s,
-            t = 1 + matrix[0, 0] + matrix[1, 1] + matrix[2, 2]
+            y = (matrix[1, 2] + matrix[2, 1]) / s,
-            Quaternion(matrix[1, 2] - matrix[2, 1], matrix[2, 0] - matrix[0, 2], matrix[0, 1] - matrix[1, 0], t)
+            z = 0.25 * s,
-        }
+        )
     }
-    return QuaternionField.invoke { q * (0.5 / sqrt(t)) }
 }

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

@@ -6,17 +6,16 @@
 package space.kscience.kmath.geometry
 
 import space.kscience.kmath.complex.Quaternion
+import space.kscience.kmath.complex.normalized
 import space.kscience.kmath.testutils.assertBufferEquals
-import kotlin.test.Ignore
 import kotlin.test.Test
-import kotlin.test.assertEquals
 
 class RotationTest {
 
     @Test
     fun rotations() = with(Euclidean3DSpace) {
         val vector = Vector3D(1.0, 1.0, 1.0)
-        val q = Quaternion(1.0, 2.0, -3.0, 4.0)
+        val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
         val rotatedByQ = rotate(vector, q)
         val matrix = q.toRotationMatrix()
         val rotatedByM = rotate(vector,matrix)
@@ -25,13 +24,12 @@ class RotationTest {
     }
 
     @Test
-    @Ignore
     fun rotationConversion() {
 
-        val q = Quaternion(1.0, 2.0, -3.0, 4.0)
+        val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
 
         val matrix = q.toRotationMatrix()
 
-        assertEquals(q, Quaternion.fromRotationMatrix(matrix))
+        assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix))
     }
 }