From 221efd9fbcd6dfad54b475725e7c485e3b123894 Mon Sep 17 00:00:00 2001 From: David Wursteisen Date: Sat, 7 Mar 2020 15:02:54 +0100 Subject: [PATCH] Add interpolation to Quaternion --- .../com/curiouscreature/kotlin/math/Matrix.kt | 62 +++++++++++++++++++ .../curiouscreature/kotlin/math/Quaternion.kt | 19 ++++++ .../curiouscreature/kotlin/math/Assertions.kt | 21 +++++++ .../curiouscreature/kotlin/math/MatrixTest.kt | 23 +++++-- .../kotlin/math/QuaternionTest.kt | 13 ---- .../curiouscreature/kotlin/math/Assertions.kt | 6 -- 6 files changed, 120 insertions(+), 24 deletions(-) create mode 100644 src/commonTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt rename src/{jvmTest => commonTest}/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt (95%) delete mode 100644 src/jvmTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt diff --git a/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Matrix.kt b/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Matrix.kt index e8d90c4..ad6895a 100644 --- a/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Matrix.kt +++ b/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Matrix.kt @@ -239,6 +239,68 @@ data class Mat4( } fun identity() = Mat4() + + fun from(quaternion: Quaternion): Mat4 = quaternion.let { + val xx: Float = it.x * it.x + val xy: Float = it.x * it.y + val xz: Float = it.x * it.z + val xw: Float = it.x * it.w + val yy: Float = it.y * it.y + val yz: Float = it.y * it.z + val yw: Float = it.y * it.w + val zz: Float = it.z * it.z + val zw: Float = it.z * it.w + val mx = Float4( + 1f - 2f * (yy + zz), + 2f * (xy - zw), + 2f * (xz + yw), + 0f + ) + val my = Float4( + 2f * (xy + zw), + 1f - 2f * (xx + zz), + 2f * (yz - xw), + 0f + ) + val mz = Float4( + 2f * (xz - yw), + 2f * (yz + xw), + 1f - 2f * (xx + yy), + 0f + ) + val mw = Float4( + 0f, + 0f, + 0f, + 1f + ) + return Mat4( + x = mx, + y = my, + z = mz, + w = mw + ) + /* + // Set matrix from quaternion + matrix.get(Matrix4.M00) = 1 - 2 * (yy + zz) + matrix.get(Matrix4.M01) = 2 * (xy - zw) + matrix.get(Matrix4.M02) = 2 * (xz + yw) + matrix.get(Matrix4.M03) = 0 + matrix.get(Matrix4.M10) = 2 * (xy + zw) + matrix.get(Matrix4.M11) = 1 - 2 * (xx + zz) + matrix.get(Matrix4.M12) = 2 * (yz - xw) + matrix.get(Matrix4.M13) = 0 + matrix.get(Matrix4.M20) = 2 * (xz - yw) + matrix.get(Matrix4.M21) = 2 * (yz + xw) + matrix.get(Matrix4.M22) = 1 - 2 * (xx + yy) + matrix.get(Matrix4.M23) = 0 + matrix.get(Matrix4.M30) = 0 + matrix.get(Matrix4.M31) = 0 + matrix.get(Matrix4.M32) = 0 + matrix.get(Matrix4.M33) = 1 + + */ + } } inline var right: Float3 diff --git a/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Quaternion.kt b/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Quaternion.kt index 8e19106..5e29e4c 100644 --- a/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Quaternion.kt +++ b/src/commonMain/kotlin/com/curiouscreature/kotlin/math/Quaternion.kt @@ -62,3 +62,22 @@ fun normalize(quaternion: Quaternion): Quaternion { val mag = sqrt(w * w + x * x + y * y + z * z) return Quaternion(x / mag, y / mag, z / mag, w / mag) } + +fun interpolate(a: Quaternion, b: Quaternion, blend: Float): Quaternion { + val dot = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z + val blendI = 1f - blend + val result = if (dot < 0) { + val w = blendI * a.w + blend * -b.w + val x = blendI * a.x + blend * -b.x + val y = blendI * a.y + blend * -b.y + val z = blendI * a.z + blend * -b.z + Quaternion(x, y, z, w) + } else { + val w = blendI * a.w + blend * b.w + val x = blendI * a.x + blend * b.x + val y = blendI * a.y + blend * b.y + val z = blendI * a.z + blend * b.z + Quaternion(x, y, z, w) + } + return normalize(result) +} diff --git a/src/commonTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt new file mode 100644 index 0000000..4103ca1 --- /dev/null +++ b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt @@ -0,0 +1,21 @@ +package com.curiouscreature.kotlin.math + +import kotlin.math.abs +import kotlin.test.fail + +fun assertEquals( + expected: Float, + actual: Float, + delta: Float = 0.000001f, + message: String = "$expected != $actual (𝝙 $delta)" +) { + if (abs(expected - actual) > delta) { + fail(message) + } +} + +fun assertArrayEquals(expected: FloatArray, actual: FloatArray, delta: Float = 0.0001f) { + expected.zip(actual).forEach { + assertEquals(it.first, it.second, delta) + } +} diff --git a/src/jvmTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt similarity index 95% rename from src/jvmTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt rename to src/commonTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt index 476162e..1298e97 100644 --- a/src/jvmTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt +++ b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt @@ -16,8 +16,9 @@ package com.curiouscreature.kotlin.math +import kotlin.test.Test import kotlin.test.assertEquals -import org.junit.Test +import kotlin.test.assertFails class MatrixTest { @Test @@ -32,9 +33,11 @@ class MatrixTest { ) } - @Test(expected = IllegalArgumentException::class) + @Test fun `Mat3 of fails if less than 9 arguments`() { - Mat3.of(*8.floatArray()) + assertFails { + Mat3.of(*8.floatArray()) + } } @Test @@ -55,9 +58,11 @@ class MatrixTest { ) } - @Test(expected = IllegalArgumentException::class) + @Test fun `Mat4 of fails if less than 16 arguments`() { - Mat4.of(*15.floatArray()) + assertFails { + Mat4.of(*15.floatArray()) + } } @Test @@ -339,6 +344,14 @@ class MatrixTest { ) } + @Test + fun fromQuaternion() { + val fromQuaternion = Mat4.from(Quaternion(-0.5f, 0f, -0.5f, 0.70710677f)) + val fromRotation = rotation(normalize(Float3(1f, 0f, 1f)), 90f) + + assertArrayEquals(fromQuaternion.toFloatArray(), fromRotation.toFloatArray()) + } + companion object { private val MAT_3 = Mat3( Float3(1f, 4f, 7f), diff --git a/src/commonTest/kotlin/com/curiouscreature/kotlin/math/QuaternionTest.kt b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/QuaternionTest.kt index 71d1b6a..b67e8ab 100644 --- a/src/commonTest/kotlin/com/curiouscreature/kotlin/math/QuaternionTest.kt +++ b/src/commonTest/kotlin/com/curiouscreature/kotlin/math/QuaternionTest.kt @@ -1,9 +1,7 @@ package com.curiouscreature.kotlin.math -import kotlin.math.abs import kotlin.test.Test import kotlin.test.assertTrue -import kotlin.test.fail class QuaternionTest { @@ -48,16 +46,5 @@ class QuaternionTest { assertEquals(expected.z, actual.z) assertEquals(expected.w, actual.w) } - - fun assertEquals( - expected: Float, - actual: Float, - delta: Float = 0.000001f, - message: String = "$expected != $actual (𝝙 $delta)" - ) { - if (abs(expected - actual) > delta) { - fail(message) - } - } } } diff --git a/src/jvmTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt b/src/jvmTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt deleted file mode 100644 index 77611d5..0000000 --- a/src/jvmTest/kotlin/com/curiouscreature/kotlin/math/Assertions.kt +++ /dev/null @@ -1,6 +0,0 @@ -package com.curiouscreature.kotlin.math - -import org.junit.Assert - -fun assertArrayEquals(expected: FloatArray, actual: FloatArray, delta: Float = 0.0001f) = - Assert.assertArrayEquals(expected, actual, delta)