commonMain.korlibs.math.geom.EulerRotation.kt Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of korma-jvm Show documentation
Show all versions of korma-jvm Show documentation
Mathematic library for Multiplatform Kotlin 1.3
The newest version!
package korlibs.math.geom
import korlibs.memory.*
import kotlin.math.*
/**
* Rotations around Z axis, then X axis, then Y axis in that order.
*/
inline class EulerRotation private constructor(val data: Vector4) {
val config: Config get() = Config(data.w.toInt())
val order: Order get() = config.order
val coordinateSystem: CoordinateSystem get() = config.coordinateSystem
enum class Order(
val x: Int, val y: Int, val z: Int, val w: Int, val str: String,
) {
INVALID(0, 0, 0, 0, "XXX"),
XYZ(+1, -1, +1, -1, "XYZ"),
XZY(-1, -1, +1, +1, "XZY"),
YXZ(+1, -1, -1, +1, "YXZ"),
YZX(+1, +1, -1, -1, "YZX"),
ZXY(-1, +1, +1, -1, "ZXY"),
ZYX(-1, +1, -1, +1, "ZYX"),
;
fun withCoordinateSystem(coordinateSystem: CoordinateSystem) = if (coordinateSystem.sign < 0) reversed() else this
fun reversed(): Order = when (this) {
INVALID -> INVALID
XYZ -> ZYX
XZY -> YZX
YXZ -> ZXY
YZX -> XZY
ZXY -> YXZ
ZYX -> XYZ
}
fun indexAt(pos: Int, reversed: Boolean = false): Int = str[(if (reversed) 2 - pos else pos) umod 3] - 'X'
override fun toString(): String = "$name [$x, $y, $z, $w]"
companion object {
val VALUES = values()
val DEFAULT = XYZ
}
}
//enum class Normalized { NO, FULL_ANGLE, HALF_ANGLE }
inline class Config(val id: Int) {
//constructor(order: Order, coordinateSystem: CoordinateSystem) : this(order.ordinal * coordinateSystem.sign)
constructor(order: Order, coordinateSystem: CoordinateSystem) : this(order.withCoordinateSystem(coordinateSystem).ordinal)
val order: Order get() = Order.VALUES[id.absoluteValue]
val coordinateSystem: CoordinateSystem get() = if (id < 0) CoordinateSystem.LEFT_HANDED else CoordinateSystem.RIGHT_HANDED
override fun toString(): String = "EulerRotation.Config(order=$order, coordinateSystem=$coordinateSystem)"
companion object {
val UNITY get() = Config(Order.ZXY, CoordinateSystem.LEFT_HANDED)
//val UNITY get() = LIBGDX
val UNREAL get() = Config(Order.ZYX, CoordinateSystem.LEFT_HANDED)
//val UNREAL get() = THREEJS
val GODOT get() = Config(Order.YXZ, CoordinateSystem.RIGHT_HANDED)
val LIBGDX get() = Config(Order.YXZ, CoordinateSystem.RIGHT_HANDED)
val THREEJS get() = Config(Order.XYZ, CoordinateSystem.RIGHT_HANDED)
// Same as Three.JS
val DEFAULT get() = Config(Order.XYZ, CoordinateSystem.RIGHT_HANDED)
}
}
enum class CoordinateSystem(val sign: Int) {
LEFT_HANDED(-1), RIGHT_HANDED(+1);
val rsign = -sign
}
val roll: Angle get() = Angle.fromRatio(data.x)
val pitch: Angle get() = Angle.fromRatio(data.y)
val yaw: Angle get() = Angle.fromRatio(data.z)
@Deprecated("", ReplaceWith("roll")) val x: Angle get() = roll
@Deprecated("", ReplaceWith("pitch")) val y: Angle get() = pitch
@Deprecated("", ReplaceWith("yaw")) val z: Angle get() = yaw
override fun toString(): String = "EulerRotation(roll=$roll, pitch=$pitch, yaw=$yaw)"
fun copy(roll: Angle = this.roll, pitch: Angle = this.pitch, yaw: Angle = this.yaw): EulerRotation = EulerRotation(roll, pitch, yaw)
constructor() : this(Angle.ZERO, Angle.ZERO, Angle.ZERO)
constructor(roll: Angle, pitch: Angle, yaw: Angle, config: Config = Config.DEFAULT)
: this(Vector4(roll.ratio, pitch.ratio, yaw.ratio, config.id.toFloat()))
fun normalized(): EulerRotation = EulerRotation(roll.normalized, pitch.normalized, yaw.normalized)
fun normalizedHalf(): EulerRotation = EulerRotation(roll.normalizedHalf, pitch.normalizedHalf, yaw.normalizedHalf)
fun toMatrix(): Matrix4 = toQuaternion().toMatrix()
fun toQuaternion(): Quaternion = _toQuaternion(x, y, z, config)
fun isAlmostEquals(other: EulerRotation, epsilon: Float = 0.00001f): Boolean =
this.data.isAlmostEquals(other.data, epsilon)
companion object {
fun toQuaternion(roll: Angle, pitch: Angle, yaw: Angle, config: Config = Config.DEFAULT): Quaternion {
return _toQuaternion(roll, pitch, yaw, config)
}
// http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
private fun _toQuaternion(x: Angle, y: Angle, z: Angle, config: Config = Config.DEFAULT): Quaternion {
val order = config.order
val coordinateSystem = config.coordinateSystem
val sign = coordinateSystem.sign
//println("ORDER=$order, coordinateSystem=$coordinateSystem, sign=$sign")
val c1 = cos(x / 2)
val c2 = cos(y / 2)
val c3 = cos(z / 2)
val s1 = sin(x / 2)
val s2 = sin(y / 2)
val s3 = sin(z / 2)
return Quaternion(
((s1 * c2 * c3) + ((c1 * s2 * s3) * order.x * sign)),
((c1 * s2 * c3) + ((s1 * c2 * s3) * order.y * sign)),
((c1 * c2 * s3) + ((s1 * s2 * c3) * order.z * sign)),
((c1 * c2 * c3) + ((s1 * s2 * s3) * order.w * sign)),
)
}
fun fromRotationMatrix(m: Matrix3, config: Config = Config.DEFAULT): EulerRotation {
//val config = if (config == Config.UNITY) Config.LIBGDX else config
val order = config.order
val coordinateSystem = config.coordinateSystem
val sign = coordinateSystem.sign
//val m = if (sign < 0) m.transposed() else m
//val m = m
val m11 = m.v00
val m12 = m.v01
val m13 = m.v02
val m21 = m.v10
val m22 = m.v11
val m23 = m.v12
val m31 = m.v20
val m32 = m.v21
val m33 = m.v22
val x: Angle
val y: Angle
val z: Angle
when (order) {
Order.XYZ -> {
x = if (m13.absoluteNotAlmostOne) Angle.atan2(-m23, m33) else Angle.atan2(m32, m22)
y = Angle.asin(m13.clamp(-1f, +1f))
z = if (m13.absoluteNotAlmostOne) Angle.atan2(-m12, m11) else Angle.ZERO
}
Order.YXZ -> {
x = Angle.asin(-(m23.clamp(-1f, +1f)))
y = if (m23.absoluteNotAlmostOne) Angle.atan2(m13, m33) else Angle.atan2(-m31, m11)
z = if (m23.absoluteNotAlmostOne) Angle.atan2(m21, m22) else Angle.ZERO
}
Order.ZXY -> {
y = Angle.asin(m32.clamp(-1f, +1f))
x = if (m32.absoluteNotAlmostOne) Angle.atan2(-m31, m33) else Angle.ZERO
z = if (m32.absoluteNotAlmostOne) Angle.atan2(-m12, m22) else Angle.atan2(m21, m11)
}
Order.ZYX -> {
x = if (m31.absoluteNotAlmostOne) Angle.atan2(m32, m33) else Angle.ZERO
y = Angle.asin(-(m31.clamp(-1f, +1f)))
z = if (m31.absoluteNotAlmostOne) Angle.atan2(m21, m11) else Angle.atan2(-m12, m22)
}
Order.YZX -> {
x = if (m21.absoluteNotAlmostOne) Angle.atan2(-m23, m22) else Angle.ZERO
y = if (m21.absoluteNotAlmostOne) Angle.atan2(-m31, m11) else Angle.atan2(m13, m33)
z = Angle.asin(m21.clamp(-1f, +1f))
}
Order.XZY -> {
x = if (m12.absoluteNotAlmostOne) Angle.atan2(m32, m22) else Angle.atan2(-m23, m33)
y = if (m12.absoluteNotAlmostOne) Angle.atan2(m13, m11) else Angle.ZERO
z = Angle.asin(-(m12.clamp(-1f, +1f)))
}
Order.INVALID -> error("Invalid")
}
//println("order=$order, coordinateSystem=$coordinateSystem : ${coordinateSystem.sign}, x=$x, y=$y, z=$z")
//val sign = coordinateSystem.sign
//return EulerRotation(x * coordinateSystem.sign, y * coordinateSystem.sign, z * coordinateSystem.sign, config)
//return EulerRotation(x * sign, y * sign, z * sign, config)
return EulerRotation(x, y, z, config)
}
private val Float.absoluteNotAlmostOne: Boolean get() = absoluteValue < 0.9999999
fun fromQuaternion(q: Quaternion, config: Config = Config.DEFAULT): EulerRotation {
return fromRotationMatrix(q.toMatrix3(), config)
/*
//return fromQuaternion(q.x, q.y, q.z, q.w, config)
val extrinsic = false
// intrinsic/extrinsic conversion helpers
val angle_first: Int
val angle_third: Int
val reversed: Boolean
if (extrinsic) {
angle_first = 0
angle_third = 2
reversed = false
} else {
reversed = true
//reversed = false
//seq = seq[:: - 1]
angle_first = 2
angle_third = 0
}
val quat = q
val i = config.order.indexAt(0, reversed = reversed)
val j = config.order.indexAt(1, reversed = reversed)
val symmetric = i == j
var k = if (symmetric) 3 - i - j else config.order.indexAt(2, reversed = reversed)
val sign = (i - j) * (j - k) * (k - i) / 2
println("ORDER: $i, $j, $k")
val eps = 1e-7f
val _angles = FloatArray(3)
//_angles = angles[ind, :]
// Step 1
// Permutate quaternion elements
val a: Float
val b: Float
val c: Float
val d: Float
if (symmetric) {
a = quat[3]
b = quat[i]
c = quat[j]
d = quat[k] * sign
} else {
a = quat[3] - quat[j]
b = quat[i] + quat[k] * sign
c = quat[j] + quat[3]
d = quat[k] * sign - quat[i]
}
// Step 2
// Compute second angle...
_angles[1] = 2 * atan2(hypot(c, d), hypot(a, b))
// ... and check if equal to is 0 or pi, causing a singularity
val case = when {
abs(_angles[1]) <= eps -> 1
abs(_angles[1] - PIF) <= eps -> 2
else -> 0 // normal case
}
// Step 3
// compute first and third angles, according to case
val half_sum = atan2(b, a)
val half_diff = atan2(d, c)
if (case == 0) { // no singularities
_angles[angle_first] = half_sum - half_diff
_angles[angle_third] = half_sum + half_diff
} else { // any degenerate case
_angles[2] = 0f
if (case == 1) {
_angles[0] = 2 * half_sum
} else {
_angles[0] = 2 * half_diff * (if (extrinsic) -1 else 1)
}
}
// for Tait-Bryan angles
if (!symmetric) {
_angles[angle_third] *= sign.toFloat()
_angles[1] -= PIF / 2
}
for (idx in 0 until 3) {
if (_angles[idx] < -PIF) {
_angles[idx] += 2 * PIF
} else if (_angles[idx] > PIF) {
_angles[idx] -= 2 * PIF
}
}
if (case != 0) {
println(
"Gimbal lock detected. Setting third angle to zero " +
"since it is not possible to uniquely determine " +
"all angles."
)
}
return EulerRotation(_angles[0].radians, _angles[2].radians, _angles[1].radians * config.coordinateSystem.sign)
*/
}
fun fromQuaternion(x: Float, y: Float, z: Float, w: Float, config: Config = Config.DEFAULT): EulerRotation {
return fromQuaternion(Quaternion(x, y, z, w), config)
/*
val t = y * x + z * w
// Gimbal lock, if any: positive (+1) for north pole, negative (-1) for south pole, zero (0) when no gimbal lock
val pole = if (t > 0.499f) 1 else if (t < -0.499f) -1 else 0
println("pole=$pole")
println(Angle.atan2(2f * (y * w + x * z), 1f - 2f * (y * y + x * x)))
return EulerRotation(
roll = when (pole) {
0 -> Angle.asin((2f * (w * x - z * y)).clamp(-1f, +1f))
else -> (pole.toFloat() * PIF * .5f).radians
},
pitch = when (pole) {
0 -> Angle.atan2(2f * (y * w + x * z), 1f - 2f * (y * y + x * x))
else -> Angle.ZERO
},
yaw = when (pole) {
0 -> Angle.atan2(2f * (w * z + y * x), 1f - 2f * (x * x + z * z))
else -> Angle.atan2(y, w) * pole.toFloat() * 2f
},
)
*/
}
}
}