forked from kscience/kmath
Add rotations converter to Quaternions
This commit is contained in:
parent
5af0c91f0a
commit
978de59b7a
@ -117,6 +117,11 @@ public val Quaternion.reciprocal: Quaternion
|
|||||||
return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2)
|
return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//TODO consider adding a-priory normalized quaternions
|
||||||
|
/**
|
||||||
|
* Produce a normalized version of this quaternion
|
||||||
|
*/
|
||||||
public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) }
|
public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -27,8 +27,9 @@ public interface Vector3D : Point<Double>, Vector {
|
|||||||
override operator fun iterator(): Iterator<Double> = listOf(x, y, z).iterator()
|
override operator fun iterator(): Iterator<Double> = listOf(x, y, z).iterator()
|
||||||
}
|
}
|
||||||
|
|
||||||
@Suppress("FunctionName")
|
public operator fun Vector3D.component1(): Double = x
|
||||||
public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z)
|
public operator fun Vector3D.component2(): Double = y
|
||||||
|
public operator fun Vector3D.component3(): Double = z
|
||||||
|
|
||||||
public fun Buffer<Double>.asVector3D(): Vector3D = object : Vector3D {
|
public fun Buffer<Double>.asVector3D(): Vector3D = object : Vector3D {
|
||||||
init {
|
init {
|
||||||
@ -51,6 +52,9 @@ private data class Vector3DImpl(
|
|||||||
override val z: Double,
|
override val z: Double,
|
||||||
) : Vector3D
|
) : Vector3D
|
||||||
|
|
||||||
|
|
||||||
|
public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z)
|
||||||
|
|
||||||
public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector3D> {
|
public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector3D> {
|
||||||
override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) }
|
override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) }
|
||||||
|
|
||||||
@ -67,4 +71,8 @@ public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector
|
|||||||
|
|
||||||
override fun Vector3D.dot(other: Vector3D): Double =
|
override fun Vector3D.dot(other: Vector3D): Double =
|
||||||
x * other.x + y * other.y + z * other.z
|
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)
|
||||||
}
|
}
|
||||||
|
@ -7,11 +7,9 @@ package space.kscience.kmath.geometry
|
|||||||
|
|
||||||
import space.kscience.kmath.complex.Quaternion
|
import space.kscience.kmath.complex.Quaternion
|
||||||
import space.kscience.kmath.complex.QuaternionField
|
import space.kscience.kmath.complex.QuaternionField
|
||||||
|
import space.kscience.kmath.complex.normalized
|
||||||
import space.kscience.kmath.complex.reciprocal
|
import space.kscience.kmath.complex.reciprocal
|
||||||
import space.kscience.kmath.linear.LinearSpace
|
import space.kscience.kmath.linear.*
|
||||||
import space.kscience.kmath.linear.Matrix
|
|
||||||
import space.kscience.kmath.linear.linearSpace
|
|
||||||
import space.kscience.kmath.linear.matrix
|
|
||||||
import space.kscience.kmath.misc.UnstableKMathAPI
|
import space.kscience.kmath.misc.UnstableKMathAPI
|
||||||
import space.kscience.kmath.operations.DoubleField
|
import space.kscience.kmath.operations.DoubleField
|
||||||
import kotlin.math.pow
|
import kotlin.math.pow
|
||||||
@ -22,16 +20,25 @@ internal fun Vector3D.toQuaternion(): Quaternion = Quaternion(0.0, x, y, z)
|
|||||||
/**
|
/**
|
||||||
* Angle in radians denoted by this quaternion rotation
|
* Angle in radians denoted by this quaternion rotation
|
||||||
*/
|
*/
|
||||||
public val Quaternion.theta: Double get() = kotlin.math.acos(w) * 2
|
public val Quaternion.theta: Radians get() = (kotlin.math.acos(normalized().w) * 2).radians
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Create a normalized Quaternion from rotation angle and rotation vector
|
||||||
|
*/
|
||||||
|
public fun Quaternion.Companion.fromRotation(theta: Angle, vector: Vector3D): Quaternion {
|
||||||
|
val s = sin(theta / 2)
|
||||||
|
val c = cos(theta / 2)
|
||||||
|
val norm = with(Euclidean3DSpace) { vector.norm() }
|
||||||
|
return Quaternion(c, vector.x * s / norm, vector.y * s / norm, vector.z * s / norm)
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* An axis of quaternion rotation
|
* An axis of quaternion rotation
|
||||||
*/
|
*/
|
||||||
public val Quaternion.vector: Vector3D
|
public val Quaternion.vector: Vector3D
|
||||||
get() {
|
get() {
|
||||||
val sint2 = sqrt(1 - w * w)
|
|
||||||
|
|
||||||
return object : Vector3D {
|
return object : Vector3D {
|
||||||
|
private val sint2 = sqrt(1 - w * w)
|
||||||
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
|
||||||
@ -50,6 +57,7 @@ public fun Euclidean3DSpace.rotate(vector: Vector3D, q: Quaternion): Vector3D =
|
|||||||
/**
|
/**
|
||||||
* Use a composition of quaternions to create a rotation
|
* 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: Vector3D, composition: QuaternionField.() -> Quaternion): Vector3D =
|
||||||
rotate(vector, QuaternionField.composition())
|
rotate(vector, QuaternionField.composition())
|
||||||
|
|
||||||
@ -114,3 +122,86 @@ public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix<Double>): Quat
|
|||||||
)
|
)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public enum class RotationOrder {
|
||||||
|
// proper Euler
|
||||||
|
XZX,
|
||||||
|
XYX,
|
||||||
|
YXY,
|
||||||
|
YZY,
|
||||||
|
ZYZ,
|
||||||
|
ZXZ,
|
||||||
|
|
||||||
|
//Tait–Bryan
|
||||||
|
XZY,
|
||||||
|
XYZ,
|
||||||
|
YXZ,
|
||||||
|
YZX,
|
||||||
|
ZYX,
|
||||||
|
ZXY
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Based on https://github.com/mrdoob/three.js/blob/master/src/math/Quaternion.js
|
||||||
|
*/
|
||||||
|
public fun Quaternion.Companion.fromEuler(
|
||||||
|
a: Angle,
|
||||||
|
b: Angle,
|
||||||
|
c: Angle,
|
||||||
|
rotationOrder: RotationOrder,
|
||||||
|
): Quaternion {
|
||||||
|
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)
|
||||||
|
|
||||||
|
return when (rotationOrder) {
|
||||||
|
|
||||||
|
RotationOrder.XYZ -> Quaternion(
|
||||||
|
c1 * c2 * c3 - s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 + c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 - s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 + s1 * s2 * c3
|
||||||
|
)
|
||||||
|
|
||||||
|
RotationOrder.YXZ -> Quaternion(
|
||||||
|
c1 * c2 * c3 + s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 + c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 - s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 - s1 * s2 * c3
|
||||||
|
)
|
||||||
|
|
||||||
|
RotationOrder.ZXY -> Quaternion(
|
||||||
|
c1 * c2 * c3 - s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 - c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 + s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 + s1 * s2 * c3
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
RotationOrder.ZYX -> Quaternion(
|
||||||
|
c1 * c2 * c3 + s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 - c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 + s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 - s1 * s2 * c3
|
||||||
|
)
|
||||||
|
|
||||||
|
RotationOrder.YZX -> Quaternion(
|
||||||
|
c1 * c2 * c3 - s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 + c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 + s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 - s1 * s2 * c3
|
||||||
|
)
|
||||||
|
|
||||||
|
RotationOrder.XZY -> Quaternion(
|
||||||
|
c1 * c2 * c3 + s1 * s2 * s3,
|
||||||
|
s1 * c2 * c3 - c1 * s2 * s3,
|
||||||
|
c1 * s2 * c3 - s1 * c2 * s3,
|
||||||
|
c1 * c2 * s3 + s1 * s2 * c3
|
||||||
|
)
|
||||||
|
else -> TODO("Proper Euler rotation orders are not supported yet")
|
||||||
|
}
|
||||||
|
}
|
@ -7,13 +7,14 @@ package space.kscience.kmath.geometry
|
|||||||
|
|
||||||
import space.kscience.kmath.complex.Quaternion
|
import space.kscience.kmath.complex.Quaternion
|
||||||
import space.kscience.kmath.complex.normalized
|
import space.kscience.kmath.complex.normalized
|
||||||
|
import space.kscience.kmath.structures.DoubleBuffer
|
||||||
import space.kscience.kmath.testutils.assertBufferEquals
|
import space.kscience.kmath.testutils.assertBufferEquals
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
|
|
||||||
class RotationTest {
|
class RotationTest {
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun rotations() = with(Euclidean3DSpace) {
|
fun differentRotations() = with(Euclidean3DSpace) {
|
||||||
val vector = Vector3D(1.0, 1.0, 1.0)
|
val vector = Vector3D(1.0, 1.0, 1.0)
|
||||||
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
|
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
|
||||||
val rotatedByQ = rotate(vector, q)
|
val rotatedByQ = rotate(vector, q)
|
||||||
@ -24,7 +25,7 @@ class RotationTest {
|
|||||||
}
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun rotationConversion() {
|
fun matrixConversion() {
|
||||||
|
|
||||||
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
|
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
|
||||||
|
|
||||||
@ -32,4 +33,18 @@ class RotationTest {
|
|||||||
|
|
||||||
assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix))
|
assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix))
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun fromRotation() {
|
||||||
|
val q = Quaternion.fromRotation(0.3.radians, Vector3D(1.0, 1.0, 1.0))
|
||||||
|
|
||||||
|
assertBufferEquals(DoubleBuffer(0.9887711, 0.0862781, 0.0862781, 0.0862781), q)
|
||||||
|
}
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun fromEuler() {
|
||||||
|
val q = Quaternion.fromEuler(0.1.radians, 0.2.radians, 0.3.radians, RotationOrder.ZXY)
|
||||||
|
|
||||||
|
assertBufferEquals(DoubleBuffer(0.9818562, 0.0342708, 0.1060205, 0.1534393), q)
|
||||||
|
}
|
||||||
}
|
}
|
@ -470,7 +470,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
}
|
}
|
||||||
|
|
||||||
private inline fun submit(signature: Map<Symbol, UInt>, lazyCoefficient: Ring<C>.() -> C) {
|
private inline fun submit(signature: Map<Symbol, UInt>, lazyCoefficient: Ring<C>.() -> C) {
|
||||||
submit(signature, lazyCoefficient, { it + lazyCoefficient() })
|
submit(signature, lazyCoefficient) { it + lazyCoefficient() }
|
||||||
}
|
}
|
||||||
|
|
||||||
private fun submit(signature: Map<Symbol, UInt>, coefficient: C) {
|
private fun submit(signature: Map<Symbol, UInt>, coefficient: C) {
|
||||||
@ -478,9 +478,9 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
}
|
}
|
||||||
|
|
||||||
// TODO: `@submit` will be resolved differently. Change it to `@C`.
|
// TODO: `@submit` will be resolved differently. Change it to `@C`.
|
||||||
private fun C.submit() = submit(emptyMap(), { this@submit })
|
private fun C.submitSelf() = submit(emptyMap()) { this@submitSelf }
|
||||||
|
|
||||||
private fun Symbol.submit() = submit(mapOf(this to 1u), { one })
|
private fun Symbol.submit() = submit(mapOf(this to 1u)) { one }
|
||||||
|
|
||||||
private fun Term.submit(): Submit {
|
private fun Term.submit(): Submit {
|
||||||
submit(signature, coefficient)
|
submit(signature, coefficient)
|
||||||
@ -490,7 +490,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
public object Submit
|
public object Submit
|
||||||
|
|
||||||
public operator fun C.unaryPlus(): Submit {
|
public operator fun C.unaryPlus(): Submit {
|
||||||
submit()
|
submitSelf()
|
||||||
return Submit
|
return Submit
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -500,12 +500,12 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
}
|
}
|
||||||
|
|
||||||
public operator fun C.plus(other: C): Submit {
|
public operator fun C.plus(other: C): Submit {
|
||||||
submit(emptyMap(), { this@plus + other })
|
submit(emptyMap()) { this@plus + other }
|
||||||
return Submit
|
return Submit
|
||||||
}
|
}
|
||||||
|
|
||||||
public operator fun C.minus(other: C): Submit {
|
public operator fun C.minus(other: C): Submit {
|
||||||
submit(emptyMap(), { this@minus - other })
|
submit(emptyMap()) { this@minus - other }
|
||||||
return Submit
|
return Submit
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -541,7 +541,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
|
|
||||||
public operator fun Symbol.plus(other: C): Submit {
|
public operator fun Symbol.plus(other: C): Submit {
|
||||||
this.submit()
|
this.submit()
|
||||||
other.submit()
|
other.submitSelf()
|
||||||
return Submit
|
return Submit
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -599,7 +599,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
|
|||||||
|
|
||||||
public operator fun Term.plus(other: C): Submit {
|
public operator fun Term.plus(other: C): Submit {
|
||||||
this.submit()
|
this.submit()
|
||||||
other.submit()
|
other.submitSelf()
|
||||||
return Submit
|
return Submit
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user