Add interpolation to Quaternion

Author: dwursteisen

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

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)
+}

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)
+    }
+}

…uriouscreature/kotlin/math/MatrixTest.kt …uriouscreature/kotlin/math/MatrixTest.ktsrc/jvmTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt renamed to src/commonTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt src/jvmTest/kotlin/com/curiouscreature/kotlin/math/MatrixTest.kt renamed to 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),

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)
-            }
-        }
     }
 }