Skip to content

Commit

Permalink
Some 3D additions: Matrix3+Quaternion compatibility, some missing met…
Browse files Browse the repository at this point in the history 
…hods in Vector3 and Matrix4. Support EulerAngles in arbitrary rotation orders. (#1725)
  • Loading branch information
@soywiz
soywiz authored Jun 26, 2023
1 parent 6060a27 commit 0dd7c48
Show file tree
Hide file tree
Showing 13 changed files with 1,110 additions and 92 deletions.
22 changes: 22 additions & 0 deletions korma/src/commonMain/kotlin/korlibs/math/geom/Angle.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,13 @@ inline class Angle @PublishedApi internal constructor(
fun isAlmostEquals(other: Angle, epsilon: Double): Boolean = isAlmostEquals(other, epsilon.toFloat())
fun isAlmostZero(epsilon: Double): Boolean = isAlmostZero(epsilon.toFloat())

/** Normalize between 0..1 ... 0..(PI*2).radians ... 0..360.degrees */
val normalized: Angle get() = fromRatio(ratioF umod 1f)
/** Normalize between -.5..+.5 ... -PI..+PI.radians ... -180..+180.degrees */
val normalizedHalf: Angle get() {
val res = normalized
return if (res > Angle.HALF) -Angle.FULL + res else res
}

override operator fun compareTo(other: Angle): Int = this.ratio.compareTo(other.ratio)

Expand Down Expand Up @@ -184,6 +190,22 @@ inline class Angle @PublishedApi internal constructor(
inline fun atan2(x: Double, y: Double, up: Vector2 = Vector2.UP): Angle = fromRadians(kotlin.math.atan2(x, y)).adjustFromUp(up)
inline fun atan2(p: Point, up: Vector2 = Vector2.UP): Angle = atan2(p.xD, p.yD, up)

inline fun asin(v: Double): Angle = kotlin.math.asin(v).radians
inline fun asin(v: Float): Angle = kotlin.math.asin(v).radians

inline fun acos(v: Double): Angle = kotlin.math.acos(v).radians
inline fun acos(v: Float): Angle = kotlin.math.acos(v).radians

fun arcCosine(v: Double): Angle = kotlin.math.acos(v).radians
fun arcCosine(v: Float): Angle = kotlin.math.acos(v).radians

fun arcSine(v: Double): Angle = kotlin.math.asin(v).radians
fun arcSine(v: Float): Angle = kotlin.math.asin(v).radians

fun arcTangent(x: Double, y: Double): Angle = kotlin.math.atan2(x, y).radians
fun arcTangent(x: Float, y: Float): Angle = kotlin.math.atan2(x, y).radians
fun arcTangent(v: Vector2): Angle = kotlin.math.atan2(v.x, v.y).radians

inline fun ratioToDegrees(ratio: Double): Double = ratio * 360.0
inline fun ratioToRadians(ratio: Double): Double = ratio * PI2
inline fun degreesToRatio(degrees: Double): Double = degrees / 360.0
Expand Down
336 changes: 303 additions & 33 deletions korma/src/commonMain/kotlin/korlibs/math/geom/EulerRotation.kt
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,77 @@
package korlibs.math.geom

import korlibs.math.normalizeAlmostZero
import korlibs.memory.*
import kotlin.math.*

inline class EulerRotation internal constructor(private val data: Vector3) {
/**
* 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)
Expand All @@ -16,45 +84,247 @@ inline class EulerRotation internal constructor(private val data: Vector3) {

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) : this(Vector3(roll.ratio, pitch.ratio, yaw.ratio))
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 = Companion.toQuaternion(roll, pitch, yaw)
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): Quaternion {
val cr = cos(roll * 0.5f)
val sr = sin(roll * 0.5f)
val cp = cos(pitch * 0.5f)
val sp = sin(pitch * 0.5f)
val cy = cos(yaw * 0.5f)
val sy = sin(yaw * 0.5f)
//println("roll=$roll, pitch=$pitch, yaw=$yaw, [cr=$cr,sr=$sr], [cp=$cp,sp=$sp], [cy=$cy,sy=$sy]")
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(
((cy * cp * sr) - (sy * sp * cr)),
((sy * cp * sr) + (cy * sp * cr)),
((sy * cp * cr) - (cy * sp * sr)),
((cy * cp * cr) + (sy * sp * sr)),
((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 fromQuaternion(quat: Quaternion): EulerRotation {
val (x, y, z, w) = quat
//println("$x, $y, $z, $w")
val sinrCosp = (+2f * (w * x + y * z)).normalizeAlmostZero()
val cosrCosp = (+1f - 2f * (x * x + y * y)).normalizeAlmostZero()
val roll = atan2(sinrCosp, cosrCosp)
//println("roll=$roll, sinrCosp=$sinrCosp, cosrCosp=$cosrCosp")
val sinp = (+2f * (w * y - z * x)).normalizeAlmostZero()
val pitch: Float = when {
abs(sinp) > 1f -> if (sinp > 0) PI2F / 2 else -PI2F / 2
else -> asin(sinp)
}
val sinyCosp = (+2f * (w * z + x * y)).normalizeAlmostZero()
val cosyCosp = (+1f - 2f * (y * y + z * z)).normalizeAlmostZero()
val yaw = atan2(sinyCosp, cosyCosp)
//println("x=$x, y=$y, z=$z, w=$w, sinrCosp=$sinrCosp, cosrCosp=$cosrCosp, roll=$roll, sinp=$sinp, pitch=$pitch, sinyCosp=$sinyCosp, cosyCosp=$cosyCosp, yaw=$yaw")
return EulerRotation(roll.radians, pitch.radians, yaw.radians)
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
},
)
*/
}
}
}
Loading

0 comments on commit 0dd7c48

Please sign in to comment.