From f19e3c50fc52846bf0e41c4a22dd756aefdc9d0c Mon Sep 17 00:00:00 2001 From: soywiz Date: Sun, 25 Jun 2023 23:17:33 +0200 Subject: [PATCH 1/8] Adds Vector3.fromArray --- korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt index 0e1572c9ad..aab580a0c3 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt @@ -42,6 +42,9 @@ data class Vector3(val x: Float, val y: Float, val z: Float) { fun length(x: Float, y: Float, z: Float): Float = sqrt(lengthSq(x, y, z)) fun lengthSq(x: Float, y: Float, z: Float): Float = x * x + y * y + z * z + fun fromArray(array: FloatArray, offset: Int): Vector3 = + Vector3(array[offset + 0], array[offset + 1], array[offset + 2]) + inline fun func(func: (index: Int) -> Float): Vector3 = Vector3(func(0), func(1), func(2)) } From 57d4183a663e4840920941871ef45d2822721192 Mon Sep 17 00:00:00 2001 From: soywiz Date: Sun, 25 Jun 2023 23:17:45 +0200 Subject: [PATCH 2/8] Added Matrix4.isAlmostEquals --- korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt index a8fdaa9f5d..9d5616cba3 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt @@ -280,6 +280,11 @@ data class Matrix4 private constructor( ) } + fun isAlmostEquals(other: Matrix4, epsilon: Float = 0.00001f): Boolean = c0.isAlmostEquals(other.c0, epsilon) + && c1.isAlmostEquals(other.c1, epsilon) + && c2.isAlmostEquals(other.c2, epsilon) + && c3.isAlmostEquals(other.c3, epsilon) + companion object { const val M00 = 0 const val M10 = 1 From b15fb1bceaf0f378509e8f43cc9d74fe05140318 Mon Sep 17 00:00:00 2001 From: soywiz Date: Sun, 25 Jun 2023 23:17:58 +0200 Subject: [PATCH 3/8] Added Matrix3 --- .../kotlin/korlibs/math/geom/Matrix3.kt | 232 ++++++++++++++++++ .../kotlin/korlibs/math/geom/AssertExt.kt | 2 + .../kotlin/korlibs/math/geom/Matrix3Test.kt | 147 +++++++++++ 3 files changed, 381 insertions(+) create mode 100644 korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt create mode 100644 korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt new file mode 100644 index 0000000000..24a2767233 --- /dev/null +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt @@ -0,0 +1,232 @@ +package korlibs.math.geom + +import kotlin.math.* + +data class Matrix3 private constructor( + private val data: FloatArray, +) { + override fun equals(other: Any?): Boolean = other is Matrix3 && this.data.contentEquals(other.data) + override fun hashCode(): Int = data.contentHashCode() + + private constructor( + v00: Float, v10: Float, v20: Float, + v01: Float, v11: Float, v21: Float, + v02: Float, v12: Float, v22: Float, + ) : this( + floatArrayOf( + v00, v10, v20, + v01, v11, v21, + v02, v12, v22, + ) + ) + + init { + check(data.size == 9) + } + + val v00: Float get() = data[0] + val v10: Float get() = data[1] + val v20: Float get() = data[2] + val v01: Float get() = data[3] + val v11: Float get() = data[4] + val v21: Float get() = data[5] + val v02: Float get() = data[6] + val v12: Float get() = data[7] + val v22: Float get() = data[8] + + val c0: Vector3 get() = Vector3.fromArray(data, 0) + val c1: Vector3 get() = Vector3.fromArray(data, 3) + val c2: Vector3 get() = Vector3.fromArray(data, 6) + fun c(column: Int): Vector3 { + if (column < 0 || column >= 3) error("Invalid column $column") + return Vector3.fromArray(data, column * 3) + } + + val r0: Vector3 get() = Vector3(v00, v01, v02) + val r1: Vector3 get() = Vector3(v10, v11, v12) + val r2: Vector3 get() = Vector3(v20, v21, v22) + + fun r(row: Int): Vector3 = when (row) { + 0 -> r0 + 1 -> r1 + 2 -> r2 + else -> error("Invalid row $row") + } + + operator fun get(row: Int, column: Int): Float { + if (column !in 0..2 || row !in 0..2) error("Invalid index $row,$column") + return data[row * 3 + column] + } + + fun transform(v: Vector3): Vector3 = Vector3(r0.dot(v), r1.dot(v), r2.dot(v)) + + operator fun unaryMinus(): Matrix3 = Matrix3( + -v00, -v10, -v20, + -v01, -v11, -v21, + -v02, -v12, -v22, + ) + operator fun unaryPlus(): Matrix3 = this + + operator fun minus(other: Matrix3): Matrix3 = Matrix3( + v00 - other.v00, v10 - other.v10, v20 - other.v20, + v01 - other.v01, v11 - other.v11, v21 - other.v21, + v02 - other.v02, v12 - other.v12, v22 - other.v22, + ) + operator fun plus(other: Matrix3): Matrix3 = Matrix3( + v00 + other.v00, v10 + other.v10, v20 + other.v20, + v01 + other.v01, v11 + other.v11, v21 + other.v21, + v02 + other.v02, v12 + other.v12, v22 + other.v22, + ) + + operator fun times(other: Matrix3): Matrix3 = Matrix3.multiply(this, other) + operator fun times(scale: Float): Matrix3 = Matrix3( + v00 * scale, v10 * scale, v20 * scale, + v01 * scale, v11 * scale, v21 * scale, + v02 * scale, v12 * scale, v22 * scale, + ) + operator fun div(scale: Float): Matrix3 = this * (1f / scale) + + fun inv(): Matrix3 = inverted() + + val determinant: Float get() = v00 * (v11 * v22 - v21 * v12) - + v01 * (v10 * v22 - v12 * v20) + + v02 * (v10 * v21 - v11 * v20) + + fun inverted(): Matrix3 { + val determinant = this.determinant + + if (determinant == 0.0f) throw ArithmeticException("Matrix is not invertible") + + val invDet = 1.0f / determinant + + return fromRows( + (v11 * v22 - v21 * v12) * invDet, + (v02 * v21 - v01 * v22) * invDet, + (v01 * v12 - v02 * v11) * invDet, + (v12 * v20 - v10 * v22) * invDet, + (v00 * v22 - v02 * v20) * invDet, + (v10 * v02 - v00 * v12) * invDet, + (v10 * v21 - v20 * v11) * invDet, + (v20 * v01 - v00 * v21) * invDet, + (v00 * v11 - v10 * v01) * invDet, + ) + } + + override fun toString(): String = buildString { + append("Matrix3(\n") + for (row in 0 until 3) { + append(" [ ") + for (col in 0 until 3) { + if (col != 0) append(", ") + val v = get(row, col) + if (floor(v) == v) append(v.toInt()) else append(v) + } + append(" ],\n") + } + append(")") + } + + fun transposed(): Matrix3 = Matrix3.fromColumns(r0, r1, r2) + + fun isAlmostEquals(other: Matrix3, epsilon: Float = 0.00001f): Boolean = c0.isAlmostEquals(other.c0, epsilon) + && c1.isAlmostEquals(other.c1, epsilon) + && c2.isAlmostEquals(other.c2, epsilon) + + companion object { + const val M00 = 0 + const val M10 = 1 + const val M20 = 2 + + const val M01 = 3 + const val M11 = 4 + const val M21 = 5 + + const val M02 = 6 + const val M12 = 7 + const val M22 = 8 + + const val M03 = 9 + const val M13 = 10 + const val M23 = 11 + + val INDICES_BY_COLUMNS = intArrayOf( + M00, M10, M20, + M01, M11, M21, + M02, M12, M22, + ) + val INDICES_BY_ROWS = intArrayOf( + M00, M01, M02, + M10, M11, M12, + M20, M21, M22, + ) + + val IDENTITY = Matrix3( + 1f, 0f, 0f, + 0f, 1f, 0f, + 0f, 0f, 1f, + ) + + fun fromRows( + r0: Vector3, r1: Vector3, r2: Vector3 + ): Matrix3 = Matrix3( + r0.x, r1.x, r2.x, + r0.y, r1.y, r2.y, + r0.z, r1.z, r2.z, + ) + + fun fromColumns( + c0: Vector3, c1: Vector3, c2: Vector3 + ): Matrix3 = Matrix3( + c0.x, c0.y, c0.z, + c1.x, c1.y, c1.z, + c2.x, c2.y, c2.z, + ) + + fun fromColumns( + v00: Float, v10: Float, v20: Float, + v01: Float, v11: Float, v21: Float, + v02: Float, v12: Float, v22: Float, + ): Matrix3 = Matrix3( + v00, v10, v20, + v01, v11, v21, + v02, v12, v22, + ) + + fun fromRows( + v00: Float, v01: Float, v02: Float, + v10: Float, v11: Float, v12: Float, + v20: Float, v21: Float, v22: Float, + ): Matrix3 = Matrix3( + v00, v10, v20, + v01, v11, v21, + v02, v12, v22, + ) + + fun multiply(l: Matrix3, r: Matrix3): Matrix3 = Matrix3.fromRows( + (l.v00 * r.v00) + (l.v01 * r.v10) + (l.v02 * r.v20), + (l.v00 * r.v01) + (l.v01 * r.v11) + (l.v02 * r.v21), + (l.v00 * r.v02) + (l.v01 * r.v12) + (l.v02 * r.v22), + + (l.v10 * r.v00) + (l.v11 * r.v10) + (l.v12 * r.v20), + (l.v10 * r.v01) + (l.v11 * r.v11) + (l.v12 * r.v21), + (l.v10 * r.v02) + (l.v11 * r.v12) + (l.v12 * r.v22), + + (l.v20 * r.v00) + (l.v21 * r.v10) + (l.v22 * r.v20), + (l.v20 * r.v01) + (l.v21 * r.v11) + (l.v22 * r.v21), + (l.v20 * r.v02) + (l.v21 * r.v12) + (l.v22 * r.v22), + ) + } +} + +fun Matrix4.toMatrix3(): Matrix3 = Matrix3.fromRows( + v00, v01, v02, + v10, v11, v12, + v20, v21, v22 +) + +fun Matrix3.toMatrix4(): Matrix4 = Matrix4.fromRows( + v00, v01, v02, 0f, + v10, v11, v12, 0f, + v20, v21, v22, 0f, + 0f, 0f, 0f, 1f, +) diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt index 2e94b4e584..20572e59d7 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt @@ -35,6 +35,8 @@ private fun T?.isAlmostEqualsGeneric( is Ray -> e.isAlmostEquals((a as? Ray?) ?: return false, absoluteTolerance.toFloat()) is Point -> e.isAlmostEquals((a as? Point?) ?: return false, absoluteTolerance.toFloat()) is Vector3 -> e.isAlmostEquals((a as? Vector3?) ?: return false, absoluteTolerance.toFloat()) + is Matrix3 -> e.isAlmostEquals((a as? Matrix3?) ?: return false, absoluteTolerance.toFloat()) + is Matrix4 -> e.isAlmostEquals((a as? Matrix4?) ?: return false, absoluteTolerance.toFloat()) is MPoint -> e.isAlmostEquals((a as? MPoint?) ?: return false, absoluteTolerance) is Float -> { if (a !is Float?) return false diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt new file mode 100644 index 0000000000..03eb1de4f2 --- /dev/null +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt @@ -0,0 +1,147 @@ +package korlibs.math.geom + +import korlibs.datastructure.* +import kotlin.test.* + +class Matrix3Test { + @Test + fun testRows() { + val m = Matrix3.fromRows( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + assertEquals(Vector3(1f, 2f, 3f), m.r0) + assertEquals(Vector3(-4f, -5f, -6f), m.r1) + assertEquals(Vector3(7f, -8f, 9f), m.r2) + assertEquals(m.r0, m.r(0)) + assertEquals(m.r1, m.r(1)) + assertEquals(m.r2, m.r(2)) + + assertEquals(Vector3(1f, -4f, 7f), m.c0) + assertEquals(Vector3(2f, -5f, -8f), m.c1) + assertEquals(Vector3(3f, -6f, 9f), m.c2) + assertEquals(m.c0, m.c(0)) + assertEquals(m.c1, m.c(1)) + assertEquals(m.c2, m.c(2)) + + assertEquals(m.c0, Vector3(m.v00, m.v10, m.v20)) + assertEquals(m.c1, Vector3(m.v01, m.v11, m.v21)) + assertEquals(m.c2, Vector3(m.v02, m.v12, m.v22)) + + assertEquals(Vector3(m[0, 0], m[1, 0], m[2, 0]), Vector3(m.v00, m.v01, m.v02)) + assertEquals(Vector3(m[0, 1], m[1, 1], m[2, 1]), Vector3(m.v10, m.v11, m.v12)) + assertEquals(Vector3(m[0, 2], m[1, 2], m[2, 2]), Vector3(m.v20, m.v21, m.v22)) + } + + @Test + fun testColumns() { + val m = Matrix3.fromColumns( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + + assertEquals(Vector3(1f, 2f, 3f), m.c0) + assertEquals(Vector3(-4f, -5f, -6f), m.c1) + assertEquals(Vector3(7f, -8f, 9f), m.c2) + } + + @Test + fun testTranspose() { + assertEquals( + Matrix3.fromRows( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ), + Matrix3.fromColumns( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ).transposed() + ) + } + + @Test + fun testConstructorRows() { + val m1 = Matrix3.fromRows( + Vector3(1f, 2f, 3f), + Vector3(-4f, -5f, -6f), + Vector3(7f, -8f, 9f) + ) + val m2 = Matrix3.fromRows( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + + assertEquals(m1, m2) + assertEquals(m1.hashCode(), m2.hashCode()) + //assertNotEquals(m1.identityHashCode(), m2.identityHashCode()) + + assertNotEquals(m1, m2.transposed()) + assertNotEquals(m1.hashCode(), m2.transposed().hashCode()) + } + + @Test + fun testConstructorCols() { + val m1 = Matrix3.fromColumns( + Vector3(1f, 2f, 3f), + Vector3(-4f, -5f, -6f), + Vector3(7f, -8f, 9f) + ) + val m2 = Matrix3.fromColumns( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + + assertEquals(m1, m2) + assertEquals(m1.hashCode(), m2.hashCode()) + assertNotEquals(m1.identityHashCode(), m2.identityHashCode()) + } + + @Test + fun testInverse() { + val m = Matrix3.fromRows( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + + assertEqualsFloat(Matrix3.IDENTITY, m * m.inverted()) + } + + @Test + fun testMultiply() { + val m = Matrix3.fromRows( + 1f, 2f, 3f, + -4f, -5f, -6f, + 7f, -8f, 9f + ) + + assertEqualsFloat(Matrix3.IDENTITY, m * m.inverted()) + } + + @Test + fun testScale() { + assertEqualsFloat(Matrix3.fromRows( + 2f, 0f, 0f, + 0f, 2f, 0f, + 0f, 0f, 2f, + ), Matrix3.IDENTITY * 2f) + } + + @Test + fun testUnary() { + assertEqualsFloat(Matrix3.IDENTITY * (-1f), -Matrix3.IDENTITY) + assertEqualsFloat(Matrix3.IDENTITY, +Matrix3.IDENTITY) + } + + @Test + fun testDeterminant() { + assertEqualsFloat(1f, Matrix3.IDENTITY.determinant) + assertEqualsFloat(96f, Matrix3.fromRows(1f, 2f, 3f, -4f, -5f, -6f, 7f, -8f, 9f).determinant) + } +} From 008764f68ef641f62119576a75272024f695ec93 Mon Sep 17 00:00:00 2001 From: soywiz Date: Sun, 25 Jun 2023 23:27:20 +0200 Subject: [PATCH 4/8] Make Quaternion compatible with Matrix3 --- .../kotlin/korlibs/math/geom/Matrix3.kt | 2 + .../kotlin/korlibs/math/geom/Quaternion.kt | 88 +++++++++++++------ .../kotlin/korlibs/math/geom/AssertExt.kt | 1 + .../kotlin/korlibs/math/geom/Matrix3Test.kt | 14 +++ 4 files changed, 80 insertions(+), 25 deletions(-) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt index 24a2767233..4cde2b0d1a 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt @@ -230,3 +230,5 @@ fun Matrix3.toMatrix4(): Matrix4 = Matrix4.fromRows( v20, v21, v22, 0f, 0f, 0f, 0f, 1f, ) + +fun Matrix3.toQuaternion(): Quaternion = Quaternion.fromRotationMatrix(this) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt index df1c070bfd..c82f9ade6e 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt @@ -1,5 +1,6 @@ package korlibs.math.geom +import korlibs.math.* import korlibs.memory.pack.* import kotlin.math.* @@ -36,6 +37,25 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { constructor(x: Double, y: Double, z: Double, w: Double) : this(x.toFloat(), y.toFloat(), z.toFloat(), w.toFloat()) fun toMatrix(): Matrix4 { + val v = _toMatrix() + return Matrix4.fromRows( + v[0], v[1], v[2], 0f, + v[3], v[4], v[5], 0f, + v[6], v[7], v[8], 0f, + 0f, 0f, 0f, 1f, + ) + } + + fun toMatrix3(): Matrix3 { + val v = _toMatrix() + return Matrix3.fromRows( + v[0], v[1], v[2], + v[3], v[4], v[5], + v[6], v[7], v[8], + ) + } + + private fun _toMatrix(): FloatArray { val xx = x * x val xy = x * y val xz = x * z @@ -46,11 +66,10 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { val zz = z * z val zw = z * w - return Matrix4.fromRows( - 1 - 2 * (yy + zz), 2 * (xy - zw), 2 * (xz + yw), 0f, - 2 * (xy + zw), 1 - 2 * (xx + zz), 2 * (yz - xw), 0f, - 2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xx + yy), 0f, - 0f, 0f, 0f, 1f + return floatArrayOf( + 1 - 2 * (yy + zz), 2 * (xy - zw), 2 * (xz + yw), + 2 * (xy + zw), 1 - 2 * (xx + zz), 2 * (yz - xw), + 2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xx + yy), ) } @@ -115,6 +134,11 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { } fun toEuler(): EulerRotation = toEuler(x, y, z, w) + fun isAlmostEquals(other: Quaternion, epsilon: Float = 0.00001f): Boolean = + this.x.isAlmostEquals(other.x, epsilon) + && this.y.isAlmostEquals(other.y, epsilon) + && this.z.isAlmostEquals(other.z, epsilon) + && this.w.isAlmostEquals(other.w, epsilon) companion object { val IDENTITY = Quaternion() @@ -204,26 +228,40 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { ) } - fun fromRotationMatrix(m: Matrix4): Quaternion { - m.apply { - val t = v00 + v11 + v22 - return when { - t > 0 -> { - val s = .5f / sqrt(t + 1f) - Quaternion(((v21 - v12) * s), ((v02 - v20) * s), ((v10 - v01) * s), (0.25f / s)) - } - v00 > v11 && v00 > v22 -> { - val s = 2f * sqrt(1f + v00 - v11 - v22) - Quaternion((0.25f * s), ((v01 + v10) / s), ((v02 + v20) / s), ((v21 - v12) / s)) - } - v11 > v22 -> { - val s = 2f * sqrt(1f + v11 - v00 - v22) - Quaternion(((v01 + v10) / s), (0.25f * s), ((v12 + v21) / s), ((v02 - v20) / s)) - } - else -> { - val s = 2f * sqrt(1f + v22 - v00 - v11) - Quaternion(((v02 + v20) / s), ((v12 + v21) / s), (0.25f * s), ((v10 - v01) / s)) - } + fun fromRotationMatrix(m: Matrix4): Quaternion = fromRotationMatrix( + m.v00, m.v10, m.v20, + m.v01, m.v11, m.v21, + m.v02, m.v12, m.v22, + ) + + fun fromRotationMatrix(m: Matrix3): Quaternion = fromRotationMatrix( + m.v00, m.v10, m.v20, + m.v01, m.v11, m.v21, + m.v02, m.v12, m.v22, + ) + + fun fromRotationMatrix( + v00: Float, v10: Float, v20: Float, + v01: Float, v11: Float, v21: Float, + v02: Float, v12: Float, v22: Float, + ): Quaternion { + val t = v00 + v11 + v22 + return when { + t > 0 -> { + val s = .5f / sqrt(t + 1f) + Quaternion(((v21 - v12) * s), ((v02 - v20) * s), ((v10 - v01) * s), (0.25f / s)) + } + v00 > v11 && v00 > v22 -> { + val s = 2f * sqrt(1f + v00 - v11 - v22) + Quaternion((0.25f * s), ((v01 + v10) / s), ((v02 + v20) / s), ((v21 - v12) / s)) + } + v11 > v22 -> { + val s = 2f * sqrt(1f + v11 - v00 - v22) + Quaternion(((v01 + v10) / s), (0.25f * s), ((v12 + v21) / s), ((v02 - v20) / s)) + } + else -> { + val s = 2f * sqrt(1f + v22 - v00 - v11) + Quaternion(((v02 + v20) / s), ((v12 + v21) / s), (0.25f * s), ((v10 - v01) / s)) } } } diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt index 20572e59d7..0d594d59ea 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt @@ -37,6 +37,7 @@ private fun T?.isAlmostEqualsGeneric( is Vector3 -> e.isAlmostEquals((a as? Vector3?) ?: return false, absoluteTolerance.toFloat()) is Matrix3 -> e.isAlmostEquals((a as? Matrix3?) ?: return false, absoluteTolerance.toFloat()) is Matrix4 -> e.isAlmostEquals((a as? Matrix4?) ?: return false, absoluteTolerance.toFloat()) + is Quaternion -> e.isAlmostEquals((a as? Quaternion?) ?: return false, absoluteTolerance.toFloat()) is MPoint -> e.isAlmostEquals((a as? MPoint?) ?: return false, absoluteTolerance) is Float -> { if (a !is Float?) return false diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt index 03eb1de4f2..93b4458d72 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/Matrix3Test.kt @@ -144,4 +144,18 @@ class Matrix3Test { assertEqualsFloat(1f, Matrix3.IDENTITY.determinant) assertEqualsFloat(96f, Matrix3.fromRows(1f, 2f, 3f, -4f, -5f, -6f, 7f, -8f, 9f).determinant) } + + @Test + fun testQuaternion() { + val quat = Quaternion.fromEuler(30.degrees, 15.degrees, 90.degrees) + val vec = Vector3(1f, -2f, 3f) + assertEqualsFloat( + quat, + quat.toMatrix3().toQuaternion(), + ) + assertEqualsFloat( + quat.transform(vec), + quat.toMatrix3().transform(vec) + ) + } } From 6a4e9f316b04f1f7c3e7a63a5ae7f1fc83674e81 Mon Sep 17 00:00:00 2001 From: soywiz Date: Mon, 26 Jun 2023 21:43:45 +0200 Subject: [PATCH 5/8] More Angle utilities --- .../kotlin/korlibs/math/geom/Angle.kt | 22 +++++++++++++ .../kotlin/korlibs/math/geom/AngleTest.kt | 32 +++++++++++++++++++ 2 files changed, 54 insertions(+) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Angle.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Angle.kt index 84c3adddab..217c2bb4b4 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Angle.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Angle.kt @@ -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) @@ -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 diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/AngleTest.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/AngleTest.kt index add55bdcff..01ad4df1e2 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/AngleTest.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/AngleTest.kt @@ -189,4 +189,36 @@ class AngleTest { assertEqualsFloat(listOf(0.0, 1.0), listOf(Angle.QUARTER.cosineD(Vector2.UP), Angle.QUARTER.sineD(Vector2.UP))) assertEqualsFloat(listOf(0.0, -1.0), listOf(Angle.QUARTER.cosineD(Vector2.UP_SCREEN), Angle.QUARTER.sineD(Vector2.UP_SCREEN))) } + + @Test + fun testNormalizeHalf() { + assertEquals((-90).degrees, 270.degrees.normalizedHalf) + assertEquals(180.degrees, 180.degrees.normalizedHalf) + assertEquals((-179).degrees, 181.degrees.normalizedHalf) + + assertEquals(0.degrees, (-360).degrees.normalizedHalf) + assertEquals(90.degrees, (-270).degrees.normalizedHalf) + assertEquals(180.degrees, (-180).degrees.normalizedHalf) + assertEquals((-90).degrees, (-90).degrees.normalizedHalf) + assertEquals(0.degrees, 0.degrees.normalizedHalf) + assertEquals(90.degrees, 90.degrees.normalizedHalf) + assertEquals(180.degrees, 180.degrees.normalizedHalf) + assertEquals((-90).degrees, 270.degrees.normalizedHalf) + assertEquals(0.degrees, 360.degrees.normalizedHalf) + assertEquals(0.degrees, 720.degrees.normalizedHalf) + } + + @Test + fun testNormalize() { + assertEquals(0.degrees, (-360).degrees.normalized) + assertEquals(90.degrees, (-270).degrees.normalized) + assertEquals(180.degrees, (-180).degrees.normalized) + assertEquals(270.degrees, (-90).degrees.normalized) + assertEquals(0.degrees, 0.degrees.normalized) + assertEquals(90.degrees, 90.degrees.normalized) + assertEquals(180.degrees, 180.degrees.normalized) + assertEquals(270.degrees, 270.degrees.normalized) + assertEquals(0.degrees, 360.degrees.normalized) + assertEquals(0.degrees, 720.degrees.normalized) + } } From 5af2b29dfd0cd80b1556757e6f0822ce46c16e50 Mon Sep 17 00:00:00 2001 From: soywiz Date: Mon, 26 Jun 2023 21:43:58 +0200 Subject: [PATCH 6/8] Small additions --- korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt | 7 ++++++- korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt index 4cde2b0d1a..296f8e058f 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix3.kt @@ -2,8 +2,11 @@ package korlibs.math.geom import kotlin.math.* +/** + * Useful for representing rotations and scales. + */ data class Matrix3 private constructor( - private val data: FloatArray, + internal val data: FloatArray, ) { override fun equals(other: Any?): Boolean = other is Matrix3 && this.data.contentEquals(other.data) override fun hashCode(): Int = data.contentHashCode() @@ -46,6 +49,8 @@ data class Matrix3 private constructor( val r1: Vector3 get() = Vector3(v10, v11, v12) val r2: Vector3 get() = Vector3(v20, v21, v22) + fun v(index: Int): Float = data[index] + fun r(row: Int): Vector3 = when (row) { 0 -> r0 1 -> r1 diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt index 0d594d59ea..827caed260 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/AssertExt.kt @@ -38,7 +38,9 @@ private fun T?.isAlmostEqualsGeneric( is Matrix3 -> e.isAlmostEquals((a as? Matrix3?) ?: return false, absoluteTolerance.toFloat()) is Matrix4 -> e.isAlmostEquals((a as? Matrix4?) ?: return false, absoluteTolerance.toFloat()) is Quaternion -> e.isAlmostEquals((a as? Quaternion?) ?: return false, absoluteTolerance.toFloat()) + is EulerRotation -> e.isAlmostEquals((a as? EulerRotation?) ?: return false, absoluteTolerance.toFloat()) is MPoint -> e.isAlmostEquals((a as? MPoint?) ?: return false, absoluteTolerance) + is Angle -> e.isAlmostEquals((a as? Angle?) ?: return false, absoluteTolerance.toFloat()) is Float -> { if (a !is Float?) return false if (e.isNaN() && a.isNaN()) return true From 256764361749941f2f7934c351a32b0a8b5ee337 Mon Sep 17 00:00:00 2001 From: soywiz Date: Mon, 26 Jun 2023 21:51:07 +0200 Subject: [PATCH 7/8] Fixes Vector3 & Vector4.normalized --- .../kotlin/korlibs/math/geom/Vector3.kt | 16 ++++++++++------ .../kotlin/korlibs/math/geom/Vector4.kt | 6 +++++- .../kotlin/korlibs/math/geom/Vector3Test.kt | 6 +++++- .../kotlin/korlibs/math/geom/Vector4Test.kt | 6 +++++- 4 files changed, 25 insertions(+), 9 deletions(-) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt index aab580a0c3..8caffc2a48 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector3.kt @@ -1,9 +1,8 @@ package korlibs.math.geom -import korlibs.memory.pack.* +import korlibs.math.* import korlibs.math.annotations.* import korlibs.math.internal.* -import korlibs.math.isAlmostEquals import korlibs.memory.* import kotlin.math.* @@ -34,9 +33,9 @@ data class Vector3(val x: Float, val y: Float, val z: Float) { operator fun invoke(): Vector3 = ZERO fun cross(a: Vector3, b: Vector3): Vector3 = Vector3( - (a.y * b.z - a.z * b.y), - (a.z * b.x - a.x * b.z), - (a.x * b.y - a.y * b.x), + ((a.y * b.z) - (a.z * b.y)), + ((a.z * b.x) - (a.x * b.z)), + ((a.x * b.y) - (a.y * b.x)), ) fun length(x: Float, y: Float, z: Float): Float = sqrt(lengthSq(x, y, z)) @@ -54,7 +53,12 @@ data class Vector3(val x: Float, val y: Float, val z: Float) { val lengthSquared: Float get() = (x * x) + (y * y) + (z * z) val length: Float get() = sqrt(lengthSquared) - fun normalized(): Vector3 = this / length + fun normalized(): Vector3 { + val length = this.length + //if (length.isAlmostZero()) return Vector3.ZERO + if (length == 0f) return Vector3.ZERO + return this / length + } // https://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector // 𝑟=𝑑−2(𝑑⋅𝑛)𝑛 diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector4.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector4.kt index d4b97dbb59..642852cdc1 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Vector4.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Vector4.kt @@ -54,7 +54,11 @@ inline class Vector4(val data: Float4Pack) { val lengthSquared: Float get() = (x * x) + (y * y) + (z * z) + (w * w) val length: Float get() = sqrt(lengthSquared) - fun normalized(): Vector4 = this / length + fun normalized(): Vector4 { + val length = this.length + if (length == 0f) return Vector4.ZERO + return this / length + } operator fun get(index: Int): Float = when (index) { 0 -> x diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/Vector3Test.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/Vector3Test.kt index 13feb90f3a..6440d69077 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/Vector3Test.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/Vector3Test.kt @@ -10,7 +10,11 @@ class Vector3Test { @Test fun testToString() = assertEquals("Vector3(1, 2, 3)", Vector3(1f, 2f, 3f).toString()) @Test - fun testNormalized() = assertEquals(1f, Vector3(1f, 2f, 4f).normalized().length, 0.00001f) + fun testNormalized() { + assertEquals(1f, Vector3(1f, 2f, 4f).normalized().length, 0.00001f) + assertEquals(Vector3.ZERO, Vector3(0f, 0f, 0f).normalized()) + assertEquals(0f, Vector3(0f, 0f, 0f).normalized().length, 0.00001f) + } @Test fun testEquals() { assertEquals(true, Vector3(1f, 2f, 3f) == Vector3(1f, 2f, 3f)) diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/Vector4Test.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/Vector4Test.kt index 2678732a08..a2ca604218 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/Vector4Test.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/Vector4Test.kt @@ -10,7 +10,11 @@ class Vector4Test { @Test fun testToString() = assertEquals("Vector4(1, 2, 3, 4)", Vector4(1f, 2f, 3f, 4f).toString()) @Test - fun testNormalized() = assertEquals(1f, Vector4(1f, 2f, 4f, 8f).normalized().length, 0.00001f) + fun testNormalized() { + assertEquals(1f, Vector4(1f, 2f, 4f, 8f).normalized().length, 0.00001f) + assertEquals(Vector4.ZERO, Vector4(0f, 0f, 0f, 0f).normalized()) + assertEquals(0f, Vector4(0f, 0f, 0f, 0f).normalized().length, 0.00001f) + } @Test fun testEquals() { assertEquals(true, Vector4(1f, 2f, 3f, 4f) == Vector4(1f, 2f, 3f, 4f)) From 9a5385ff1fc5dfe31972b4c7ee9de133acb69678 Mon Sep 17 00:00:00 2001 From: soywiz Date: Mon, 26 Jun 2023 21:52:27 +0200 Subject: [PATCH 8/8] Support EulerRotation with different orders --- .../kotlin/korlibs/math/geom/EulerRotation.kt | 336 ++++++++++++++++-- .../kotlin/korlibs/math/geom/Matrix4.kt | 3 + .../kotlin/korlibs/math/geom/Quaternion.kt | 95 +++-- .../korlibs/math/geom/QuaternionTest.kt | 185 ++++++++++ 4 files changed, 558 insertions(+), 61 deletions(-) diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/EulerRotation.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/EulerRotation.kt index 7a4f2a38cd..5b2bf20d9f 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/EulerRotation.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/EulerRotation.kt @@ -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) @@ -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 + }, + ) + + */ } } } diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt index 9d5616cba3..e2ee35b2e4 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Matrix4.kt @@ -10,6 +10,9 @@ import kotlin.math.* //@KormaExperimental //@KormaValueApi //inline class Matrix4 private constructor( +/** + * Useful for representing complete transforms: rotations, scales, translations, projections, etc. + */ data class Matrix4 private constructor( private val data: FloatArray, //val c0: Vector4, val c1: Vector4, val c2: Vector4, val c3: Vector4, diff --git a/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt b/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt index c82f9ade6e..e925c36137 100644 --- a/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt +++ b/korma/src/commonMain/kotlin/korlibs/math/geom/Quaternion.kt @@ -1,7 +1,8 @@ package korlibs.math.geom import korlibs.math.* -import korlibs.memory.pack.* +import korlibs.math.interpolation.* +import korlibs.math.isAlmostZero import kotlin.math.* // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -111,16 +112,13 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { return Quaternion(x / length, y / length, z / length, w / length) } + /** Also known as conjugate */ fun inverted(): Quaternion { val q = this val lengthSquared = q.lengthSquared - return when (lengthSquared) { - 0f -> { - val num = 1f / lengthSquared - Quaternion(q.x * -num, q.y * -num, q.z * -num, q.w * num) - } - else -> q - } + if (lengthSquared.isAlmostZero()) error("Zero quaternion doesn't have invesrse") + val num = 1f / lengthSquared + return Quaternion(q.x * -num, q.y * -num, q.z * -num, q.w * num) } fun transform(v: Vector3): Vector3 { @@ -133,18 +131,27 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { return Vector3(resultQuaternion.x, resultQuaternion.y, resultQuaternion.z) } - fun toEuler(): EulerRotation = toEuler(x, y, z, w) + fun toEuler(config: EulerRotation.Config = EulerRotation.Config.DEFAULT): EulerRotation = EulerRotation.fromQuaternion(this, config) fun isAlmostEquals(other: Quaternion, epsilon: Float = 0.00001f): Boolean = this.x.isAlmostEquals(other.x, epsilon) && this.y.isAlmostEquals(other.y, epsilon) && this.z.isAlmostEquals(other.z, epsilon) && this.w.isAlmostEquals(other.w, epsilon) + fun interpolated(other: Quaternion, t: Float): Quaternion = interpolated(this, other, t) + fun interpolated(other: Quaternion, t: Ratio): Quaternion = interpolated(this, other, t.toFloat()) + fun angleTo(other: Quaternion): Angle = angleBetween(this, other) + companion object { val IDENTITY = Quaternion() fun dotProduct(l: Quaternion, r: Quaternion): Float = l.x * r.x + l.y * r.y + l.z * r.z + l.w * r.w + fun angleBetween(a: Quaternion, b: Quaternion): Angle { + val dot = dotProduct(a, b) + return Angle.arcCosine(2 * (dot * dot) - 1) + } + inline fun func(callback: (Int) -> Float) = Quaternion(callback(0), callback(1), callback(2), callback(3)) inline fun func(l: Quaternion, r: Quaternion, func: (l: Float, r: Float) -> Float) = Quaternion( func(l.x, r.x), @@ -181,10 +188,10 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { fun interpolated(left: Quaternion, right: Quaternion, t: Float): Quaternion = slerp(left, right, t) - fun fromVectors(v1: Vector3, v2: Vector3): Quaternion { + fun fromVectors(from: Vector3, to: Vector3): Quaternion { // Normalize input vectors - val start = v1.normalized() - val dest = v2.normalized() + val start = from.normalized() + val dest = to.normalized() val dot = start.dot(dest) @@ -228,6 +235,22 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { ) } + // @TODO: Check + fun lookRotation(forward: Vector3, up: Vector3 = Vector3.UP): Quaternion { + //if (up == Vector3.UP) return fromVectors(Vector3.FORWARD, forward.normalized()) + val z = forward.normalized() + val x = (up.normalized() cross z).normalized() + + //println("x=$x, z=$z") + if (x.lengthSquared.isAlmostZero()) { + // COLLINEAR + return Quaternion.fromVectors(Vector3.FORWARD, z) + } + + val y = z cross x + return fromRotationMatrix(Matrix3.fromColumns(x, y, z)) + } + fun fromRotationMatrix(m: Matrix4): Quaternion = fromRotationMatrix( m.v00, m.v10, m.v20, m.v01, m.v11, m.v21, @@ -246,22 +269,27 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { v02: Float, v12: Float, v22: Float, ): Quaternion { val t = v00 + v11 + v22 + //println("t=$t, v00=$v00, v11=$v11, v22=$v22") return when { - t > 0 -> { + t >= 0 -> { val s = .5f / sqrt(t + 1f) + //println("[0]") Quaternion(((v21 - v12) * s), ((v02 - v20) * s), ((v10 - v01) * s), (0.25f / s)) } v00 > v11 && v00 > v22 -> { val s = 2f * sqrt(1f + v00 - v11 - v22) + //println("[1]") Quaternion((0.25f * s), ((v01 + v10) / s), ((v02 + v20) / s), ((v21 - v12) / s)) } v11 > v22 -> { val s = 2f * sqrt(1f + v11 - v00 - v22) - Quaternion(((v01 + v10) / s), (0.25f * s), ((v12 + v21) / s), ((v02 - v20) / s)) + //println("[2]") + Quaternion(((v01 + v10) / s), (.25f * s), ((v12 + v21) / s), ((v02 - v20) / s)) } else -> { val s = 2f * sqrt(1f + v22 - v00 - v11) - Quaternion(((v02 + v20) / s), ((v12 + v21) / s), (0.25f * s), ((v10 - v01) / s)) + //println("[3]") + Quaternion(((v02 + v20) / s), ((v12 + v21) / s), (.25f * s), ((v10 - v01) / s)) } } } @@ -269,19 +297,30 @@ data class Quaternion(val x: Float, val y: Float, val z: Float, val w: Float) { fun fromEuler(e: EulerRotation): Quaternion = e.toQuaternion() fun fromEuler(roll: Angle, pitch: Angle, yaw: Angle): Quaternion = EulerRotation(roll, pitch, yaw).toQuaternion() - fun toEuler(x: Float, y: Float, z: Float, w: Float): EulerRotation { - val sinrCosp = +2.0 * (w * x + y * z) - val cosrCosp = +1.0 - 2.0 * (x * x + y * y) - val roll = atan2(sinrCosp, cosrCosp) - val sinp = +2.0 * (w * y - z * x) - val pitch = when { - abs(sinp) >= 1 -> if (sinp > 0) PI / 2 else -PI / 2 - else -> asin(sinp) - } - val sinyCosp = +2.0 * (w * z + x * y) - val cosyCosp = +1.0 - 2.0 * (y * y + z * z) - val yaw = atan2(sinyCosp, cosyCosp) - return EulerRotation(roll.radians, pitch.radians, yaw.radians) + fun toEuler(x: Float, y: Float, z: Float, w: Float, config: EulerRotation.Config = EulerRotation.Config.DEFAULT): EulerRotation { + return EulerRotation.Companion.fromQuaternion(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 + 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 + }, + ) + + */ } } } + +fun Angle.Companion.between(a: Quaternion, b: Quaternion): Angle = Quaternion.angleBetween(a, b) diff --git a/korma/src/commonTest/kotlin/korlibs/math/geom/QuaternionTest.kt b/korma/src/commonTest/kotlin/korlibs/math/geom/QuaternionTest.kt index 0f58c3169e..7dc93e172a 100644 --- a/korma/src/commonTest/kotlin/korlibs/math/geom/QuaternionTest.kt +++ b/korma/src/commonTest/kotlin/korlibs/math/geom/QuaternionTest.kt @@ -12,6 +12,7 @@ class QuaternionTest { fun testTransformQuat() { assertEqualsFloat(Vector3.RIGHT, Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).transform(Vector3.UP)) } + @Test fun testScaled() { assertEqualsFloat(Vector3.UP, Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).scaled(0f).transform(Vector3.UP)) @@ -19,4 +20,188 @@ class QuaternionTest { assertEqualsFloat(Vector3.LEFT, Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).scaled(-1f).transform(Vector3.UP)) assertEqualsFloat(Vector3.DOWN, Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).scaled(2f).transform(Vector3.UP)) } + + @Test + fun testInverse() { + assertEqualsFloat( + Quaternion.IDENTITY, + Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT) * Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).inverted() + ) + assertEqualsFloat( + Quaternion.fromVectors(Vector3.UP, Vector3.LEFT), + Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT).inverted() + ) + assertEqualsFloat( + Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT), + Quaternion.fromVectors(Vector3.UP, Vector3.LEFT).inverted() + ) + } + + @Test + fun testAngleBetween() { + assertEqualsFloat( + 90.degrees, + Angle.between( + Quaternion.fromVectors(Vector3.UP, Vector3.UP), + Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT) * 1f + ) + ) + assertEqualsFloat( + 45.degrees, + Angle.between( + Quaternion.IDENTITY, + Quaternion.interpolated( + Quaternion.fromVectors(Vector3.UP, Vector3.UP), + Quaternion.fromVectors(Vector3.UP, Vector3.RIGHT), + .5f + ) + ) + ) + assertEqualsFloat( + 90.degrees, + Quaternion.IDENTITY.angleTo( + Quaternion.fromVectors(Vector3.UP, Vector3.UP) + .interpolated( + Quaternion.fromVectors(Vector3.UP, Vector3.DOWN), + .5f + ) + ) + ) + } + + @Test + //@Ignore // Failing for now + fun testToEulerUnity() { + assertEqualsFloat( + EulerRotation(90.degrees, (-90).degrees, 0.degrees, EulerRotation.Config.UNITY).normalized(), + Quaternion(-.5f, .5f, -.5f, -.5f).toEuler(EulerRotation.Config.UNITY).normalized() + ) + assertEqualsFloat( + EulerRotation(315.degrees, 30.degrees, 90.degrees, EulerRotation.Config.UNITY).normalized(), + Quaternion(-.09230f, .43046f, .70106f, .56099f).toEuler(EulerRotation.Config.UNITY).normalized() + ) + assertEqualsFloat( + EulerRotation(30.degrees, 315.degrees, 0.degrees, EulerRotation.Config.UNITY).normalized(), + Quaternion(.23912f, -.36964f, .09905f, .89240f).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + } + + @Test + fun testFromEulerUnity() { + assertEqualsFloat( + Quaternion(-.5f, .5f, -.5f, -.5f), + EulerRotation(90.degrees, 180.degrees, (-90).degrees, EulerRotation.Config.UNITY).toQuaternion() + ) + assertEqualsFloat( + Quaternion(-.09230f, .43046f, .70106f, .56099f), + EulerRotation((-45).degrees, 30.degrees, 90.degrees, EulerRotation.Config.UNITY).toQuaternion() + ) + assertEqualsFloat( + Quaternion(.23912f, -.36964f, .09905f, .89240f), + EulerRotation(30.degrees, (-45).degrees, 0.degrees, EulerRotation.Config.UNITY).toQuaternion() + ) + } + + @Test + fun testFromEulerLibgdx() { + assertEqualsFloat( + Quaternion(-.5f, .5f, -.5f, -.5f), + EulerRotation(90.degrees, 180.degrees, (-90).degrees, EulerRotation.Config.LIBGDX).toQuaternion() + ) + assertEqualsFloat( + Quaternion(-.09230f, .43046f, .70106f, .56099f), + EulerRotation((-45).degrees, 30.degrees, 90.degrees, EulerRotation.Config.LIBGDX).toQuaternion() + ) + assertEqualsFloat( + Quaternion(.23912f, -.36964f, .09905f, .89240f), + EulerRotation(30.degrees, (-45).degrees, 0.degrees, EulerRotation.Config.LIBGDX).toQuaternion() + ) + } + + @Test + fun testFromEulerThreejs() { + assertEqualsFloat( + Quaternion(-0.5f, 0.5f, 0.5f, 0.5f), + EulerRotation(90.degrees, 180.degrees, (-90).degrees, EulerRotation.Config.THREEJS).toQuaternion() + ) + assertEqualsFloat( + Quaternion(-.09230f, .43046f, .56099f, .70106f), + EulerRotation((-45).degrees, 30.degrees, 90.degrees, EulerRotation.Config.THREEJS).toQuaternion() + ) + assertEqualsFloat( + Quaternion(.23912f, -.36964f, -.09905f, .89240f), + EulerRotation(30.degrees, (-45).degrees, 0.degrees, EulerRotation.Config.THREEJS).toQuaternion() + ) + } + + //val quat = //Quaternion.lookRotation(Vector3.LEFT, Vector3.FORWARD) + val quat = Quaternion(-0.5, 0.5, 0.5, 0.5) + + @Test + fun testToFromEulerDefault() { + assertEqualsFloat(quat, quat.toEuler(EulerRotation.Config.DEFAULT).toQuaternion()) + } + + @Test + //@Ignore // Failing for now + fun testToFromEulerUnity() { + assertEqualsFloat(quat, quat.toEuler(EulerRotation.Config.UNITY).toQuaternion()) + } + + @Test + //@Ignore // Failing for now + fun testToFromEulerUnreal() { + assertEqualsFloat(quat, quat.toEuler(EulerRotation.Config.UNREAL).toQuaternion()) + } + + @Test + fun testToFromEulerLibgdx() { + assertEqualsFloat(quat, quat.toEuler(EulerRotation.Config.LIBGDX).toQuaternion()) + } + + @Test + fun testToFromEulerThreejs() { + assertEqualsFloat(quat, quat.toEuler(EulerRotation.Config.THREEJS).toQuaternion()) + } + + @Test + //@Ignore // Not passing because EulerRotation not working in ZXY-left + fun testLookRotation() { + assertEqualsFloat( + EulerRotation(0.degrees, 270.degrees, 0.degrees), + Quaternion.lookRotation(Vector3.LEFT, Vector3.UP).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + assertEqualsFloat( + EulerRotation(0.degrees, (90).degrees, 0.degrees), + Quaternion.lookRotation(Vector3.RIGHT).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + assertEqualsFloat(Quaternion(0f, 0f, 0f, 1f), Quaternion.IDENTITY) + assertEqualsFloat(Quaternion(0f, 1f, 0f, 0f), Quaternion.lookRotation(Vector3.BACK)) + assertEqualsFloat( + EulerRotation(0.degrees, (180).degrees, 0.degrees), + Quaternion.lookRotation(Vector3.BACK).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + //println("Quaternion.fromVectors(Vector3.UP, Vector3.FORWARD)=${Quaternion.fromVectors(Vector3.UP, Vector3.FORWARD)}") + assertEqualsFloat( + EulerRotation(90.degrees, 0.degrees, 0.degrees), + Quaternion.fromVectors(Vector3.UP, Vector3.FORWARD).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + } + + @Test + @Ignore // Not working for now + fun testLookRotation2() { + assertEqualsFloat( + EulerRotation((-90).degrees, 0.degrees, 0.degrees), + Quaternion.lookRotation(Vector3.UP, Vector3.DOWN).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + assertEqualsFloat( + Quaternion(.5f, -.5f, -.5f, .5f), + Quaternion.lookRotation(Vector3.LEFT, Vector3.FORWARD), + ) + assertEqualsFloat( + EulerRotation((-90).degrees, 0.degrees, 90.degrees), + Quaternion.lookRotation(Vector3.LEFT, Vector3.FORWARD).toEuler(EulerRotation.Config.UNITY).normalized(), + ) + } }