From eff70eb69099ae3fedea5086cd9a1cda5f3c61c2 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 13 Aug 2023 14:51:50 +0300 Subject: [PATCH] Refactor rotations. Add Rotation matrix to Euler conversion --- .../space/kscience/kmath/geometry/Vector2D.kt | 2 +- .../space/kscience/kmath/geometry/Vector3D.kt | 2 +- .../kmath/geometry/euclidean2d/Circle2D.kt | 4 +- .../geometry/euclidean2d/Float32Space2D.kt | 16 +- .../geometry/euclidean2d/Float64Space2D.kt | 11 +- .../geometry/euclidean3d/Float32Space3D.kt | 16 +- .../geometry/euclidean3d/Float64Space3D.kt | 8 +- .../kmath/geometry/euclidean3d/rotations3D.kt | 193 ++++++++++++++++-- .../kmath/geometry/quaternionOperations.kt | 55 ----- .../kmath/geometry/vectorPrecision.kt | 9 +- .../kscience/kmath/geometry/RotationTest.kt | 23 ++- 11 files changed, 228 insertions(+), 111 deletions(-) delete mode 100644 kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/quaternionOperations.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector2D.kt index 9eced7ba9..58069981e 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector2D.kt +++ b/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 : Point, Vector { +public interface Vector2D : Point { public val x: T public val y: T override val size: Int get() = 2 diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector3D.kt index 7e7c6c4ed..c7fb70fc6 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Vector3D.kt +++ b/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 : Point, Vector { +public interface Vector3D : Point { public val x: T public val y: T public val z: T diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Circle2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Circle2D.kt index 077735b28..1c1305c8b 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Circle2D.kt +++ b/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, ) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float32Space2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float32Space2D.kt index 89c3dc204..0cba56bb5 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float32Space2D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float32Space2D.kt @@ -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 +public interface Float32Vector2D : Vector2D -public object Float32Space2D : - GeometrySpace, - ScaleOperations { +public object Float32Space2D : GeometrySpace { @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) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float64Space2D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float64Space2D.kt index 320924fbf..ae7cefd8f 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean2d/Float64Space2D.kt +++ b/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 public typealias Float64Vector2D = Vector2D @@ -30,10 +28,9 @@ public val Vector2D.r: Double get() = Float64Space2D.norm(this) /** * 2D Euclidean space */ -public object Float64Space2D : - GeometrySpace, - ScaleOperations, - Norm { +public object Float64Space2D : GeometrySpace { + + public val linearSpace: Float64LinearSpace = Float64LinearSpace @Serializable @SerialName("Float64Vector2D") diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float32Space3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float32Space3D.kt index 1413a885b..ce6bbd8fd 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float32Space3D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float32Space3D.kt @@ -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 +public interface Float32Vector3D : Vector3D -public object Float32Space3D : - GeometrySpace, - ScaleOperations{ +public object Float32Space3D : GeometrySpace { @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) diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float64Space3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float64Space3D.kt index b51796012..e808f6129 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/Float64Space3D.kt +++ b/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 public val DoubleVector3D.r: Double get() = Float64Space3D.norm(this) -public object Float64Space3D : GeometrySpace, ScaleOperations, - Norm { +public object Float64Space3D : GeometrySpace{ + + public val linearSpace: Float64LinearSpace = Float64LinearSpace @Serializable @SerialName("Float64Vector3D") diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/rotations3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/rotations3D.kt index deb8b0a93..bf34e2ed2 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/euclidean3d/rotations3D.kt +++ b/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.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): 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): 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") + + 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 { + 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, + 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") } \ No newline at end of file diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/quaternionOperations.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/quaternionOperations.kt deleted file mode 100644 index 49ceda04c..000000000 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/quaternionOperations.kt +++ /dev/null @@ -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() -} \ No newline at end of file diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/vectorPrecision.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/vectorPrecision.kt index 7d3fc7867..dd82a2926 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/vectorPrecision.kt +++ b/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.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) /** diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt index 1d354bf72..7d1687ecb 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt +++ b/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) } + } \ No newline at end of file